MPU6050 DMP直接输出姿态角，无需卡尔曼 MSP430主控 -

#include <msp430f149.h>
#include <math.h>
#include "Config.h"
#include "1602.c"
#include "mpu6050.c"
#include "mpu60500.h"
#include "mpu60500.c"
#include "mpuiic.h"
#include "mpuiic.c"
#include "IOI2C.h"
#include "IOI2C.c"
#include "dmpKey.h"
#include "dmpmap.h"
#include "inv_mpu.h"
#include "inv_mpu.c"
#include "inv_mpu_dmp_motion_driver.h"
#include "inv_mpu_dmp_motion_driver.c"
#include "kaerman.c"
struct quaternion{
float w;
float x;
float y;
float z;
}quaternion;
float gyr[3],acc[3];
float Pitch=1.0,Roll,Yaw ;
//q30格式,long转float时的除数.
#define q30 1073741824.0f
void mix_gyrAcc_crossMethod(struct quaternion *attitude,const float gyr[3],const float acc[3],float interval);
void quaternion_normalize(struct quaternion*q);
void main()
{
WDT_Init(); //看门狗设置
Clock_Init(); //系统时钟设置
Port_init(); //系统初始化，设置IO口属性
delay_ms(100); //延时100ms
LCD_init(); //液晶参数初始化设置
LCD_clear(); //清屏
//InitMPU6050();
uchar e=mpu_dmp_init();
float yy=e*1.0;
delay_ms(300);
/*
P1DIR|=0x01;
P1OUT&=0XFE;
TA0CCTL1=OUTMOD_7+CCIE;
TACTL=TASSEL_2+MC_1+TAIE;
TA0CCR0=655;
TA0CCR1=654;
*/
//float aaa=0.0,bbb=0.0,ccc=0.0;
quaternion.w=1;
quaternion.x=0;
quaternion.y=0;
quaternion.z=0;
_EINT();
while(1)
{
Disp(yy,0,0);
/*
gyr[0]=-GetData(GYRO_XOUT_H)/16.4;
gyr[1]=-GetData(GYRO_YOUT_H)/16.4;
gyr[2]=-GetData(GYRO_ZOUT_H)/16.4;
acc[0]=GetData(ACCEL_XOUT_H)/16384;
acc[1]=GetData(ACCEL_YOUT_H)/16384;
acc[2]=GetData(ACCEL_ZOUT_H)/16384;
Disp(Pitch,0,0);
Disp(Roll,7,0);
Disp(Yaw,0,1);
Disp(quaternion.z,7,1);
mpu_dmp_get_data(&aaa,&bbb,&ccc);
Disp(aaa,0,0);
Display10BitData(GetData(ACCEL_XOUT_H),0,0); //显示X轴加速度
Display10BitData(GetData(ACCEL_YOUT_H),5,0); //显示Y轴加速度
Display10BitData(GetData(ACCEL_ZOUT_H),10,0); //显示Z轴加速度
Display10BitData(GetData(GYRO_XOUT_H),0,1); //显示X轴角速度
Display10BitData(GetData(GYRO_YOUT_H),5,1); //显示Y轴角速度
Display10BitData(GetData(GYRO_ZOUT_H),10,1); //显示Z轴角速度 */
}
}
/*
#pragma vector=TIMERA1_VECTOR
__interrupt void P1(void)
{
P1OUT|=0X01;
if(TAIV==2)
{_NOP();}
mix_gyrAcc_crossMethod(&quaternion,gyr,acc,0.01);
}
*/
void mix_gyrAcc_crossMethod(struct quaternion *attitude,const float gyr[3],const float acc[3],float interval)
{
const static float FACTOR = 0.001;//取接近0的数
//
float w_q = attitude->w;
float x_q = attitude->x;
float y_q = attitude->y;
float z_q = attitude->z;
float x_q_2 = x_q * 2;
float y_q_2 = y_q * 2;
float z_q_2 = z_q * 2;
//
// 加速度计的读数，单位化。
float a_rsqrt = sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]);
float x_aa = acc[0] * a_rsqrt;
float y_aa = acc[1] * a_rsqrt;
float z_aa = acc[2] * a_rsqrt; //加速度计测量出的加速度向量(载体坐标系下)
//
// 载体坐标下的重力加速度向量，单位化。
float x_ac = x_q*z_q_2 - w_q*y_q_2;
float y_ac = y_q*z_q_2 + w_q*x_q_2; //通过四元数旋转矩阵与地理坐标系下的重力加速度向量[0 0 0 1]叉乘得到载体坐标系下的重力加速度向量
float z_ac = 1 - x_q*x_q_2 - y_q*y_q_2;//（主要）角速度计测出的四元数表示的载体坐标系下的重力加速度向量（这里已转换成载体坐标系下）
//
// 测量值与常量的叉积。
float x_ca = y_aa * z_ac - z_aa * y_ac;
float y_ca = z_aa * x_ac - x_aa * z_ac;
float z_ca = x_aa * y_ac - y_aa * x_ac;//角速度计测出的角度误差，叠加的FACTOR大小可以实验试凑
//
// 构造增量旋转。
float delta_x = gyr[0] * interval / 2 + x_ca * FACTOR;
float delta_y = gyr[1] * interval / 2 + y_ca * FACTOR;
float delta_z = gyr[2] * interval / 2 + z_ca * FACTOR;
//
// 融合，四元数乘法。
attitude->w = w_q - x_q*delta_x - y_q*delta_y - z_q*delta_z;
attitude->x = w_q*delta_x + x_q + y_q*delta_z - z_q*delta_y;
attitude->y = w_q*delta_y - x_q*delta_z + y_q + z_q*delta_x;
attitude->z = w_q*delta_z + x_q*delta_y - y_q*delta_x + z_q;
quaternion_normalize(attitude);//归一化
float q0=0.0,q1=0.0,q2=0.0,q3=0.0;
q0=attitude->w;
q1=attitude->x;
q2=attitude->y;
q3=attitude->z;
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;
}
void quaternion_normalize(struct quaternion*q)
{
float qlength_inv = 1.0/(sqrt(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z));
//这里只应该开根号x*x+y*y+z*z
// now normalize
q->w=q->w*qlength_inv;
q->x=q->x*qlength_inv;
q->y=q->y*qlength_inv;
q->z=q->z*qlength_inv;
}

复制代码