目录

  • 1 DMP 官方库介绍
    • 1.1 DMP与MPL(Motion Processing Libraries)功能
    • 1.2 运行MPL的硬件要求
    • 1.3 Motion Driver 6.12 的架构
      • 1.3.1 MD6.12 驱动层（Driver Layer）
      • 1.3.2 MPL Library
      • 1.3.3 eMPL-HAL
  • 2 ESP32 DMP移植过程记录
    • 2.1 inv_mpu.c
      • 2.1.1 修改预处理部分
      • 2.1.2 修改数据定义部分
      • 2.1.3 修改函数部分
    • 2.2 inv_mpu_dmp_motion_driver.c
      • 2.2.1 修改预处理部分
      • 2.2.2 函数修改
    • 2.3 功能函数封装
      • 2.3.1 预处理部分
      • 2.3.2 数据定义部分
      • 2.3.2 函数部分
  • 3 测试工程开发
  • 4 MPU6050 测试上位机开发
  • 5 源文件获取

先上运行效果：

1 DMP 官方库介绍

Motion Driver是传感器驱动层的嵌入式软件堆栈，可轻松配置和利用Invense运动跟踪解决方案的许多功能。支持的运动设备为MPU6050/MPU6500/MPU9150/MPU9250。硬件和板载数字运动处理器（DMP）的许多功能都封装在模块化API中，可供使用和参考。

1.1 DMP与MPL(Motion Processing Libraries)功能

  • DMP：
    DMP是一种快速、低功耗、可编程的嵌入式轻量级微处理器。它的设计目的是从MCU中释放传感器融合和手势识别等功能，以节省系统的整体功耗。

DMP有很多功能，并且可以在运行过程中动态配置。除计步器外，所有DMP数据均输出至FIFO。DMP还可以编程，通过手势或数据准备就绪生成中断。

DMP的功能：

  • 3轴低功耗四元数：仅陀螺仪
  • 6轴低功耗四元数：陀螺仪和加速度计
  • 方向手势识别
  • 点击手势识别
  • 计步器手势识别
  • DMP中断

Motion Driver 6.12 的功能：

  • DMP 功能

    • 3/6 轴低功耗四元数
    • 点击、方向和计步器手势识别
  • MPL 算法
    • 运行中陀螺校准
    • 运行中陀螺温度补偿
    • 运行中罗盘校正
    • 运行中磁干扰抑制
    • 3/6/9轴传感器融合
  • 硬件功能
    • 校准
    • 自检
    • 保存和加载传感器状态
    • 低功耗加速模式
    • 低功耗运动中断模式
    • 寄存器转储

1.2 运行MPL的硬件要求

运行 Motion Driver 6.12 的 MCU 的硬件要求

  • Flash和RAM

    • 16位MCU：128k Flash , 128k RAM
    • 32位MCU：68k Flash, 10k RAM
  • Long Long 数据格式的支持
  • 中断
  • 采样率
    传感器融合需要来自MCU的大量计算能力。这会影响每个样本的处理量，并限制采样率。需要确保每帧数据能及时处理。

1.3 Motion Driver 6.12 的架构

1.3.1 MD6.12 驱动层（Driver Layer）

MD6.12 驱动层（Driver Layer）包含的文件：

  • inv_mpu
    底层驱动，包含I2C读写、时钟、日志打印等功能，这也是我们移植的时候需要适配平台的部分。
  • inv_mpu_dmp_motion_driver
    包含dmp映像的驱动程序以及加载和配置dmp的API。
  • dmpKey.h
    包含DMP特性的DMP内存位置的定义
  • dmpmap.h
    包含DMP内存位置的定义

我们在移植的时候，需要基于自己的平台提供下面的API给MD6.12使用：

  • #define i2c_write msp430_i2c_write
  • #define i2c_read msp430_i2c_read
  • #define delay_ms msp430_delay_ms
  • #define get_ms msp430_get_clock_ms
  • #define log_i MPL_LOGI
  • #define log_e MPL_LOGE
    *以上是msp430平台的函数定义，我们改成自己的名字就行，这里我命名为
