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

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_ms
void 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 = ;
for(; n>; n--);
} //定义完成后,需要将myiic.c和mpu6050.c文件中的所有delay(x)修改为x个delay(),如:
void IIC_Stop(void)
{
SDA_OUT();
IIC_SCL=;
IIC_SDA=;
delay_us();delay_us();delay_us();delay_us();//原为delay_us(4)
IIC_SCL=;
IIC_SDA=;
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[];
unsigned char regs[];
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[];
unsigned short fifo_count;
if (!st.chip_cfg.dmp_on)
return -;
if (!st.chip_cfg.sensors)
return -; if (i2c_read(st.hw->addr, st.reg->fifo_count_h, , tmp))
return -; //fifo_count = (tmp[0] << 8) | tmp[1];//将该行语句修改为下面三行,原因为CS+不会自动将tmp[]数组转换为16位数据类型,需要手动转换
fifo_count=tmp[];
fifo_count=fifo_count<<;
fifo_count|=tmp[];
……
} //然后在inv_mpu.c中添加以下代码,以方便调用
#define q30 1073741824.0f int mget_ms(unsigned long *time)
{
return ;
} static signed char gyro_orientation[] = { , , ,
, , ,
, , }; 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 + ) << ;
scalar |= inv_row_2_scale(mtx + ) << ; return scalar;
} unsigned short inv_row_2_scale(const signed char *row)
{
unsigned short b; if (row[] > )
b = ;
else if (row[] < )
b = ;
else if (row[] > )
b = ;
else if (row[] < )
b = ;
else if (row[] > )
b = ;
else if (row[] < )
b = ;
else
b = ; // error
return b;
} uint8_t mpu_dmp_init(void)
{
uint8_t res=;
long gyro[], accel[];
if(mpu_init()==)
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);
if(res)return ;
res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
if(res)return ;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ);
if(res)return ;
res=dmp_load_motion_driver_firmware();
if(res)return ;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
if(res)return ;
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 ;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);
if(res)return ;
res=mpu_run_self_test(gyro, accel);
if(res==0x3)return ;
res=mpu_set_dmp_state();
if(res)return ;
return ;
}
else{
return ;
}
} 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[], accel[], sensors;
unsigned char more;
long quat[];
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return ;
/* 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[] / q30;
q1 = quat[] / q30;
q2 = quat[] / q30;
q3 = quat[] / q30;
*pitch = asin(- * q1 * q3 + * q0* q2)* 57.3; // pitch
*roll = atan2( * q2 * q3 + * q0 * q1, - * q1 * q1 - * q2* q2 + )* 57.3; // roll
*yaw = atan2(*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
}else return ;
return ;
}

至此,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,建议成功获取数据后再开始相关操作
……
} //其他库函数使用请参照DMP官方手册

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

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

最新文章

  1. Nginx负载均衡的详细配置及使用案例详解.
  2. 20160928_CentOS6.4x64_Oracle11gR2x64
  3. 15套漂亮的 PSD 格式的图标,不一样的视觉效果
  4. 测试一个C段网络的联通性
  5. python爬取某些网站出错的解决办法
  6. 自定义UICollectionViewController之后 如何设置UICollectionView的布局方式--备用
  7. [BZOJ1003] [ZJOI2006] 物流运输trans (最短路 &amp; dp)
  8. 全废话SQL Server统计信息(1)——统计信息简介
  9. Numpy系列(十二)- 矩阵运算
  10. mysql实现简单的增删改查,放入xmapp自带数据库中
  11. redis发布订阅、事务、脚本
  12. Python: Ubuntu 安装numpy,scipy,matplotlib
  13. Python全栈学习_day003知识点
  14. Azure ARM (15) 根据现有VHD文件,创建ARM VM
  15. saliency map [转]
  16. linux 协议栈tcp的rst报文中,seq的选取问题
  17. hibernate关联映射注解
  18. Thunder团队Alpha周贡献分规则及贡献分分配结果
  19. Java多线程多个线程wait(),一个notify()唤醒,唤醒的顺序
  20. uvalive 7299 Boggle

热门文章

  1. 最短的数字判断代码 js
  2. Droid@screen:在PC屏幕上显示Android手机屏幕
  3. 十大Intellij IDEA快捷键
  4. jScrollPane 美化滚动条
  5. Dropplets – 极简的 Markdown 博客平台
  6. javascript的switch的使用注意
  7. reactjs学习一(环境搭配react+es6+webpack热部署)
  8. 编译安装mysql(Ubuntu10 64位)
  9. Gradle基础
  10. IOS开发之小实例--创建一个简单的用于视频录制和回放的应用程序