F7 独轮机器人 MPU6050-DMP移植

ketose

F7 独轮机器人 MPU6050-DMP移植 [复制链接]

     这篇主要谈独轮机器人的软件部分，独轮机器人要想能够站立不倒，那么必须有个反馈把现在的状态反馈给MCU。这个反馈在该项目中使用的是MPU6050这个模块，所以我们就要把MPU6050的数据读出来。MPU6050是什么东西？MPU-6000（6050）的角速度全格感测范围为±250、±500、±1000与±2000°/sec (dps)，可准确追踪快速与慢速动作，并且，用户可程式控制的加速器全格感测范围为±2g、±4g±8g与±16g。产品传输可透过最高至400kHz的IC或最高达20MHz的SPI（MPU-6050没有SPI）。MPU-6000可在不同电压下工作，VDD供电电压介为2.5V±5%、3.0V±5%或3.3V±5%，逻辑接口VVDIO供电为1.8V± 5%（MPU6000仅用VDD）。MPU-6000的包装尺寸4x4x0.9mm(QFN)，在业界是革命性的尺寸。其他的特征包含内建的温度感测器、包含在运作环境中仅有±1%变动的振荡器。
    以数字输出6轴或9轴的旋转矩阵、四元数(quaternion)、欧拉角格式(Euler Angle forma)的融合演算数据。 具有131 LSBs/°/sec 敏感度与全格感测范围为±250、±500、±1000与±2000°/sec 的3轴角速度感测器(陀螺仪)。 可程式控制，且程式控制范围为±2g、±4g、±8g和±16g的3轴加速器。 移除加速器与陀螺仪轴间敏感度，降低设定给予的影响与感测器的飘移。 数字运动处理(DMP: Digital Motion Processing)引擎可减少复杂的融合演算数据、感测器同步化、姿势感应等的负荷。 运动处理数据库支持Android、Linux与Windows 内建之运作时间偏差与磁力感测器校正演算技术，免除了客户须另外进行校正的需求。
   读MPU6050的数据有两种方案，一种通过寄存器直接读出原始数据，然后进行数据融合，计算出机器人的倾斜角度和角速度。还有一种是使用MPU6050数字运动处理(DMP: Digital Motion Processing)引擎。我们的项目中使用DMP，官方只给出了DMP在MSP430上的例子，今天就来说说MPU6050的DMP在STM32F7上的移植。
先看下电路图：

从原理上看，使用了PB8,PB9两个引角，所以在配置的时候，我们配置这两个引角

另外我们项目使用到FreeRTOS所以这个也必须选上，还有就是使用串口来给上位机传输调试信息，所以串口也要选上。
时钟配置如下：

FreeRTOS再添加两个任务。堆栈由原来的128改为256.因为MPU6050 DMP库使用的堆栈超过了128.

一切配置完后生成项目。
在生成的项目文件夹里新建Drv文件夹，然后把MPU6050 DMP官方例子里的eMPL文件夹全部复制过来，并添加到项目里。

一切准备工作就绪，下面就来看如何移植到F7里了。其实就一个文件inv_mpu.c把这个文件里所有MPS430的注释到，然后实现I2C的读写函数。

1. #if defined MOTION_DRIVER_TARGET_STM32F7
   
2. <font color="Green">/*#include "msp430.h"
   
3. #include "msp430_i2c.h"
   
4. #include "msp430_clock.h"
   
5. #include "msp430_interrupt.h" */</font>
   
6. 
   
7. #define i2c_write   i2cWrite
   
8. #define i2c_read    i2cRead
   
9. #define delay_ms    delay_ms
   
10. #define get_ms      get_ms
    
11. 
    
12. <font color="Green">//static int reg_int_cb(struct int_param_s *int_param)
    
13. //{
    
14. //    /*return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
    
15. //        int_param->active_low);*/
    
16. //                return 0;
    
17. //}         
    
18. //#define log_i(...)     do {} while (0)
    
19. //#define log_e(...)     do {} while (0)</font>
    
20. #define log_e    printf
    
21. #define log_i    printf
    
22. <font color="Green">/* labs is already defined by TI's toolchain. */
    
23. /* fabs is for doubles. fabsf is for floats. */</font>
    
24. #define fabs        fabsf
    
25. #define min(a,b) ((a<b)?a:b)

复制代码

实现硬件相关的几个函数：

1. void get_ms(unsigned long *time)
   
