四旋翼飞行器的飞控实现

67 q2 += qDot3 * (1.0f / sampleFreq);

68 q3 += qDot4 * (1.0f / sampleFreq);

69

70 // Normalise quaternion

71 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

72 q0 *= recipNorm;

73 q1 *= recipNorm;

74 q2 *= recipNorm;

75 q3 *= recipNorm;

76 }

77

78

79 float invSqrt(float x) {

80 float halfx = 0.5f * x;

81 float y = x;

82 long i = *(long*)&y;

83 i = 0x5f3759df - (i>>1);

84 y = *(float*)&i;

85 y = y * (1.5f - (halfx * y * y));

86 return y;

87 }

四元数转欧拉角的算法可参考 http://www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html

4.获取期望姿态;

也就是遥控部分了，让用户介入控制。

本着拿来主义的原则，用上”圆点博士开源项目”提供的安卓的开源蓝牙控制端。

圆点博士给出了数据包格式，同过HC-06蓝牙模块接连到STM32串口1，再无线连接到控制端，这样我们就可以获得控制端不断发送的数据包了，并实时更新期望姿态角，这里只需要注意输出的姿态角和实时姿态角方向一致以及数据包的校验就行了。

5.PID控制算法;

由于简单的线性控制不可能满足四轴飞行器这个灵敏的系统，引入PID控制器来更好的纠正系统。

简介：PID实指“比例proportional”、“积分integral”、“微分derivative”，这三项构成PID基本要素。每一项完成不同任务，对系统功能产生不同的影响。

以Pitch为例：

error为期望角减去实时角度得到的误差;

iState为积分i参数对应累积过去时间里的误差总和;

if语句限定iState范围，繁殖修正过度;

微分d参数为当前姿态减去上次姿态，估算当前速度(瞬间速度);

总调整量为p,i,d三者之和;

这样，P代表控制系统的响应速度，越大，响应越快。

I，用来累积过去时间内的误差，修正P无法达到的期望姿态值(静差);

D，加强对机体变化的快速响应，对P有抑制作用。

PID各参数的整定需要综合考虑控制系统的各个方面，才能达到最佳效果。

输出PWM信号：

PID计算完成之后，便可以通过STM32自带的定时资源很容易的调制出四路pwm信号，采用的电调pwm格式为50Hz，高电平持续时间0.5ms-2.5ms;

我以1.0ms-2.0ms为每个电机的油门行程，这样，1ms的宽度均匀的对应电调的从最低到最高转速。

至此，一个用stm32和mpu6050搭建的飞控系统就算实现了。

剩下的调试PID参数了。