移植MPU6050-DMP库实现姿态角PRY解算

//得到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;

}

#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

软件 IIC 引脚定义连接

#define SEEKFREE_SCL        B13                                                                                                                        // 定义SCL引脚

#define SEEKFREE_SDA        B15                                                                                                                        // 定义SDA引脚  

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

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

}

#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

        }

}