2. {
   
3. 
   
4. }
   
5. <font color="Green">/**
   
6. ****************************************************************************************
   
7. * @brief  delay function
   
8. * @description
   
9. *  This function is to use timer7 to delay for mpu6050 sensor.
   
10. *****************************************************************************************
    
11. */</font>
    
12. __weak void mdelay(uint32_t ms)
    
13. {
    
14.         extern int delayCount;
    
15.         
    
16.         delayCount = ms;
    
17.         HAL_TIM_Base_Start_IT(&htim7);
    
18.         while(delayCount != 0);
    
19.         HAL_TIM_Base_Stop_IT(&htim7);
    
20. }
    
21. <font color="Green">/**
    
22. ****************************************************************************************
    
23. * @brief  I2C function for write mpu data
    
24. * @description
    
25. *  This function is implemented using the i2c bus protocol write data to the mpu6050 sensor.
    
26. *****************************************************************************************
    
27. */</font>
    
28. __weak int i2cwrite(uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data)
    
29. {
    
30.         if(HAL_I2C_Mem_Write(&hi2c1,addr<<1,reg,1,data,len,5000) == HAL_OK)
    
31.                 return 0;
    
32.         else
    
33.                 return 1;
    
34. }
    
35. <font color="Green">/**
    
36. ****************************************************************************************
    
37. * @brief  I2C function for read mpu data
    
38. * @description
    
39. *  This function is to impletmented using i2c bus protocol read data from mpu6050 sensor.
    
40. *****************************************************************************************
    
41. */</font>
    
42. __weak int i2cread(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *data)
    
43. {
    
44.         if(HAL_I2C_Mem_Read(&hi2c1,addr<<1,reg,1,data,len,5000) == HAL_OK)
    
45.                 return 0;
    
46.         else
    
47.                 return 1;
    
48. }

复制代码

OK了，移植完成了，很简单，再增加两个辅助函数，一个是MPU6050 DMP的初始化，一个就是读MPU6050的数据。

1. <font color="Green">/**
   
2. ****************************************************************************************
   
3. * @brief  start mpu6050 dmp
   
4. * @description
   
5. *  This function is used to initialize mpu6050 dmp.
   
6. *****************************************************************************************
   
7. */</font>
   
8. void mpu_start()
   
9. {
   
10.     int result = mpu_init(NULL);
    
11.         while(result != MPU_OK)
    
12.         {
    
13.                 result = mpu_init(NULL);
    
14.         }
    
15.     if(result == MPU_OK)
    
16.     {
    
17.         printf("mpu initialization complete......\n ");                   //mpu_set_sensor
    
18.         if(mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) == MPU_OK)
    
19.             printf("mpu_set_sensor complete ......\n");
    
20.         else
    
21.             printf("mpu_set_sensor come across error ......\n");
    
22.         if(mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) == MPU_OK)                     //mpu_configure_fifo
    
23.             printf("mpu_configure_fifo complete ......\n");
    
24.         else
    
25.             printf("mpu_configure_fifo come across error ......\n");
    
26.         if(mpu_set_sample_rate(DEFAULT_MPU_HZ) == MPU_OK)                     //mpu_set_sample_rate
    
27.             printf("mpu_set_sample_rate complete ......\n");
    
28.         else
    
29.             printf("mpu_set_sample_rate error ......\n");
    
30.         if(dmp_load_motion_driver_firmware() == MPU_OK)             //dmp_load_motion_driver_firmvare
    
31.             printf("dmp_load_motion_driver_firmware complete ......\n");
    
32.         else
    
33.             printf("dmp_load_motion_driver_firmware come across error ......\n");
    
34.         if(dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)) == MPU_OK)           //dmp_set_orientation
    
35.             printf("dmp_set_orientation complete ......\n");
    
36.         else
    
37.             printf("dmp_set_orientation come across error ......\n");
    
38.         if(dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
    
39.                               DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
    
40.                               DMP_FEATURE_GYRO_CAL) == MPU_OK)                             //dmp_enable_feature
    
41.             printf("dmp_enable_feature complete ......\n");
    
42.         else
    
43.             printf("dmp_enable_feature come across error ......\n");
    
44.         if(dmp_set_fifo_rate(DEFAULT_MPU_HZ) == MPU_OK)             //dmp_set_fifo_rate
    
45.             printf("dmp_set_fifo_rate complete ......\n");
    
