ESP32 - mpu6050

[toc]

硬件

引脚说明

和ESP32的连接

I2C

I2C数据帧

• 起始位：SCL高电平，SDA下降沿；
• 设备地址：7位；
• 读写位：0：主设备向从设备写数据，1：主设备向从设备读数据；
• 应答信号：从设备应答，从设备正常应答时，SDA为低电平，从设备忙，SDA为高电平；
• 寄存器地址：8位；
• 应答信号：
• 数据：写入寄存器数据（8位）；
• 停止位：SCL高电平，SDA上升沿；

ESP-idf提供I2C驱动程序，因此I2C的配置比较简单。

步骤：

1. 配置驱动程序：设置初始化参数（如主机模式或从机模式，SDA 和 SCL 使用的 GPIO 管脚，时钟速度等），使用结构体i2c_config_t conf
2. 安装驱动程序：激活一个 I2C 控制器的驱动，该控制器可为主机也可为从机；使用函数i2c_driver_install()

#define I2C_MASTER_SCL_IO           8       // 时钟线
#define I2C_MASTER_SDA_IO           18      // 数据线
#define I2C_MASTER_NUM              0       // I2C master i2c port number
#define I2C_MASTER_FREQ_HZ          400000  // I2C频率                 
#define I2C_MASTER_TX_BUF_DISABLE   0       /*!< I2C master doesn't need buffer */
#define I2C_MASTER_RX_BUF_DISABLE   0       /*!< I2C master doesn't need buffer */

esp_err_t i2c_master_init(void)
{
    int i2c_master_port = I2C_MASTER_NUM;
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = I2C_MASTER_SDA_IO,
        .scl_io_num = I2C_MASTER_SCL_IO,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = I2C_MASTER_FREQ_HZ,
    };

    i2c_param_config(i2c_master_port, &conf);

    return i2c_driver_install(i2c_master_port, conf.mode, I2C_MASTER_RX_BUF_DISABLE, I2C_MASTER_TX_BUF_DISABLE, 0);
}

MPU6050的相关名词

• 陀螺仪：测量的是绕xyz轴转动的角速度，单位：Degree Per Second的缩写°/S ；

• 加速度：测量的是xyz方向受到的加速度，静止状态下，三个方向的合加速度为重力加速度；

• DMP：Digital Motion Processor，直接输出四元数，并非完全开源；

• 四元数：四元数可以方便的表示3维空间的旋转。基本形式：$q_0+q_1i+q_2j+q_3*k$，一个实部和三个虚部；

• 欧拉角：pitch：俯仰（绕y轴旋转）、roll滚动（绕X轴旋转）：、yaw：偏航（绕Z轴旋转）的表示形式，方便观察姿态。不过存在死锁现象；

MPU6050 配置

MPU6050配置主要根据数据手册配置相关寄存器来。

#define MPU_DEVICE_ID_REG		0X75	//器件ID寄存器
#define MPU_PWR_MGMT1_REG		0X6B	//电源管理寄存器1
#define MPU_CFG_REG				0X1A	//配置寄存器
#define MPU_SAMPLE_RATE_REG		0X19	//采样频率分频器
#define MPU_GYRO_CFG_REG		0X1B	//陀螺仪配置寄存器
#define MPU_ACCEL_CFG_REG		0X1C	//加速度计配置寄存器

	/*读取设备ID，判断I2C通信是否正常*/
    ESP_ERROR_CHECK(mpu6050_register_read(MPU_DEVICE_ID_REG, data, 1));
    ESP_LOGI(TAG, "WHO_AM_I = %X", data[0]);
    /*重置设备*/
    ESP_ERROR_CHECK(mpu6050_register_write_byte(MPU_PWR_MGMT1_REG, 0x80));
    vTaskDelay(100 / portTICK_PERIOD_MS);
    /*禁用睡眠模式，选择X轴陀螺仪为时钟源*/
     ESP_ERROR_CHECK(mpu6050_register_write_byte(MPU_PWR_MGMT1_REG, 0x01));
    /*配置数字低通滤波器，带宽为5，陀螺仪输出频率为1kHz*/
    ESP_ERROR_CHECK(mpu6050_register_write_byte(MPU_CFG_REG, 0x06));
    /*配置采样频率:50Hz*/
    ESP_ERROR_CHECK(mpu6050_register_write_byte(MPU_SAMPLE_RATE_REG, 19));
    /*配置陀螺仪量程:± 2000 °/s, 不自检*/
    ESP_ERROR_CHECK(mpu6050_register_write_byte(MPU_GYRO_CFG_REG, 0x18));
    /*配置加速度计量程: ± 16g， 不自检*/
    ESP_ERROR_CHECK(mpu6050_register_write_byte(MPU_ACCEL_CFG_REG, 0x18));

