黑洞飞控代码分析（三）------姿态解算和控制

在收集需要的数据后，进行处理，解算姿态

在进入之前先看一看系统时钟的中断函数

/*
* 函数名：SysTick_Handler
* 描述  ：滴答中断函数
* 输入  ：无
* 输出  ：无
*/
void SysTick_Handler(void)
{
static u16 cnt = 0;
static u8 ms = 0;

cnt++;

Prepare_Data();  //准备数据
if(Is_WFLY_Unlock())//判断是否解锁与是否在执行解锁动作
{
Is_WFLY_Lock();		//判断是否在执行上锁动作
IMU_Update();			//姿态更新频率为1KHz
ms++;
if(ms == 2)	 			//控制频率500Hz
{
ms = 0;
WFLY_PWM_Convert();
Control(Angle,Exp_Angle);
}
if(cnt % 500 == 0)
{
LED_LEFT_BACK_TOGGLE;
LED_RIGHT_BACK_TOGGLE;
cnt = 0;
}
}
else//未解锁
{
Motor_PWM_Update(0,0,0,0);//确保安全
if(cnt % 1000 == 0)
{
LED_LEFT_BACK_TOGGLE;
LED_RIGHT_BACK_TOGGLE;
cnt = 0;
}
}

}

现在到了判断if(Is_WFLY_Unlock())//判断是否解锁与是否在执行解锁动作

怎么判断呢?

u8 unlock_flag = 0 ;					//解锁标志位，为0未解锁，为1解锁
extern vs16 throttle;
extern S_FLOAT_XYZ Exp_Angle;
extern u16 Rc_Channel_Val[6];

/*
* 函数名：Is_WFLY_Unlock
* 描述  ：判断是否执行解锁动作
* 输入  ：无
* 输出  ：0：未解锁；1：解锁成功
*/
u8 Is_WFLY_Unlock(void)
{
static u16	cnt_unlock = 0;

if( !unlock_flag )	//如果未解锁,只有在未解锁的清况才能解锁
{
if((Rc_Channel_Val[0]	<	1200) && (Rc_Channel_Val[1] < 1200) )			//左摇杆打到右下角
{
if(((Rc_Channel_Val[2]	<	1200) && (Rc_Channel_Val[3]) > 1800) )//右摇杆打到左下角
{       类似大疆的外八解锁
cnt_unlock++;
if( (cnt_unlock % 20) == 0)								 //解锁过程中，每隔20ms闪烁一次
{
LED_LEFT_BACK_TOGGLE;//左边LED闪烁
LED_RIGHT_BACK_TOGGLE;
}
}
}
else cnt_unlock = 0;													 //如果未解锁或解锁中断，则解锁时间计数清零

if( cnt_unlock > 1000)												 //解锁动作持续1秒，则解锁
{
cnt_unlock = 0;
SysTick->CTRL &= ~ SysTick_CTRL_ENABLE_Msk;//关闭中断
AHRS_Date_Init(); 										 	 //每次解锁后都先初始化导航数据
MPU6500_Date_Offset(5000);								 //校准陀螺仪
SysTick->CTRL |=  SysTick_CTRL_ENABLE_Msk; //开启中断
unlock_flag = 1;													 //成功解锁标志
return 1;
}
else return 0;
}
else return 1;//如果解锁了，则返回1
}

其中说一下

#define  LED_LEFT_BACK_TOGGLE   GPIOC->ODR ^= GPIO_Pin_13;//a与b的对应位进行异或运算
#define  LED_RIGHT_BACK_TOGGLE  GPIOB->ODR ^= GPIO_Pin_12;

而另外有定义

#define GPIO_Pin_12                ((uint16_t)0x1000)  /*!< Pin 12 selected */
#define GPIO_Pin_13                ((uint16_t)0x2000)  /*!< Pin 13 selected */

当掰杆计时达到1000以后，先关闭中断，先初始化导航数据

extern S_FLOAT_XYZ Exp_Angle;

