MPU6050四元數(shù) 歐拉角 程序

1、#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"stat

2、ic signed char gyro_orientation9 = -1, 0, 0, 0,-1, 0, 0, 0, 1;#define q30 1073741824.0ffloat q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;char num50;float Pitch,Roll,Yaw;unsigned long sensor_timestamp;short gyro3, accel3, sensors;unsigned char more;long quat4;int main(void)/ u16 i; int result; Stm32_Clock_Init(9

3、);/系統(tǒng)時(shí)鐘設(shè)置delay_init(72);/延時(shí)初始化uart_init(72,9600); /串口1初始化 LCD_Init();POINT_COLOR=RED;i2cInit();/ result = mpu_init();/ if(!result) / PrintChar("mpu initialization complete.n "); /mpu_set_sensor/ if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL) mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)

4、; / PrintChar("mpu_set_sensor complete .n"); / else / PrintChar("mpu_set_sensor come across error .n"); if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL) /mpu_configure_fifo PrintChar("mpu_configure_fifo complete .n"); else PrintChar("mpu_configure_fifo come acr

5、oss error .n"); if(!mpu_set_sample_rate(DEFAULT_MPU_HZ) /mpu_set_sample_rate PrintChar("mpu_set_sample_rate complete .n"); else PrintChar("mpu_set_sample_rate error .n"); if(!dmp_load_motion_driver_firmware() /dmp_load_motion_driver_firmvare PrintChar("dmp_load_motion_d

6、river_firmware complete .n"); else PrintChar("dmp_load_motion_driver_firmware come across error .n"); if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation) /dmp_set_orientation PrintChar("dmp_set_orientation complete .n"); else PrintChar("dmp_set_or

7、ientation come across error .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_feature complete .n"); else PrintCh

8、ar("dmp_enable_feature come across error .n"); if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ) /dmp_set_fifo_rate PrintChar("dmp_set_fifo_rate complete .n"); else PrintChar("dmp_set_fifo_rate come across error .n"); run_self_test(); if(!mpu_set_dmp_state(1) PrintChar("mpu_se

9、t_dmp_state complete .n"); else PrintChar("mpu_set_dmp_state come across error .n"); while(1) dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more); /* Gyro and accel data are written to the FIFO by the DMP in chip * frame and hardware units. This behavior i

10、s 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 w

11、ritten 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=quat0 / q30; q1=quat1 / q30; q2=quat2 / q30; q3=quat3 / q30; Pitch = asin(2 * q1 * q3 - 2 * q0* q2)* 57.3; / pitch Roll = a

12、tan2(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);/in Calculating quaternion steps./sprintf(num,"%f",q1);sprintf(num,"%f,%f,%f",Pitch,Roll,Yaw); PrintChar(num)

13、; 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,&quo

14、t;TFT TEST");/delay_ms(500);/LCD_ShowNum(30,70,sca610(0,5),6,16); /Hanzi(50,110,BLUE,0,4,1); /* $License: Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. See included License.txt for License information. $ */* * addtogroup DRIVERS Sensor Driver Layer * brief Hardware driver

15、s to communicate with sensors via I2C. * * * file inv_mpu_dmp_motion_driver.c * brief DMP image and interface functions. * details All functions are preceded by the dmp_ prefix to * differentiate among MPL and general driver function calls. */#include "stm32f10x.h"#include <stdio.h>#

16、include <stdint.h>#include <stdlib.h>#include <string.h>#include <math.h>#include "inv_mpu.h"#include "inv_mpu_dmp_motion_driver.h"#include "dmpKey.h"#include "dmpmap.h"#include "Time.h"#include "delay.h"#define MOTI

17、ON_DRIVER_TARGET_MSP430/* The following functions must be defined for this platform: * i2c_write(unsigned char slave_addr, unsigned char reg_addr, * unsigned char length, unsigned char const *data) * i2c_read(unsigned char slave_addr, unsigned char reg_addr, * unsigned char length, unsigned char *da

18、ta) * delay_ms(unsigned long num_ms) * get_ms(unsigned long *count) */#if defined MOTION_DRIVER_TARGET_MSP430/#include "msp430.h"/#include "msp430_clock.h"#define delay_ms delay_ms#define get_ms get_ms#define log_i(.) do while (0)#define log_e(.) do while (0)#elif defined EMPL_TA

19、RGET_MSP430#include "msp430.h"#include "msp430_clock.h"#include "log.h"#define delay_ms msp430_delay_ms#define get_ms msp430_get_clock_ms#define log_i MPL_LOGI#define log_e MPL_LOGE#elif defined EMPL_TARGET_UC3L0/* Instead of using the standard TWI driver from the ASF l

20、ibrary, we're using * a TWI driver that follows the slave address + register address convention. */#include "delay.h"#include "sysclk.h"#include "log.h"#include "uc3l0_clock.h"/* delay_ms is a function already defined in ASF. */#define get_ms uc3l0_get_clo

21、ck_ms#define log_i MPL_LOGI#define log_e MPL_LOGE#else/#error Gyro driver is missing the system layer implementations.#endif/* These defines are copied from dmpDefaultMPU6050.c in the general MPL * releases. These defines may change for each DMP image, so be sure to modify * these values when switch

22、ing to a new image. */#define CFG_LP_QUAT (2712)#define END_ORIENT_TEMP (1866)#define CFG_27 (2742)#define CFG_20 (2224)#define CFG_23 (2745)#define CFG_FIFO_ON_EVENT (2690)#define END_PREDICTION_UPDATE (1761)#define CGNOTICE_INTR (2620)#define X_GRT_Y_TMP (1358)#define CFG_DR_INT (1029)#define CFG_

23、AUTH (1035)#define UPDATE_PROP_ROT (1835)#define END_COMPARE_Y_X_TMP2 (1455)#define SKIP_X_GRT_Y_TMP (1359)#define SKIP_END_COMPARE (1435)#define FCFG_3 (1088)#define FCFG_2 (1066)#define FCFG_1 (1062)#define END_COMPARE_Y_X_TMP3 (1434)#define FCFG_7 (1073)#define FCFG_6 (1106)#define FLAT_STATE_END

24、 (1713)#define SWING_END_4 (1616)#define SWING_END_2 (1565)#define SWING_END_3 (1587)#define SWING_END_1 (1550)#define CFG_8 (2718)#define CFG_15 (2727)#define CFG_16 (2746)#define CFG_EXT_GYRO_BIAS (1189)#define END_COMPARE_Y_X_TMP (1407)#define DO_NOT_UPDATE_PROP_ROT (1839)#define CFG_7 (1205)#def

25、ine FLAT_STATE_END_TEMP (1683)#define END_COMPARE_Y_X (1484)#define SKIP_SWING_END_1 (1551)#define SKIP_SWING_END_3 (1588)#define SKIP_SWING_END_2 (1566)#define TILTG75_START (1672)#define CFG_6 (2753)#define TILTL75_END (1669)#define END_ORIENT (1884)#define CFG_FLICK_IN (2573)#define TILTL75_START

26、 (1643)#define CFG_MOTION_BIAS (1208)#define X_GRT_Y (1408)#define TEMPLABEL (2324)#define CFG_ANDROID_ORIENT_INT (1853)#define CFG_GYRO_RAW_DATA (2722)#define X_GRT_Y_TMP2 (1379)#define D_0_22 (22+512)#define D_0_24 (24+512)#define D_0_36 (36)#define D_0_52 (52)#define D_0_96 (96)#define D_0_104 (1

27、04)#define D_0_108 (108)#define D_0_163 (163)#define D_0_188 (188)#define D_0_192 (192)#define D_0_224 (224)#define D_0_228 (228)#define D_0_232 (232)#define D_0_236 (236)#define D_1_2 (256 + 2)#define D_1_4 (256 + 4)#define D_1_8 (256 + 8)#define D_1_10 (256 + 10)#define D_1_24 (256 + 24)#define D_

28、1_28 (256 + 28)#define D_1_36 (256 + 36)#define D_1_40 (256 + 40)#define D_1_44 (256 + 44)#define D_1_72 (256 + 72)#define D_1_74 (256 + 74)#define D_1_79 (256 + 79)#define D_1_88 (256 + 88)#define D_1_90 (256 + 90)#define D_1_92 (256 + 92)#define D_1_96 (256 + 96)#define D_1_98 (256 + 98)#define D_

29、1_106 (256 + 106)#define D_1_108 (256 + 108)#define D_1_112 (256 + 112)#define D_1_128 (256 + 144)#define D_1_152 (256 + 12)#define D_1_160 (256 + 160)#define D_1_176 (256 + 176)#define D_1_178 (256 + 178)#define D_1_218 (256 + 218)#define D_1_232 (256 + 232)#define D_1_236 (256 + 236)#define D_1_24

30、0 (256 + 240)#define D_1_244 (256 + 244)#define D_1_250 (256 + 250)#define D_1_252 (256 + 252)#define D_2_12 (512 + 12)#define D_2_96 (512 + 96)#define D_2_108 (512 + 108)#define D_2_208 (512 + 208)#define D_2_224 (512 + 224)#define D_2_236 (512 + 236)#define D_2_244 (512 + 244)#define D_2_248 (512

31、+ 248)#define D_2_252 (512 + 252)#define CPASS_BIAS_X (35 * 16 + 4)#define CPASS_BIAS_Y (35 * 16 + 8)#define CPASS_BIAS_Z (35 * 16 + 12)#define CPASS_MTX_00 (36 * 16)#define CPASS_MTX_01 (36 * 16 + 4)#define CPASS_MTX_02 (36 * 16 + 8)#define CPASS_MTX_10 (36 * 16 + 12)#define CPASS_MTX_11 (37 * 16)#

