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_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 = 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.0f int 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代碼。