#define i2c_write   esp32_i2c_write
#define i2c_read    esp32_i2c_read
#define delay_ms    esp32_delay_ms
#define get_ms      esp32_get_clock_ms

i2c_write/i2c_read:
需要输入以下4个参数：

  • unsigned char slave_addr：器件地址
  • unsigned char reg_addr：寄存器地址
  • unsigned char length：数据长度
  • unsigned char *data：数据buffer

对于I2C读写函数，ESP32 IDF平台调用官方的 i2c 基本读写命令即可：

int esp32_i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data)
{i2c_cmd_handle_t cmd = i2c_cmd_link_create();i2c_master_start(cmd);i2c_master_write_byte(cmd, (slave_addr << 1) | WRITE_BIT, ACK_CHECK_EN);i2c_master_write_byte(cmd, reg_addr, ACK_CHECK_EN);i2c_master_write(cmd, data, length, ACK_CHECK_EN);i2c_master_stop(cmd);esp_err_t ret = i2c_master_cmd_begin(0, cmd, 1000 / portTICK_RATE_MS);i2c_cmd_link_delete(cmd);// printf("esp32_i2c_write\n");return ret;
}int esp32_i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data)
{if (length == 0) {return ESP_OK;}i2c_cmd_handle_t cmd = i2c_cmd_link_create();i2c_master_start(cmd);i2c_master_write_byte(cmd, (slave_addr << 1) | WRITE_BIT, ACK_CHECK_EN);i2c_master_write_byte(cmd, reg_addr, ACK_CHECK_EN);i2c_master_start(cmd);i2c_master_write_byte(cmd, (slave_addr << 1) | READ_BIT, ACK_CHECK_EN);if (length > 1) {i2c_master_read(cmd, data, length - 1, ACK_VAL);}i2c_master_read_byte(cmd, data + length - 1, NACK_VAL);i2c_master_stop(cmd);esp_err_t ret = i2c_master_cmd_begin(0, cmd, 1000 / portTICK_RATE_MS);i2c_cmd_link_delete(cmd);return ret;
}

delay_ms：延时ms
延时也采用官方的延时函数：

int esp32_delay_ms(unsigned long num_ms)
{vTaskDelay(num_ms / portTICK_RATE_MS);return 0;
}

get_ms：获取时间

int esp32_get_clock_ms(unsigned long *count)
{*count = (unsigned long)esp_timer_get_time();return 0;
}

log_i/log_e: 打印信息和错误
直接用printf就行。就不区分信息还是错误了。

#define log_i     printf
#define log_e     printf

1.3.2 MPL Library

MPL库是专有Invense运动应用程序算法的核心，由Mllite和MPL目录组成。MPL不需要移植。您可能需要包含特定于系统的头文件，以支持mllite包中的memcpy、memset等函数调用。

1.3.3 eMPL-HAL

包含从MPL库获取各种数据的API。

2 ESP32 DMP移植过程记录

移植的过程我是参考小马哥STM32课程系列直播-第十一讲（MPU6050 官方DMP库的移植），小马哥是移植MPL到STM32，因为基本的过程是差不多的，只是一些细节略有差异，现在将移植的过程记录如下：

2.1 inv_mpu.c

2.1.1 修改预处理部分

1 首先需要选择器件
因为这个驱动是支持多种MCU(MSP430、UC3L0) 和 MPU(MPU9150、MPU6050、MPU9250等) 的，所以文件中有很多条件编译。我们需要选择在某一个器件的基础上进行移植。
在“ inv_mpu.h ”添加宏定义即可：
c #define MOTION_DRIVER_TARGET_MSP430 #define MPU6050
2 将msp430相关的头文件注释掉，由于IDF平台是在make时会自己索引平台驱动，所以这里我们只需要注释掉就行。

