当前位置： 首页 > news >正文

STM32F103 MPU6500 DMP库姿态解算

news 2025/10/7 6:38:13

最近查资料发现MPU6500的性能要优于MPU6050，而且目前MPU6050芯片已经停产，最近准备将需要陀螺仪的部分全部替换为MPU6500,所以尝试一下DMP库的姿态解算
代码开源地址:gitee仓库地址

下载DMP库

我在这里使用的是motion_driver6.1 具体的文件可以在仓库里面找到
在这里插入图片描述

下载后是这个样子:
在这里插入图片描述

将里面的.c/.h文件都添加到自己的工程文件中,我这里使用的是Clion进行开发，当然用keil也是一点问题没有的,接下来就可以开始移植代码了

DMP代码移植

官方提供的是基于msp430的工程文件，所以我们要将里面的一些宏定义进行替换为我们自己的代码(比如IIC读写代码,延时代码)\

inv_mpu.h

首先打开inv_mpu.h,在31行的位置有这样的代码
在这里插入图片描述

这段代码主要是为了适应不同开发平台的定义，我们这里不需要，只保留void *arg部分

struct int_param_s {void *arg;
};

inv_mpu.c

接着打开inv_mpu.c文件 将27-161行全部删除，这段代码的宏定义主要是陀螺仪型号的选择以及一些函数的实现(IIC读写,延时,调试信息打印).我们将这部分全部替换为自己的代码，具体如下

#define MPU6500//根据自己的陀螺仪信号进行修改，两者的解算代码是不通用的
#include "main.h"
extern I2C_HandleTypeDef hi2c1;  //注意替换为自己的i2c端口,还有下面的iic写入读取函数
#define i2c_write(dev_addr, reg_addr, data_size, p_data) \
HAL_I2C_Mem_Write(&hi2c1, dev_addr, reg_addr, I2C_MEMADD_SIZE_8BIT, p_data, data_size, 0x100)
#define i2c_read(dev_addr, reg_addr, data_size, p_data) \
HAL_I2C_Mem_Read(&hi2c1, dev_addr, reg_addr, I2C_MEMADD_SIZE_8BIT, p_data, data_size, 0x100)
#define delay_ms HAL_Delay
#define get_ms(p) do{ *p = HAL_GetTick();}while(0)
#define log_i(...) do {} while (0)//我们这里不需要打印调试信息
#define log_e(...) do {} while (0)
/* labs is already defined by TI's toolchain. */
/* fabs is for doubles. fabsf is for floats. */
#define fabs fabsf
#define min(a,b) ((a<b)?a:b)

利用Ctrl+F打开搜索栏，搜索EMPL_TARGET_STM32F4
在这里插入图片描述

删除ifndef中的代码
接着Ctrl+F搜索hw_s，修改器件地址,一共有两个，分别对应MPU6500和MPU6050所使用的地址，可根据自己的需要修改,将.addr修改为0xd0即可
在这里插入图片描述

在这里插入图片描述

还有一个地方的宏定义我们需要修改,搜索HWST_MAX_PACKET_LENGTH
在这里插入图片描述

将512替换为1024,否则之后自检会出现数组溢出导致程序进入hardfault卡死

inv_mpu_dmp_motion_driver.c

接着我们打开inv_mpu_dmp_motion_driver.c，删除35-76行代码，替换为如下的代码

#include "main.h"
extern I2C_HandleTypeDef hi2c1;
#define delay_ms HAL_Delay
#define get_ms(p) do{ *p = HAL_GetTick();}while(0)
#define log_i(...) do {} while (0)
#define log_e(...) do {} while (0)

然后利用Ctrl+F,搜索_no_operation
在这里插入图片描述

将_no_operation替换为__NOP()😭_no_operation是msp430中使用的NOP是STM32使用的)

验证函数

在代码修改完成后，便可编写初始化代码对DMP库进行调用,这里我参照别的博主的方法。原视频:B站视频地址
但没法直接用在MPU6500上。
在代码的68行左右的自检函数中，需要根据自己的陀螺仪型号选择合适的自检函数
MPU6500使用的是mpu_run_6500_self_test(gyro, accel,0);(0代表不打印调试信息)
MPU6050使用的是mpu_run_self_test(gyro, accel)
需要注意区分
在这里插入图片描述

