mpu6050 DMP庫的移植

https://www.amobbs.com/thread-5528472-1-1.html

官方的運動庫，必須通過這個才能啟用MPU6050的DMP引擎(數據手冊里完全不提這個東西，必須在官網注冊登錄后才有·····)
里面的代碼是基於msp430的，不過很容易就能移植到stm32上面

唯一需要的就是得提供i2c讀寫接口，然后在
#define i2c_write(a, b, c, d)   !I2C_BufferWrite(d, c, b, a)
這里替換

剩下的就是照着范例文件依次調用函數
主要是這幾個

1. mpu_init();
2. mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
3. mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);mpu_set_sample_rate(DEFAULT_MPU_HZ);
4. dmp_load_motion_driver_firmware();
5. dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
6. dmp_enable_feature(hal.dmp_features);
7. dmp_set_fifo_rate(DEFAULT_MPU_HZ);                        
   
8. run_self_test();
9. mpu_set_dmp_state(1);

復制代碼

其中dmp_load_motion_driver_firmware()這個很重要，失敗的話就沒法開啟DMP
run_self_test();是自檢，用來消除漂移的

初始化完成后在死循環里調用dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);來讀取FIFO
(sensors & INV_WXYZ_QUAT)為真就說明讀到四元數，默認是long，得轉換成float后除以 1073741824.0f，然后就能使用了，順序是wxyz