六轴加速度传感器MPU6050官方DMP库到瑞萨RL78/G13的移植

2015年的电赛已经结束了。赛前接到器件清单的时候，看到带防护圈的多旋翼飞行器赫然在列，又给了一个瑞萨RL78/G13的MCU，于是自然联想到13年的电赛，觉得多半是拿RL78/G13做四旋翼的主控，虽然事后证实我的猜测是错的，但是在赛前我还是完成了相关代码的准备，这其中就包括了MPU6050的DMP库移植。在移植前我大概搜了一下，发现网上还没有相关的源代码。一起准备电赛的同学还买过一份RL78/G13的飞控代码，虽然也是使用MPU6050进行姿态获取，但是对MPU6050的读取并不是通过DMP进行，而且竟然在注释里写明DMP的RL78/G13移植受到某网站的专利保护。最后只好自己动手移植了，下面就简单说一下我的移植过程：

一、MPU6050简介

MPU6050 是 InvenSense 公司推出的全球首款整合性 6 轴运动处理组件，相较于多组件方案，免除了组合陀螺仪与加速器时之轴间差的问题，减少了安装空间。

MPU6050 内部整合了 3 轴陀螺仪和 3 轴加速度传感器，并且含有一个第二 IIC 接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。有了 DMP，我们可以使用 InvenSense 公司提供的运动处理资料库，非常方便的实现姿态解算，降低了运动处理运算对操作系统的负荷，同时大大降低了开发难度。

MPU6050 的特点包括：
1、以数字形式输出 6 轴或 9 轴（需外接磁传感器）的旋转矩阵、四元数(quaternion)、欧拉角格式(Euler Angle forma)的融合演算数据（需 DMP 支持）
2、具有 131 LSBs/°/sec 敏感度与全格感测范围为±250、±500、±1000 与±2000°/sec的 3 轴角速度感测器(陀螺仪)
3、集成可程序控制，范围为±2g、±4g、±8g 和±16g 的 3 轴加速度传感器
4、移除加速器与陀螺仪轴间敏感度，降低设定给予的影响与感测器的飘移
5、自带数字运动处理(DMP: Digital Motion Processing)引擎可减少 MCU 复杂的融合演算数据、感测器同步化、姿势感应等的负荷
6、内建运作时间偏差与磁力感测器校正演算技术，免除了客户须另外进行校正的需求
7、自带一个数字温度传感器
8、带数字输入同步引脚(Sync pin)支持视频电子影相稳定技术与 GPS
9、可程序控制的中断(interrupt)，支持姿势识别、摇摄、画面放大缩小、滚动、快速下降中断、high-G 中断、零动作感应、触击感应、摇动感应功能
10、VDD 供电电压为 2.5V±5%、3.0V±5%、3.3V±5%；VLOGIC 可低至 1.8V± 5%
11、陀螺仪工作电流：5mA，陀螺仪待机电流：5uA；加速器工作电流：500uA，加速器省电模式电流：40uA@10Hz

12、自带 1024 字节 FIFO，有助于降低系统功耗

13、高达 400Khz 的 IIC 通信接口
14、超小封装尺寸：4x4x0.9mm（QFN）
MPU6050 传感器的检测轴如图所示：

MPU6050 的内部框图如图所示：

其中，SCL 和 SDA 是连接 MCU 的 IIC 接口，MCU 通过这个 IIC 接口来控制 MPU6050，另外还有一个 IIC 接口：AUX_CL 和 AUX_DA，这个接口可用来连接外部从设备，比如磁传感器，这样就可以组成一个九轴传感器。VLOGIC 是 IO 口电压，该引脚最低可以到 1.8V，我们一般直接接 VDD 即可。AD0 是从 IIC 接口（接 MCU）的地址控制引脚，该引脚控制 IIC 地址的最低位。如果接 GND，则 MPU6050 的 IIC 地址是：0X68，如果接 VDD，则是 0X69，注意：这里的地址是不包含数据传输的最低位的（最低位用来表示读写）！！

二、瑞萨RL78/G13简介