具体代码如下:
MPU6500.c

#include "MPU6500.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "math.h"/* 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 thei* driver(s).* TODO: The following matrices refer to the configuration on an internal test* board at Invensense. If needed, please modify the matrices to match the* chip-to-body matrix for your particular set up.*/
static signed char gyro_orientation[9] = {-1, 0, 0,0,-1, 0,0, 0, 1};/* These next two functions converts the orientation matrix (see* gyro_orientation) to a scalar representation for use by the DMP.* NOTE: These functions are borrowed from Invensense's MPL.*/
static unsigned short inv_row_2_scale(const signed char *row)
{unsigned short b;if (row[0] > 0)b = 0;else if (row[0] < 0)b = 4;else if (row[1] > 0)b = 1;else if (row[1] < 0)b = 5;else if (row[2] > 0)b = 2;else if (row[2] < 0)b = 6;elseb = 7;      // errorreturn b;
}static unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
{unsigned short scalar;/*XYZ  010_001_000 Identity MatrixXZY  001_010_000YXZ  010_000_001YZX  000_010_001ZXY  001_000_010ZYX  000_001_010*/scalar = inv_row_2_scale(mtx);scalar |= inv_row_2_scale(mtx + 3) << 3;scalar |= inv_row_2_scale(mtx + 6) << 6;return scalar;
}static int run_self_test(void)
{int result;long gyro[3], accel[3];result = mpu_run_6500_self_test(gyro, accel,0);//这里需要根据自己的陀螺仪进行修改if (result == 0x07) {/* Test passed. We can trust the gyro data here, so let's push it down* to the DMP.*/float sens;unsigned short accel_sens;mpu_get_gyro_sens(&sens);gyro[0] = (long)(gyro[0] * sens);gyro[1] = (long)(gyro[1] * sens);gyro[2] = (long)(gyro[2] * sens);dmp_set_gyro_bias(gyro);mpu_get_accel_sens(&accel_sens);accel[0] *= accel_sens;accel[1] *= accel_sens;accel[2] *= accel_sens;dmp_set_accel_bias(accel);} else {return -1;}return 0;
}
int MPU6050_DMP_init(void)
{int ret;struct int_param_s int_param;//mpu_initret = mpu_init(&int_param);if(ret != 0){return ERROR_MPU_INIT;}//设置传感器ret = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);if(ret != 0){return ERROR_SET_SENSOR;}//设置fiforet = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);if(ret != 0){return ERROR_CONFIG_FIFO;}//设置采样率ret = mpu_set_sample_rate(DEFAULT_MPU_HZ);if(ret != 0){return ERROR_SET_RATE;}//加载DMP固件ret = dmp_load_motion_driver_firmware();if(ret != 0){return ERROR_LOAD_MOTION_DRIVER;}//设置陀螺仪方向ret = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));if(ret != 0){return ERROR_SET_ORIENTATION;}//设置DMP功能ret = dmp_enable_feature(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);if(ret != 0){return ERROR_ENABLE_FEATURE;}//设置输出速率ret = dmp_set_fifo_rate(DEFAULT_MPU_HZ);if(ret != 0){return ERROR_SET_FIFO_RATE;}//自检ret = run_self_test();if(ret != 0){return ERROR_SELF_TEST;}//使能DMPret = mpu_set_dmp_state(1);if(ret != 0){return ERROR_DMP_STATE;}return 0;
}
//DMP解算的结果为四元数，需要进行换算
int MPU6050_DMP_Get_Date(float *pitch, float *roll, float *yaw)
{float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;short gyro[3];short accel[3];long quat[4];unsigned long timestamp;short sensors;unsigned char more;if(dmp_read_fifo(gyro, accel, quat, &timestamp, &sensors, &more)){return -1;}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 * (q0 * q3 + q1 * q2), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3; // yaw}return 0;
}

MPU6500.h

