本帖最后由 ketose 于 2015-9-9 09:40 编辑
前一篇学习了使用FireBLE的I2C来读MPU6050的原始数据，可是如果要做四轴这些是不够的。四轴需要计算出飞行器在空中的姿态，也就是 Roll，Pitch，Yaw 三个角度，如果采用原始数据经过滤波、数据融合，如果这些计算都交给MCU来做，难免会对MCU造成一定的压力，MCU还是专注控制的好。而且MPU6050本身提供了DMP库，还是直接让MPU6050把计算好的数据直接给我们吧。今天我们学习在FireBLE 开发板上移植MPU6050的DMP库。
实验目的：
在FireBLE开发板移植MPU6050的DMP库，读出四元数，计算出Roll,Pitch,Yaw三个角度。
实验电路：
参见： https://bbs.elecfans.com/jishu_510257_1_1.html
具体步骤：
1、首先从InvenSense官网上下载MPU6050的DMP库，这个DMP库官网已经移植了，不过是MSP430的项目，我们需要修改。
2、新建QN902X项目，把eMPL文件夹复制到项目文件夹下。然后添加dmp组，把eMPL文件夹里的inv_mpu.c和inv_mpu_dmp_mo tion_driver.c添加进来，如下图：
我们就是主要修改inv_mpu.c这个文件，在这个文件里也只是修改一小部分就是I2C读写部分，如下：
上框里的内容就是我们要修改的。其实下面的部分条件编译也是没用的，我把他们都一直干掉换成自己的代码如下：
- #ifdef QN_902X
- #define i2c_write i2cwrite
- #define i2c_read i2cread
- #define delay_ms mdelay
- #define get_ms get_ms
- #define log_e printf
- #define log_i printf
- //#define fabs fabsf
- #define min fmin
- #else
- #error Gyro driver is missing the system layer implementations.
- #endif
reg_int_cb 这个函数也干掉不用。然后就是要自己实验i2cwrite,i2cread,mdelay,get_ms四个函数如下：
- void get_ms(unsigned long *time)
- {
- }
- void mdelay(uint32_t ms)
- {
- timer_delay(QN_TIMER0,TIMER_PSCAL_DIV,TIMER_COUNT_MS(ms, TIMER_PSCAL_DIV));
- }
- int i2cwrite(uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data)
- {
- I2C_nBYTE_WRITE(addr, reg, data, len);
- return 0;
- }
- int i2cread(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
- {
- I2C_nBYTE_READ(addr, reg, buf, len);
- return 0;
- }
记得把相关的头文件引进来
#include "i2c.h"
#include "uart.h"
#include "timer.h"
OK移植完工了。。。
下面是主程序文件了：
- /****************************************************************************
- * $Id:: i2c_example.c $
- * Project: I2C read mpu6050 data by dmp example
- *
- * Description:
- * This file contains I2C driver usage.
- *
- ****************************************************************************/
- #include "i2c.h"
- #include "uart.h"
- #include "timer.h"
- #include "system.h"
- #include "inv_mpu.h"
- #include "inv_mpu_dmp_motion_driver.h"
- #include
- #include
- uint8_t i2cbuffer[104];
- static signed char gyro_orientation[9] = {-1, 0, 0,
- 0,-1, 0,
- 0, 0, 1};
- char num[50];
-
- #define q30 1073741824.0f
- #define u8 uint8_t
- #define MPU_OK 0
- #define MPU_ERR -1
- //传送数据给匿名四轴上位机软件(V2.6版本)
- //fun:功能字. 0XA0~0XAF
- //data:数据缓存区,最多28字节!!
- //len:data区有效数据个数
- void usart1_niming_report(u8 fun,u8*data,u8 len)
- {
- u8 send_buf[32];
- u8 i;
- if(len>28)return; //最多28字节数据
- send_buf[len+3]=0; //校验数置零
- send_buf[0]=0X88; //帧头
- send_buf[1]=fun; //功能字
- send_buf[2]=len; //数据长度
- for(i=0;i
- for(i=0;i
- for(i=0;i
- uart_write_one_byte(QN_UART0,send_buf[i]); //发送数据到串口1
- }
- //发送加速度传感器数据和陀螺仪数据
- //aacx,aacy,aacz:x,y,z三个方向上面的加速度值
- //gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值
- void mpu6050_send_data(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz)
- {
- u8 tbuf[12];
- tbuf[0]=(aacx>>8)&0XFF;
- tbuf[1]=aacx&0XFF;
- tbuf[2]=(aacy>>8)&0XFF;
- tbuf[3]=aacy&0XFF;
- tbuf[4]=(aacz>>8)&0XFF;
- tbuf[5]=aacz&0XFF;
- tbuf[6]=(gyrox>>8)&0XFF;
- tbuf[7]=gyrox&0XFF;
- tbuf[8]=(gyroy>>8)&0XFF;
- tbuf[9]=gyroy&0XFF;
- tbuf[10]=(gyroz>>8)&0XFF;
- tbuf[11]=gyroz&0XFF;
- usart1_niming_report(0XA1,tbuf,12);//自定义帧,0XA1
- }
-
- //通过串口1上报结算后的姿态数据给电脑
- //aacx,aacy,aacz:x,y,z三个方向上面的加速度值
- //gyrox,gyroy,gyroz:x,y,z三个方向上面的陀螺仪值
- //roll:横滚角.单位0.01度。 -18000 -> 18000 对应 -180.00 -> 180.00度
- //pitch:俯仰角.单位 0.01度。-9000 - 9000 对应 -90.00 -> 90.00 度
- //yaw:航向角.单位为0.1度 0 -> 3600 对应 0 -> 360.0度
- void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,short gyroz,short roll,short pitch,short yaw)
- {
- u8 tbuf[28];
- u8 i;
- for(i=0;i<28;i++)tbuf[i]=0;//清0
- tbuf[0]=(aacx>>8)&0XFF;
- tbuf[1]=aacx&0XFF;
- tbuf[2]=(aacy>>8)&0XFF;
- tbuf[3]=aacy&0XFF;
- tbuf[4]=(aacz>>8)&0XFF;
- tbuf[5]=aacz&0XFF;
- tbuf[6]=(gyrox>>8)&0XFF;
- tbuf[7]=gyrox&0XFF;
- tbuf[8]=(gyroy>>8)&0XFF;
- tbuf[9]=gyroy&0XFF;
- tbuf[10]=(gyroz>>8)&0XFF;
- tbuf[11]=gyroz&0XFF;
- tbuf[18]=(roll>>8)&0XFF;
- tbuf[19]=roll&0XFF;
- tbuf[20]=(pitch>>8)&0XFF;
- tbuf[21]=pitch&0XFF;
- tbuf[22]=(yaw>>8)&0XFF;
- tbuf[23]=yaw&0XFF;
- usart1_niming_report(0XAF,tbuf,28);//飞控显示帧,0XAF
- }
- /* Main Program */
- int main (void)
- {
- SystemInit();
-
- //Initialize UART
- uart_init(QN_UART0,__USART_CLK,UART_115200);
- uart_tx_enable(QN_UART0,MASK_ENABLE);
- uart_rx_enable(QN_UART0,MASK_ENABLE);
- uart_clock_on(QN_UART0);
-
- //Initialize Timer
- timer_init(QN_TIMER0, NULL);
- timer_config(QN_TIMER0, TIMER_PSCAL_DIV, TIMER_COUNT_US(1000, TIMER_PSCAL_DIV));
- timer_enable(QN_TIMER0, MASK_ENABLE);
-
- //Initialize I2C master
- i2c_init(I2C_SCL_RATIO(100000), i2cbuffer, 104);
- int result = mpu_init(NULL);
- if(result == MPU_OK)
- {
- printf("mpu initialization complete......n "); //mpu_set_sensor
- if(mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) == MPU_OK)
- printf("mpu_set_sensor complete ......n");
- else
- printf("mpu_set_sensor come across error ......n");
- if(mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) == MPU_OK) //mpu_configure_fifo
- printf("mpu_configure_fifo complete ......n");
- else
- printf("mpu_configure_fifo come across error ......n");
- if(mpu_set_sample_rate(DEFAULT_MPU_HZ) == MPU_OK) //mpu_set_sample_rate
- printf("mpu_set_sample_rate complete ......n");
- else
- printf("mpu_set_sample_rate error ......n");
- if(dmp_load_motion_driver_firmware() == MPU_OK) //dmp_load_motion_driver_firmvare
- printf("dmp_load_motion_driver_firmware complete ......n");
- else
- printf("dmp_load_motion_driver_firmware come across error ......n");
- if(dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)) == MPU_OK) //dmp_set_orientation
- printf("dmp_set_orientation complete ......n");
- else
- printf("dmp_set_orientation come across error ......n");
- if(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) == MPU_OK) //dmp_enable_feature
- printf("dmp_enable_feature complete ......n");
- else
- printf("dmp_enable_feature come across error ......n");
- if(dmp_set_fifo_rate(DEFAULT_MPU_HZ) == MPU_OK) //dmp_set_fifo_rate
- printf("dmp_set_fifo_rate complete ......n");
- else
- printf("dmp_set_fifo_rate come across error ......n");
- run_self_test();
- if(mpu_set_dmp_state(1) == MPU_OK)
- printf("mpu_set_dmp_state complete ......n");
- else
- printf("mpu_set_dmp_state come across error ......n");
- }
-
-
- while (1) /* Loop forever */
- {
- float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
-
- float Pitch,Roll,Yaw;
- unsigned long sensor_timestamp;
- short gyro[3], accel[3], sensors;
- unsigned char more;
- long quat[4];
-
- dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more);
- /* 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.
- */
- /* 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;
- usart1_report_imu(accel[0],
- accel[1],
- accel[2],
- gyro[0],
- gyro[1],
- gyro[2],
- (short)(Roll*100),
- (short)(Pitch*100),
- (short)(Yaw*10));
- }
- //delay(5000*10);
- }
- }
主程序文件把读出的四元数据转换成欧拉角，然后送上位机。
实验视频：
0
|
|
|
|
|
|
|
电子发烧友论坛
/6 
工商网监
湘ICP备2023018690号
8232
淘帖
937
发表评论
赞
回复
