代码收藏家技术教程 2023-07-29

MPU6050_DMP初始化校准设置和取消上电零度详解

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

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

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

解决方案1：

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

//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中的偏置，所以就会导致上电零度的情况。

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

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

//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;使其重力校准失效！！

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;
}