/*
* 函数名：AHRS_Date_Init
* 描述  ：航姿数据初始化
* 输入  ：无
* 输出  ：无
*/
void AHRS_Date_Init(void)
{
int cnt;

for(cnt	=0;cnt	<FILTER_LENGTH;cnt++)//FILTER_LENGTH=20

{
Acc_Buf[cnt].X = 0;
Acc_Buf[cnt].Y = 0;
Acc_Buf[cnt].Z = 0;
}

q0 = 1;
q1 = 0;
q2 = 0;
q3 = 0;

q0_yaw = 1;
q1_yaw = 0;
q2_yaw = 0;
q3_yaw = 0;

Angle.X = 0;
Angle.Y = 0;
Angle.Z = 0;

Exp_Angle.X = 0;
Exp_Angle.Y = 0;
Exp_Angle.Z = 0;
}

其中有结构体S_FLOAT_XYZ

typedef struct
{
float X;
float Y;
float Z;
}S_FLOAT_XYZ;

接着进入陀螺仪校准MPU6500_Date_Offset，这里校准5000次说的是采集5000次姿态数据除了机体Z轴方向的加速度

/*
* 函数名：MPU6500_Date_Offset
* 描述  ：MPU6500数据校准
* 输入  ：校准次数
* 输出  ：无
*/
void MPU6500_Date_Offset(u16 cnt)
{
static S_INT32_XYZ Temp_Gyro , Temp_Acc;
int i = 0;

Temp_Gyro.X =	0;
Temp_Gyro.Y =	0;
Temp_Gyro.Z =	0;

Temp_Acc.X = 0;
Temp_Acc.Y = 0;
Temp_Acc.Z = 0;

offset_flag = 1;//进入MPU6500校准模式
for(i = cnt; i > 0; i--)
{
MPU6500_ReadValue();

Temp_Acc.X	+=	MPU6500_Acc.X;
Temp_Acc.Y	+=	MPU6500_Acc.Y;
//Temp_Acc.Z	+=	MPU6500_Acc.Z;
Temp_Gyro.X	+=	MPU6500_Gyro.X;
Temp_Gyro.Y	+=	MPU6500_Gyro.Y;
Temp_Gyro.Z	+=	MPU6500_Gyro.Z;

}

MPU6500_Acc_Offset.X 	=	Temp_Acc.X	/	cnt;
MPU6500_Acc_Offset.Y 	=	Temp_Acc.Y	/	cnt;
//MPU6500_Acc_Offset.Z  =	Temp_Acc.Z	/	cnt;
MPU6500_Gyro_Offset.X	= Temp_Gyro.X	/	cnt;
MPU6500_Gyro_Offset.Y	= Temp_Gyro.Y	/	cnt;
MPU6500_Gyro_Offset.Z =	Temp_Gyro.Z	/	cnt;

offset_flag = 0;//退出MPU6500校准模式
}

意思就是从飞机放平MPU6500里读取5000次加速度和陀螺仪数据求和取平均然后做为MPU6500的零偏

再打开中断

到此为止也就解锁完成，回到系统时钟的中断函数接下来有Is_WFLY_Lock();判断是否在执行上锁动作

具体定义

/*
* 函数名：Is_WFLY_Unlock
* 描述  ：判断是否执行上锁动作
* 输入  ：无
* 输出  ：0：上锁成功；1：未上锁
*/
u8 Is_WFLY_Lock(void)
{
static u16	cnt_lock = 0;
if( unlock_flag )	//如果没有上锁
{
if((Rc_Channel_Val[0]	> 1800) && (Rc_Channel_Val[1] < 1200) )//左摇杆打到左下角
{
cnt_lock++;
if( (cnt_lock % 20) == 0)//上锁过程中，每隔40ms闪烁一次
{
LED_LEFT_BACK_TOGGLE;
LED_RIGHT_BACK_TOGGLE;
}
}
else cnt_lock = 0;		//如果未解锁或上锁中断，则解锁时间计数清零

if( cnt_lock == 500)	//上锁动作持续500ms，则上锁
{
cnt_lock = 0;
unlock_flag = 0;	//成功上锁标志
return 1;
}
else return 0;
}
else return 1;					//如果已经上锁了，则返回1
}

左摇杆打到左下角动作持续500ms上锁

接下来姿态更新，也就是姿态解算

IMU_Update();			//姿态更新频率为1KHz

定义有：

float ex_int = 0, ey_int = 0, ez_int = 0;   //X、Y、Z轴的比例误差
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    //定义四元素
float q0_yaw = 1, q1_yaw = 0, q2_yaw = 0, q3_yaw = 0;    //弥补Mahony算法在无地磁情况解算Yaw轴满足不了大扰动要求的现象

