【MPU6050_DMP】dmp初始化校准设置，取消上电零度【MPU6050_DMP】dmp初始化校准设置，取消上电零度

130 阅读 0 评论 86 点赞

我是靠谱客的博主刻苦秋天，这篇文章主要介绍【MPU6050_DMP】dmp初始化校准设置，取消上电零度【MPU6050_DMP】dmp初始化校准设置，取消上电零度，现在分享给大家，希望可以做个参考。

【MPU6050_DMP】dmp初始化校准设置，取消上电零度

相信很多移植过正点原子或者其他的mpu6050 dmp解算代码，都遇到过这个问题，即陀螺仪每次解算的姿态角都是上电零度。即每次你都需要将陀螺仪的位置摆的非常正。
但是往往很多时候我们需要记住这个零初始值，以避免每次都需要摆正上电。

== 这个问题其实是源于dmp代码初始化的时候，自检设备，启用基于DMP的陀螺仪校准，然后将这个上电产生的新的偏差，则覆盖写入此位置的偏差。偏置陀螺仪在q16中的偏置==

解决方案1：

mpu6050 dmp初始化函数中，res=run_self_test();//自检函数中

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
//mpu6050,dmp初始化
//返回值:0,正常
//
其他,失败
unsigned char mpu_dmp_init(void)
{
unsigned char res=0;
//simiic_init();
if(mpu_init()==0)	//初始化MPU6050
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
if(res)return 1;
res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO
if(res)return 2;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ);	//设置采样率
if(res)return 3;
res=dmp_load_motion_driver_firmware();
//加载dmp固件
if(res)return 4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
if(res)return 5;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|	//设置dmp功能
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL);
if(res)return 6;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);	//设置DMP输出速率(最大不超过200Hz)
if(res)return 7;
res=run_self_test();
//自检
if(res)return 8;
res=mpu_set_dmp_state(1);	//使能DMP
if(res)return 9;
}
return 0;
}

注释掉两行函数
dmp_set_gyro_bias(gyro);
dmp_set_accel_bias(accel);
仔细查看就可以看出，这两行代码就是将读取到的角度和角速度信息，赋值给了陀螺仪在q16中的偏置，所以就会导致上电零度的情况。

其实这两行代码的作用就是：

对于运动，我们都要为其设立一个参考系，因为运动都是相对的，陀螺仪输出了值，这个值是相对与谁呢？？我们在地球上，相对参考系当然是地面。假如我们在动车上做实验，陀螺仪输出的值是相对与谁呢？？当然是动车!!! 那为什么是动车呢？那我就告诉你，因为开机陀螺仪初始化的时候陀螺仪是相对火车静止的，故开机对谁静止，陀螺仪输出的值就是相对于谁的运动速率。

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
//MPU6050自测试
//返回值:0,正常
//
其他,失败
unsigned char 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 == 0x3)
{
/* 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);
return 0;
}else return 1;
}

解决方案2：

改掉源代码就是在使用accel_sens前加一行： accel_sens=0;使其重力校准失效！！

复制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
unsigned char 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 == 0x3)
{
/* 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);
accel_sens = 0;
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);
return 0;
}else return 1;
}