STM32实现水下四旋翼（五）自定义航行数据

一. 绪论

这一篇算作番外篇。只是在写其他章节的时候绕不开很多自定义数据类型，所以干脆另成一篇。

为什么要自定义数据类型（准确来说就是结构体），因为一方面有其物理意义，一方面非常便于编程，大大提高代码的可读性（可以理解为C++的对象）。水下四旋翼（包括空中四旋翼）在航行过程中有大量的数据需要处理，如果单独定义为一个个的变量会非常复杂。所以按照物理意义和功能，定义为不同类型的数据（结构体），有相近物理意义和功能的数据作为结构体的成员，新增数据只需在结构体中增加成员即可，开发效率会大大提高。

此系列博文的开发过程中参考了正点原子的开源小四轴的源码，尤其是本文的数据类型定义部分很多沿用了里面的命名。在此说明，同时表示致谢（真的是宝藏卖家呀呀呀~~~）。

二. 航行数据类型声明与定义

1. 系统参数

系统参数包括所有的需要调参的数据，主要就是pid参数。

建立一个pid.h和pid.c文件。

系统设置相关的结构体类型定义在pid.h中，如下。可以看到经过层层嵌套，我们最后只需要使用configParam_t这个结构体就行了。它包含了机器人运行所有的PID数据，包括定深航行时的油门基准值（这个后面专门讲解）。

#ifndef __PID_H
#define __PID_H
#include "sys.h"
#include "stdbool.h"

typedef struct
{
	float kp;
	float ki;
	float kd;
} pidInit_t;

typedef struct
{
	pidInit_t roll;
	pidInit_t pitch;
	pidInit_t yaw;
} pidParam_t;

typedef struct
{
	pidInit_t vx;
	pidInit_t vy;
	pidInit_t vz;
} pidParamPos_t;

typedef struct
{
	pidParam_t pidAngle;  /*角度PID*/
	pidParam_t pidRate;	  /*角速度PID*/
	pidParamPos_t pidPos; /*位置PID*/
	float thrustBase;		  /*油门基础值*/
	u8 cksum;
} configParam_t;

extern configParam_t configParam;
extern configParam_t configParamDefault;

#endif /* __PID_H */

pid.c文件中定义结构体变量。定义了两个configParam_t 类型的数据，其中configParam没有初始化，后面通过上位机更改参数就依靠这个变量了。configParamDefault 初始化了，这是默认的参数，将来调整参数完毕可以更新到这里，作为默认参数写入flash。

#include "pid.h"

configParam_t configParam;
configParam_t configParamDefault =
{
	.pidAngle=	/*角度PID*/
	{	
		.roll=
		{
			.kp=3.0,
			.ki=0.0,
			.kd=0.0,
		},
		.pitch=
		{
			.kp=3.0,
			.ki=0.0,
			.kd=0.0,
		},
		.yaw=
		{
			.kp=3.0,
			.ki=0.0,
			.kd=0.0,
		},
	},	
	.pidRate=	/*角速度PID*/
	{	
		.roll=
		{
			.kp=30.0,
			.ki=0.2,
			.kd=0.0,
		},
		.pitch=
		{
			.kp=30.0,
			.ki=0.2,
			.kd=0.0,
		},
		.yaw=
		{
			.kp=30.0,
			.ki=0.2,
			.kd=0.0,
		},
	},	
	.pidPos=	/*位置PID*/
	{	
		.vx=
		{
			.kp=0.0,
			.ki=0.0,
			.kd=0.0,
		},
		.vy=
		{
			.kp=0.0,
			.ki=0.0,
			.kd=0.0,
		},
		.vz=
		{
			.kp=35.0,
			.ki=0.1,
			.kd=0.0,
		},
	},
	.thrustBase = -2000.0,
};

当然了，pid.c和pid.h还有重要的功能，就是PID计算，这个我暂时省略了，后面讲PID章节会加入相关内容。

2. 状态参数

状态参数包括机器人获取的传感器信息、机器人的实际状态。

建立一个stabilizer.h和stabilizer.c文件，stabilizer.h中添加如下内容：

#ifndef __STABILIZER_TYPES_H
#define __STABILIZER_TYPES_H
#include "sys.h"

#if defined(__CC_ARM)
	#pragma anon_unions
#endif

struct vec3_s
{
	float x;
	float y;
	float z;
};

typedef struct vec3_s positon_t;
typedef struct vec3_s velocity_t;
typedef struct vec3_s acc_t;

//姿态集
typedef struct
{
	float roll;
	float pitch;
	float yaw;
} attitude_t;

typedef struct
{
	Axis3f acc;
	Axis3f gyro;
	Axis3f mag;
	positon_t position;  
	float height;
	float depth;  
	float voltage;
	float current;
	float water_temp;
	float water_press;
	float air_press_robot;		// 这个值是 机器人 舱内的压力
	float imu_temp;
} sensorData_t;

typedef struct
{
	attitude_t realAngle;
	positon_t position;
	velocity_t velocity;
	acc_t acc;
	attitude_t realRate;
	float realDepth;
	float preDepth;
} state_t;

typedef struct
{
	attitude_t expectedAngle;
	positon_t position;
	velocity_t velocity;
	attitude_t expectedRate;
	float expectedDepth;
	float thrust;
	bool isAltHold;
} setstate_t;

// 控制量，速度环输出
typedef struct
{
	s16 roll;
	s16 pitch;
	s16 yaw;
	float thrust;			// 油门值 针对四轴
	float forward_thrust;
	float hightOut;
	float depthOut;
	bool isLandMode;
	bool preMode;
} control_t;  

// 四个电机的油门值
typedef struct 
{
	u16 m1;
	u16 m2;
	u16 m3;
	u16 m4;
}motorPWM_t;

3. 控制参数

控制参数是设置量，包括遥控器和上位机设置的机器人目标位置，模式等，以及各个环pid计算出的控制量等。

从命名可以很容易理解，sensorData_t是所有传感器的数据（已经从原始数据计算、滤波处理），state_t是机器人当前状态，包括姿态角、位置、速度等，setstate_t是机器人的期望状态，也包括机器人的姿态角、位置、速度等，同时还有油门、是否是定深模式等。control_t是控制量，由PID计算出，将会直接作用到电机上控制电机运动。motorPWM_t也是控制量，因为最后的控制量需要转化为PWM值用以控制无刷电机。

stabilizer.c中很简单，就是定义相应的结构体变量。

#include "stabilizer.h"

control_t control = {0};
setstate_t setstate = {0};
state_t state = {0};
sensorData_t sensorData = {0};

motorPWM_t motorPWM = {0};
motorPWM_t motorPWMSet = {0, 0, 0, 0};

本章完结。