最近在用MPU6050的DMP输出姿态,基本上就是照搬网上的代码,然后略微修改。不过有两个地方可能需要特别注意一下:
1.自检。
自检就是在MPU初始化函数中的下面这句话:
run_self_test();
查看函数定义如下:
void run_self_test(void)
{
int result;
// char test_packet[4] = {0};
long gyro[3], accel[3];
result = mpu_run_self_test(gyro, accel);
if (result == 0x07) {
//if(1) {
/* Test passed. We can trust the gyro data here, so let's push it down
* to the DMP.
*/
float sens;
unsigned short accel_sens;
mpu_get_gyro_sens(&sens);
gyro[0] = (long)(gyro[0] * sens);
gyro[1] = (long)(gyro[1] * sens);
gyro[2] = (long)(gyro[2] * sens);
dmp_set_gyro_bias(gyro);
mpu_get_accel_sens(&accel_sens);
accel[0] *= accel_sens;
accel[1] *= accel_sens;
accel[2] *= accel_sens;
dmp_set_accel_bias(accel);
PrintChar("setting bias succesfully ......\n");
}
else
{
PrintChar("bias has not been modified ......\n");
}
}
总之很多人在使用DMP时的那句"bias has not been modified"出处就在这里了。
仔细看一下代码,这个自检不成功的原因是result不等于0x07,再经过一层又一层第剖析函数定义后,发现result意义如下:
0x07即0b00000111,前面5个固定是0,后面的3个1从左往右分别代表磁力计,加速度计,陀螺仪校准是否成功,1为成功。
很显然MPU6050没有磁力计所以绝不可能是0x07,加速度计貌似在绝对水平时才能为1,有参考价值的只有陀螺仪的校准。所以改成“result == 0x01”即可。当然不作判断也可以,反正都能用。
我在自检中还碰到一个问题:DMP初始化稳定时间太长,尤其是在“底朝天”姿态时。对此我强制把accel_sens设为0(大概就是不考虑当前姿态,以绝对姿态作参考吧),不过Pitch稳定下来还是要30s左右。。。
2.姿态换算。
原来的Pitch范围是-90~90,Roll范围-180~180,Yaw范围-180~180,四元素换算代码如下:
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
Pitch范围扩展为-180~180,四元素换算代码如下:
Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
if(fabs(Roll) < 90.0) Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
else Pitch = (fabs(q0* q2 - q1 * q3)/(q0* q2 - q1 * q3)) *(180.0 - fabs(asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3));
Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
好处是强迫症看起来比较爽,坏处是在-90和90度时,Pitch会突变(其实Roll的变化也很剧烈了)