欧美日韩中文,一二三四视频社区在线,在线看国产一区

#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;
}

復制代碼

wwwxxx国产_337p日本欧洲亚洲大胆张筱雨_免费在线看成人av_日本黄色不卡视频_国产精品成熟老女人_99视频一区_亚洲精品97久久中文字幕_免费精品视频在线_亚洲色图欧美视频_欧美一区二三区