前言

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配置

文件移植

代碼移植

結論

「Pygors跨平臺GUI」1：Pygors跨平臺GUI應用研究

[轉帖]

python列出centos7內存使用前50的進程信息

「Pygors跨平臺GUI」2：安裝MinGW-w64、MSYS2還是WSL2

一鍵自動化博客發佈工具,用過的人都說好(掘金篇)

通義千問 2.5 “客串” ChatGPT4，你分的清嗎？

Garnet：微軟官方基於.NET開源的高性能分佈式緩存存儲數據庫

Flink執行圖

Java響應式編程

評估統計算法在銀行僞造鈔票檢測中的價值

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以太網驅動總結