【你的偏航角还在飘吗】STM32CUBEMX+MPU6050+MPL运动处理库移植教程
目录
- 前言
- 环境及平台介绍
- STM32CUBEMX配置
- 文件移植
- 代码移植
- 结论
前言
MPL是Invensense公司推出的一个运动处理库，用于欧拉角解算的效果亲测很棒。不过目前网上貌似并没有什么教程讲这个库的移植，在此跟大家分享一下自己的移植过程。
环境及平台介绍
- STM32F407VE
- MPU6050
- STM32CUBEMX 4.26.1
- MDK5
- MPL 6.12
官网：https://www.invensense.com/ 真的特别慢，下了好久好久。
链接：https://pan.baidu.com/s/1vX4UFOOXi2n9awqI_DQVGg
提取码：putd
STM32CUBEMX配置
新建工程，选择MCU型号就不说了。
- 打开I2C2、RCC HSE选择Crystal。
- 如果需要的话，可以打开USB CDC用作调试
- 时钟配置
- 外设配置
- 生成代码配置，生成好之后打开工程即可。
文件移植
- 解压motion_driver_6.12.zip，打开motion_driver_6.12\arm\STM32F4_MD6\Projects\eMD6
这个是官方给的例程，将Core文件夹复制到刚才生成的工程的根目录下
- 再将motion_driver_6.12\mpl libraries\arm\Keil中的libmpllib_Keil_M4.zip解压至libmpllib_Keil_M4
- 复制libmpllib.lib到工程文件夹下的IMU_test\core\mpl文件夹里
- 在IMU_test\Drivers目录下新建文件夹Devices，在其中新建mpu_module.c与mpu_module.h文件
代码移植
- 打开IMU_test\MDK-ARM\IMU_test.uvprojx
- 在Options for Target里添加预定义，在原有的之后加上“,MPL_LOG_NDEBUG=1,EMPL,MPU6050,EMPL_TARGET_STM32F4”
3. 继续添加包含目录
要添加下面这些。
- 添加分组，命名为MPL
- 右键点击MPL，添加一些文件进来，这一步要把core下子目录的所有的.c文件都添加进来，包括刚才复制的lib文件。
- 把刚才新建的mpu_module.c添加到User组下面
- 开始撸代码！
在mpu_module.c下添加如下代码。
/* Includes ------------------------------------------------------------------*/
//#include "inv_mpu20608.h"
#include "invensense.h"
#include "invensense_adv.h"
#include "eMPL_outputs.h"
#include "mltypes.h"
#include "mpu.h"
#include "log.h"
#include "packet.h"
#include "inv_mpu.h"
#include "mpu_module.h"
/* Private define ------------------------------------------------------------*/
#ifdef MPU_DEBUG
/* Data read from MPL. */
#define PRINT_ACCEL (0x01)
#define PRINT_GYRO (0x02)
#define PRINT_QUAT (0x04)
#define PRINT_EULER (0x10)
#define PRINT_ROT_MAT (0x20)
#define PRINT_HEADING (0x40)
#define PRINT_PEDO (0x80)
#define PRINT_LINEAR_ACCEL (0x100)
#define PRINT_GRAVITY_VECTOR (0x200)
#endif
/* Switch */
#define ACCEL_ON (0x01)
#define GYRO_ON (0x02)
/* Starting sampling rate. */
#define DEFAULT_mpu_HZ (1000)
#define TEMP_READ_TICK (500)
/* Private typedef -----------------------------------------------------------*/
struct hal_s {
unsigned char lp_accel_mode;
unsigned char lp_6axis_mode;
unsigned char sensors;
unsigned char watermark;
//volatile unsigned char new_sensor;
unsigned char motion_int_mode;
unsigned long next_temp_tick;
unsigned int report;
};
/* Platform-specific information. Kinda like a boardfile. */
struct platform_data_s {
signed char orientation[9];
};
/* Private variables ---------------------------------------------------------*/
/* The sensors can be mounted onto the board in any orientation. The mounting
* matrix seen below tells the MPL how to rotate the raw data from the driver(s).
*/
static struct platform_data_s gyro_pdata = {
.orientation = { 1, 0, 0,
0, 1, 0,
0, 0, 1}
};
static struct hal_s hal = {0};
unsigned char *mpl_key = (unsigned char*)"eMPL 5.1";
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
#ifdef MPU_DEBUG
/**
* @brief 从MPL库中读出规格数据
* @return 无
* @attention 通过UART返回
*/
static void read_from_mpl(void)
{
long data[9];
int8_t accuracy;
unsigned long timestamp;
float float_data[3] = {0};
if (inv_get_sensor_type_quat(data, &accuracy, (inv_time_t*)×tamp)) {
/* Sends a quaternion packet to the PC. Since this is used by the Python
* test app to visually represent a 3D quaternion, it's sent each time
* the MPL has new data.
*/
eMPL_send_quat(data);
/* Specific data packets can be sent or suppressed using USB commands. */
if (hal.report & PRINT_QUAT)
eMPL_send_data(PACKET_DATA_QUAT, data);
}
if (hal.report & PRINT_ACCEL) {
if (inv_get_sensor_type_accel(data, &accuracy,
(inv_time_t*)×tamp))
eMPL_send_data(PACKET_DATA_ACCEL, data);
}
if (hal.report & PRINT_GYRO) {
if (inv_get_sensor_type_gyro(data, &accuracy,
(inv_time_t*)×tamp))
eMPL_send_data(PACKET_DATA_GYRO, data);
}
if (hal.report & PRINT_EULER) {
if (inv_get_sensor_type_euler(data, &accuracy,
(inv_time_t*)×tamp))
eMPL_send_data(PACKET_DATA_EULER, data);
}
if (hal.report & PRINT_ROT_MAT) {
if (inv_get_sensor_type_rot_mat(data, &accuracy,
(inv_time_t*)×tamp))
eMPL_send_data(PACKET_DATA_ROT, data);
}
if (hal.report & PRINT_HEADING) {
if (inv_get_sensor_type_heading(data, &accuracy,
(inv_time_t*)×tamp))
eMPL_send_data(PACKET_DATA_HEADING, data);
}
if (hal.report & PRINT_LINEAR_ACCEL) {
if (inv_get_sensor_type_linear_acceleration(float_data, &accuracy,
(inv_time_t*)×tamp))
MPL_LOGI("Linear Accel: %7.5f %7.5f %7.5f\r\n",
float_data[0], float_data[1], float_data[2]);
}
if (hal.report & PRINT_GRAVITY_VECTOR) {
if (inv_get_sensor_type_gravity(float_data, &accuracy,
(inv_time_t*)×tamp))
MPL_LOGI("Gravity Vector: %7.5f %7.5f %7.5f\r\n",
float_data[0], float_data[1], float_data[2]);
}
}
#endif
/**
* @brief 包装器，包装inv_get_sensor_type_euler()函数
* @return 1 if data was updated.
* @attention
*/
int8_t mpu_read_euler(long *data, unsigned long *timestamp) {
int8_t tmp;
inv_get_sensor_type_euler(data, &tmp, timestamp);
return tmp;
}
/**
* @brief MPU模块初始化函数
* @return 无
* @attention
*/
int module_mpu_init(void)
{
inv_error_t result;
unsigned char accel_fsr;
unsigned short gyro_rate, gyro_fsr;
result = mpu_init();
if (result) {
MPL_LOGE("Could not initialize sensors.\n");
}
result = inv_init_mpl();
if (result) {
MPL_LOGE("Could not initialize MPL.\n");
}
/* Compute 6-axis quaternions. 计算6轴融合数据后的四元数 */
inv_enable_quaternion();
inv_enable_9x_sensor_fusion();
/* Update gyro biases when not in motion.
* 自动更新静止时的陀螺仪偏差。这三个函数为互斥函数。
*/
inv_enable_fast_nomot();
/* inv_enable_motion_no_motion(); */
/* inv_set_no_motion_time(1000); */
/* Update gyro biases when temperature changes. 温度变化后更新陀螺仪偏差*/
inv_enable_gyro_tc();
/* Allows use of the MPL APIs in read_from_mpl. 对数据输出提供支持*/
inv_enable_eMPL_outputs();
result = inv_start_mpl();
if (result == INV_ERROR_NOT_AUTHORIZED) {
while (1) {
MPL_LOGE("Not authorized.\n");
}
}
if (result) {
MPL_LOGE("Could not start the MPL.\n");
}
/* Get/set hardware configuration. Start gyro. */
/* Wake up all sensors. */
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
/* Push both gyro and accel data into the FIFO. */
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sample_rate(DEFAULT_mpu_HZ);
/* Read back configuration in case it was set improperly. */
mpu_get_sample_rate(&gyro_rate);
mpu_get_gyro_fsr(&gyro_fsr);
mpu_get_accel_fsr(&accel_fsr);
/* Sync driver configuration with MPL. */
/* Sample rate expected in microseconds. */
inv_set_gyro_sample_rate(1000000L / gyro_rate);
inv_set_accel_sample_rate(1000000L / gyro_rate);
/* Set chip-to-body orientation matrix.
* Set hardware units to dps/g's/degrees scaling factor.
*/
inv_set_gyro_orientation_and_scale(
inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
(long)gyro_fsr<<15);
inv_set_accel_orientation_and_scale(
inv_orientation_matrix_to_scalar(gyro_pdata.orientation),
(long)accel_fsr<<15);
/* Initialize HAL state variables. */
hal.sensors = ACCEL_ON | GYRO_ON;
hal.report = 0;
hal.next_temp_tick = 0;
return 0;
}
/**
* @brief 从MPU的FIFO中读取数据并处理。
其中也包含了mpl库对温度变化的处理。
* @return 无
* @attention 应该至少为10ms执行一次
*/
int mpu_module_sampling()
{
unsigned char sensors, more,new_temp = 0;
static unsigned long ticktime = 0;
unsigned long sensor_timestamp,cycletime = 0;
int new_data = 0;
ticktime++;
/* 温度没有必要每次都读取。按照一定时间间隔读取 */
if (ticktime > hal.next_temp_tick) {
hal.next_temp_tick = ticktime + TEMP_READ_TICK;
new_temp = 1; //start task temp;
}
/* 没有任何sensor打开时 */
if (!hal.sensors) return 0;
do {
short gyro[3], accel_short[3];
long accel[3], temperature;
cycletime++;
mpu_read_fifo(gyro, accel_short, &sensor_timestamp,
&sensors, &more);
if (sensors & INV_XYZ_GYRO) {
/* 将数据送入MPL. */
inv_build_gyro(gyro, sensor_timestamp);
new_data = 1;
if (new_temp) {
new_temp = 0;
/* Temperature only used for gyro temp comp. */
mpu_get_temperature(&temperature, &sensor_timestamp);
inv_build_temp(temperature, sensor_timestamp);
}
}
if (sensors & INV_XYZ_ACCEL) {
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
inv_build_accel(accel, 0, sensor_timestamp);
new_data = 1;
}
if (new_data) {
if(inv_execute_on_data()) {
MPL_LOGE("ERROR execute on data\n");
}
#ifdef MPU_DEBUG
read_from_mpl();
#endif
}
}
while(more);
return cycletime;
}
在mpu_module.h中添加如下代码
#ifndef __MPU_MODULE__
#define __MPU_MODULE__
/* Includes ------------------------------------------------------------------*/
/* Function typedef -----------------------------------------------------------*/
/**
* @brief MPU模块初始化函数
* @return 无
* @attention
*/
int module_mpu_init(void);
/**
* @brief 从MPU的FIFO中读取数据并处理。
其中也包含了mpl库对温度变化的处理。
* @return 循环次数，用于优化
* @attention 应该至少为10ms执行一次
*/
int mpu_module_sampling(void);
/**
* @brief 包装器，包装inv_get_sensor_type_euler()函数
* @return 1 if data was updated.
* @attention
*/
signed char mpu_read_euler(long *data, unsigned long *timestamp);
#endif
- 在main.c中添加如下函数，注意注释位置，在main.h中添加对应的函数声明
/* USER CODE BEGIN 4 */
void mdelay(unsigned long num_ms)
{
HAL_Delay(num_ms);
}
void get_tick_count(unsigned long *count)
{
*count = HAL_GetTick();
}
/* USER CODE END 4 */
- 在i2c.c下添加代码，在对应的i2c.h文件中添加函数声明
/* USER CODE BEGIN 1 */
uint8_t Sensors_I2C_WriteRegister(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data)
{
return HAL_I2C_Mem_Write(&hi2c2, slave_addr, (uint16_t)reg_addr, length, data, 1, 3000);
}
uint8_t Sensors_I2C_ReadRegister(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data)
{
return HAL_I2C_Mem_Read(&hi2c2, slave_addr, (uint16_t)reg_addr, I2C_MEMADD_SIZE_8BIT, data, length, 3000);
}
/* USER CODE END 1 */
- 在inv_mpu.c文件下找到503行这段代码，把addr改成0xD0，这是一个坑，和MPU6050的地址根本对不上，应该是官方留的bug。
const struct hw_s hw = {
.addr = 0xD0,
.max_fifo = 1024,
.num_reg = 118,
.temp_sens = 340,
.temp_offset = -521,
.bank_size = 256
#if defined AK89xx_SECONDARY
,.compass_fsr = AK89xx_FSR
#endif
};
- 主函数中添加代码
/* USER CODE BEGIN 2 */
module_mpu_init();
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
timestamp = HAL_GetTick(); //¶ÁÈ¡µ±Ç°Ê±¼ä
mpu_module_sampling(); //MPL²ÉÑùÊý¾Ý
if (mpu_read_euler(data_buf, ×tamp)) //¶ÁÈ¡MPLÊä³öµÄÅ·À½ÇÊý¾Ý
{
pitch = 1.0f*data_buf[0]/65536.f; //½«longÀàÐ͵ÄMPLÊä³öÊý¾Ýת»»³É½Ç¶Èµ¥Î»
roll = 1.0f*data_buf[1]/65536.f;
yaw = 1.0f*data_buf[2]/65536.f;
usbprint("pitch:%lf\t\troll:%lf\t\tyaw:%lf\t\tt:%ld\t\t\r\n", pitch, roll, yaw, timestamp);//如果需要的话可以把这里换成串口的输出。
}
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
- 到这里应该就可以进行编译了，如果编译不成功就自己再找找bug吧orz，也欢迎评论说一下问题，抽空回复并更新在这里。
结论
本文详细介绍了MPL库的移植过程。希望能帮到大家~
听说在官网上下载5.12版本，里面会有API手册！6.12很奇怪的没有这个，所以只能看着例程写一写代码这样子。大家感兴趣可以去官网下载呀。