陀螺儀mpu6050的調試與dmp庫的移植經歷

通過三天的痛苦掙扎終於調通了mpu以及移植好了dmp庫

一開始動手使用mpu6050的時候移植了一個例程想試下效果

不曾想調用dmp的時候就出錯了，然後嘗試了各種方法試圖定位

錯誤，一開始懷疑模擬IIC有問題，然後仔細覈對了一下IIC的時序

例程程序：

//MPU IIC 延時
void MPU_IIC_Delay(void)
{
	delay_us(2);
}
//產生II起始信號
void MPU_IIC_Start(void)
{
	MPU_SDA_OUT();     
	MPU_IIC_SDA=1;	  	  
	MPU_IIC_SCL=1;
	MPU_IIC_Delay();
 	MPU_IIC_SDA=0;
	MPU_IIC_Delay();
	MPU_IIC_SCL=0;
}	  

可以發現延時有點不對，可以對照時序圖

然後以爲是時序問題，就索性自己寫的模擬IIC程序代替了，結果還是不行，排除時序問題，實際上例程這麼寫是爲了加快讀取數據的速度，

然後自己便嘗試了硬件iic讀取mpu6050的數據，看代碼

#include "delay.h"

#include "IICMPU.h"

//初始化IIC
void MPU_IIC_INIT(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
	I2C_InitTypeDef I2C_InitStructure; 
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1,ENABLE);  

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; //
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; 
	GPIO_Init(GPIOB, &GPIO_InitStructure); 
  I2C_InitStructure.I2C_Mode = I2C_Mode_I2C ; 
	I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; 
	I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; 
	I2C_InitStructure.I2C_ClockSpeed = 50000; 

	I2C_Init(I2C1, &I2C_InitStructure);	   	
	I2C_Cmd  (I2C1,ENABLE); 
	I2C_AcknowledgeConfig(I2C1, ENABLE); 


}
/*********寫一個字節到寄存器
					REG_Address 幾首數據的iic設備寄存器的地址
					REG_data 待寫入的數據******************************/
void I2C_ByteWrite(uint8_t REG_Address,uint8_t REG_data)
{

I2C_GenerateSTART(I2C1,ENABLE);
while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_MODE_SELECT));
I2C_Send7bitAddress(I2C1,SlaveAddress,I2C_Direction_Transmitter);
while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));
I2C_SendData(I2C1,REG_Address);
while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_BYTE_TRANSMITTED));
I2C_SendData(I2C1,REG_data);
while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_BYTE_TRANSMITTED));
I2C_GenerateSTOP(I2C1,ENABLE);
}
/*************?MPU6050讀一個字節**********************************/
uint8_t I2C_ByteRead(uint8_t REG_Address)
{
uint8_t REG_data;

while(I2C_GetFlagStatus(I2C1,I2C_FLAG_BUSY));
I2C_GenerateSTART(I2C1,ENABLE);//起始信號
while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_MODE_SELECT));
I2C_Send7bitAddress(I2C1,SlaveAddress,I2C_Direction_Transmitter);//發送設備地址+寫信號
while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED));//
I2C_Cmd(I2C1,ENABLE);
I2C_SendData(I2C1,REG_Address);//發送儲存單元，從0開始
while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_BYTE_TRANSMITTED));
I2C_GenerateSTART(I2C1,ENABLE);//起始信號
while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_MODE_SELECT));
I2C_Send7bitAddress(I2C1,SlaveAddress,I2C_Direction_Receiver);//發送設備地址+讀信號
while(!I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED));
I2C_AcknowledgeConfig(I2C1,DISABLE);
I2C_GenerateSTOP(I2C1,ENABLE);
while(!(I2C_CheckEvent(I2C1,I2C_EVENT_MASTER_BYTE_RECEIVED)));
REG_data=I2C_ReceiveData(I2C1);//讀出數據寄存器