#ifndef INC_MPU6050_H_
#define INC_MPU6050_H_#define ERROR_MPU_INIT      -1 //错误代码所代表的含义
#define ERROR_SET_SENSOR    -2
#define ERROR_CONFIG_FIFO   -3
#define ERROR_SET_RATE      -4
#define ERROR_LOAD_MOTION_DRIVER    -5
#define ERROR_SET_ORIENTATION       -6
#define ERROR_ENABLE_FEATURE        -7
#define ERROR_SET_FIFO_RATE         -8
#define ERROR_SELF_TEST             -9
#define ERROR_DMP_STATE             -10#define DEFAULT_MPU_HZ  100
#define Q30  1073741824.0fint MPU6050_DMP_init(void);//DMP初始化函数
int MPU6050_DMP_Get_Date(float *pitch, float *roll, float *yaw);//DMP调用函数#endif /* INC_MPU6050_H_ */

主函数验证

调用函数写完后，我们便可以在主函数中进行验证,我在这里使用OLED打印解算结果(使用CLion的记得打开浮点数打印支持,仓库里有具体说明)
我这里根据自己的OLED代码进行调用，放在这里作为参考，可根据自己的喜好替换为串口打印或别的方式

/* USER CODE BEGIN 2 */HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_SET);OLED_Init();OLED_Clear();int ret=0;do {ret=MPU6050_DMP_init();//DMP初始化} while (ret);/* USER CODE END 2 *//* Infinite loop *//* USER CODE BEGIN WHILE */while (1){/* USER CODE END WHILE *//* USER CODE BEGIN 3 */MPU6050_DMP_Get_Date(&pitch,&roll,&yaw);//DMP数据获取sprintf(String,"pitch:%.1f",pitch);OLED_ShowString(0,0,String,8);sprintf(String,"roll:%.1f",roll);OLED_ShowString(0,16,String,8);sprintf(String,"yaw:%.1f",yaw);OLED_ShowString(0,32,String,8);OLED_Update();}/* USER CODE END 3 */
Date(&pitch,&roll,&yaw);//DMP数据获取sprintf(String,"pitch:%.1f",pitch);OLED_ShowString(0,0,String,8);sprintf(String,"roll:%.1f",roll);OLED_ShowString(0,16,String,8);sprintf(String,"yaw:%.1f",yaw);OLED_ShowString(0,32,String,8);OLED_Update();}/* USER CODE END 3 */

实验现象

接线

我所有的IIC设备都挂在hi2c1下面直接串接即可
3V3–3V3
PB6–SCL
PB7–SDA
GND–GND

现象

在这里插入图片描述

http://www.dtcms.com/a/449617.html

相关文章：

  • 使用第三方库
  • 腾讯企业邮箱登录入口app云优化seo软件
  • 【操作系统-Day 40】文件的“身份证”：深入解析文件定义、属性与核心操作
  • 磁共振成像原理（理论）19：基本成像原理 (Basic Imaging Methods) - 三维成像
  • 线程池——线程池
  • WebSocket细谈
  • 公司网站怎么建站微网站如何做微信支付宝支付
  • Ubuntu 原地升级 MongoDB 全攻略
  • 网站变灰色代码安徽省建设工程信息网官网是什么网站
  • Hexo博客搭建系列（四）：透明居中导航栏+鼠标悬停放大效果
  • 【STM32项目开源】基于STM32的智能仓库火灾检测系统
  • 陕西省建设监理协会网站证书wordpress 图片外链
  • 做模板网站企业网站类型
  • 24H2壁纸显示错误修复（针对vb.net的紧急加更）
  • 兰州做网站 东方商易怎么样做美术招生信息网站
  • 酒店客房管理系统|基于SpringBoot和Vue的酒店客房管理系统(源码+数据库+文档)
  • AI编程开发系统019-基于Vue+SpringBoot的邮件收发系统（源码+部署说明+演示视频+源码介绍+lw）
  • 做海免费素材网站排版设计模板
  • 212-基于Python的老人健康管理系统
  • 万能格式文件查看工具，支持查看图像、音视频和文档等，免安装超方便！
  • 做食品企业网站的费用wordpress文章图片全屏浏览
  • 韩国免费行情网站的推荐理由外贸 wordpress
  • 嵌入式开发核心知识点详解教程
  • 操作系统应用开发(二十六)RustDesk tls证书不匹配错误—东方仙盟筑基期
  • 如何制作个人网站住建局查询房产信息
  • 西安网站建设hyk123wordpress帖子添加代码
  • 乐理知识学习内容
  • 新手SEO教程：高效提升网站访问量的实用技巧与策略
  • 代码文件内容
  • 一款基于ESP32的导航小车