7、微型四轴软件原理 - XDU-Educational-UAV/Drone_Master_PID GitHub Wiki

7.1 STM32G030 程序解读

7.1.1 时钟树配置

本项目使用STM32CubeMX配置时钟树，使用STM32CubeMX的一个好处就是图形化配置，CubeMX能自动检测时钟树配置的冲突并自动给出解决方案。在时钟配置方面，主要了解清楚高级外设总线（APB1、APB2）控制哪些外设。

首先配置RCC（复位和时钟控制），选择外部高速时钟，如下图所示：

image65

图7.1.1 STM32CubeMX时钟配置

通过查询MCU芯片手册，我们可以知道TIM1挂在APB2总线上，TIM2挂在APB1总线上。

进入时钟配置，STM32G030开发板外部晶振是8MHz的，在Input frequency 输入8，在HCLK 那里会提示最大频率168MHz，输入168，然后自动寻求配置，注意第一次配置出来的还是内部时钟HSI的，要选择为HSE，配置好后如下图。

image66

图7.1.2 STM32CubeMX时钟树配置

设置定时器，以TIM1为例，APB2时钟频率168MHz=168,000,000Hz，选取预分频系数(Prescaler)（16位存储，预分频系数最大65535）为16799，则定时器的时钟频率为168,000,000 / 16800 = 10000 Hz，频率为1万赫兹，选取计数周期(Counter Period，16位存储）为9999， 所以定时周期为1s 。

牢记如下公式：

时钟周期=时钟频率/((时钟预分频系数+1)*(计数器自动重装载值+1))

在这里计数器自动重装载值(AutoReload Register)等于计数周期(Counter Period)，因为计算机计数是从0开始的，因此这里我们需要给位于上式分母的两个值各加一。

同理TIM2也是同样计算，如果APB的时钟频率改变，一样安装上面的方法进行设置。

7.1.2 LED指示灯配置

通过搜索框找到引脚的位置，这里我们分别设置PB5，PB4，PC6，PA9为GPIO输出模式（即GPIO_Output），并输入用户标签分别为L1、L2、L3、L4（这里为了方便按照机臂LED顺序分别标注）。选中后，配置为输出模式后，该引脚变为绿色。

image67

图7.1.2 STM32CubeMX引脚配置

接下来分别对两个IO进行详细配置，点击左边的System Core，然后选择GPIO,这时候右边可以选中具体的引脚。我们分别配置PB5、PB4、PC6、PA9引脚的默认输出电平、GPIO工作模式、上下拉(这里配置为No pull-up and no pull-down)。

image68

图7.1.3 GPIO引脚配置

使用手机控制时需要 WiFi 摄像头模块的支持。有关 ATKP 格式的说明，大家可以参考《MiniFly 遥控器开发指南.pdf》。stabilizerTask 才是我们的重点，主要包括姿态解算和 PID 控制输出，下面我们就讲解一下姿态解算和 PID 算法流程。

7.2 姿态解算和PID算法流程图

微型四轴的姿态解算和 PID 算法流程图如图 6.3.1 所示：

image69

图 6.3.1 算法流程图

关于姿态解算，微型四轴采用互补滤波算法进行姿态解算，更新周期 500Hz。MCU 通过 IIC(模拟 IIC)读取加速计和陀螺仪数据寄存器，然后对加速计数据 IIR 低通滤波，对陀螺仪数据加偏置调整，然后对加计数据和陀螺数据进行融合，输出姿态数据（roll/pitch/yaw）, 互补滤波算法具体内容请看 7.4 节。

角度环 PID 控制器，更新周期 500Hz。期望角度来自手机蓝牙发送的指令，测量角度来自数据融合，期望角度减去测量角度得到偏差角度，这个偏差值作为角度环的输入，经过角度环 PID 后输出期望角速度，角度环 PID 详细内容请看 7.4 节。

角速度环 PID 控制器，更新周期 500Hz。测量角速度来自陀螺数据，期望角速度减去测量角速度得到一组偏差值，这组偏差值作为角速度环的输入，经过角速度环 PID 后输出的姿态控制量，用作控制电机，角速度环 PID 详细内容请看 7.5 节。

得到实际油门值和姿态控制量数据，我们就可以把油门值和姿态控制量数据整合，整合周期 1000Hz，然后通过控制 PWM 控制电机，从而控制四轴，关于油门值和姿态控制量数据融合详细内容请看 6.7 节。

7.3 基于四元数的姿态解算互补滤波算法

微型四轴的姿态解算算法采用四元数的互补滤波算法。在一个 IMU 系统中，一般集成有加速度计传感器、陀螺仪传感器、磁传感器等。目前常见的飞控系统中只使用一个姿态传感器芯片，这个芯片集成了加速度计、陀螺仪以及磁传感器，四轴使用一颗 6 轴传感器芯片 MPU6050。接下来说说如何把传感器数据通过四元数和欧拉角解算成飞行器的姿态。

姿态解算关键代码如下：

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) /*数据融合 互补滤波*/ 
{ float norm;
	float vx, vy, vz;
	float ex, ey, ez;
	
//四元数积分，求得当前的姿态
float q0_last=q0;	
float q1_last = q1;	
float q2_last = q2;	
float q3_last = q3;

下面把四元数换算成方向余弦中的第三行的三个元素。vecxZ、vecyZ、veczZ 就是上一次的欧拉角（四元数）的机体坐标参考系换算出来的重力的单位向量。

//把加速度计的三维向量转成单位向量
norm = invSqrt(ax*ax + ay*ay + az*az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;

//估计重力加速度方向在飞行器坐标系中的表示，为四元数表示的旋转矩阵的第三行
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

ax、ay、az 是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向量，vx、vy、vz 是根据陀螺仪数据推算出的重力向量，它们都是机体坐标参照系上的重力向量。

那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，ex、ey、ez 就是两个重力向量的叉积。

 //加速度计读取的方向与重力加速度方向的差值，用向量叉乘计算
ex = ay*vz - az*vy;
ey = az*vx - ax*vz;
ez = ax*vy - ay*vx;

这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。

用叉积误差来做 PI 修正陀螺零偏

//误差累积，已与积分常数相乘 exInt = exInt + exKi; eyInt = eyInt + eyKi; ezInt = ezInt + ez*Ki;

//用叉积误差来做PI修正陀螺零偏，即抵消陀螺读数中的偏移量	
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;

四元数微分方程，其中 q0_last 、q1_last 、q2_last 、q3_last分别为上一次的四元数值 q0、q1、q2、q3， halfT 为测量的半周期，gx、gy、gz为陀螺仪角速度，以下都是已知量，这里使用了一阶毕卡算法求解四元数微分方程：

//一阶近似算法，四元数运动学方程的离散化形式和积分
q0 = q0_last + (-q1_last*gx - q2_last*gy - q3_last*gz)*halfT;
q1 = q1_last + ( q0_last*gx + q2_last*gz - q3_last*gy)*halfT;
q2 = q2_last + ( q0_last*gy - q1_last*gz + q3_last*gx)*halfT;
q3 = q3_last + ( q0_last*gz + q1_last*gy - q2_last*gx)*halfT;

最后根据四元数方向余弦阵和欧拉角的转换关系，把四元数转换成欧拉角 pitch、roll、

yaw 以及去除重力加速度后的 Z 轴加速度：

//四元数规范化
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 * norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;
	
out_angle.yaw  +=  filter_gyro.z *RawData_to_Angle * 0.001f;
//四元数转欧拉角
void Get_Eulerian_Angle(struct _out_angle *angle)
{	
	angle->pitch = -atan2(2.0f*(q0*q1 + q2*q3),q0*q0 - q1*q1 - q2*q2 + q3*q3)*Radian_to_Angle;
	angle->roll  =  asin (2.0f*(q0*q2 - q1*q3))*Radian_to_Angle;
}

7.4 角度环 PID 和角速度环 PID

在讲角度 PID 之前，首先讲一下 PID 更新函数，PID 采用的标准 PID，其数学公式如下：

将数学公式转换为 C 代码，PID 更新函数是这样的：

float pidUpdate(PidObject* pid,  const float error) 
{ 
 	float output; 
 
 	pid->error = error;     	
 	pid->integ += pid->error * pid->dt;
  	if (pid->integ > pid->iLimit) 
 	{ 
 	 	pid->integ = pid->iLimit; 
 	} 
 	else if (pid->integ < pid->iLimitLow) 
 	{ 
 	        Pid->integ = pid->iLimitLow; 
 	}
 	pid->deriv = (pid->error - pid->prevError) / pid->dt; 
 	pid->outP = pid->kp * pid->error; 
 	pid->outI = pid->ki * pid->integ; 
 	pid->outD = pid->kd * pid->deriv; 
 	output = pid->outP + pid->outI + pid->outD; pid->prevError = pid->error; 
 	return output;
}

PidObject 为 PID 对象结构体数据类型，第一个参数为将被更新的 PID 结构体对象，第二个参数则是偏差（期望值-测量值），积分项为偏差对时间的积分，微分项则是偏差对时间的微分，然后函数里面有三个参数 pid->kp，pid->ki，pid->kd 分别指的是该 pid 对象的比例项，积分项和微分项系数，每个 pid 对象都有属于自己的 PID 系数，PID 初始化 pid 对象的时候会设定一组默认的系数，同时这组系数是可以调整的，我们常说的 PID 参数整定，其实就是调整这组系数，让它满足你的系统。

接下来我们说说角度环 PID，其函数原型如下

void Control_Angle(struct _out_angle *angle,struct _Rc *rc)
{
 	static struct _out_angle control_angle;
 	static struct _out_angle last_angle;
 	if(rc->ROLL>1490 && rc->ROLL<1510)	
		rc->ROLL=1500;
	if(rc->PITCH>1490 && rc->PITCH<1510)	
		rc->PITCH=1500;
	if(rc->AUX1>1495 && rc->AUX1<1505)	
		rc->AUX1=1500;
	if(rc->AUX2>1495 && rc->AUX2<1505)	
		rc->AUX2=1500;
	control_angle.roll  = angle->roll  - (rc->ROLL  -1500)/13.0f + (rc->AUX2 -1500)/100.0f;
	control_angle.pitch = angle->pitch - (rc->PITCH -1500)/13.0f - (rc->AUX1 -1500)/100.0f;
	if(control_angle.roll >  angle_max)	//ROLL
		roll.integral +=  angle_max;
	if(control_angle.roll < -angle_max)
		roll.integral += -angle_max;
	else
		roll.integral += control_angle.roll;
	if(roll.integral >  angle_integral_max)
	   	roll.integral =  angle_integral_max;
	if(roll.integral < -angle_integral_max)
	   	roll.integral = -angle_integral_max;
	if(control_angle.pitch >  angle_max)//PITCH
		pitch.integral +=  angle_max;
	if(control_angle.pitch < -angle_max)
		pitch.integral += -angle_max;
	else
		pitch.integral += control_angle.pitch;
	if(pitch.integral >  angle_integral_max)
		pitch.integral =  angle_integral_max;
	if(pitch.integral < -angle_integral_max)
		pitch.integral = -angle_integral_max;
	if(rc->THROTTLE<1200)//油门较小时，积分清零
	{
		roll.integral  = 0;
		pitch.integral = 0;
	}
	roll.output  = roll.kp *control_angle.roll  + roll.ki *roll.integral  + roll.kd *(control_angle.roll -last_angle.roll );
	pitch.output = pitch.kp*control_angle.pitch + pitch.ki*pitch.integral + pitch.kd*(control_angle.pitch-last_angle.pitch);
	last_angle.roll =control_angle.roll;
	last_angle.pitch=control_angle.pitch;
}

attitude_t 是一个姿态数据结构类型，函数参数 actualAngle 是一个结构体指针，指向实际角度结构体变量（数据融合输出值）state->attitude, desiredAngle 指向期望角度结构体变量（设置的角度）attitudeDesired，outDesiredRate 则是角度环的输出，指向期望角速度结构体变量 rateDesired。此处 PID 更新的分别是角度环的 3 个 pid 对象结构体 ，输入偏差为期望角度减去测量角度（actualAngle- desiredAngle）。YawError 处理那段，是为了快速计算并限制其范围在-180°到+180°。

然后是角速度环 PID，其函数原型如下：

/* 角速度环 PID */ 
void Control_Gyro(struct _SI_float *gyro,struct _Rc *rc,uint8_t Lock)
{
	static struct _out_angle control_gyro;
	static struct _out_angle last_gyro;
	int16_t throttle1,throttle2,throttle3,throttle4;
	if(rc->YAW>1400 && rc->YAW<1600)
		rc->YAW=1500;
	if(rc->AUX3>1495 && rc->AUX3<1505)	
		rc->AUX3=1500;
	control_gyro.roll  = -roll.output - gyro->y*Radian_to_Angle;
	control_gyro.pitch = pitch.output - gyro->x*Radian_to_Angle;
	if(rc->AUX4 & Lock_Mode)
		control_gyro.yaw   = - gyro->z*Radian_to_Angle - (rc->AUX3 -1500)/100.0f;//锁尾模式
	else
		control_gyro.yaw   = -(rc->YAW-1500)/2.0f - gyro->z*Radian_to_Angle + (rc->AUX3 -1500)/50.0f;//非锁尾模式
	if(control_gyro.roll >  gyro_max)	//GYRO_ROLL
		gyro_roll.integral +=  gyro_max;
	if(control_gyro.roll < -gyro_max)
		gyro_roll.integral += -gyro_max;
	else
		gyro_roll.integral += control_gyro.roll;
	if(gyro_roll.integral >  gyro_integral_max)
	   	gyro_roll.integral =  gyro_integral_max;
	if(gyro_roll.integral < -gyro_integral_max)
	   	gyro_roll.integral = -gyro_integral_max;
	if(control_gyro.pitch >  gyro_max)//GYRO_PITCH
		gyro_pitch.integral +=  gyro_max;
	if(control_gyro.pitch < -gyro_max)
		gyro_pitch.integral += -gyro_max;
	else
		gyro_pitch.integral += control_gyro.pitch;
	if(gyro_pitch.integral >  gyro_integral_max)
	   	gyro_pitch.integral =  gyro_integral_max;
	if(gyro_pitch.integral < -gyro_integral_max)
		gyro_pitch.integral = -gyro_integral_max;
	gyro_yaw.integral += control_gyro.yaw;
	if(gyro_yaw.integral >  gyro_integral_max)
	   	gyro_yaw.integral =  gyro_integral_max;
	if(gyro_yaw.integral < -gyro_integral_max)
	   	gyro_yaw.integral = -gyro_integral_max;
	if(rc->THROTTLE<1200)//油门较小时，积分清零
		{	gyro_yaw.integral  = 0;		}
	gyro_roll.output = gyro_roll.kp *control_gyro.roll + gyro_roll.ki *gyro_roll.integral + gyro_roll.kd *(control_gyro.roll -last_gyro.roll );
	gyro_pitch.output = gyro_pitch.kp*control_gyro.pitch + gyro_pitch.ki*gyro_pitch.integral + gyro_pitch.kd*(control_gyro.pitch-last_gyro.pitch);
	gyro_yaw.output   = gyro_yaw.kp  *control_gyro.yaw   + gyro_yaw.ki  *gyro_yaw.integral   + gyro_yaw.kd  *(control_gyro.yaw  -last_gyro.yaw  );
	last_gyro.roll =control_gyro.roll;
	last_gyro.pitch=control_gyro.pitch;
	last_gyro.yaw  =control_gyro.yaw;
	if(rc->THROTTLE>1200 && Lock==0){
		throttle1 = rc->THROTTLE - 1050 + gyro_pitch.output + gyro_roll.output - gyro_yaw.output;
		throttle2 = rc->THROTTLE - 1050 + gyro_pitch.output - gyro_roll.output + gyro_yaw.output;
		throttle3 = rc->THROTTLE - 1050 - gyro_pitch.output + gyro_roll.output + gyro_yaw.output;
		throttle4 = rc->THROTTLE - 1050 - gyro_pitch.output - gyro_roll.output - gyro_yaw.output;
	}
	else{
		throttle1=0;
		throttle2=0;
		throttle3=0;
		throttle4=0;
	}
	Motor_Out(throttle1,throttle2,throttle3,throttle4);
}

Axis3f 是一个 3 轴数据结构体类型，control_t 是控制数据结构体类型。参数 actualRate 指向 3 轴陀螺结构体变量 sensors->gyro，desiredRate 则指向角度环 PID 的输出 rateDesired， control_t 则指向 control 结构体变量，control 结构体包含了角速度环的输出数据—姿态控制量数据，这个数据用作控制电机。此处 PID 更新的分别是角速度环的 3 个 pid 对象结构体 ，输入偏差为期望角速度减去测量角速度（actualRate- desiredRate），pidOutLimit 函数用作限制姿态控制量的调整范围（-32768~+32767），防止调整量过大，难以控制。

7.5 姿态控制量和油门值整合

微型四轴为 X 模式飞行，电机转向和姿态解算正方向（箭头指示正方向）如图 6.7.1 所示：

image70

图 6.7.1 电机转向和姿态解算方向

rc->THROTTLE 为油门控制量，这个值增大四轴升高，减小则下降。rc->ROLL，rc->PITCH，rc->YAW 为 PID输出的姿态控制量。油门控制量和姿态控制量整合后控制电机，整合代码在power_control.c文件的函数powerControl()中实现，代码如下：

void RC_Limit(struct _Rc *rc)
{
	rc->THROTTLE = (rc->THROTTLE<=1000)?1000:rc->THROTTLE; 
	rc->THROTTLE = (rc->THROTTLE>=2000)?2000:rc->THROTTLE; 
	rc->PITCH = (rc->PITCH<=1000)?1000:rc->PITCH; 
	rc->PITCH = (rc->PITCH>=2000)?2000:rc->PITCH; 
	rc->ROLL = (rc->ROLL<=1000)?1000:rc->ROLL; 
	rc->ROLL = (rc->ROLL>=2000)?2000:rc->ROLL; 
	rc->YAW  = (rc->YAW<=1000)?1000:rc->YAW; 
	rc->YAW  = (rc->YAW>=2000)?2000:rc->YAW; 
	rc->AUX1 = (rc->AUX1<=1000)?1000:rc->AUX1; 
	rc->AUX1 = (rc->AUX1>=2000)?2000:rc->AUX1; 
	rc->AUX2 = (rc->AUX2<=1000)?1000:rc->AUX2; 
	rc->AUX2 = (rc->AUX2>=2000)?2000:rc->AUX2; 
	rc->AUX3 = (rc->AUX3<=1000)?1000:rc->AUX3; 
	rc->AUX3 = (rc->AUX3>=2000)?2000:rc->AUX3; 
}

如图 6.7.1 所示，微型四轴右上角为 M1 机臂，在代码中对应电机为motorPWM.m1，然后顺时针依次为 M2，M3，M4。如果四轴受外力干扰导致 PITCH 轴向前倾斜 3 度，那么M1 和 M4 需要提高升力，M2 和 M3 减小升力来恢复平衡状态，所以有以下规则：

Roll 方向（向右为正）旋转，为了恢复平衡，则 M3 和 M4 同侧出力，M1 和 M2 反向出力（m1 和 m2 的 Roll 为-，m3 和 m4 的 Roll 为+）;

Pitch 方向（向前为正）旋转，为了恢复平衡，则 M2 和 M3 同侧出力，M1 和 M4 反向出力（m1 和 m4 的 Pitch 为-，m2 和 m3 的 Pitch 为+）;

Yaw 方向（逆时针为正）旋转，为了恢复平衡，则 M1 和 M3 同侧出力，M2 和 M4 反向出力，（m2 和 m4 的 Yaw 为-，m1 和 m3 的 Yaw 为+）;

bool 型变量 motorSetEnable，为 true，使能手动设置电机占空比，这样可以方便单独调试某几个电机，默认不使能。

motorsSetRatio()当然就是设定对应电机定时器通道占空比的函数了，设定的占空比作用到 MOS 管，然后控制电机，从而控制四轴。

7.6 蓝牙无线串口通讯协议

APP和飞控之间的通信协议使用了MWC飞控协议(MSP, Multiwii Serial Protocol)，详见MSP协议格式。

MWC协议规定了飞控和上位机（或者手机APP）信息交流的基本格式。

MWC具体实现，可以查看Android APP源代码中的Protocol.java文件。

串口通信采用UART通讯方式，数组名为Bluetooth_RXDATA，一次发送八个字节，低八位读取。若发送的位数超过8位，则使用两个数组元素来存放一个通道的数据。

通信协议格式如下所示：

串口数组 作用
Bluetooth_RXDATA[0] 数据校验，内容为$
Bluetooth_RXDATA[1] 数据校验，内容为M
Bluetooth_RXDATA[2] 发送标志， 若为四轴发送给上位机，则内容为> 若为上位机发送给四轴，则内容为<
Bluetooth_RXDATA[3] 串口长度，用于校验
Bluetooth_RXDATA[4] 功能桢标志
Bluetooth_RXDATA[5] ~ Bluetooth_RXDATA[20] 取决于功能帧的标志

7.7 手机控制软件框架

Android手机控制软件采用Android Studio开发，并采用最新版本29版本的Android SDK，操作系统最高支持Android 9.0，最低支持Android 4.3.0。BLE技术需要Android4.3及以上系统支持。源码采用Java语言编写而成，通过使用Bluetooth BLE4.0协议，可通过Android手机实现近距离控制配套的微型四轴。

源码的主要java代码文件在

⚠️ **GitHub.com Fallback** ⚠️