MM32使用DMP库处理MPU6050数据、姿态解算

只看该作者 · 2022-8-31 23:30

mpu6050介绍
MPU6050 是 InvenSense 公司推出的全球首款整合性 6 轴运动处理组件，相较于多组件
方案，免除了组合陀螺仪与加速器时之轴间差的问题，减少了安装空间。

MPU6050 内部整合了 3 轴陀螺仪和 3 轴加速度传感器，并且含有一个第二 IIC 接口（本
模块未引出），可用于连接外部磁力传感器，并利用自带的数字运动处理器（ DMP: Digital
Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。
我们可以使用 InvenSense 公司提供的运动处理DMP资料库，非常方便的实现姿态算，降低了运动处理运算对操作系统的负荷，同时大大降低了开发难度。

只看该作者 · 2022-8-31 23:32

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）

只看该作者 · 2022-8-31 23:33

灵动MM32-DMP库移植
InvenSense 提供的 MPU6050 运动驱动库是基于 MSP430 的，我们需要将其移植一下，
才可以用到 MM32上面。

只看该作者 · 2022-8-31 23:35

添加库文件
首先需要添加这六个文件到工程中，这六个文件是我从正点原子那里复制过来的，里边大部分不需要改动，只有下面少数几个地方。最后我会将工程文件上传到百度网盘。

只看该作者 · 2022-8-31 23:35

官方 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

只看该作者 · 2022-8-31 23:38

其中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[i]);        //发送数据                

        }    

    simiic_stop();         

        return 0;        

}

只看该作者 · 2022-8-31 23:39

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;        

}

只看该作者 · 2022-8-31 23:40

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

}

只看该作者 · 2022-8-31 23:41

mget_ms函数为空函数未用到位置在inv_mpu.h文件中

复制

void mget_ms(unsigned long *time)



}

只看该作者 · 2022-8-31 23:41

inv_mpu.c 文件移植
包括头文件调用及 4 个函数： i2c_write， i2c_read，delay_ms 和 get_ms*函数重定义

只看该作者 · 2022-8-31 23:44

复制

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

只看该作者 · 2022-8-31 23:45

修改两个函数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;

}

只看该作者 · 2022-8-31 23:47

①：调用mpu_init 函数（MSP430库函数，不用管），初始化 MPU6050。
②：设置 DMP 所用传感器、 FIFO、采样率和加载固件等一系列操作。
③：最后通过 mpu_set_dmp_state(1)使能 DMP 功能，在使能成功以后，我们便可以通过 mpu_dmp_get_data 来读取姿态解算后的数据了。
————————————————

只看该作者 · 2022-8-31 23:47

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[3], accel[3], sensors;

        unsigned char more;

        long quat[4]; 

        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[0] / q30;        //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

        }else return 2;

        return 0;

}

只看该作者 · 2022-8-31 23:49

这里就用到了我们前面介绍的四元数转欧拉角公式，将 dmp_read_fifo 函数读到的 q30 格式四元数转换成欧拉角。

其中 quat[0]~ quat[3]是 MPU6050 的 DMP 解算后的四元数， q30 格式，所以要除以一个2 的 30 次方，其中 q30 是一个常量： 1073741824，即 2 的 30 次方，然后带入公式，计算出欧拉角。

上述计算公式的 57.3 是弧度转换为角度，即 180/π，这样得到的结果就是以度（°）为单位的。

关于四元数与欧拉角的公式推导，这里我们不进行讲解，感兴趣的朋友，可以自行查阅相关资料学习。

此函数用于得到 DMP 姿态解算后的俯仰角、横滚角和航向角。

只看该作者 · 2022-8-31 23:49

inv_mpu_dmp_motion_driver.c移植
这个文件只需要修改头文件调用和函数重新定义，不需要修改函数。

复制

#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 "dmpKey.h"

#include "dmpmap.h"



#include "SEEKFREE_IIC.h"

#include "zf_systick.h"



//定义目标板采用MSP430

#define  MOTION_DRIVER_TARGET_MSP430



/* 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)

 */

#if defined MOTION_DRIVER_TARGET_MSP430

//#include "msp430.h"

//#include "msp430_clock.h"

#define delay_ms    systick_delay_ms

#define get_ms      mget_ms

#define log_i                 printf

#define log_e                  printf

只看该作者 · 2022-8-31 23:50

MM32程序编写

复制

软件 IIC 引脚定义连接

#define SEEKFREE_SCL        B13                                                                                                                        // 定义SCL引脚

#define SEEKFREE_SDA        B15                                                                                                                        // 定义SDA引脚

只看该作者 · 2022-8-31 23:50

MPU6050初始化

复制

//-------------------------------------------------------------------------------------------------------------------

// @brief                初始化MPU6050

// @param                NULL

// @return                void

//-------------------------------------------------------------------------------------------------------------------

void mpu6050_init(void)

{

        simiic_init();                                                                                                                        //模拟IIC的初始化

        systick_delay_ms(100);                                                                                                        // 上电延时



        mpu6050_self1_check();                                                                                                        //上电自检

        simiic_write_reg(MPU6050_DEV_ADDR, PWR_MGMT_1, 0x00);                                        // 解除休眠状态

        simiic_write_reg(MPU6050_DEV_ADDR, SMPLRT_DIV, 0x07);                                        // 125HZ采样率

        simiic_write_reg(MPU6050_DEV_ADDR, MPU6050_CONFIG, 0x04);                                // 

        simiic_write_reg(MPU6050_DEV_ADDR, GYRO_CONFIG, 0x18);                                        // 2000

        simiic_write_reg(MPU6050_DEV_ADDR, ACCEL_CONFIG, 0x10);                                        // 8g

        simiic_write_reg(MPU6050_DEV_ADDR, User_Control, 0x00);

        simiic_write_reg(MPU6050_DEV_ADDR, INT_PIN_CFG, 0x02);

}

只看该作者 · 2022-8-31 23:51

主函数编写
1、引入头文件
2、调用mpu6050初始化
3、dmp初始化
4、得到加速度值
5、解算欧拉角
6、屏幕显示

只看该作者 · 2022-8-31 23:51

#include "headfile.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
// **************************** 代码区域 ****************************

int main(void)
{
float pitch,roll,yaw; //欧拉角
mpu6050_init();
while(mpu_dmp_init()!=0)
{
LCD_ShowString(0,0,"dmp-error",MAGENTA);
}
LCD_ShowString(0,0,"dmp-init-ok",MAGENTA);

while(1)
{
get_gyro();
mpu_dmp_get_data(&pitch,&roll,&yaw);

LCD_ShowFloat(0,40,pitch,5,BLACK);
LCD_ShowFloat(0,60,roll,5,BLACK);
LCD_ShowFloat(0,80,yaw,5,BLACK
}
}