MPU9250/MPU6050與運動數據處理與卡爾曼濾波（1）

第一篇——概述和MPU6050及其自帶的DMP輸出四元數

概述

　　InvenSense（國內一般譯為應美盛）公司產的數字運動傳感器在國內非常流行，我用過它的兩款，9250和6050。出於被國產芯片慣壞的習慣，我自然而然地認為其封裝引腳和寄存器都是兼容的，所以這成功地讓我打廢兩次板，這兩款芯片的封裝並不是一樣的，MPU9250的要小很多，而兩者都引腳也不一樣，雖然他們都是24pin的，可能是出於MPU9250多一個地磁傳感器，AK8963。

所以兩者的差異點主要在於：

1，封裝（塑體大小）；

2，管腳功能；

3，MPU9250較MPU6050多一個三軸地磁傳感器AK8963;

4，部分寄存器（待補充）；

　　MPU6050是款加速度和角速度傳感器，有人也將因為其角速度傳感器的功能將其稱為陀螺儀，我其實並不能理解，我覺得能直接輸出姿態數據比如歐拉角或者四元數的傳感器才是陀螺儀。多年以前我剛進入大學時聽過一個東大微電子學院教授的講座，講的是MEMS技術，我只記得中間陳提出了一個MEMS能否用來制造晶振的問題，讓我記住了這個大二的。后來也有同事說過MEMS技術中封裝也是很重要的，我一想也有道理，能把AK8963封裝進去肯定不簡單啊。MPU6050能測三軸加速度和三軸加速度，加速度的量程為±2/4/6/8/16g，角速度量程為±250/500/1000/2000角度每秒，16位ADC，輸出速率好像能達到幾千赫茲，當然6050只支持IIC，時鍾最快400KHz。3.3V供電，幾個毫安的功耗。帶一個IIC主機接口，用來外掛其他的IIC傳感器比如GNSS或者地磁傳感器。

MPU6050的DMP

　　一般運動傳感器都是要靠處理器跑算法來進行角度融合以得到最終能直接使用的表示當前自身姿態的歐拉角或者四元數的。我之前用的是卡爾曼濾波。要自己寫代碼大家自然會覺得多個流程，當然有時也會覺得自己算的才靠譜，其實也是，靠6050自帶的DMP算的並不比單片機算的准，而且DMP算得慢，有時是不夠用的。但單片機根據原始的加速度和角速度算四元數逃不過要寫數字信號處理算法要用到大量的乘法或者FPU，有時並不能算過來，所以使用DMP算出來的數據也未嘗不可。DMP，數字運動處理器，MPU6050/9250內部的一個組成部分，可以用來直接向外部吐四元數。

　　DMP的使用我並不想去深究，設備並不重要，數據才重要。應美盛針對6050提供了一套代碼和文檔，加一個跑在MSP430上的例程。例程資源鏈接如下：

　　官方提供的代碼需要實現其中的延時，IIC序列讀和序列寫函數。移植起來對於各位來說，只要1，C語言合格，2，對IDE使用熟練，3了解IIC時序，那么應該是非常簡單的。

IIC驅動代碼

　　而我使用的是STM32F103C8T6和CH32V103C8T6，后者是RISC-V核的前者兼容MCU。我遇到的三個問題中，IIC的時序確實搞了我一下，寫軟件的IIC沒啥，問題有二，1，速度上到100K后就提不上去了，2，收發會被中斷打斷，這點很頭疼。寫單片機程序最頭疼的是你要時刻提醒自己你的業務邏輯流程可能在任意時刻被中斷打斷。所以我還是使用硬件的IIC，32的IIC設計得沒必要得復雜，我需要推一個數據到數據寄存器里，然后等相應的標志位被置位，事實上這些標志位總是莫名其妙地等不到被置位，但是CPU刻意去讀時它又是置位的，就很神奇。我是按照參考手冊的精神去寫的，但是這個也未必能每次都不出問題，我實測在72MHz主頻，400KHz波特率下還是不怎么會報異常的，如果還有異常你就干脆用延時代替讀標志位吧。

