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。