MPU6050四元数 欧拉角 程序.docx
《MPU6050四元数 欧拉角 程序.docx》由会员分享,可在线阅读,更多相关《MPU6050四元数 欧拉角 程序.docx(59页珍藏版)》请在冰豆网上搜索。
![MPU6050四元数 欧拉角 程序.docx](https://file1.bdocx.com/fileroot1/2023-2/2/2fff66c9-b81d-4185-a85f-5711655099cc/2fff66c9-b81d-4185-a85f-5711655099cc1.gif)
MPU6050四元数欧拉角程序
#include"sys.h"
#include"usart.h"
#include"delay.h"
#include"chinese.h"
#include"lcd.h"
#include"IIC.h"
#include"inv_mpu.h"
#include"inv_mpu_dmp_motion_driver.h"
#include"upload.h"
#include"math.h"
staticsignedchargyro_orientation[9]={-1,0,0,
0,-1,0,
0,0,1};
#defineq301073741824.0f
floatq0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
charnum[50];
floatPitch,Roll,Yaw;
unsignedlongsensor_timestamp;
shortgyro[3],accel[3],sensors;
unsignedcharmore;
longquat[4];
intmain(void)
{
//u16i;
intresult;
Stm32_Clock_Init(9);//系统时钟设置
delay_init(72);//延时初始化
uart_init(72,9600);//串口1初始化
LCD_Init();
POINT_COLOR=RED;
i2cInit();
//result=mpu_init();
//if(!
result)
{
//PrintChar("mpuinitializationcomplete......\n");//mpu_set_sensor
//if(!
mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL))
mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);
//PrintChar("mpu_set_sensorcomplete......\n");
//else
//PrintChar("mpu_set_sensorcomeacrosserror......\n");
if(!
mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL))//mpu_configure_fifo
PrintChar("mpu_configure_fifocomplete......\n");
else
PrintChar("mpu_configure_fifocomeacrosserror......\n");
if(!
mpu_set_sample_rate(DEFAULT_MPU_HZ))//mpu_set_sample_rate
PrintChar("mpu_set_sample_ratecomplete......\n");
else
PrintChar("mpu_set_sample_rateerror......\n");
if(!
dmp_load_motion_driver_firmware())//dmp_load_motion_driver_firmvare
PrintChar("dmp_load_motion_driver_firmwarecomplete......\n");
else
PrintChar("dmp_load_motion_driver_firmwarecomeacrosserror......\n");
if(!
dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation)))//dmp_set_orientation
PrintChar("dmp_set_orientationcomplete......\n");
else
PrintChar("dmp_set_orientationcomeacrosserror......\n");
if(!
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL))//dmp_enable_feature
PrintChar("dmp_enable_featurecomplete......\n");
else
PrintChar("dmp_enable_featurecomeacrosserror......\n");
if(!
dmp_set_fifo_rate(DEFAULT_MPU_HZ))//dmp_set_fifo_rate
PrintChar("dmp_set_fifo_ratecomplete......\n");
else
PrintChar("dmp_set_fifo_ratecomeacrosserror......\n");
run_self_test();
if(!
mpu_set_dmp_state
(1))
PrintChar("mpu_set_dmp_statecomplete......\n");
else
PrintChar("mpu_set_dmp_statecomeacrosserror......\n");
}
while
(1)
{
dmp_read_fifo(gyro,accel,quat,&sensor_timestamp,&sensors,
&more);
/*GyroandacceldataarewrittentotheFIFObytheDMPinchip
*frameandhardwareunits.Thisbehaviorisconvenientbecauseit
*keepsthegyroandacceloutputsofdmp_read_fifoand
*mpu_read_fifoconsistent.
*/
/*if(sensors&INV_XYZ_GYRO)
send_packet(PACKET_TYPE_GYRO,gyro);
if(sensors&INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL,accel);*/
/*Unlikegyroandaccel,quaternionsarewrittentotheFIFOin
*thebodyframe,q30.Theorientationissetbythescalarpassed
*todmp_set_orientationduringinitialization.
*/
if(sensors&INV_WXYZ_QUAT)
{
q0=quat[0]/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;
//sprintf(num,"%f",q0);//inCalculatingquaternionsteps.....
//sprintf(num,"%f",q1);
sprintf(num,"%f,%f,%f",Pitch,Roll,Yaw);
PrintChar(num);
PrintChar("\n");
//LCD_ShowString(30,190,200,16,32,num);
//LCD_Show2Num(30,150,Pitch,16,8,0);
//LCD_Show2Num(30,170,Roll,16,8,0);
//LCD_Show2Num(30,190,Yaw,16,8,0);
//UART1_ReportIMU(Yaw*10,Pitch*10,Roll*10,0,0,0,100);
}
LCD_ShowNum(30,80,result,2,16);
POINT_COLOR=RED;
LCD_ShowString(30,50,"TFTTEST");
//delay_ms(500);
//LCD_ShowNum(30,70,sca610(0,5),6,16);
//Hanzi(50,110,BLUE,0,4,1);
}
}
/*
$License:
Copyright(C)2011-2012InvenSenseCorporation,AllRightsReserved.
SeeincludedLicense.txtforLicenseinformation.
$
*/
/**
*@addtogroupDRIVERSSensorDriverLayer
*@briefHardwaredriverstocommunicatewithsensorsviaI2C.
*
*@{
*@fileinv_mpu_dmp_motion_driver.c
*@briefDMPimageandinterfacefunctions.
*@detailsAllfunctionsareprecededbythedmp_prefixto
*differentiateamongMPLandgeneraldriverfunctioncalls.
*/
//#include"stm32f10x.h"
#include
#include
#include
#include
#include
#include"inv_mpu.h"
#include"inv_mpu_dmp_motion_driver.h"
#include"dmpKey.h"
#include"dmpmap.h"
#include"Time.h"
#include"delay.h"
#defineMOTION_DRIVER_TARGET_MSP430
/*Thefollowingfunctionsmustbedefinedforthisplatform:
*i2c_write(unsignedcharslave_addr,unsignedcharreg_addr,
*unsignedcharlength,unsignedcharconst*data)
*i2c_read(unsignedcharslave_addr,unsignedcharreg_addr,
*unsignedcharlength,unsignedchar*data)
*delay_ms(unsignedlongnum_ms)
*get_ms(unsignedlong*count)
*/
#ifdefinedMOTION_DRIVER_TARGET_MSP430
//#include"msp430.h"
//#include"msp430_clock.h"
#definedelay_msdelay_ms
#defineget_msget_ms
#definelog_i(...)do{}while(0)
#definelog_e(...)do{}while(0)
#elifdefinedEMPL_TARGET_MSP430
#include"msp430.h"
#include"msp430_clock.h"
#include"log.h"
#definedelay_msmsp430_delay_ms
#defineget_msmsp430_get_clock_ms
#definelog_iMPL_LOGI
#definelog_eMPL_LOGE
#elifdefinedEMPL_TARGET_UC3L0
/*InsteadofusingthestandardTWIdriverfromtheASFlibrary,we'reusing
*aTWIdriverthatfollowstheslaveaddress+registeraddressconvention.
*/
#include"delay.h"
#include"sysclk.h"
#include"log.h"
#include"uc3l0_clock.h"
/*delay_msisafunctionalreadydefinedinASF.*/
#defineget_msuc3l0_get_clock_ms
#definelog_iMPL_LOGI
#definelog_eMPL_LOGE
#else
//#errorGyrodriverismissingthesystemlayerimplementations.
#endif
/*ThesedefinesarecopiedfromdmpDefaultMPU6050.cinthegeneralMPL
*releases.ThesedefinesmaychangeforeachDMPimage,sobesuretomodify
*thesevalueswhenswitchingtoanewimage.
*/
#defineCFG_LP_QUAT(2712)
#defineEND_ORIENT_TEMP(1866)
#defineCFG_27(2742)
#defineCFG_20(2224)
#defineCFG_23(2745)
#defineCFG_FIFO_ON_EVENT(2690)
#defineEND_PREDICTION_UPDATE(1761)
#defineCGNOTICE_INTR(2620)
#defineX_GRT_Y_TMP(1358)
#defineCFG_DR_INT(1029)
#defineCFG_AUTH(1035)
#defineUPDATE_PROP_ROT(1835)
#defineEND_COMPARE_Y_X_TMP2(1455)
#defineSKIP_X_GRT_Y_TMP(1359)
#defineSKIP_END_COMPARE(1435)
#defineFCFG_3(1088)
#defineFCFG_2(1066)
#defineFCFG_1(1062)
#defineEND_COMPARE_Y_X_TMP3(1434)
#defineFCFG_7(1073)
#defineFCFG_6(1106)
#defineFLAT_STATE_END(1713)
#defineSWING_END_4(1616)
#defineSWING_END_2(1565)
#defineSWING_END_3(1587)
#defineSWING_END_1(1550)
#defineCFG_8(2718)
#defineCFG_15(2727)
#defineCFG_16(2746)
#defineCFG_EXT_GYRO_BIAS(1189)
#defineEND_COMPARE_Y_X_TMP(1407)
#defineDO_NOT_UPDATE_PROP_ROT(1839)
#defineCFG_7(1205)
#defineFLAT_STATE_END_TEMP(1683)
#defineEND_COMPARE_Y_X(1484)
#defineSKIP_SWING_END_1(1551)
#defineSKIP_SWING_END_3(1588)
#defineSKIP_SWING_END_2(1566)
#defineTILTG75_START(1672)
#defineCFG_6(2753)
#defineTILTL75_END(1669)
#defineEND_ORIENT(1884)
#defineCFG_FLICK_IN(2573)
#defineTILTL75_START(1643)
#defineCFG_MOTION_BIAS(1208)
#defineX_GRT_Y(1408)
#defineTEMPLABEL(2324)
#defineCFG_ANDROID_ORIENT_INT(1853)
#defineCFG_GYRO_RAW_DATA(2722)
#defineX_GRT_Y_TMP2(1379)
#defineD_0_22(22+512)
#defineD_0_24(24+512)
#defineD_0_36(36)
#defineD_0_52(52)
#defineD_0_96(96)
#defineD_0_104(104)
#defineD_0_108(108)
#defineD_0_163(163)
#defineD_0_188(188)
#defineD_0_192(192)
#defineD_0_224(224)
#defineD_0_228(228)
#defineD_0_232(232)
#defineD_0_236(236)
#defineD_1_2(256+2)
#defineD_1_4(256+4)
#defineD_1_8(256+8)
#defineD_1_10(256+10)
#defineD_1_24(256+24)
#defineD_1_28(256+28)
#defineD_1_36(256+36)
#defineD_1_40(256+40)
#defineD_1_44(256+44)
#defineD_1_72(256+72)
#defineD_1_74(256+74)
#defineD_1_79(256+79)
#defineD_1_88(256+88)
#defineD_1_90(256+90)
#defineD_1_92(256+92)
#defineD_1_96(256+96)
#defineD_1_98(256+98)
#defineD_1_106(256+106)
#defineD_1_108(256+108)
#defineD_1_112(256+112)
#defineD_1_128(256+144)
#defineD_1_152(256+12)
#defineD_1_160(256+160)
#defineD_1_176(256+176)
#defineD_1_178(256+178)
#defineD_1_218(256+218)
#defineD_1_232(256+232)
#defineD_1_236(256+236)
#defineD_1_240(256+240)
#defineD_1_244(256+244)
#defineD_1_250(256+250)
#defineD_1_252(256+252)
#defineD_2_12(512+12)
#defineD_2_96(512+96)
#defineD_2_108(512+108)
#defineD_2_208(512+208)
#defineD_2_224(512+224)
#defineD_2_236(512+236)
#defineD_2_244(512+244)
#defineD_2_248(512+248)
#defineD_2_252(512+252)
#defineCPASS_BIAS_X(35*16+4)
#defineCPASS_BIAS_Y(35*16+8)
#defineCPASS_BIAS_Z(35*16+12)
#defineCPASS_MTX_00(36*16)
#defineCPASS_MTX_01(36*16+4)
#defineCPASS_MTX_02(36*16+8)
#defineCPASS_MTX_10(36*16+12)
#defineCPASS_MTX_11(37*16)
#defineCPASS_MTX_12(37*16+4)
#defineCPASS_MTX_20(37*16+8)
#defineCPASS_MTX_21(37*16+12)
#defineCPASS_MTX_22(43*16+12)
#defineD_EXT_GYRO_BIAS_X(61*16)
#defineD_EXT_GYRO_BIAS_Y(61*16)+4
#defineD_EXT_GYRO_BIAS_Z(61*16)+8
#defineD_ACT0(40*16)
#defineD_ACSX(40*