1 WHO_AM_I

读取mpu6050的ID；默认为0x68，可以用来验证IIC底层是否配置好。其最后一位由ADO决定。如果ADO=1，寄存器的的值为0x69，否则为0x68；

ESP_ERROR_CHECK(mpu6050_register_read(MPU_DEVICE_ID_REG, data, 1));

2 Power Management 1

配置电源和时钟源，用于重置设备和禁用温度传感器；

/*重置设备*/
ESP_ERROR_CHECK(mpu6050_register_write_byte(MPU_PWR_MGMT1_REG, 0x80));
vTaskDelay(100 / portTICK_PERIOD_MS);
/*禁用睡眠模式，选择X轴陀螺仪为时钟源*/
ESP_ERROR_CHECK(mpu6050_register_write_byte(MPU_PWR_MGMT1_REG, 0x01));

• 参数：

一般配置:

1. 重置寄存器，Bit7 = 1，所以，Power_Management_1 = 0x80；
2. 解除休眠，时钟源设置为X轴，Bit0=1：Power_Management_1 = 0x01；

注意：重置寄存器后SLEEP自动为1， 所以必须解除休眠，否则没有数据输出

3 Configuration

/*配置数字低通滤波器，带宽为5，陀螺仪输出频率为1kHz*/
 ESP_ERROR_CHECK(mpu6050_register_write_byte(MPU_CFG_REG, 0x06));

主要用于配置数字低通滤波器（DLPF_CFG）,确定陀螺仪的输出频率。

如果启用数字低通滤波器（DLPF_CFG != 0 orDLPF_CFG != 7 ），则陀螺仪的采样频率只能为1KHz，

配置：

• Bit2 =1 and Bit1 = 1，所以，Configuration = 0x06

4 SMPRT_DIV

/*配置采样频率:50Hz*/
ESP_ERROR_CHECK(mpu6050_register_write_byte(MPU_SAMPLE_RATE_REG, 19));

设置传感器的采样速率

$$ 采样率 = \frac{陀螺仪输出频率}{(1 + SMPLRT_DIV)} $$ 陀螺仪的输出频率由DLPF_CFG （寄存器26）进行配置，目前陀螺仪输出频率配置为1KHz。

配置：

• 采样速率配置为50Hz，由$50 = \frac{1000}{(1 + SMPLRT_DIV)}$知，SMPRT_DIV = 19

5 Gyroscope Configuration

是否启用自检和设置陀螺仪量程

 /*配置陀螺仪量程:± 2000 °/s, 不自检*/
 ESP_ERROR_CHECK(mpu6050_register_write_byte(MPU_GYRO_CFG_REG, 0x18));

：

自检：简单理解为重新设置零位（有可能理解不对）。

量程设置

设置：

• 一般设置为不自检，最大满量程：Bit4 = 1 & Bit3 = 1, 所以Gyroscope Configuration = 0x18

6 Accelerometer Configuration

 /*配置加速度计量程: ± 16g， 不自检*/
 ESP_ERROR_CHECK(mpu6050_register_write_byte(MPU_ACCEL_CFG_REG, 0x18));

