MPU6050移植DMP读出三轴角度

先决条件

• MPU6050移植完成，能够正确读出加速度和陀螺仪原始数据

DMP移植

官方包代码复制

• 解压demo，打开motion_driver_6.12\arm\STM32F4_MD6\Projects\eMD6\core\driver，复制路径下的eMPL到自己工程
  

代码移植

• 需要修改的有三个文件，inv_mpu.h inv_mpu.c inv_mpu_dmp_motion_driver.c

首先是inv_mpu.h，在#define _INV_MPU_H_下添加define

#define _INV_MPU_H_#define EMPL_TARGET_STM32F4
#define MPU6050

inv_mpu.c需要实现#if defined EMPL_TARGET_STM32F4下的宏定义，在上方注释有每个函数的接口定义

#if defined EMPL_TARGET_STM32F4
// #include "i2c.h"   
// #include "main.h"
// #include "log.h"
// #include "board-st_discovery.h"
#include "./i2c/bsp_i2c.h"  
#include "./systick/bsp_SysTick.h"#define i2c_write   Soft_DMP_I2C_Write
#define i2c_read    Soft_DMP_I2C_Read 
#define delay_ms    mdelay
#define get_ms      get_tick_count
#define log_i       printf    
#define log_e       printf    
#define min(a,b) ((a<b)?a:b)

• Soft_DMP_I2C_Write和Soft_DMP_I2C_Read可以参照如下代码修改

#define I2C_Direction_Transmitter ((uint8_t)0x00)
#define I2C_Direction_Receiver ((uint8_t)0x01)uint8_t Soft_DMP_I2C_Write(uint8_t soft_dev_addr, uint8_t soft_reg_addr, uint8_t soft_i2c_len, unsigned char *soft_i2c_data_buf)
{uint8_t i, result = 0;i2c_Start();i2c_SendByte(soft_dev_addr << 1 | I2C_Direction_Transmitter);result = i2c_WaitAck();if (result != 0)return result;i2c_SendByte(soft_reg_addr);result = i2c_WaitAck();if (result != 0)return result;for (i = 0; i < soft_i2c_len; i++){i2c_SendByte(soft_i2c_data_buf[i]);result = i2c_WaitAck();if (result != 0)return result;}i2c_Stop();return 0x00;
}uint8_t Soft_DMP_I2C_Read(uint8_t soft_dev_addr, uint8_t soft_reg_addr, uint8_t soft_i2c_len, unsigned char *soft_i2c_data_buf)
{uint8_t result;i2c_Start();i2c_SendByte(soft_dev_addr << 1 | I2C_Direction_Transmitter);result = i2c_WaitAck();if (result != 0)return result;i2c_SendByte(soft_reg_addr);result = i2c_WaitAck();if (result != 0)return result;i2c_Start();i2c_SendByte(soft_dev_addr << 1 | I2C_Direction_Receiver);result = i2c_WaitAck();if (result != 0)return result;//while (soft_i2c_len){if (soft_i2c_len == 1)*soft_i2c_data_buf = i2c_ReadByte(0);else*soft_i2c_data_buf = i2c_ReadByte(1);soft_i2c_data_buf++;soft_i2c_len--;}i2c_Stop();return 0x00;
}

• mdelay和get_tick_count比较简单，通过滴答定时器累减和累加即可

void mdelay(unsigned long nTime)
{TimingDelay = nTime;while (TimingDelay != 0);
}int get_tick_count(unsigned long *count)
{count[0] = g_ul_ms_ticks;return 0;
}
// 放入定时器中断
void TimingDelay_Decrement(void)
{if (TimingDelay != 0x00)TimingDelay--;
}void TimeStamp_Increment(void)
{g_ul_ms_ticks++;
}

inv_mpu_dmp_motion_driver.c需要实现#if defined EMPL_TARGET_STM32F4下的宏定义

• 还需要注意在该文件的630多行有个__no_operation();需要进行修改

// __no_operation();__ASM("nop");

相关变量与函数

• 变量和函数有的是初始化使用，有的是主循环使用，自己根据情况选择放置位置

static unsigned short inv_row_2_scale(const signed char *row)
{unsigned short b;if (row[0] > 0)b = 0;else if (row[0] < 0)b = 4;else if (row[1] > 0)b = 1;else if (row[1] < 0)b = 5;else if (row[2] > 0)b = 2;else if (row[2] < 0)b = 6;elseb = 7;		// errorreturn b;
}
unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
{unsigned short scalar;/*XYZ  010_001_000 Identity MatrixXZY  001_010_000YXZ  010_000_001YZX  000_010_001ZXY  001_000_010ZYX  000_001_010*/scalar = inv_row_2_scale(mtx);scalar |= inv_row_2_scale(mtx + 3) << 3;scalar |= inv_row_2_scale(mtx + 6) << 6;return scalar;
}static signed char gyro_orientation[9] = {1, 0, 0,0, 1, 0,0, 0, 1};#define DEFAULT_MPU_HZ (20)
#define q30 1073741824.0fuint8_t result;
unsigned char accel_fsr = 0;
unsigned short gyro_rate, gyro_fsr;
struct int_param_s int_param;short gyro[3], accel_short[3], sensors;
unsigned char more;
long accel[3], quat[4], temperature;
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
float pitch = 0.0f, roll = 0.0f, yaw = 0.0f;
unsigned long sensor_timestamp;

主函数初始化

// I2C初始化i2c_GPIO_Config();printf("mpu 6050 test start\n");result = mpu_init(&int_param);if (result){printf("one failed\n");}else{printf("one success\n");}// 使能陀螺仪和加速度mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);// 使能FIFO，并设置采样率mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);mpu_set_sample_rate(DEFAULT_MPU_HZ);// 获取采样率，存到gyro_ratempu_get_sample_rate(&gyro_rate);// 获取量程，存到gyro_ratempu_get_gyro_fsr(&gyro_fsr);mpu_get_accel_fsr(&accel_fsr);// 加载dmp固件dmp_load_motion_driver_firmware();// 设置方向矩阵dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));// 启用配置,六轴四元数计算，敲击检测， 屏幕旋转检测，原始加速度发送到FIFO，校准陀螺仪发送到FIFO，陀螺仪校准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);// 设置向FIFO中输出数据的频率dmp_set_fifo_rate(DEFAULT_MPU_HZ);// 启动dmpmpu_set_dmp_state(1);

主函数循环

  while (1){if (bsp_CheckTimer(KEY_TIMER)){dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more);if (sensors & INV_WXYZ_QUAT){q0 = quat[0] / q30; // q30格式转换为浮点数q1 = quat[1] / q30;q2 = quat[2] / q30;q3 = quat[3] / q30;// 计算得到俯仰角/横滚角/航向角pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;                                    // pitchroll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3;     // rollyaw = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3; // yawprintf("pitch: %f, roll: %f, yaw: %f\n", roll, pitch, yaw);}}}