移植MPU6050-DMP库实现姿态角PRY解算
本帖最后由 芯圣电子官方QQ 于 2023-7-20 10:37 编辑【灵动MM32-姿态角解算】移植MPU6050-DMP库实现姿态角PRY解算MM32使用DMP库处理MPU6050数据、姿态解算
实现途径：
①MM32F3277G9P单片机 ②MPU6050模块 ③智能车逐飞MM32开源库 ④正点原子dmp开源库
开源库链接: 逐飞科技MM32F327X_G9P开源库
MM32使用DMP库处理MPU6050数据、姿态解算
一、mpu6050介绍
二、灵动MM32-DMP库移植
1、添加库文件
2、官方 DMP 驱动库移植
3、inv_mpu.c 文件移植
4、 inv_mpu_dmp_motion_driver.c移植
三、MM32程序编写
1、MPU6050初始化
2、主函数编写
四、工程源码
一、mpu6050介绍
MPU6050 是 InvenSense 公司推出的全球首款整合性 6 轴运动处理组件，相较于多组件
方案，免除了组合陀螺仪与加速器时之轴间差的问题，减少了安装空间。
MPU6050 内部整合了 3 轴陀螺仪和 3 轴加速度传感器，并且含有一个第二 IIC 接口（本
模块未引出），可用于连接外部磁力传感器，并利用自带的数字运动处理器（ DMP: Digital
Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。
我们可以使用 InvenSense 公司提供的运动处理DMP资料库，非常方便的实现姿态算，降低了运动处理运算对操作系统的负荷，同时大大降低了开发难度。 MPU6050 的特点包括：
① 以数字形式输出 6 轴或 9 轴（需外接磁传感器）的旋转矩阵、四元数(quaternion)、
欧拉角格式(Euler Angle forma)的融合演算数据（需 DMP 支持）
② 具有 131 LSBs/° /sec 敏感度与全格感测范围为±250、±500、±1000 与±2000°
/sec 的 3 轴角速度感测器(陀螺仪)
③ 集成可程序控制，范围为±2g、±4g、±8g 和±16g 的 3 轴加速度传感器
④ 移除加速器与陀螺仪轴间敏感度，降低设定给予的影响与感测器的飘移
⑤ 自带数字运动处理(DMP: Digital Motion Processing)引擎可减少 MCU 复杂的融合演
算数据、感测器同步化、姿势感应等的负荷
⑥ 内建运作时间偏差与磁力感测器校正演算技术， 免除了客户须另外进行校正的需求
⑦ 自带一个数字温度传感器
⑧ 带数字输入同步引脚(Sync pin)支持视频电子影相稳定技术与 GPS
⑨ 可程序控制的中断(interrupt)，支持姿势识别、摇摄、画面放大缩小、滚动、快速
下降中断、 high-G 中断、零动作感应、触击感应、摇动感应功能
⑩ VDD 供电电压为 2.5V±5%、 3.0V±5%、 3.3V±5%； VLOGIC 可低至1.8V±5%
⑪ 陀螺仪工作电流： 5mA，陀螺仪待机电流： 5uA；加速器工作电流： 500uA，加速
器省电模式电流： 40uA@10Hz
⑫ 自带 1024 字节 FIFO，有助于降低系统功耗
⑬ 高达 400Khz 的 IIC 通信接口
⑭ 超小封装尺寸： 4x4x0.9mm（QFN） 二、灵动MM32-DMP库移植
InvenSense 提供的 MPU6050 运动驱动库是基于 MSP430 的，我们需要将其移植一下，
才可以用到 MM32上面。 添加库文件
首先需要添加这六个文件到工程中，这六个文件是我从正点原子那里复制过来的，里边大部分不需要改动，只有下面少数几个地方。最后我会将工程文件上传到百度网盘。
官方 DMP 驱动库移植
主要是实现这 4 个函数： i2c_write， i2c_read，delay_ms 和 get_ms
#define i2c_write MPU_Write_Len
#define i2c_read MPU_Read_Len
#define delay_ms systick_delay_ms
#define get_ms mget_m
其中MPU_Write_Len是我根据逐飞库改变而来，位置在SEEKREE_IIC.c文件之中
//IIC连续写
//addr:器件地址
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
// 其他,错误代码
uint8_t MPU_Write_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
uint8_t i;
simiic_start();
send_ch((addr<<1)|0);//发送器件地址+写命令
send_ch(reg); //写寄存器地址
for(i=0;i<len;i++)
{
send_ch(buf); //发送数据
}
simiic_stop();
return 0;
}
MPU_Read_Len是我根据逐飞库改变而来，位置在SEEKREE_IIC.c文件之中
//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
// 其他,错误代码
uint8_t MPU_Read_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
simiic_start();
send_ch((addr<<1)|0);//发送器件地址+写命令
send_ch(reg); //写寄存器地址
simiic_start();
send_ch((addr<<1)|1);//发送器件地址+读命令
while(len)
{
if(len==1)*buf=read_ch(0); //读数据,发送nACK
else *buf=read_ch(1); //读数据,发送ACK
len--;
buf++;
}
simiic_stop(); //产生一个停止条件
return 0;
}
systick_delay_ms函数为逐飞库内毫秒级systick延时函数，位置在zf_systick.h文件中
//-------------------------------------------------------------------------------------------------------------------
// @brief 毫秒级systick延时函数
// @param time 延时多少毫秒
// @return void
// Sample usage: systick_delay_ms(1000); //延时1000毫秒
//-------------------------------------------------------------------------------------------------------------------
void systick_delay_ms (uint32 time)
{
while(time--) systick_delay(SystemCoreClock / 1000);
}
mget_ms函数为空函数未用到 位置在inv_mpu.h文件中 void mget_ms(unsigned long *time)
}
inv_mpu.c 文件移植
包括头文件调用及 4 个函数： i2c_write， i2c_read，delay_ms 和 get_ms*函数重定义 #include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "mpu6050.h"
#include "SEEKFREE_IIC.h"
#include "zf_systick.h"
#define MPU6050 //定义我们使用的传感器为MPU6050
#define MOTION_DRIVER_TARGET_MSP430 //定义驱动部分,采用MSP430的驱动(移植到STM32F4)
#if defined MOTION_DRIVER_TARGET_MSP430
//#include "msp430.h"
//#include "msp430_i2c.h"
//#include "msp430_clock.h"
//#include "msp430_interrupt.h"
#define i2c_write MPU_Write_Len
#define i2c_read MPU_Read_Len
#define delay_ms systick_delay_ms
#define get_ms mget_ms
//static inline int reg_int_cb(struct int_param_s *int_param)
//{
// return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
// int_param->active_low);
//}
#define log_i printf //打印信息
#define log_e printf //打印信息
/* 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)
修改两个函数mpu_dmp_init 、mpu_dmp_get_data
mpu_dmp_init是 MPU6050 DMP 初始化函数 该函数代码如下：
//mpu6050,dmp初始化
//返回值:0,正常
// 其他,失败
u8 mpu_dmp_init(void)
{
u8 res=0;
//simiic_init(); //iic总线初始化，这里不需要加，因为在主函数中dmp初始化已经调用过了
if(mpu_init()==0) //初始化MPU6050
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
if(res)return 1;
res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO
if(res)return 2;
res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率
if(res)return 3;
res=dmp_load_motion_driver_firmware(); //加载dmp固件
if(res)return 4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
if(res)return 5;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL);
if(res)return 6;
res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz)
if(res)return 7;
res=run_self_test(); //自检
if(res)return 8;
res=mpu_set_dmp_state(1); //使能DMP
if(res)return 9;
}
return 0;
}
①：调用mpu_init 函数（MSP430库函数，不用管），初始化 MPU6050。
②：设置 DMP 所用传感器、 FIFO、采样率和加载固件等一系列操作。
③：最后通过 mpu_set_dmp_state(1)使能 DMP 功能，在使能成功以后，我们便可以通过 mpu_dmp_get_data 来读取姿态解算后的数据了。 mpu_dmp_get_data 函数代码如下：
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
//roll:横滚角精度:0.1° 范围:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
//返回值:0,正常
// 其他,失败
u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short gyro, accel, sensors;
unsigned char more;
long quat;
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;
/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
* This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
**/
/*if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
* The orientation is set by the scalar passed to dmp_set_orientation during initialization.
**/
if(sensors&INV_WXYZ_QUAT)
{
q0 = quat / q30; //q30格式转换为浮点数
q1 = quat / q30;
q2 = quat / q30;
q3 = quat / 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
}else return 2;
return 0;
}
页:
[1]
2