• 描述：配置加速度是否自检和量程范围

量程设置

设置：

• 一般设置为不自检，最大满量程：Bit4 = 1 & Bit3 = 1, 所以Gyroscope Configuration = 0x18

读取原始数据

以下代码读取温度，陀螺仪，加速度计的原始数据

/**
* @brief 读取温度值
*/
esp_err_t mpu6050_get_temperature(float *temp)
{
    int ret;
    uint8_t data[2];
    int16_t raw;
    ret = mpu6050_register_read(MPU_TEMP_OUTH_REG, data, 2);
    raw = (int16_t)((data[0] << 8) + data[1]);
    *temp = 36.53 + raw/340.0;
    return ret;
}
/**
 * @brief 读取陀螺仪值
*/
esp_err_t mpu6050_get_gyroscope(float *gxyz)
{
    int ret;
    uint8_t data[6];
    int16_t raw_gxyz[3];
    float LSB[4] = {131.0, 65.5, 32.8, 16.4};
    /*读取原始数据*/
    ret = mpu6050_register_read(MPU_GYRO_XOUTH_REG, data, 6);
    if (ret == ESP_OK)
    {
        /*转化原始数据*/
        raw_gxyz[0] = (int16_t)((data[0] << 8) + data[1]);
        raw_gxyz[1] = (int16_t)((data[2] << 8) + data[3]);
        raw_gxyz[2] = (int16_t)((data[4] << 8) + data[5]);
    }
    /*读取量程*/
    ret = mpu6050_register_read(MPU_GYRO_CFG_REG, data, 1);
        
    if (ret == ESP_OK)
    {
        uint8_t a = (data[0] & 0X18) >> 3;
        printf("ga = %d\n", a);
        // 实际值
        for (uint8_t i = 0; i < 3; i++)
        {
            gxyz[i] =raw_gxyz[i] / LSB[a];
        }
    }
    
    return ret; 
}
/**
 * @brief 读取加速度值
*/
esp_err_t mpu6050_get_accelerometer(float *axyz)
{
    int ret;
    uint8_t data[6];
    int16_t raw_axyz[3];
    float LSB[4] = {16384.0, 8192.0, 4096.0, 2048.0};
    /*原始值*/
    ret = mpu6050_register_read(MPU_ACCEL_XOUTH_REG, data, 6);
    if (ret == ESP_OK)
    {
        raw_axyz[0] = (int16_t)((data[0] << 8) + data[1]);
        raw_axyz[1] = (int16_t)((data[2] << 8) + data[3]);
        raw_axyz[2] = (int16_t)((data[4] << 8) + data[5]);
    }
    /*读取量程*/
    ret = mpu6050_register_read(MPU_ACCEL_CFG_REG, data, 1);
    if (ret == ESP_OK)
    {
        uint8_t a = (data[0] & 0X18) >> 3 ;
        printf("aa = %d\n", a);
        /*转化为实际值*/
        for (uint8_t i = 0; i < 3; i++)
        {
            axyz[i] =raw_axyz[i] / LSB[a];
        }
        
    }
    return ret; 
}

7 Temperature Measurement

读取温度的值

函数esp_err_t mpu6050_get_temperature(float *temp)

寄存器

每个数据由两个寄存器组成，数据更新速度由寄存器25的采样速度控制。

和实际温度的转化关系： *temp = 36.53 + raw/340.0;没有在手册中查到，抄别人的代码。

8 Gyroscope Measurements

读取陀螺仪的数据

esp_err_t mpu6050_get_gyroscope(float *gxyz)

寄存器：

每个数据由两个寄存器组成，数据更新速度由寄存器25的采样速度控制

由于XYZ的数据连续，所以直接读了6个值，并根据LSB转化为实际值。

9 Accelerometer Measurements

读取加速度的值