RL78/G13是瑞萨电子出品的16位单片机，它的特点包括：

 1、最短指令执行时间可在高速(0.03125us: @ 32 MHz 高速片上振荡器时钟运行时)至超低速(30.5us: @ 32.768 kHz 副系统时钟运行时)之间更改
 2、通用寄存器: 8 位32 个寄存器(8 位8 个寄存器4 组)
 3、ROM: 16 至 512 KB，RAM: 2 至 32 KB，数据闪存: /4/8 KB
 4、内置高速片上振荡器时钟
     ·可从 32 MHz (TYP.) , 24 MHz (TYP.), 16 MHz (TYP.), 12 MHz (TYP.), 8 MHz (TYP.), 4 MHz (TYP.)和 1 MHz(TYP.)中选择
 5、内置单电源闪存(具有禁止块擦除/写入功能)
 6、支持自编程功能（具有引导交换功能/flash 屏蔽窗口功能）
 7、On-chip 调试功能
 8、内置上电复位(POR)电路和电压检测电路(LVD)
 9、内置看门狗定时器(可在专用低速片上振荡器时钟下运行)
 10、内置乘除法器和乘加器
    ·16 位  16 位 = 32 位 (无符号或者有符号)
    ·32 位  32 位 = 32 位 (无符号)
    ·16 位  16 位 + 32 位 = 32 位 (无符号或者有符号)
 11、内置按键中断功能
 12、内置时钟输出/蜂鸣器输出控制电路
 13、内置十进制调整(BCD)电路
 14、输入/输出端口: 16 至 120 (N 沟开漏：0 至 4)
 15、定时器
    ·16 位定时器：  8 至 16 通道
    ·看门狗定时器：  1 通道
    ·实时时钟：  1 通道 (校正时钟输出)
    ·12 位间隔定时器： 1 通道
 16、串行接口
    ·CSI：  2 至 8 通道
    ·UART/UART (支持 LIN-bus)： 2 至 4 通道
    ·I2C/简易 I2C 通信：  2 至 8 通道
 17、不同电位接口：可以连接 1.8/2.5/3 V 运行的器件
 18、8/10 位分辨率 A/D 转换器 (V DD = EV DD =1.6 至 5.5 V): 6 至 26 通道
 19、待机功能: HALT, STOP, SNOOZE 模式
 20、电源电压: VDD = 1.6 至 5.5 V
 21、运行环境温度: T A = -40 至 +85C

三、移植过程

移植DMP库主要需要实现4个函数，即i2c_write，i2c_read，delay_ms 和 get_ms，这四个函数的原形分别如下：

i2c_write(unsigned char slave_addr, unsigned char reg_addr,unsigned char length, unsigned char const *data)//i2c写入函数，要求至少能连续写入16字节数据i2c_read(unsigned char slave_addr, unsigned char reg_addr,unsigned char length, unsigned char *data)//i2c读取函数，要求至少能连续读取16字节数据delay_ms(unsigned long num_ms)//延时函数get_ms(unsigned long *count)//一般用不到这个函数

1、i2c_write和i2c_read

  RL78/G13的硬件i2c需要接上拉电阻，使用比较麻烦，而稳定的模拟i2c在网上很容易获得，因此这两个函数我是基于正点原子的模拟IIC（源代码见附件，包括myiic.h,myiic.c,mpu6050.h,mpu6050.c）修改而成，主要修改管脚映射宏和IIC初始化函数即可，如下：

//宏定义位于myiic.h#define u8 uint8_t  //正点原子的模拟IIC使用了数据类型u8，该类型在RL78/G13中对应的类型为uint8_t#define SDA_IN() PM7.5=1  //管脚7.5使用为SDA口，PM为端口配置寄存器，该位为1时输入，为0时输出#define SDA_OUT() PM7.5=0#define SCL_IN() PM7.6=1 //管脚7.6使用为SCL口#define SCL_OUT() PM7.6=0 #define IIC_SCL  P7.6 //P为端口输出寄存器#define IIC_SDA  P7.5 #define READ_SDA  P7.5 //IIC初始化函数位于myiic.c，删去了大量代码，因为RL78/G13的管脚配置可以通过代码生成器直接完成，不用手动配置void IIC_Init(void){         SDA_OUT();  SCL_OUT();  IIC_SCL=1;  IIC_SDA=1;}

2、delay_ms和delay_us

delay_ms主要由DMP库调用，而delay_us则由模拟iic调用，代码如下：

