devbox
笔触狂放9
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

esp32系列(11)：ESP32 IDF平台 mpu6050 DMP 驱动移植及测试上位机开发_esp-idf写mpu6050驱动

作者：笔触狂放9 | 2024-07-24 17:24:42

赞

踩

esp-idf写mpu6050驱动

目录

先上运行效果：
在这里插入图片描述

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

  • 1
  • 2
  • 3
  • 4

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;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34

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

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

  • 1
  • 2
  • 3
  • 4
  • 5

get_ms：获取时间

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

  • 1
  • 2
  • 3
  • 4
  • 5

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

#define log_i     printf
#define log_e     printf

  • 1
  • 2

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

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13

我的处理是将上诉函数全部打包在一个名为 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);

  • 1

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

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

  • 1
  • 2

2.2 inv_mpu_dmp_motion_driver.c

2.2.1 修改预处理部分

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

#define MOTION_DRIVER_TARGET_MSP430

  • 1

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

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

  • 1
  • 2

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

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

  • 1
  • 2
  • 3
  • 4

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

#include "empl_driver.h"

  • 1

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"

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23

math 用于姿态角的计算。

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

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

  • 1

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
// };

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10

2.3.2 函数部分

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

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

  • 1
  • 2
  • 3
  • 4

用不到的函数：

// 开关传感器  
// 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)

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12

2 改
重写复位函数：

// static inline void msp430_reset(void)
// {
//     PMMCTL0 |= PMMSWPOR;
// }

// 重启系统  
void system_reset(void)
{
    esp_restart();
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10

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;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20

自检函数，因为我们只用了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);

    }

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16

< 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);

}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11

基本的处理流程为：

  • 初始化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 源文件获取

在这里插入图片描述

文件夹包含了:

  • 0 官方库文件
    MD5.1.3 与 MD6.12 两个版本的官方库文件。
  • 1 ESP32 IDF 平台MPU DMP驱动文件
    移植好的ESP32 IDF 平台MPU DMP驱动文件。
  • 2 测试工程
    已经测试后的测试工程。
  • 3 上位机源码与exe
    及上位机的源码和打包发布了的应用程序 mpu_display.exe。
    上面的移植过程记录的很清楚了，推荐自己移植试试，挺有意思的。如果需要下载移植好的驱动和上位机源码的欢迎￥1.99下载支持
    声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/笔触狂放9/article/detail/875986
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。