- 以STM32F4为控制核心的四轴航拍飞行器的设计与实现

ex = (ay*vz - az*vy);                                                        

ey = (az*vx - ax*vz);

ez = (ax*vy - ay*vx);

halfT=0.002;

gx = gx + ex*FACTOR/halfT;                                 //校正陀螺仪测量值    用叉积误差来做PI修正陀螺零偏                                            

gy = gy + ey*FACTOR/halfT;

gz = gz + ez*FACTOR/halfT;        

delta_2=(2*halfT*gx)*(2*halfT*gx)+(2*halfT*gy)*(2*halfT*gy)+(2*halfT*gz)*(2*halfT*gz);   

q0 = (1-delta_2/8)*q0 + (-q1*gx - q2*gy - q3*gz)*halfT;                    // 整合四元数率       四元数微分方程       四元数更新算法，二阶毕卡法

q1 = (1-delta_2/8)*q1 + (q0*gx + q2*gz - q3*gy)*halfT;

q2 = (1-delta_2/8)*q2 + (q0*gy - q1*gz + q3*gx)*halfT;

q3 = (1-delta_2/8)*q3 + (q0*gz + q1*gy - q2*gx)*halfT;                    

norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);        // 正常化四元

q0 = q0 / norm;

q1 = q1 / norm;

q2 = q2 / norm;

q3 = q3 / norm;

//转换为欧拉角

Q_ANGLE.Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;                                   // pitch

Q_ANGLE.Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;    // roll

Q_ANGLE.Yaw = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3 * q3 + 1)* 57.3; // yaw

}

STM32/STM8

意法半导体/ST/STM