|
当固定MPU6050 一定角度时,融合后的Anglex,Angley会匀速变大或变小,大约10变化2度,平放时候为0不会发生变化。
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 q0q0 = q0 * q0;
float q0q1 = q0 * q1;
float q0q2 = q0 * q2;
float q0q3 = q0 * q3;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q3q3 = q3 * q3;
float q0y_q0y = q0_y * q0_y;
float q1y_q1y = q1_y * q1_y;
float q2y_q2y = q2_y * q2_y;
float q3y_q3y = q3_y * q3_y;
float q1y_q2y = q1_y * q2_y;
float q0y_q3y = q0_y * q3_y;
/* Yaw轴四元素的微分方程 */
q0_y = q0_y + (-q1_y * gx - q2_y * gy - q3_y * gz) * halfT;
q1_y = q1_y + (q0_y * gx + q2_y * gz - q3_y * gy) * halfT;
q2_y = q2_y + (q0_y * gy - q1_y * gz + q3_y * gx) * halfT;
q3_y = q3_y + (q0_y * gz + q1_y * gy - q2_y * gx) * halfT;
/* 规范化yaw */
norm = sqrt( q0y_q0y + q1y_q1y + q2y_q2y + q3y_q3y );
q0_y = q0_y / norm;
q1_y = q1_y / norm;
q2_y = q2_y / norm;
q3_y = q3_y / norm;
/* 规范化加速度计值 */
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计算 */
exInt = exInt + ex * Ki;
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki;
/* 校正陀螺仪 */
gx = gx + Kp * ex + exInt;
gy = gy + Kp * ey + eyInt;
gz = gz + Kp * ez + ezInt;
/* 四元素的微分方程 */
q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;
/* 规范化Pitch、Roll轴四元数 */
norm = sqrt( q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3 );
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
Anglex = atan2( 2 * q2q3 + 2 * q0q1, -2 * q1q1 - 2 * q2q2 + 1 ) * 57.2957795f;
Angley = asin( -2 * q1q3 + 2 * q0q2 ) * 57.2957795f;
Anglez = atan2( 2 * q1y_q2y + 2 * q0y_q3y, -2 * q2y_q2y - 2 * q3y_q3y + 1 ) * 57.2957795f;
}
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
阿莫论坛20周年了!感谢大家的支持与爱护!!
一只鸟敢站在脆弱的枝条上歇脚,它依仗的不是枝条不会断,而是自己有翅膀,会飞。
|