return REG_data;
}
//mpu初始化
void InitMPU6050(void)
{
	I2C_ByteWrite(PWR_MGMT_1,0x00);//解除休眠狀態
	I2C_ByteWrite(SMPLRT_DIV,0x07);//採樣頻率125hz
	I2C_ByteWrite(CONFIG,0x06);//
	I2C_ByteWrite(GYRO_CONFIG,0x18);//陀螺儀量程設置+-2000deg/s,靈敏度16.4LSB/led/s
	I2C_ByteWrite(ACCEL_CONFIG,0x01);//加速度傳感器量程設置+-4g,靈敏度8190LSB/g
}


/*******************獲取數據*******************************/
unsigned int GetData(unsigned char REG_Address)
{
	char H,L;                                         
	H=I2C_ByteRead(REG_Address);
	L=I2C_ByteRead(REG_Address+1);
	return (H<<8)+L;   //合成數據
}

然後讀取數據正常，但是這樣移植起DMP庫就麻煩多了，具體能不能實現移植還沒有去嘗試，可以參考http://bbs.eeworld.com.cn/thread-444810-1-1.html
就這樣排除了IIC的問題，直接把問題指向mpu初始化配置

通過分析終於找到了問題所在，直接看圖

看註釋的那條語句，註釋掉就可以完美運行，直接使用默認的時鐘，但是這樣勢必加大了最後的出歐拉角的誤差。

至於爲什麼在註釋這條語句就會到至錯誤我自己也想不大明白，如果有讀者發現了問題，希望交流交流，

通常情況下DMP庫的自檢也會導致一些錯誤，看圖

可以嘗試註釋掉自檢；對於自檢函數，我們打開來看看裏面到底是什麼

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

下面的文字應用某貼：

細心的人如果將它打開後會發現這裏面實際也是一個校零程序，矯正三個方向的陀螺儀的零點漂移，第二個就是糾正重力方向，也就是開機時刻的加速度方向（通俗一點就是我們認爲的水平方向）
result = mpu_run_self_test(gyro, accel);  用於獲得當前各軸的角速度和加速度，並且反饋芯片的狀態。
                                 mpu_get_gyro_sens(&sens);                  這個就不用說了，用於獲得角速度的計算比例；好戲在下頭。
                                dmp_set_gyro_bias(gyro);                      這個把開機當前的角速度計算完成後送到了DMP庫

我們知道，對於運動，我們都要爲其設立一個參考系，因爲運動都是相對的，陀螺儀輸出了值，這個值是相對與誰呢？？我們在地球上，相對參考系當然是地面。假如我們在動車上做實驗，陀螺儀輸出的值是相對與誰呢？？當然是動車!!!!   那爲什麼是動車呢？那我就告訴你，因爲開機陀螺儀初始化的時候陀螺儀是相對火車靜止的，故開機對誰靜止，陀螺儀輸出的值就是相對於誰的運動速率。
         回到主題，DMP自檢時刻保存了開機時的陀螺儀速率，其實就是讓陀螺儀檢測到的速率去除開機時刻的速率，自然得到了陀螺儀與開機時刻參考系的相對速率，也就是我們需要的有效值。這也就是抑制溫漂的原理。
         再來看看加速度重心的校準程序，與陀螺儀一樣，均是修正相對參考系的速率和位置。但是加速度傳感器不一樣，只要傳感器不動，其重力加速度方向是不變的，默認的是-Y是重力加速的的正方向。如果在模塊非水平情況下進行了重力校準，那麼-Y就不是重力方向了，爲了得到無論模塊處於什麼狀態是都需要—y是重力的正方向，我們就有這樣的一個高大上的辦法。那就是“不要對重力進行校準”。
        改掉源代碼就是在使用accel_sens前加一行：  accel_sens=0;使其重力校準失效！！！即可！！！

經修改後，模塊的初始方位不再收開機時刻模塊位置的影響，使用DMP一樣需要8S左右的時間穩定，起碼不用再擔心開機需要去將小車扶正吧！（如果是其他用途，需要開機修正方位的程序，這樣就不行了喲！目測比較少

要注意的就是修改以上的定義，如上圖修改即可