[资源共享] 移植MPU6050-DMP库实现姿态角PRY解算

#define i2c_write   MPU_Write_Len

#define i2c_read    MPU_Read_Len

#define delay_ms    systick_delay_ms

#define get_ms      mget_m

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

} 

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

}

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

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

}

void mget_ms(unsigned long *time)



}

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

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

}

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

}