　　這里放下初始化代碼：

 

void IIC_Init( u32 bound, u16 address )
{
    GPIO_InitTypeDef GPIO_InitStructure;
    I2C_InitTypeDef I2C_InitTSturcture;

    RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE );
    GPIO_PinRemapConfig(GPIO_Remap_I2C1, ENABLE);
    RCC_APB1PeriphClockCmd( RCC_APB1Periph_I2C1, ENABLE );

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;/* 注意硬件IIC和模擬IIC的管腳配置時不一致的  */
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init( GPIOB, &GPIO_InitStructure );

    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init( GPIOB, &GPIO_InitStructure );

    I2C_InitTSturcture.I2C_ClockSpeed = bound;
    I2C_InitTSturcture.I2C_Mode = I2C_Mode_I2C;
    I2C_InitTSturcture.I2C_DutyCycle = I2C_DutyCycle_16_9;
    I2C_InitTSturcture.I2C_OwnAddress1 = address;
    I2C_InitTSturcture.I2C_Ack = I2C_Ack_Enable;
    I2C_InitTSturcture.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;
    I2C_Init( I2C1, &I2C_InitTSturcture );

    I2C_Cmd( I2C1, ENABLE );

    I2C_AcknowledgeConfig( I2C1, ENABLE );

}

 

　　序列讀取：

 

fn_status IIC_receive_byte(uint8_t devi_addr, uint8_t reg_addr, uint16_t read_length, uint8_t *pbuf)
{
    uint16_t timeout_cnt = 0;
    uint16_t len = read_length;
    volatile uint16_t cnt = 0;

//    printf("IIC_receive_byte.%d bytes.\n",len);
    /* 第一步，等待總線空閑。100毫秒內未等待到其空閑即報錯：超時 */
    while( I2C_GetFlagStatus( I2C1, I2C_FLAG_BUSY ) != RESET )
    {
        jiance();
    }

    /* 第二步，發送開始標志。 */
    I2C_GenerateSTART( I2C1, ENABLE );/* 發送一個起始信號 */
    while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_MODE_SELECT ) )
    {
        jiance();
    }
    timeout_cnt = 0;
    /* 第三步，發送7位設備地址，寫地址。 */
    devi_addr=devi_addr<<1;
    I2C_Send7bitAddress( I2C1, devi_addr, I2C_Direction_Transmitter );

    while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED ) )
    {
        jiance();
    }
    timeout_cnt = 0;
    /* 第四位，發送八位寄存器地址 */
    I2C_SendData( I2C1, reg_addr );/* 發送寄存器地址 */
    while(  !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_BYTE_TRANSMITTED ))/* 等待上字節發送完畢 */
    {
        jiance();
    }
    timeout_cnt = 0;
    /* 第六步，重送開始標志。 */
    I2C_GenerateSTART( I2C1, ENABLE );/* 發送一個起始信號 */
    while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_MODE_SELECT ) )
    {
        jiance();
    }
    timeout_cnt = 0;
    /* 第七步，發送7位設備地址，讀地址。 */
    I2C_Send7bitAddress( I2C1, devi_addr, I2C_Direction_Receiver );
    while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED ) )
    {
//        printf("ice I2C_CheckEvent.\n.\n");
        jiance();
    }
    timeout_cnt = 0;
    /* 第八步，讀取數據  */
    if(len==1)
    {
        I2C_AcknowledgeConfig( I2C1, DISABLE );
        while( I2C_GetFlagStatus( I2C1, I2C_FLAG_RXNE ) ==  RESET )
        {
//            printf("I2C_GetFlagStatus.cnt:%d\n",cnt);
            jiance();
        }
        pbuf[cnt] = I2C_ReceiveData( I2C1 );
    }
    else
    for(cnt=0;cnt < len;cnt++)
    {
        while( I2C_GetFlagStatus( I2C1, I2C_FLAG_RXNE ) ==  RESET )
        {
//            printf("I2C_GetFlagStatus.cnt:%d\n",cnt);
            delay_us(2);
//            jiance();
        }
        pbuf[cnt] = I2C_ReceiveData( I2C1 );
//        printf("%d:%02x\n",cnt,pbuf[cnt]);
        if( cnt==( len-2) )
            I2C_AcknowledgeConfig( I2C1, DISABLE );
    }
    /* 第九步，發送結束標志 */