//CS+没有提供官方延时函数，因此需要自定义//首先用代码生成器生成一个1ms的定时中断unsigned long systemtime;//定义全局变量//在定时器中断函数中添加 systemtime++;，该函数位于r_cg_timer_user.c__interrupt static void r_tau0_channel3_interrupt(void){  /* Start user code. Do not edit comment generated here */  systemtime++;  /* End user code. Do not edit comment generated here */}//定义delay_msvoid delay_ms(unsigned long num_ms){  unsigned long now=systemtime;  while((systemtime-now)<num_ms){    NOP();  }}//定义delay_us，该函数由网友提供，据称完成整个调用刚好1us，我没有实测过，但是可以使用void delay_us(void){  unsigned char n;  n = 1;  for(; n>0; n--);}//定义完成后，需要将myiic.c和mpu6050.c文件中的所有delay(x)修改为x个delay()，如：void IIC_Stop(void){  SDA_OUT();  IIC_SCL=0;  IIC_SDA=0;   delay_us();delay_us();delay_us();delay_us();//原为delay_us(4)  IIC_SCL=1;   IIC_SDA=1;  delay_us();delay_us();delay_us();delay_us();                  }//最后在inv_mpu.c和inv_mpu_dmp_motion_driver.c中添加包含delay_ms函数所在的头文件

3、inv_mpu.c和inv_mpu_dmp_motion_driver.c

这是DMP代码移植的主要部分，官方DMP库共包含6个文件，分别为：dmpKey.h,dmpmap.h,inv_mpu.h,inv_mpu.c,inv_mpu_dmp_motion_driver.h,inv_mpu_dmp_motion_driver.c，其中需要修改的主要是inv_mpu.c和inv_mpu_dmp_motion_driver.c。代码如下：