46.         else
    
47.             printf("dmp_set_fifo_rate come across error ......\n");
    
48.         run_self_test();
    
49.         if(mpu_set_dmp_state(1) == MPU_OK)
    
50.             printf("mpu_set_dmp_state complete ......\n");
    
51.         else
    
52.             printf("mpu_set_dmp_state come across error ......\n");
    
53.     }
    
54. }
    

复制代码

读数据：

1. <font color="Green">/**
   
2. ****************************************************************************************
   
3. * @brief  read data from mpu6050
   
4. * @description
   
5. *  This function is to use DMP to read data from mpu6050 sensor.
   
6. *****************************************************************************************
   
7. */</font>
   
8. int mpu_read(MPU_Data* data)
   
9. {
   
10. 
    
11. #define q30          1073741824.0f
    
12. #define u8                uint8_t
    
13. 
    
14.     float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
    
15. 
    
16.     unsigned long sensor_timestamp;
    
17.     short sensors;
    
18.     unsigned char more;
    
19.     long quat[4];
    
20. 
    
21.     dmp_read_fifo(data->gyro, data->accel, quat, &sensor_timestamp, &sensors,&more);
    
22.     /* Gyro and accel data are written to the FIFO by the DMP in chip
    
23.     * frame and hardware units. This behavior is convenient because it
    
24.     * keeps the gyro and accel outputs of dmp_read_fifo and
    
25.     * mpu_read_fifo consistent.
    
26.     */
    
27. 
    
28.     /* Unlike gyro and accel, quaternions are written to the FIFO in
    
29.     * the body frame, q30. The orientation is set by the scalar passed
    
30.     * to dmp_set_orientation during initialization.
    
31.     */
    
32.     if (sensors & INV_WXYZ_QUAT )
    
33.     {
    
34.         q0=quat[0] / q30;
    
35.         q1=quat[1] / q30;
    
36.         q2=quat[2] / q30;
    
37.         q3=quat[3] / q30;
    
38.         data->Pitch  = asin(2 * q1 * q3 - 2 * q0* q2)* 57.3; // pitch
    
39.         data->Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
    
40.         data->Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
    
41.                 return MPU_OK;
    
42.     }
    
43. 
    
44.     return MPU_ERR;
    
45. }

复制代码

在freertos.c文件里添加任务代码：

1. <font color="Green">/* StartDefaultTask function */</font>
   
2. void StartDefaultTask(void const * argument)
   
3. {
   
4.     <font color="Green">/* USER CODE BEGIN StartDefaultTask */</font>
   
5.     <font color="Green">/* Infinite loop */</font>
   
6.     for(;;)
   
7.     {
   
8.         if(mpu_read(&Data) == MPU_OK)
   
9.         {
   
10.             <font color="Green">//printf("Pitch:%f\tRoll:%f\tYaw:%f\n",Data.Pitch,Data.Roll,Data.Yaw);</font>
    
11.             report_to_pc(Data.accel[0],
    
12.                          Data.accel[1],
    
13.                          Data.accel[2],
    
14.                          Data.gyro[0],
    
15.                          Data.gyro[1],
    
16.                          Data.gyro[2],
    
17.                          (short)(Data.Roll*100),
    
18.                          (short)(Data.Pitch*100),
    
19.                          (short)(Data.Yaw*100));
    
20.         }
    
21.         osDelay(10);
    
22.     }
    
23.     <font color="Green">/* USER CODE END StartDefaultTask */</font>
    
24. }

复制代码

我这里是反数据上传到匿名四轴的上位机观看状态数据。效果视频

arthasarthas

这个四轴的软件挺高大上，能否共享学习下，呵呵

紫阳大仙

学习一下

nmg

哈哈，过来看热闹

视频要是有配音就好了

天天好心情亲

楼主  我那个添加的时候好像无法使用模拟I2C 请问 是怎么回事？？？？寄存器没有设置的问题？？？？？？？

ketose

天天好心情亲 发表于 2015-11-5 08:39
楼主  我那个添加的时候好像无法使用模拟I2C 请问 是怎么回事？？？？寄存器没有设置的问题？？？？？？？

这个例子是硬件I2C，没有模拟I2C。模拟I2C应该是自己用IO口来处理，基本上和这个没有多大关系了。
现在STM32的硬件I2C挺好用的，所以能使用硬件就使用硬件。