#include \
#include \ #include \ #include \#include \#include \
#include \
#include \#include\#include\
static signed char gyro_orientation[9] = {-1, 0, 0,
0,-1, 0, 0, 0, 1}; #define q30 1073741824.0f
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f; char num[50];
float Pitch,Roll,Yaw;
unsigned long sensor_timestamp; short gyro[3], accel[3], sensors; unsigned char more; long quat[4];
int main(void) {
// u16 i;
int result; Stm32_Clock_Init(9);//系统时钟设置 delay_init(72); //延时初始化 uart_init(72,9600); //串口1初始化 LCD_Init(); POINT_COLOR=RED; i2cInit();
// result = mpu_init();
// if(!result) { // PrintChar(\ //mpu_set_sensor // if(!mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL)) mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL); // PrintChar(\
// else // PrintChar(\ if(!mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL)) //mpu_configure_fifo PrintChar(\ else PrintChar(\ if(!mpu_set_sample_rate(DEFAULT_MPU_HZ)) //mpu_set_sample_rate PrintChar(\ else PrintChar(\ if(!dmp_load_motion_driver_firmware()) //dmp_load_motion_driver_firmvare PrintChar(\ else PrintChar(\ if(!dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation))) //dmp_set_orientation PrintChar(\ else PrintChar(\ 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(\ else PrintChar(\ if(!dmp_set_fifo_rate(DEFAULT_MPU_HZ)) //dmp_set_fifo_rate PrintChar(\ else PrintChar(\ run_self_test(); if(!mpu_set_dmp_state(1)) PrintChar(\ else PrintChar(\ } 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 is 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 written 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=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,\in Calculating quaternion steps..... // sprintf(num,\ sprintf(num,\ PrintChar(num); PrintChar(\ // 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,\// 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 drivers 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 \#include
#include \#include \#include \#include \#include \
#define MOTION_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 *data) * delay_ms(unsigned long num_ms) * get_ms(unsigned long *count) */
#if defined MOTION_DRIVER_TARGET_MSP430 //#include \
//#include \#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_TARGET_MSP430 #include \
#include \#include \
#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 library, we're using * a TWI driver that follows the slave address + register address convention. */
#include \#include \#include \
#include \
/* delay_ms is a function already defined in ASF. */ #define get_ms uc3l0_get_clock_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 switching 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)