//首先注释掉#include <stdint.h>，该代码同时位于inv_mpu.c和inv_mpu_dmp_motion_driver.c，头文件stdint.h提供了几个数据类型，但其中定义了64位的int型，而RL78/G13支持的数据类型最高位为32位，不注释掉将无法通过编译//然后添加包含文件#include "mpu6050.h"//在inv_mpu.c中添加如下宏定义#define MPU6050  //define used sensor 6050#define MOTION_DRIVER_TARGET_GL78G13 //define used MCU GL78/G13#define EMPL_NO_64BIT //在inv_mpu_dmp_motion_driver.c添加如下宏定义#define MOTION_DRIVER_TARGET_GL78G13#define EMPL_NO_64BIT //修改如下代码（inv_mpu.c）#if defined MOTION_DRIVER_TARGET_MSP430#include "msp430.h"#include "msp430_i2c.h"#include "msp430_clock.h"#include "msp430_interrupt.h"#define i2c_write  msp430_i2c_write#define i2c_read  msp430_i2c_read#define delay_ms  msp430_delay_ms#define get_ms   msp430_get_clock_ms//修改为：#if defined MOTION_DRIVER_TARGET_GL78G13#define i2c_write  MPU_Write_Len#define i2c_read  MPU_Read_Len#define delay_ms  delay_ms//该行可注释掉，编译时会警告#define get_ms   mget_ms//修改如下代码（inv_mpu_dmp_motion_driver.c）#if defined MOTION_DRIVER_TARGET_MSP430#include "msp430.h"#include "msp430_clock.h"#define delay_ms  msp430_delay_ms#define get_ms   msp430_get_clock_ms#define log_i(...)   do {} while (0)#define log_e(...)   do {} while (0)//为：#if defined MOTION_DRIVER_TARGET_GL78G13#define delay_ms  delay_ms#define get_ms     mget_ms#define log_i      ;#define log_e     ;//找到如下结构体定义，注释掉unsigned char accel_cfg2;unsigned char lp_accel_odr;unsigned char accel_intel;三个成员变量，这三个值会引起数据飘逸，导致DMP初始化无法通过struct gyro_reg_s {  unsigned char who_am_i;  unsigned char rate_div;  unsigned char lpf;  unsigned char prod_id;  unsigned char user_ctrl;  unsigned char fifo_en;  unsigned char gyro_cfg;  unsigned char accel_cfg;  //unsigned char accel_cfg2;  //unsigned char lp_accel_odr;  unsigned char motion_thr;  unsigned char motion_dur;  unsigned char fifo_count_h;  unsigned char fifo_r_w;  unsigned char raw_gyro;  unsigned char raw_accel;  unsigned char temp;  unsigned char int_enable;  unsigned char dmp_int_status;  unsigned char int_status;  // unsigned char accel_intel;  unsigned char pwr_mgmt_1;  unsigned char pwr_mgmt_2;  unsigned char int_pin_cfg;  unsigned char mem_r_w;  unsigned char accel_offs;  unsigned char i2c_mst;  unsigned char bank_sel;  unsigned char mem_start_addr;  unsigned char prgm_start_h;#if defined AK89xx_SECONDARY  unsigned char s0_addr;  unsigned char s0_reg;  unsigned char s0_ctrl;  unsigned char s1_addr;  unsigned char s1_reg;  unsigned char s1_ctrl;  unsigned char s4_ctrl;  unsigned char s0_do;  unsigned char s1_do;  unsigned char i2c_delay_ctrl;  unsigned char raw_compass;  /* The I2C_MST_VDDIO bit is in this register. */  unsigned char yg_offs_tc;#endif};//结构体实例化一律将：const struct gyro_reg_s reg = {  .who_am_i    = 0x75,  .rate_div    = 0x19,  .lpf      = 0x1A,  .prod_id    = 0x0C,  .user_ctrl   = 0x6A,  .fifo_en    = 0x23,  .gyro_cfg    = 0x1B,  .accel_cfg   = 0x1C,  .motion_thr   = 0x1F,  .motion_dur   = 0x20,  .fifo_count_h  = 0x72,  .fifo_r_w    = 0x74,  .raw_gyro    = 0x43,  .raw_accel   = 0x3B,  .temp      = 0x41,  .int_enable   = 0x38,  .dmp_int_status = 0x39,  .int_status   = 0x3A,  .pwr_mgmt_1   = 0x6B,  .pwr_mgmt_2   = 0x6C,  .int_pin_cfg  = 0x37,  .mem_r_w    = 0x6F,  .accel_offs   = 0x06,  .i2c_mst    = 0x24,  .bank_sel    = 0x6D,  .mem_start_addr = 0x6E,  .prgm_start_h  = 0x70#ifdef AK89xx_SECONDARY  ,.raw_compass  = 0x49,  .yg_offs_tc   = 0x01,  .s0_addr    = 0x25,  .s0_reg     = 0x26,  .s0_ctrl    = 0x27,  .s1_addr    = 0x28,  .s1_reg     = 0x29,  .s1_ctrl    = 0x2A,  .s4_ctrl    = 0x34,  .s0_do     = 0x63,  .s1_do     = 0x64,  .i2c_delay_ctrl = 0x67#endif};//此种形式的修改为如下形式：（CS+编译器不支持该c特性）const struct gyro_reg_s reg = {  0x75,  0x19,  0x1A,  0x0C,  0x6A,  0x23,  0x1B,  0x1C,  0x1F,  0x20,  0x72,  0x74,  0x43,  0x3B,  0x41,  0x38,  0x39,  0x3A,  0x6B,  0x6C,  0x37,  0x6F,  0x06,  0x24,  0x6D,  0x6E,  0x70/*#ifdef AK89xx_SECONDARY  0x49,  0x01,  0x25,  0x26,  0x27,  0x28,  0x29,  0x2A,  0x34,  0x63,  0x64,  0x67#endif*/};//同时在结构体 gyro_reg_s实例化的地方注释掉相应的值const struct gyro_reg_s reg = {  0x75,  0x19,  0x1A,  0x0C,  0x6A,  0x23,  0x1B,  0x1C,  // 0x1D,  // 0x1E,  0x1F,  0x20,  0x72,  0x74,  0x43,  0x3B,  0x41,  0x38,  0x39,  0x3A,  //0x69,  0x6B,  0x6C,  0x37,  0x6F,  0x77,  0x24,  0x6D,  0x6E,  0x70/*#ifdef AK89xx_SECONDARY  raw_compass  = 0x49,  s0_addr    = 0x25,  s0_reg     = 0x26,  s0_ctrl    = 0x27,  s1_addr    = 0x28,  s1_reg     = 0x29,  s1_ctrl    = 0x2A,  s4_ctrl    = 0x34,  s0_do     = 0x63,  s1_do     = 0x64,  i2c_delay_ctrl = 0x67#endif*/};//找到inv_mpu_dmp_motion_driver.c中的int dmp_set_accel_bias(long *bias){  long accel_bias_body[3];  unsigned char regs[12];  long long accel_sf;//将long long 修改为long，原因为RL78/G13不支持64位数据类型，修改为long后不会影响精度  ……}//找到inv_mpu.c中的：int mpu_read_fifo_stream(unsigned short length, unsigned char *data,  unsigned char *more){  unsigned char tmp[2];  unsigned short fifo_count;  if (!st.chip_cfg.dmp_on)    return -1;  if (!st.chip_cfg.sensors)    return -1;  if (i2c_read(st.hw->addr, st.reg->fifo_count_h, 2, tmp))    return -1;    //fifo_count = (tmp[0] << 8) | tmp[1];//将该行语句修改为下面三行，原因为CS+不会自动将tmp[]数组转换为16位数据类型，需要手动转换  fifo_count+=tmp[0];  fifo_count=fifo_count<<8;  fifo_count|=tmp[1];  ……}//然后在inv_mpu.c中添加以下代码，以方便调用#define q30 1073741824.0fint mget_ms(unsigned long *time){  return 0;}static signed char gyro_orientation[9] = { 1, 0, 0,                      0, 1, 0,                      0, 0, 1};unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx){  unsigned short scalar;   /*    XYZ 010_001_000 Identity Matrix    XZY 001_010_000    YXZ 010_000_001    YZX 000_010_001    ZXY 001_000_010    ZYX 000_001_010   */  scalar = inv_row_2_scale(mtx);  scalar |= inv_row_2_scale(mtx + 3) << 3;  scalar |= inv_row_2_scale(mtx + 6) << 6;  return scalar;}unsigned short inv_row_2_scale(const signed char *row){  unsigned short b;  if (row[0] > 0)    b = 0;  else if (row[0] < 0)    b = 4;  else if (row[1] > 0)    b = 1;  else if (row[1] < 0)    b = 5;  else if (row[2] > 0)    b = 2;  else if (row[2] < 0)    b = 6;  else    b = 7;   // error  return b;}uint8_t mpu_dmp_init(void){  uint8_t res=0;  long gyro[3], accel[3];   if(mpu_init()==0)    {       res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);    if(res)return 1;     res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);    if(res)return 2;     res=mpu_set_sample_rate(DEFAULT_MPU_HZ);      if(res)return 3;     res=dmp_load_motion_driver_firmware();        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_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);      if(res)return 7;      res=mpu_run_self_test(gyro, accel);        if(res==0x3)return 8;      res=mpu_set_dmp_state(1);      if(res)return 9;      return 0;  }  else{    return 10;  }}uint8_t mpu_dmp_get_data(float *pitch,float *roll,float *yaw){  float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;  unsigned long sensor_timestamp;  short gyro[3], accel[3], sensors;  unsigned char more;  long quat[4];   if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;     /* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.   * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.  **/  /*if (sensors & INV_XYZ_GYRO )  send_packet(PACKET_TYPE_GYRO, gyro);  if (sensors & INV_XYZ_ACCEL)  send_packet(PACKET_TYPE_ACCEL, accel); */  /* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.   * The orientation is set by the scalar passed to dmp_set_orientation during initialization.   **/  if(sensors&INV_WXYZ_QUAT)   {    q0 = quat[0] / q30;    q1 = quat[1] / q30;    q2 = quat[2] / q30;    q3 = quat[3] / q30;     *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;  // pitch    *roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;  // roll    *yaw  = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;  //yaw  }else return 2;  return 0;}

至此，DMP代码已移植完毕，将所有代码文件加入工程，写好相关的包含文件就可以开始使用了。

//头文件包含#include "mpu6050.h"#include "inv_mpu.h"#include "inv_mpu_dmp_motion_driver.h"//MPU6050初始化MPU_Init();//DMP初始化 while(mpu_dmp_init()){//mpu_dmp_init()初始化成功将返回0    NOP();}//获取俯仰角，横滚角以及偏航角if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0){//成功获取偏航角返回0,建议成功获取数据后再开始相关操作 ……}//其他库函数使用请参照DMP官方手册

附：百度网盘（链接: http://pan.baidu.com/s/1c0lUMRI 密码: dsqs）包含移植完成的测试工程（可直接编译，下载测试）和DMP资料（官方源代码和手册）

注：本文的代码移植基于正点原子的stm32f407板子的MPU6050代码。

原标题：六轴加速度传感器MPU6050官方DMP库到瑞萨RL78/G13的移植

关键词：