esp_err_t mpu6050_get_accelerometer(float *axyz)

每个数据由两个寄存器组成，数据更新速度由寄存器25的采样速度控制。

由于XYZ的数据连续，所以直接读了6个值，并根据LSB转化为实际值。

其他自认为有用的寄存器

9 FIFO Enable

• 描述：决定那些寄存器的数据会被放入缓冲器中

10 Interrupt Enable

• 描述，是否是能相应中断

11 User Control

• 描述：配置是否启用FIFO，IIC的主从，重置FIFO和传感器

读取结果

将MPU6050静止平放在桌面上，获取连续3秒的数据

  while (1)
    {
        ESP_ERROR_CHECK(mpu6050_get_temperature(&temp));
        ESP_LOGI(TAG, "temp = %.2f", temp);
        ESP_ERROR_CHECK(mpu6050_get_gyroscope(gxyz));
        ESP_LOGI(TAG, "gx = %.2f\t gy = %.2f\t gz = %.2f", gxyz[0], gxyz[1], gxyz[2]);
        ESP_ERROR_CHECK(mpu6050_get_accelerometer(axyz));
         ESP_LOGI(TAG, "ax = %.2f\t ay = %.2f\t az = %.2f", axyz[0], axyz[1], axyz[2]);
        vTaskDelay(1000 / portTICK_PERIOD_MS);
    }

读取周期为1S

I (6415) mpu6050-example: temp = 25.52
I (6415) mpu6050-example: gx = -4.21     gy = -0.49      gz = -1.59
I (6415) mpu6050-example: ax = 0.11      ay = 0.03       az = 1.03
I (7415) mpu6050-example: temp = 25.55
I (7415) mpu6050-example: gx = -4.21     gy = -0.49      gz = -1.59
I (7415) mpu6050-example: ax = 0.11      ay = 0.03       az = 1.04
I (8415) mpu6050-example: temp = 25.55
I (8415) mpu6050-example: gx = -4.21     gy = -0.49      gz = -1.59
I (8415) mpu6050-example: ax = 0.11      ay = 0.03       az = 1.04

定性分析：

• 温度：芯片摸着不发烫，25度左右算正常；
• 陀螺仪：理论上禁止，陀螺仪的数据应该为0，但是实际上存在一点数据，原因为止；
• 加速度计：静止时，加速度的合力应该为9.8，目前合力估计为10左右，说明读取的数据基本正常。

从定性分析的角度来说，根据datasheed配置应该没有错。

下一步计划

原始数据意义不大，纯粹用于学习。后面将进一步融合获取欧拉角。

• 使用MDP融合；
• 使用卡尔曼滤波融合；

DMP移植

上次通过直接读取寄存器的方式获取到mpu6050的温度、陀螺仪和加速度的数据，但是mpu6040一般使用欧拉角和四元数表示姿态。mpu6050通过自带的DMP(DigitalMotion Processor)，可以在减轻控制器的计算压力下，轻松得到姿态数据。DMP的移植无论是网上的讲解还是github上的代码，都挺丰富的。基本上能做到开箱就用。原先我也是直接使用现成的代码。但是呢，目前想从头开始，想体验下从最初的源码开始移植是怎么样的。

DMP简介

DMP就是MPU6050内部的运动引擎，由Invensense提供。支持设备有MPU6050、MPU6500、MPU9150、MPU9250

能实现的功能有：

• 3 轴四元数：仅仅使用陀螺仪数据计算；
• 6轴四元数：使用陀螺仪和加速度融合计算。不能和3 轴四元数同时开启；
• 手势识别 ： 使用传感器数据来检测设备方向是否有纵向、横向、反向纵向和反向方向的变化。很大程度上取决于方向矩阵；
• 点击手势识别 -：检测设备上的多方向点击检测。此功能将检测设备在哪个轴上发生敲击事件它可以检测最多 4 次的多次抽头。API 可以配置此功能的阈值、死区时间和点击次数；
• 计步器手势识别 ：提供步数和时间戳。此功能会自动启用，但只有在检测到 5 秒的连续脚步时才会触发。；
• DMP 中断 ：可以将中断配置为在传感器数据准备就绪时或检测到点击或方向手势；

移植

既然是从头开始，就要从源码开始，本次使用Embedded MotionDriver 5.1，手中没有磁力计或者mpu9250等带有磁力计的mpu，所以就从5.1版本开始。另一个原因是Embedded MotionDriver 6没有找到最初的源码。

源码结构

+---core
|   \---driver
|       +---eMPL
|       |       dmpKey.h	// 对于DMP实现各种功能的内存定义
|       |       dmpmap.h	// DMP 内存定义
|       |       inv_mpu.c						// 驱动程序，用于适配微控制器
|       |       inv_mpu.h
|       |       inv_mpu_dmp_motion_driver.c		// DMP 加载和配置相关的API
|       |       inv_mpu_dmp_motion_driver.h
|       |
|       \---msp430
|
\---simple_apps
    \---msp430
        |   motion_driver_test.c	// 实现demo（移植参考的最原版本）
        |
        \---motion-driver-client	// 用python写的上位机程序，和motion_driver_test.c适配

从源码的结构可以看出eMPL中的文件是DMP的核心代码，motion_driver_test.c是运用DMP的参考代码。

另外，还有一份文档，是参考之一：embedded-motion-5-1-1-tutorial.pdf

移植思路

1. 在工程中新建components用于存放esp32自定义组件，组件名称为mpu6050DMP。esp32构建规定，工程目录下components文件夹中的组件能自动被依赖，可以简化CMakeCMakeLists.txt的书写；
2. 将eMPL放在mpu6050DMP中；
3. 在mpu6050DMP中新建mpu6050.c和mpu6050.h文件，用于组合DMP和esp32；
4. inv_mpu.c是用于适配微控制器的，因此从该文件的注释可以了解到，我们需要提供以下函数：
   1. i2c读函数：i2c_write()
   2. i2c写函数: i2c_read()
   3. 延迟函数: delay_ms()
   4. 获取毫秒函数: get_ms()
   5. 回调函数: reg_int_cb()
5. 编译，修正编译错误
6. motion_driver_test.c是demo函数，所以参考该文件实现DMP的配置；
7. 将DMP计算出来的四元数转化为欧拉角；
8. 编译，保证获取数据；

移植

1、 补充相关函数

通过inv_mpu.c中的注释可知，需要补充以下四个函数：

i2C写函数:i2c_write(unsigned char slave_addr, unsigned char reg_addr,unsigned char length, unsigned char const *data)和i2C读函数： i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data)

• slave_addr：从机地址；
• reg_addr：寄存器地址；
• length：读/写长度；
• data：读/写的数据指针；

/**
 * @brief 写数据
 */
esp_err_t esp32s3_i2c_write_bytes(uint8_t slave_addr, uint8_t reg_addr, uint8_t length, uint8_t *data)
{
    int ret;
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    ret = i2c_master_write_byte(cmd, (slave_addr << 1) | I2C_MASTER_WRITE, 1);
    if (ret != ESP_OK)
        return ESP_FAIL;

    ret = i2c_master_write_byte(cmd, reg_addr, 1);
    if (ret != ESP_OK)
        return ESP_FAIL;

    ret = i2c_master_write(cmd, data, length, 1);
    if (ret != ESP_OK)
        return ESP_FAIL;

    i2c_master_stop(cmd);
    i2c_master_cmd_begin(I2C_MASTER_NUM, cmd, I2C_MASTER_TIMEOUT_MS / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);
    return ret;
}
/**
 * @brief 读数据
 *      
*/
esp_err_t esp32s3_i2c_read_bytes(uint8_t slave_addr, uint8_t reg_addr,uint8_t length, uint8_t *data)
{
   int ret;
	i2c_cmd_handle_t cmd = i2c_cmd_link_create();
	i2c_master_start(cmd);
	ret = i2c_master_write_byte(cmd, (slave_addr << 1) | I2C_MASTER_WRITE, 1);
	if(ret != ESP_OK)
		return ESP_FAIL;

	ret = i2c_master_write_byte(cmd, reg_addr, 1);
	if(ret != ESP_OK)
		return ESP_FAIL;

	i2c_master_start(cmd);
	ret = i2c_master_write_byte(cmd, (slave_addr << 1) | I2C_MASTER_READ, 1);
	if(ret != ESP_OK)
		return ESP_FAIL;

	ret = i2c_master_read(cmd, data, length, I2C_MASTER_LAST_NACK);
	if(ret != ESP_OK)
		return ESP_FAIL;

	i2c_master_stop(cmd);
	i2c_master_cmd_begin(I2C_NUM_0, cmd, I2C_MASTER_TIMEOUT_MS/portTICK_PERIOD_MS);
	i2c_cmd_link_delete(cmd);

    return ret;
}

实现主要参照esp32d的在线文档；

延迟函数：delay_ms(unsigned long num_ms)

• num_ms：延迟描秒数指针；

int esp32s3_delay_ms(unsigned long num_ms)
{
    vTaskDelay(num_ms / portTICK_PERIOD_MS);
    return 0;
}

实现基于vTaskDelay()函数；

时钟函数： get_ms(unsigned long *count)

• count：毫秒数；

int esp32s3_get_clock_ms(unsigned long *count)
{
    *count = xTaskGetTickCount() * 10;
    return 0;
}

实现基于获取时钟节拍函数，再乘以时钟周期（10ms）

回调函数舍弃

最后通过宏定义指向：

/*定义传感器*/
#define MPU6050	
/*定义单片机*/					
#define MOTION_DRIVER_TARGET_ESP32S3

#if defined MOTION_DRIVER_TARGET_ESP32S3
#include <stdio.h>
#include "esp_log.h"
#include "driver/i2c.h"
#include "../mpu6050.h"
/*映射函数*/	
#define i2c_write   esp32s3_i2c_write_bytes
#define i2c_read    esp32s3_i2c_read_bytes
#define delay_ms    esp32s3_delay_ms
#define get_ms      esp32s3_get_clock_ms

#define log_i     printf
#define log_e     printf

#define min(a,b) ((a<b)?a:b)

#endif

2 编译，修错

错误1：error: implicit declaration of function '__no_operation' [-Werror=implicit-function-declaration]

• 分析：__no_operation为停止一个周期，esp32中没有。可以忽视；

• 处理：直接屏蔽；

错误2：error: implicit declaration of function 'reg_int_cb' [-Werror=implicit-function-declaration]

• 分析：没有提供回调函数导致；
• 处理：直接屏蔽；

到此为止，移植结束，代码可以正常编译了

应用

MDP的应用需要好好参考motion_driver_test.c文件，大致思路如下：

• 初始化I2C；
• 初始化mpu6050部分寄存器：mpu_init()；
• 唤醒mpu6050: mpu_set_sensors()
• 将需要的数据放在FIFO中: mpu_configure_fifo()
• 定义采样频率: mpu_set_sample_rate()
• 记载内存: dmp_load_motion_driver_firmware
• 设置方向（坐标系）: dmp_set_orientation()
• 使能MDP的功能: dmp_enable_feature()
• 启用MDP: mpu_set_dmp_state()
• 周期读取四元数： dmp_read_fifo()
• 将四元数转化为欧拉角。

MDP初始化函数：

esp_err_t MDP_init(void)
{
    int  ret;
    /*MPU6050初始化*/
    ret = mpu_init(NULL);

    if (ret != 0)
        printf("0, %d\n", ret);
    /*唤醒*/
    ret = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    /*将加速度和陀螺仪数据放入FIFO中*/
    ret=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
    /*设置采样频率*/
    mpu_set_sample_rate(100);
    /*加载内存*/
    dmp_load_motion_driver_firmware();
    /*设置方向*/
    dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
    /*启用功能
    * DMP_FEATURE_6X_LP_QUAT:产生四元数
    * DMP_FEATURE_TAP：检测敲击事件
    * DMP_FEATURE_ANDROID_ORIENT：实现了与Google Motion_driver设备兼容的显示方向
    * DMP_FEATURE_SEND_RAW_ACCEL：计步器功能
    * DMP_FEATURE_SEND_CAL_GYRO：设备处于无运动状态超过8秒，就会校准陀螺仪偏置
    * DMP_FEATURE_GYRO_CAL：将原始加速度计数据添加到FIFO
    */
    unsigned short dmp_features = DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP |
        DMP_FEATURE_ANDROID_ORIENT | DMP_FEATURE_SEND_RAW_ACCEL | DMP_FEATURE_SEND_CAL_GYRO |
        DMP_FEATURE_GYRO_CAL;
    dmp_enable_feature(dmp_features);
    dmp_set_fifo_rate(100);
    if (ret != 0)
        printf("6, %d\n", ret);
    run_self_test();
    /*启用DMP*/
    ret = mpu_set_dmp_state(1);
    if (ret != 0)
        printf("7, %d\n", ret);

    return ret;

}

周期读取和转化：

if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more) == ESP_OK)
        {
            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;	// pitch
                roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
                yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw
            }

        }

姿态数据可视化

上位机主要使用匿名上位机，通讯方式采用UDP， mpu6050为一个任务，给UDP发送数据为一个任务，两个任务之间通过队列进行数据传输：

匿名上位机对于欧拉角的传输协议如下：

对于mpu6050_task任务，主要是将姿态数据扩大100。

/*发送数据*/
mpu6050_data[0] = (int16_t)(roll * 100);
mpu6050_data[1] = (int16_t)(pitch * 100);
mpu6050_data[2] = (int16_t)(yaw * 100);

对于udp_client_task，按照协议封装数据，并通过UDP发送数据

/**
 * 发送给匿名上位机（欧拉角）
*/
void tranportdata(int16_t *esp23data, uint8_t *pcdata)
{
    pcdata[11] = 0;
    pcdata[12] = 0;
    pcdata[0] = 0XAA;
    pcdata[1] = 0XFF;
    pcdata[2] = 0X03;
    pcdata[3] = 7;
    for (uint8_t i = 0; i < 3; i++)
    {
        pcdata[4 + 2*i] = esp23data[i] & 0x00ff;
        pcdata[4 + 2*i + 1] = esp23data[i] >> 8;
    }
    /*融合状态*/
    pcdata[10] = 1;  
    /*校验位*/
    for (uint8_t i = 0; i < pcdata[3]+4; i++)
    {
        pcdata[11] += pcdata[i];
        pcdata[12] +=  pcdata[11];
    }
}

/*封装数据*/
tranportdata(mpu6050_data, euler_esp32_to_pc);
// printf("%d\n", mpu6050_data[1]);
/*发送*/
int err = sendto(sock, euler_esp32_to_pc, 13, 0, (struct socka

每个任务周期为10为10毫秒，通过上位机发现数据发送频率为101Hz，说明数据发送漏包情况不严重。

结果：

总结

• 由于MDP给msp430提供比较完整的demo，所以移植起来比较顺利，由于msp430采用中断接收数据，而我目前采用周期轮询，因此注意轮询的周期，如果周期较大，会出现MDP由于FIFO满，导致自动清除缓存数据，使dmp_read_fifo没有数据可读。
• 还有就是，在可视化情况下，当mpu6050静止时，数据传输比较稳定，但是一旦动起来，会出现卡顿的现象，原因未知。