32、define CPASS_MTX_12 (37 * 16 + 4)#define CPASS_MTX_20 (37 * 16 + 8)#define CPASS_MTX_21 (37 * 16 + 12)#define CPASS_MTX_22 (43 * 16 + 12)#define D_EXT_GYRO_BIAS_X (61 * 16)#define D_EXT_GYRO_BIAS_Y (61 * 16) + 4#define D_EXT_GYRO_BIAS_Z (61 * 16) + 8#define D_ACT0 (40 * 16)#define D_ACSX (40 * 16 +

33、4)#define D_ACSY (40 * 16 + 8)#define D_ACSZ (40 * 16 + 12)#define FLICK_MSG (45 * 16 + 4)#define FLICK_COUNTER (45 * 16 + 8)#define FLICK_LOWER (45 * 16 + 12)#define FLICK_UPPER (46 * 16 + 12)#define D_AUTH_OUT (992)#define D_AUTH_IN (996)#define D_AUTH_A (1000)#define D_AUTH_B (1004)#define D_PEDS

34、TD_BP_B (768 + 0x1C)#define D_PEDSTD_HP_A (768 + 0x78)#define D_PEDSTD_HP_B (768 + 0x7C)#define D_PEDSTD_BP_A4 (768 + 0x40)#define D_PEDSTD_BP_A3 (768 + 0x44)#define D_PEDSTD_BP_A2 (768 + 0x48)#define D_PEDSTD_BP_A1 (768 + 0x4C)#define D_PEDSTD_INT_THRSH (768 + 0x68)#define D_PEDSTD_CLIP (768 + 0x6C

