0.参考
2_小学生都能搞定的MPU6050DMP库向STM32HAL库的移植
stm32f103 MPU6050利用DMP进行姿态解算(硬件iic,hal库)
1.挂载MPU6050芯片
以ESP-IDF v5.5.0的Basic I2C Master Example为基础进行修改。
改动1：Example中使用的SDA和SDL引脚改为自己的硬件连接。
改动2：配置寄存器，完成对MPU6050初始化，使其开始工作。
ESP_ERROR_CHECK(mpu6050_register_write_byte(dev_handle, MPU6050_PWR_MGMT_1_REG_ADDR, 1 << MPU6050_RESET_BIT));
vTaskDelay(100 / portTICK_PERIOD_MS);
ESP_ERROR_CHECK(mpu6050_register_write_byte(dev_handle, MPU6050_PWR_MGMT_1_REG_ADDR, 0 << MPU6050_RESET_BIT));
ESP_ERROR_CHECK(mpu6050_register_write_byte(dev_handle, MPU6050_GYRO_CONFIG_REG_ADDR, 0x18));
ESP_ERROR_CHECK(mpu6050_register_write_byte(dev_handle, MPU6050_ACCEL_CONFIG_REG_ADDR, 0x00));
ESP_ERROR_CHECK(mpu6050_register_write_byte(dev_handle, MPU6050_INT_ENABLE_REG_ADDR, interrupt_enable));
ESP_ERROR_CHECK(mpu6050_register_write_byte(dev_handle, MPU6050_SAMPLE_RATE_DIVIDER_REG_ADDR, sample_rate_divider));
ESP_ERROR_CHECK(mpu6050_register_write_byte(dev_handle, MPU6050_DLPF_CFG_REG_ADDR, dlpf));
ESP_ERROR_CHECK(mpu6050_register_write_byte(dev_handle, MPU6050_PWR_MGMT_1_REG_ADDR, 0x01));
ESP_ERROR_CHECK(mpu6050_register_write_byte(dev_handle, MPU6050_PWR_MGMT_2_REG_ADDR, 0x00));
改动3：增加持续读取寄存器数据并打印显示的代码逻辑
/* Read the MPU6050 WHO_AM_I register, on power up the register should have the value 0x68 */
ESP_ERROR_CHECK(mpu6050_register_read(dev_handle, MPU6050_WHO_AM_I_REG_ADDR, whoami, 1));
ESP_LOGI(TAG, "WHO_AM_I = %X", whoami[0]);
///////////////////////////////////////////////////////////
/*
持续读取寄存器数据并打印显示
*/
while (whoami[0] == 0x68)
{
vTaskDelay(200 / portTICK_PERIOD_MS);
ESP_ERROR_CHECK(mpu6050_register_read(dev_handle, MPU6050_ACCEL_XOUT_H_REG_ADDR, measurements_buffer+0, 14));
//ESP_LOGI(TAG, "俯仰Pitch(-90/+90) 方向Yaw(-180/+180) 滚转Roll(-180/+180)");
ESP_LOGI(TAG, "Temperature Register Value(%d)",*temperature_ptr);
ESP_LOGI(TAG, "Gyro Register Value(%d,%d,%d)", *gyro_x_ptr, *gyro_y_ptr, *gyro_z_ptr);
ESP_LOGI(TAG, "Accel Register Value(%d,%d,%d)", *accel_x_ptr, *accel_y_ptr, *accel_z_ptr);
}监视器输出
I (6778) example: Temperature Register Value(7408)
I (6778) example: Gyro Register Value(1792,-6145,-2049)
I (6778) example: Accel Register Value(2303,-19966,30785)
I (6978) example: Temperature Register Value(6128)
I (6978) example: Gyro Register Value(1536,-6145,-2049)
I (6978) example: Accel Register Value(7423,-24062,15937)
I (7178) example: Temperature Register Value(6640)
I (7178) example: Gyro Register Value(1792,-6401,-1793)
I (7178) example: Accel Register Value(4351,-24062,29249)2.姿态解算
使用MPU6050的DMP进行姿态解算。下载DMP库，获取的文件如下图。将DMP库源码文件并入工程。
主程序改动1：将stm32f103 MPU6050利用DMP进行姿态解算(硬件iic,hal库)中的MPU5060.c.h文件整合到main中。代码无改动。
主程序改动2：主程序中持续调用mpu6050_dmp_get_gyroscope函数读取姿态并打印显示。
do {
result = mpu6050_dmp_init();
}while(result);
while(1) {
mpu6050_dmp_get_attitude(&pitch, &roll, &yaw);
ESP_LOGI(TAG, "Pitch:%.2f Yaw:%.2f Roll:%.2f", pitch, yaw, roll);
vTaskDelay(200 / portTICK_PERIOD_MS);
}主程序改动3：将DMP的源文件和头文件加入到CMakeList.txt中。
监视器输出
DMP库源码并入工程后在不改动的情况下会出现以下的编译error。分析并解决这些error来完成DMP库的移植。
inv_mpu_dmp_motion_driver.c: In function 'dmp_set_accel_bias':
inv_mpu_dmp_motion_driver.c:75:2: error: #error Gyro driver is missing the system layer implementations.
75 | #error Gyro driver is missing the system layer implementations.
inv_mpu_dmp_motion_driver.c: In function 'dmp_read_fifo':
inv_mpu_dmp_motion_driver.c:638:5: error: implicit declaration of function '__no_operation' [-Wimplicit-function-declaration]
638 | __no_operation();
inv_mpu_dmp_motion_driver.c: In function 'dmp_read_fifo':
inv_mpu_dmp_motion_driver.c:1342:5: error: implicit declaration of function 'get_ms'; did you mean 'gets'? [-Wimplicit-function-declaration]
1342 | get_ms(timestamp);
inv_mpu.c:119:2: error: #error Gyro driver is missing the system layer implementations.
119 | #error Gyro driver is missing the system layer implementations.
inv_mpu.c:123:2: error: #error Which gyro are you using? Define MPUxxxx in your compiler options.
123 | #error Which gyro are you using? Define MPUxxxx in your compiler options.
inv_mpu.c:379:1: error: empty enum is invalid
379 | };
inv_mpu.c:642:9: error: 'st' undeclared (first use in this function)
inv_mpu.c:678:13: error: implicit declaration of function 'i2c_read' [-Wimplicit-function-declaration]
678 | if (i2c_read(st.hw->addr, ii, 1, &data))
inv_mpu.c:680:9: error: implicit declaration of function 'log_i' [-Wimplicit-function-declaration]
680 | log_i("%#5x: %#5x\r\n", ii, data);
inv_mpu.c:722:5: error: implicit declaration of function 'delay_ms' [-Wimplicit-function-declaration]
722 | delay_ms(100);
inv_mpu.c:776:9: error: implicit declaration of function 'reg_int_cb' [-Wimplicit-function-declaration]
776 | reg_int_cb(int_param);
inv_mpu.c:905:9: error: implicit declaration of function 'get_ms'; did you mean 'gets'? [-Wimplicit-function-declaration]
905 | get_ms(timestamp);
inv_mpu.c:2114:19: error: 'test' undeclared (first use in this function); did you mean 'test_s'?
2114 | data[0] = test.reg_accel_fsr;
inv_mpu.c:2860:22: error: implicit declaration of function 'min'; did you mean 'fmin'? [-Wimplicit-function-declaration]
2860 | this_write = min(LOAD_CHUNK, length - ii);
inv_mpu.c:699:1: error: control reaches end of non-void function [-Werror=return-type]
inv_mpu.c:1439:1: error: control reaches end of non-void function [-Werror=return-type]
DMP库移植改动1：inv_mpu.h 增加适配ESP32S3平台的结构。
#define EMPL_TARGET_ESP32S3 1
struct int_param_s {
#elif defined EMPL_TARGET_ESP32S3
void *arg;
}DMP库移植改动2：inv_mpu.c 增加适配ESP32S3平台的接口实现。
/* The following functions must be defined for this platform:
* i2c_write(unsigned char slave_addr, unsigned char reg_addr,
* unsigned char length, unsigned char const *data)
* i2c_read(unsigned char slave_addr, unsigned char reg_addr,
* unsigned char length, unsigned char *data)
* delay_ms(unsigned long num_ms)
* get_ms(unsigned long *count)
*/
#elif defined EMPL_TARGET_ESP32S3
#include "sdkconfig.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "driver/i2c_master.h"
extern i2c_master_dev_handle_t dev_handle;
int esp32s3_i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data)
{
}
int esp32s3_i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data)
{
}
void esp32s3_delay_ms(unsigned long num_ms)
{
}
void esp32s3_get_ms(unsigned long *count)
{
}
#define i2c_write esp32s3_i2c_write
#define i2c_read esp32s3_i2c_read
#define delay_ms esp32s3_delay_ms
#define get_ms esp32s3_get_ms
#define MPU6050监视器输出
inv_mpu.c: In function 'mpu_init':
inv_mpu.c:809:9: error: implicit declaration of function 'reg_int_cb' [-Wimplicit-function-declaration]
809 | reg_int_cb(int_param);DMP库移植改动3：屏蔽掉mpu_init中的四行。
DMP库移植改动4：1.inv_mpu_dmp_motion_dirver.c中35-76行的编译控制宏与inv_mpu.c中定义的相同，没有用，删除掉；2.注释__no_operation();添加__asm__ __volatile__("nop");；3.文件头部添加extern void esp32s3_get_ms(unsigned long *count);；4.dmp_read_fifo函数中调用的get_ms改成esp32s3_get_ms；
至此，Build Successfully，运行能不能成功，不知道。
果然没有那么顺利......mpu_load_firmware()失败，返回值-2。挂载MPU6050驱动芯片时验证了I2C的读写接口都是没问题的，问题出在哪里呢？
for (ii = 0; ii < length; ii += this_write) {}
//这个循环中，mpu_write_mem 和 mpu_read_mem都没有报错。比较写入内容和读取内容时出现不一致。
//为了让代码继续执行，注释掉这部分比较的代码，先看程序能不能正常运行（即默认I2C写入成功）继续不顺利。。。mpu_run_self_test()失败，返回值0x07。查看函数代码
accel_result = accel_self_test(accel, accel_st);
gyro_result = gyro_self_test(gyro, gyro_st);
result = 0;
if (!gyro_result)
result |= 0x01;
if (!accel_result)
result |= 0x02;
#ifdef AK89xx_SECONDARY
compass_result = compass_self_test();
if (!compass_result)
result |= 0x04;
#else
result |= 0x04;//源码中通过条件编译扩展了支持的芯片，为支持MPU6050增加了标记。所以自检成功的标志应该是0x07DMP库移植改动5：将自检通过的标志改为0x7
result = mpu_run_self_test(gyro, accel);
ESP_LOGI(TAG, "mpu_run_self_test result:%d", result);
if (result == 0x7) {
/* Test passed. We can trust the gyro data here, so let's push it down
* to the DMP.
*/
}监视器输出，继续不顺利。。。感觉DMP没干活啊。难道真的是固件写入出错了？
I (261) main_task: Started on CPU0
I (281) main_task: Calling app_main()
I (281) example: I2C initialized successfully
I (1841) example: mpu6050_dmp_init result:0
I (1841) example: Pitch:0.00 Yaw:0.00 Roll:0.00
I (2041) example: Pitch:0.00 Yaw:0.00 Roll:0.00
I (2241) example: Pitch:0.00 Yaw:0.00 Roll:0.00DMP库移植改动6：检查了esp32s3_i2c_write。把mpu_load_firmware中注释的代码恢复。
监视器输出，GoodGood
I (75941) example: Pitch:-0.556098 Yaw:-23.384480 Roll:0.820657
I (76091) example: Pitch:-0.556098 Yaw:-23.384480 Roll:0.820657
I (76191) example: Pitch:-0.573516 Yaw:-23.378716 Roll:0.785903
扫一扫
1237
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