// #include "msp430.h"
// #include "msp430_i2c.h"
// #include "msp430_clock.h"
// #include "msp430_interrupt.h"
···首先修改上述基于平台的功能函数宏定义,改为
```c
#include "empl_driver.h"
#define i2c_write   esp32_i2c_write
#define i2c_read    esp32_i2c_read
#define delay_ms    esp32_delay_ms
#define get_ms      esp32_get_clock_ms

我的处理是将上诉函数全部打包在一个名为 empl_driver.c 的文件中，在 inv_mpu.c 包含 empl_driver.h 即可。具体的函数实现见 1.3.1 MD6.12 驱动层（Driver Layer）。

2.1.2 修改数据定义部分

将 结构体 gyro_reg_s 的 accel_cfg2 、 lp_accel_odr 与 accel_intel成员注释掉。

2.1.3 修改函数部分

mpu_init mpu初始化函数:
struct int_param_s *int_param是用于中断API的平台特定参数。为了简化可以不传入此参数，所以需要将函数定义改为：

int mpu_init(void);

同时返回此参数的部分需要注释掉

    // if (int_param)//     reg_int_cb(int_param);

2.2 inv_mpu_dmp_motion_driver.c

2.2.1 修改预处理部分

1 选择器件
相同的，在 inv_mpu_dmp_motion_driver.h 中加上宏定义：

#define MOTION_DRIVER_TARGET_MSP430

2 与 inv_mpu.c 的移植一样，需要注释掉与 MSP430 相关的头文件包含。

// #include "msp430.h"
// #include "msp430_clock.h"

并将相关的功能函数改成 ESP32 平台的

#define delay_ms    esp32_delay_ms
#define get_ms      esp32_get_clock_ms
#define log_i       printf
#define log_e       printf

记得加上我们的库函数文件包含：

#include "empl_driver.h"

2.2.2 函数修改

需要注释掉其中一个空载函数 __no_operation();

2.3 功能函数封装

为了方便的使用，将MD5.1.3版本下的 simple_apps\msp430\motion_driver_test.c 相关功能函数封装一下。
这一部分与小马哥的视频基本上一致，只在头文件与宏定义上需要根据自己的平台修改。
修改后将名字改为 mpu_dmp_driver.c 并添加对应的头文件 mpu_dmp_driver.h 方便后续包含使用。

2.3.1 预处理部分

同样注释掉之前与平台相关的头文件，包含ESP32平台头文件

#include <math.h>// #include "USB_eMPL/descriptors.h"// #include "USB_API/USB_Common/device.h"
// #include "USB_API/USB_Common/types.h"
// #include "USB_API/USB_Common/usb.h"// #include "F5xx_F6xx_Core_Lib/HAL_UCS.h"
// #include "F5xx_F6xx_Core_Lib/HAL_PMM.h"
// #include "F5xx_F6xx_Core_Lib/HAL_FLASH.h"// #include "USB_API/USB_CDC_API/UsbCdc.h"
// #include "usbConstructs.h"// #include "msp430.h"
// #include "msp430_clock.h"
// #include "msp430_i2c.h"
// #include "msp430_interrupt.h" #include "esp_system.h"
#include "mpu_dmp_driver.h"
#include "empl_driver.h"

math 用于姿态角的计算。

值得注意的是，MPU6050数据率在DEFAULT_MPU_HZ宏定义中设置：

#define DEFAULT_MPU_HZ  (100) // 设置MPU6050的输出数据率为100Hz

2.3.2 数据定义部分

官方示例程序是有与之对应的上位机的，是有Python实现，基于pygame。不过我跑了一下跑不通，因为我的Python水平很差，所以也懒得去研究，自己写了一个简单的显示姿态的上位机用于测试。比研究别人的实现更省时间。

motion_driver_test.c中很大一部分代码用于与上位机通信，我们直接将相关的部分注释掉就行。在数据定义部分，需要注释的有：

// 通信数据包定义
// enum packet_type_e {//     PACKET_TYPE_ACCEL,
//     PACKET_TYPE_GYRO,
//     PACKET_TYPE_QUAT,
//     PACKET_TYPE_TAP,
//     PACKET_TYPE_ANDROID_ORIENT,
//     PACKET_TYPE_PEDO,
//     PACKET_TYPE_MISC
// };

2.3.2 函数部分

1 删
注释掉与通信相关的函数：

// 发包函数
// void send_packet(char packet_type, void *data)
// 收包函数
// static void handle_input(void)

用不到的函数：

// 开关传感器
// static void setup_gyro(void)// 手势回调函数
// static void tap_cb(unsigned char direction, unsigned char count)
// static void android_orient_cb(unsigned char orientation)  // MSP430 平台初始化
// static inline void platform_init(void) // app 主函数
// void main(void)

2 改
重写复位函数：

// static inline void msp430_reset(void)
// {//     PMMCTL0 |= PMMSWPOR;
// }// 重启系统
void system_reset(void)
{esp_restart();
}

dmp初始化函数 mpu_dmp_init：
在其中加入对应 I2C 初始化函数 mpu_init_i2c，并注释掉 MSP430 初始化的部分。
给每个函数执行结果增加错误检测，方便调试知道初始化执行到哪一步，因为 ESP32 不能在线调试。

3 增
加上 mpu_init_i2c 函数 用于初始化 MPU 使用的 I2C 接口。

uint8_t mpu_init_i2c(void)
{esp_err_t esp_err;i2c_config_t conf = {.mode = I2C_MODE_MASTER,.sda_io_num = MPU_I2C_SDA,         // select GPIO specific to your project.sda_pullup_en = GPIO_PULLUP_ENABLE,.scl_io_num = MPU_I2C_SCL,         // select GPIO specific to your project.scl_pullup_en = GPIO_PULLUP_ENABLE,.master.clk_speed = 200000,  // select frequency specific to your project// .clk_flags = 0,          /*!< Optional, you can use I2C_SCLK_SRC_FLAG_* flags to choose i2c source clock here. */};esp_err = i2c_param_config(0, &conf);printf("i2c_param_config: %d \n", esp_err);esp_err = i2c_driver_install(0, I2C_MODE_MASTER, 0, 0, 0);printf("i2c_driver_install: %d \n", esp_err);return esp_err;
}

自检函数，因为我们只用了6轴，所以自检的结果是0x3 而不是 0x7。

加上姿态角的计算部分：
这部分套用公式：

    if (sensors & INV_WXYZ_QUAT){q0 = quat[0] / q30;q1 = quat[1] / q30;q2 = quat[2] / q30;q3 = quat[3] / q30;pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1)*57.3;yaw   = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 -q3 * q3)*57.3;// printf("pitch: %f\t", pitch);// printf("roll: %f\t", roll);// printf("yaw: %f\n", yaw);}

< 20220316补充 >

3 测试工程开发

值得注意的是 DMP 是通过中断通知 MPU 姿态角数据准备好了，所以需要将 MPU6050 模块的 INT 引脚接到 ESP32 的GPIO上，比如我这边将 INT 引脚接到 GPIO4。

主函数为：

void app_main(void)
{oled_init();mpu_dmp_init();gpio_init();gpio_evt_queue = xQueueCreate(10, sizeof(uint32_t));  xTaskCreate(gpio_task, "gpio_task", 2048, NULL, 10, NULL);gpio_install_isr_service(0);gpio_isr_handler_add(GPIO_MPU_INTR, gpio_intr_handle, (void*) GPIO_MPU_INTR);}

基本的处理流程为：

  • 初始化OLED
  • 初始化 MPU6050 DMP
  • 初始化GPIO
  • 新建一个 queue 用于在 GPIO 中断与 gpio_task 任务之间发送消息。其中 gpio_task 任务完成姿态数据的获取。

4 MPU6050 测试上位机开发

基于PyQt5 和 OpenGL 写了一个简单的MPU6050测试用的上位机，主要的需求：

  • 姿态显示

    • 3维模型显示
    • 姿态角数据显示
  • 串口通信
  • 通用性

结合上面的需求，我设计的程序框架设计为：

  • 采用PyQt5实现窗口界面。
  • 采用串口通信。
  • OpenGL 绘制模型实时显示姿态。
  • 采用正则进行数据解析，以满足通用性。
    这里详细说明一下，我在库文件输出姿态角的格式为：printf("pitch:%f,roll:%f,yaw:%f\n",pitch, roll, yaw);，所以我在上位机中分别直接去匹配关键词pitch:、roll:和yaw:后面的数据来获得姿态角，这样就不需要额外再定通信协议了。

UI：

使用：

5 源文件获取

文件夹包含了: