先决条件
- MPU6050移植完成,能够正确读出加速度和陀螺仪原始数据
DMP移植
官方包代码复制
- 解压demo,打开motion_driver_6.12\arm\STM32F4_MD6\Projects\eMD6\core\driver,复制路径下的eMPL到自己工程
![在这里插入图片描述](https://i-blog.csdnimg.cn/direct/9d7ee56c952f4ece8128c43f62e69564.png)
代码移植
- 需要修改的有三个文件,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/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();需要进行修改
__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;
return b;
}
unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
{
unsigned short scalar;
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_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);
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sample_rate(DEFAULT_MPU_HZ);
mpu_get_sample_rate(&gyro_rate);
mpu_get_gyro_fsr(&gyro_fsr);
mpu_get_accel_fsr(&accel_fsr);
dmp_load_motion_driver_firmware();
dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
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_set_fifo_rate(DEFAULT_MPU_HZ);
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;
q1 = quat[1] / q30;
q2 = quat[2] / q30;
q3 = quat[3] / q30;
pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;
roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3;
yaw = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3;
printf("pitch: %f, roll: %f, yaw: %f\n", roll, pitch, yaw);
}
}
}