[原创] F7 独轮机器人 MPU6050-DMP移植

ketose 2015-10-26 21:43 楼主

这篇主要谈独轮机器人的软件部分，独轮机器人要想能够站立不倒，那么必须有个反馈把现在的状态反馈给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的读写函数。

#if defined MOTION_DRIVER_TARGET_STM32F7
/*#include "msp430.h"
#include "msp430_i2c.h"
#include "msp430_clock.h"
#include "msp430_interrupt.h" */
#define i2c_write i2cWrite
#define i2c_read i2cRead
#define delay_ms delay_ms
#define get_ms get_ms
//static int reg_int_cb(struct int_param_s *int_param)
//{
// /*return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
// int_param->active_low);*/
// return 0;
//}
//#define log_i(...) do {} while (0)
//#define log_e(...) do {} while (0)
#define log_e printf
#define log_i printf
/* labs is already defined by TI's toolchain. */
/* fabs is for doubles. fabsf is for floats. */
#define fabs fabsf
#define min(a,b) ((a<b)?a:b)

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

void get_ms(unsigned long *time)
{
}
/**
****************************************************************************************
* @brief delay function
* @description
* This function is to use timer7 to delay for mpu6050 sensor.
*****************************************************************************************
*/
__weak void mdelay(uint32_t ms)
{
extern int delayCount;
delayCount = ms;
HAL_TIM_Base_Start_IT(&htim7);
while(delayCount != 0);
HAL_TIM_Base_Stop_IT(&htim7);
}
/**
****************************************************************************************
* @brief I2C function for write mpu data
* @description
* This function is implemented using the i2c bus protocol write data to the mpu6050 sensor.
*****************************************************************************************
*/
__weak int i2cwrite(uint8_t addr, uint8_t reg, uint8_t len, uint8_t * data)
{
if(HAL_I2C_Mem_Write(&hi2c1,addr<<1,reg,1,data,len,5000) == HAL_OK)
return 0;
else
return 1;
}
/**
****************************************************************************************
* @brief I2C function for read mpu data
* @description
* This function is to impletmented using i2c bus protocol read data from mpu6050 sensor.
*****************************************************************************************
*/
__weak int i2cread(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *data)
{
if(HAL_I2C_Mem_Read(&hi2c1,addr<<1,reg,1,data,len,5000) == HAL_OK)
return 0;
else
return 1;
}

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

/**
****************************************************************************************
* @brief start mpu6050 dmp
* @description
* This function is used to initialize mpu6050 dmp.
*****************************************************************************************
*/
void mpu_start()
{
int result = mpu_init(NULL);
while(result != MPU_OK)
{
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");
}
}

读数据：

/**
****************************************************************************************
* @brief read data from mpu6050
* @description
* This function is to use DMP to read data from mpu6050 sensor.
*****************************************************************************************
*/
int mpu_read(MPU_Data* data)
{
#define q30 1073741824.0f
#define u8 uint8_t
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short sensors;
unsigned char more;
long quat[4];
dmp_read_fifo(data->gyro, data->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;
data->Pitch = asin(2 * q1 * q3 - 2 * q0* q2)* 57.3; // pitch
data->Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
data->Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;
return MPU_OK;
}
return MPU_ERR;
}