35、)#define D_PEDSTD_SB (768 + 0x28)#define D_PEDSTD_SB_TIME (768 + 0x2C)#define D_PEDSTD_PEAKTHRSH (768 + 0x98)#define D_PEDSTD_TIML (768 + 0x2A)#define D_PEDSTD_TIMH (768 + 0x2E)#define D_PEDSTD_PEAK (768 + 0X94)#define D_PEDSTD_STEPCTR (768 + 0x60)#define D_PEDSTD_TIMECTR (964)#define D_PEDSTD_DECI

36、(768 + 0xA0)#define D_HOST_NO_MOT (976)#define D_ACCEL_BIAS (660)#define D_ORIENT_GAP (76)#define D_TILT0_H (48)#define D_TILT0_L (50)#define D_TILT1_H (52)#define D_TILT1_L (54)#define D_TILT2_H (56)#define D_TILT2_L (58)#define D_TILT3_H (60)#define D_TILT3_L (62)#define DMP_CODE_SIZE (3062)static

37、 const unsigned char dmp_memoryDMP_CODE_SIZE = /* bank # 0 */ 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9

38、, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0xff, 0xff, 0xff, 0xff, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, 0x00, 0x00, 0x00, 0x3c, 0xff, 0xff, 0x00, 0x00, 0x00, 0x10

39、, 0x00, 0x00, 0x38, 0x83, 0x6f, 0xa2, 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92

40、, 0x0a, 0x16, 0xc0, 0xdf, 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00

41、, 0x00, 0x00, 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, 0xd9, 0x0e, 0x9f, 0xc9, 0x1d, 0xcf, 0x4c, 0x34, 0x30, 0x00, 0x00, 0x00, 0x50, 0x00, 0x00, 0x00

42、, 0x3b, 0xb6, 0x7a, 0xe8, 0x00, 0x64, 0x00, 0x00, 0x00, 0xc8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, /* bank # 1 */ 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0

43、x00, 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, 0x00, 0

44、x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, 0xff, 0x9b, 0xfc, 0

45、x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0

46、x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0

47、x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, /* bank # 2 */ 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00

48、, 0x05, 0x00, 0x05, 0xba, 0xc6, 0x00, 0x47, 0x78, 0xa2, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00

49、, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0