前言

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*)&timestamp)) {
       /* 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*)&timestamp))
            eMPL_send_data(PACKET_DATA_ACCEL, data);
    }
    if (hal.report & PRINT_GYRO) {
        if (inv_get_sensor_type_gyro(data, &accuracy,
            (inv_time_t*)&timestamp))
            eMPL_send_data(PACKET_DATA_GYRO, data);
    }
    if (hal.report & PRINT_EULER) {
        if (inv_get_sensor_type_euler(data, &accuracy,
            (inv_time_t*)&timestamp))
            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*)&timestamp))
            eMPL_send_data(PACKET_DATA_ROT, data);
    }
    if (hal.report & PRINT_HEADING) {
        if (inv_get_sensor_type_heading(data, &accuracy,
            (inv_time_t*)&timestamp))
            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*)&timestamp))
        	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*)&timestamp))
            	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, &timestamp))		//¶ÁÈ¡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很奇怪的没有这个，所以只能看着例程写一写代码这样子。大家感兴趣可以去官网下载呀。

【你的偏航角还在飘吗】STM32CUBEMX+MPU6050+MPL运动处理库移植教程

目录

前言

环境及平台介绍

STM32CUBEMX配置

文件移植

代码移植

结论

vue项目获取富文本编辑器wangEditor内容导出为word（html转word格式并下载）

dotnet C# 创建 X11 应用时设置窗口背景颜色

Navicat安装与激活教程

TDengine docker安装方法

vue3组件通信与props

sapui5

Alpine Linux apk add DNS lookup error

部分JDK版本的发布时间

工作中用到的脚本合集

合并代码时Beyond Compare设置

PAT 1003 Emergency題解及buglist

KMP算法（複習自用）

【你的偏航角還在飄嗎】STM32CUBEMX+MPU6050+MPL運動處理庫移植教程

VESC電調配置教程

SOLIDWORKS導出URDF 注意事項 buglist 教程

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結