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;
    else
        b = 7;		// error
    return b;
}
unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
{

  unsigned short scalar;

  /*
     XYZ  010_001_000 Identity Matrix
     XZY  001_010_000
     YXZ  010_000_001
     YZX  000_010_001
     ZXY  001_000_010
     ZYX  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.0f

uint8_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_rate
  mpu_get_sample_rate(&gyro_rate);
  // 获取量程，存到gyro_rate
  mpu_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);
  // 启动dmp
  mpu_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;                                    // 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; // yaw
        printf("pitch: %f, roll: %f, yaw: %f\n", roll, pitch, yaw);
      }
    }
  }