//    printf("I2C_GenerateSTOP.\n");
    I2C_GenerateSTOP( I2C1, ENABLE );
    I2C_AcknowledgeConfig( I2C1, ENABLE );
    return 0;
}

 

　　序列寫入：

 

fn_status IIC_send_byte(uint8_t devi_addr, uint8_t reg_addr, uint16_t send_length, uint8_t *pbuf)
{
    volatile uint16_t timeout_cnt = 0;
    uint16_t len = send_length;
    uint16_t i = 0;

    while( I2C_GetFlagStatus( I2C1, I2C_FLAG_BUSY ) != RESET )   jiance();
    I2C_GenerateSTART( I2C1, ENABLE );/* 發送一個起始信號 */
    while( !I2C_CheckEvent( I2C1, I2C_EVENT_MASTER_MODE_SELECT ) )  jiance();
    devi_addr=devi_addr<<1;
    I2C_Send7bitAddress( I2C1, devi_addr, I2C_Direction_Transmitter );    //發送地址1 byte
    while( (I2C1->STAR1 != 0x0082)||(I2C1->STAR2!=0x0007) )   jiance();
    I2C_SendData( I2C1, (u8)(reg_addr&0x00FF) );
    while( (I2C1->STAR1!=0x0084)||(I2C1->STAR2!=0x0007) )   jiance();//發送寄存器地址 1 byte
    /* 發送數據 */
    for(i=0;i<len;i++)
    {
           I2C_SendData( I2C1, pbuf[i]);
           delay_us(10);
           while( (I2C1->STAR1!=0x0084)||(I2C1->STAR2!=0x0007) ) jiance2();
    }
    I2C_GenerateSTOP( I2C1, ENABLE );
    return 0;
}

 

　　里面的jiance()是一個調試用的函數，用來判斷等待當前標志位花了多久，如果超過閾值就記錄當前的情況並開始下一次的讀寫。

#define yuzhi 15/* 實測在72M主頻下可用  */
#if 1
#define jiance()      {\
          delay_us(10);\
          timeout_cnt++;\
          if(timeout_cnt >= yuzhi)\
          {\
              timeout_cnt=0;\
              printf("timeout at line:%d\n",__LINE__);\
              printf("STAR1:%04x,STAR2:%04x.\n",I2C1->STAR1,I2C1->STAR2);\
              break;\
          }\
      }
#else
#define jiance()
#endif

#define jiance2()      {\
          delay_us(10);\
          timeout_cnt++;\
          if(timeout_cnt >= yuzhi)\
          {\
              timeout_cnt=0;\
              break;\
          }\
      }

 

實物操作：

　　原理圖直接照抄的官方的。注意電容容值。32側我隨意找了個IIC口。

 

PCB圖沒啥好放的，簡單地連線。

這里放個CH32V103C8T6的工程，IDE是MRS。讀取四元數然后由單片機轉成歐拉角。晶振用8MHz的。鏈接如下。

https://share.weiyun.com/zDZ5EC0P

輸出的打印就像這樣。

MRS的注意點：

　　MRS很明顯能看出有eclipse的影子，我用了下覺得繼承了eclipse的友好的界面，不過沒有MDK成熟，使用它還是需要對編譯過程或者原理要有些許的了解的。這里專門記錄下MRS的一些可能會對你產生困惑的點。

1，無法直接使用math.h，需要在庫鏈接里加上一個m，否則編譯器會報缺文件。

2，使用浮點數打印（printf("%f")）時，需要勾選使用定制的庫。否則打印不出東西。

以上兩條針對1.42版本的MRS。