/*
* 函数名：IMU_Update
* 描述  ：姿态数据更新
* 输入  ：无
* 输出  ：无
*/
void IMU_Update(void)
{
float norm;
float gx = MPU6500_Gyro.X	*	GYRO_GR,gy = MPU6500_Gyro.Y	*	GYRO_GR,gz = MPU6500_Gyro.Z	*	GYRO_GR;
float ax = Acc_Avg.X,ay = Acc_Avg.Y,az = Acc_Avg.Z;

float q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q1q1 = q1 * q1;
float q1q3 = q1 * q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
float vx, vy, vz;
float ex, ey, ez;

float	q0_yawq0_yaw = q0_yaw * q0_yaw;
float	q1_yawq1_yaw = q1_yaw * q1_yaw;
float	q2_yawq2_yaw = q2_yaw * q2_yaw;
float	q3_yawq3_yaw = q3_yaw * q3_yaw;
float	q1_yawq2_yaw = q1_yaw * q2_yaw;
float	q0_yawq3_yaw = q0_yaw * q3_yaw;

//**************************Yaw轴计算******************************

//Yaw轴四元素的微分方程，先单独解出yaw的姿态
q0_yaw = q0_yaw + (-q1_yaw * gx - q2_yaw * gy - q3_yaw * gz) * SAMPLE_HALF_T;SAMPLE_HALF_T是采样时间的一半
q1_yaw = q1_yaw + (q0_yaw * gx + q2_yaw * gz - q3_yaw * gy) * SAMPLE_HALF_T;
q2_yaw = q2_yaw + (q0_yaw * gy - q1_yaw * gz + q3_yaw * gx) * SAMPLE_HALF_T;
q3_yaw = q3_yaw + (q0_yaw * gz + q1_yaw * gy - q2_yaw * gx) * SAMPLE_HALF_T;

//规范化Yaw轴四元数
norm = sqrt(q0_yawq0_yaw + q1_yawq1_yaw + q2_yawq2_yaw + q3_yawq3_yaw);
q0_yaw = q0_yaw / norm;
q1_yaw = q1_yaw / norm;
q2_yaw = q2_yaw / norm;
q3_yaw = q3_yaw / norm;

if(ax * ay * az	== 0)//如果加速度数据无效，或者自由坠落，不结算
return ;

//规范化加速度计值
norm = sqrt(ax * ax + ay * ay + az * az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;

//估计重力方向和流量/变迁，重力加速度在机体系的投影
vx = 2 * (q1q3 - q0q2);
vy = 2 * (q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ;

//向量外积再相减得到差分就是误差
ex = (ay * vz - az * vy) ;
ey = (az * vx - ax * vz) ;
ez = (ax * vy - ay * vx) ;

//对误差进行PI计算
ex_int = ex_int + ex * IMU_KI;
ey_int = ey_int + ey * IMU_KI;
ez_int = ez_int + ez * IMU_KI;

//校正陀螺仪
gx = gx + IMU_KP * ex + ex_int;
gy = gy + IMU_KP * ey + ey_int;
gz = gz + IMU_KP * ez + ez_int;

//四元素的微分方程
q0 = q0 + (-q1 * gx - q2	*	gy - q3	*	gz)	*	SAMPLE_HALF_T;
q1 = q1 + (q0	*	gx + q2	*	gz - q3	*	gy)	*	SAMPLE_HALF_T;
q2 = q2 + (q0	*	gy - q1	*	gz + q3	*	gx)	*	SAMPLE_HALF_T;
q3 = q3 + (q0	*	gz + q1	*	gy - q2	*	gx)	*	SAMPLE_HALF_T;

//规范化Pitch、Roll轴四元数
norm = sqrt(q0q0 + q1q1 + q2q2 + q3q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;

//求解欧拉角
Angle.X = atan2(2 * q2q3 + 2 * q0q1, -2 * q1q1 - 2 * q2q2 + 1) * 57.3f;
Angle.Y = asin(-2 * q1q3 + 2 * q0q2) * 57.3f;
Angle.Z = atan2(2 * q1_yawq2_yaw + 2 * q0_yawq3_yaw, -2 * q2_yawq2_yaw - 2 * q3_yawq3_yaw + 1)	* 57.3f;

}

总的来说这里先结算偏航的四元素数再解算俯仰和横滚角，为什么？这样算法依据？

然后

ms++;
if(ms == 2)	 			//控制频率500Hz

这两句代码将中断的1KHZ转化成控制的频率500HZ

下面接收遥控信号结算成期望的欧拉角

volatile float angle_z;
/*
* 函数名：WFLY_PWM_Convert
* 描述  ：遥控器信号转换
* 输入  ：无
* 输出  ：无
*/
void WFLY_PWM_Convert(void)
{
static u16 cnt;

cnt++;

Exp_Angle.X = (float)(Rc_Channel_Val[2]- 1500) * 0.07f;
if((Exp_Angle.X > -2) && (Exp_Angle.X < 2))	Exp_Angle.X = 0;

Exp_Angle.Y = (float)(Rc_Channel_Val[3]- 1500) * 0.07f;
if((Exp_Angle.Y > -2) && (Exp_Angle.Y < 2))	Exp_Angle.Y = 0;

throttle	=	(vs16)(Rc_Channel_Val[1] - 1000) * 2;
if(cnt == 20)
{
cnt = 0;
if((Rc_Channel_Val[0] > 1600) || (Rc_Channel_Val[0] < 1400))//防止死区累计误差
{
if(throttle > LAUNCH_THROTTLE)	Exp_Angle.Z += -(float)(Rc_Channel_Val[0]- 1500) * 0.006f;
}
if((Exp_Angle.Z > 180) && (Angle.Z > 0))
{
angle_z = Angle.Z - 360;
}
else if((Exp_Angle.Z < -180) && (Angle.Z < 0))
{
angle_z = Angle.Z + 360;
}
else angle_z = Angle.Z;

if(Exp_Angle.Z > 360)  Exp_Angle.Z = (float)((s32)Exp_Angle.Z % 360);
if(Exp_Angle.Z < -360) Exp_Angle.Z = (float)((s32)Exp_Angle.Z % - 360);
}
}

这里2通道横滚，3通道俯仰，1通道油门，0通道偏航

下面就是控制了

volatile u16 motor[5];这里应该是4而不是5；

vs16 throttle = 0;

PID PID_Roll_Angle,PID_Pitch_Angle,PID_Yaw_Angle;	//外环：角度PID环
PID PID_Roll_Rate,PID_Pitch_Rate,PID_Yaw_Rate;		//内环：角速度PID环
S_FLOAT_XYZ Exp_Angle;

extern u8 unlock_flag;//解锁标志位，为0未解锁，为1解锁
extern volatile float angle_z;

/*
* 函数名：Control
* 描述  ：根据姿态信息控制四旋翼
* 输入  ：Now_Angle：当前姿态；Exp_Angle：期望姿态
* 输出  ：无
*/
void Control(S_FLOAT_XYZ	Now_Angle, S_FLOAT_XYZ Exp_Angle)
{
//期望角+测量角，两者符号相反，实质为偏差
float rol = Exp_Angle.Y + Now_Angle.Y;
float pit = Exp_Angle.X + Now_Angle.X;
float yaw = Exp_Angle.Z + angle_z;

PID_Roll_Angle.Pout = PID_Roll_Angle.P * rol;//误差乘以比例项
PID_Pitch_Angle.Pout = PID_Pitch_Angle.P * pit;//
PID_Yaw_Angle.Pout = PID_Yaw_Angle.P * yaw;

PID_Pitch_Angle.Dout = PID_Pitch_Angle.D * MPU6500_Gyro.X;//误差乘以微分项
PID_Roll_Angle.Dout = PID_Roll_Angle.D * MPU6500_Gyro.Y;
PID_Yaw_Angle.Dout = PID_Yaw_Angle.D * MPU6500_Gyro.Z;

PID_Roll_Angle.Out = PID_Roll_Angle.Pout + PID_Roll_Angle.Iout + PID_Roll_Angle.Dout;//PD控制输出
PID_Pitch_Angle.Out = PID_Pitch_Angle.Pout + PID_Pitch_Angle.Iout + PID_Pitch_Angle.Dout;
PID_Yaw_Angle.Out = PID_Yaw_Angle.Pout + PID_Yaw_Angle.Iout + PID_Yaw_Angle.Dout;

if((throttle	>	LAUNCH_THROTTLE) && (unlock_flag) && (Is_WFLY_Connected()))//当油门大于100，解锁了，且遥控器连接正常情况下
{
motor[1] = ADD_THROTTLE + throttle - PID_Roll_Angle.Out - PID_Pitch_Angle.Out - PID_Yaw_Angle.Out;
motor[2] = ADD_THROTTLE + throttle - PID_Roll_Angle.Out + PID_Pitch_Angle.Out + PID_Yaw_Angle.Out;
motor[3] = ADD_THROTTLE + throttle + PID_Roll_Angle.Out + PID_Pitch_Angle.Out - PID_Yaw_Angle.Out;
motor[4] = ADD_THROTTLE + throttle + PID_Roll_Angle.Out - PID_Pitch_Angle.Out + PID_Yaw_Angle.Out;
}//ADD_THROTTLE=1500

else
{
motor[1]  = 0;
motor[2]  = 0;
motor[3]  = 0;
motor[4]  = 0;
}

Motor_PWM_Update(motor[1] ,motor[2] ,motor[3] ,motor[4]);//pwm输出}

在进入这个函数之前首先要明确几个定义

volatile u16 motor[5];

其中volatile提醒编译器它后面所定义的变量随时都有可能改变，因此编译后的程序每次需要存储或读取这个变量的时候，都会直接从变量地址中读取数据。如果没有volatile关键字，则编译器可能优化读取和存储，可能暂时使用寄存器中的值，如果这个变量由别的程序更新了的话，将出现不一致的现象。

PID PID_Roll_Angle,PID_Pitch_Angle,PID_Yaw_Angle;	//外环：角度PID环
PID PID_Roll_Rate,PID_Pitch_Rate,PID_Yaw_Rate;		//内环：角速度PID环

PID是个结构体。

typedef struct PID{float P,Pout,I,Iout,D,Dout,I_Max,Out;}PID;

另外

S_FLOAT_XYZ Exp_Angle;

在控制初始化过程中

PID_Roll_Angle.P = 14;
PID_Roll_Angle.I = 0;
PID_Roll_Angle.D = 0.25;

PID_Pitch_Angle.P = 14;
PID_Pitch_Angle.I = 0;
PID_Pitch_Angle.D = 0.25;

PID_Yaw_Angle.P = 20;
PID_Yaw_Angle.I = 0;
PID_Yaw_Angle.D = 1;

积分项等于0；其实就是PD控制

输出PWM

/*
* 函数名：Motor_PWM_Update
* 描述  ：电机PWM输出更新函数
* 输入  ：motorx_pwm：电机PWM输入值
* 输出  ：无
*/
void Motor_PWM_Update(s16 motor1_pwm,s16 motor2_pwm,s16 motor3_pwm,s16 motor4_pwm)
{
if(motor1_pwm	>	MOTOR_PWM_MAX)	motor1_pwm = MOTOR_PWM_MAX;//限幅
if(motor2_pwm	>	MOTOR_PWM_MAX)	motor2_pwm = MOTOR_PWM_MAX;
if(motor3_pwm	>	MOTOR_PWM_MAX)	motor3_pwm = MOTOR_PWM_MAX;
if(motor4_pwm	>	MOTOR_PWM_MAX)	motor4_pwm = MOTOR_PWM_MAX;

if(motor1_pwm	<	0)	motor1_pwm = 0;
if(motor2_pwm	<	0)	motor2_pwm = 0;//限幅
if(motor3_pwm	<	0)	motor3_pwm = 0;
if(motor4_pwm	<	0)	motor4_pwm = 0;

TIM4->CCR2 = motor1_pwm;//控制占空比，定时器4输出pwm
TIM4->CCR3 = motor2_pwm;
TIM4->CCR4 = motor3_pwm;
TIM4->CCR1 = motor4_pwm;
}

下面会带系统时钟的中断函数

if(cnt % 500 == 0)
{
LED_LEFT_BACK_TOGGLE;
LED_RIGHT_BACK_TOGGLE;
cnt = 0;
}

当cnt =500时，亮灯，cnt =0；

当未解锁时：

else//未解锁
{
Motor_PWM_Update(0,0,0,0);//确保安全pwm掌控比为0
if(cnt % 1000 == 0)
{
LED_LEFT_BACK_TOGGLE;
LED_RIGHT_BACK_TOGGLE;
cnt = 0;
}
}

系统时钟中断函数到此结束。

下一篇分析飞控遥控接收。