mpu6050 四元数 欧拉角 程序

《mpu6050 四元数 欧拉角 程序》由会员上传分享，免费在线阅读，更多相关内容在学术论文-天天文库。

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"staticsignedchargyro_orientation[9]={-1,0,0,0,-1,0,0,0,1};#defineq301073741824.0ffloatq0=1.0f,q1=0.0f,q2=0.

2、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()

3、;//if(!result){//PrintChar("mpuinitializationcomplete......");//mpu_set_sensor//if(!mpu_set_sensors(INV_XYZ_GYRO

4、INV_XYZ_ACCEL))mpu_set_sensors(INV_XYZ_GYRO

5、INV_XYZ_ACCEL);//PrintChar("mpu_set_sensorcomplete......");//else//PrintChar("mpu_set_sensorcomeacrosserror......");if(!mpu_configur

6、e_fifo(INV_XYZ_GYRO

7、INV_XYZ_ACCEL))//mpu_configure_fifoPrintChar("mpu_configure_fifocomplete......");elsePrintChar("mpu_configure_fifocomeacrosserror......");if(!mpu_set_sample_rate(DEFAULT_MPU_HZ))//mpu_set_sample_ratePrintChar("mpu_set_sample_ratecomplete......");elsePrintChar("mpu_set_

8、sample_rateerror......");if(!dmp_load_motion_driver_firmware())//dmp_load_motion_driver_firmvarePrintChar("dmp_load_motion_driver_firmwarecomplete......");elsePrintChar("dmp_load_motion_driver_firmwarecomeacrosserror......");if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_or

9、ientation)))//dmp_set_orientationPrintChar("dmp_set_orientationcomplete......");elsePrintChar("dmp_set_orientationcomeacrosserror......");if(!dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT

10、DMP_FEATURE_TAP

11、DMP_FEATURE_ANDROID_ORIENT

12、DMP_FEATURE_SEND_RAW_ACCEL

13、DMP_FEATURE_SEND_CAL_GYRO

14、DMP_FEATURE

15、_GYRO_CAL))//dmp_enable_featurePrintChar("dmp_enable_featurecomplete......");elsePrintChar("dmp_enable_featurecomeacrosserror......");if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ))//dmp_set_fifo_ratePrintChar("dmp_set_fifo_ratecomplet