MPU6050四元数 欧拉角 程序.docx
《MPU6050四元数 欧拉角 程序.docx》由会员分享，可在线阅读，更多相关《MPU6050四元数 欧拉角 程序.docx（59页珍藏版）》请在冰豆网上搜索。
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*
升级会员 