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);
}
}
}
MPL移植
先决条件
- dmp移植完成，可以正常输出欧拉角
- 测试只移植mpl，将IIC读出的数据直接传入应该不行，读不出角度值
MPL移植
- 在移植完dmp后，MPL移植简单，就是单纯文件多
官方包代码复制
- 解压demo，打开motion_driver_6.12\arm\STM32F4_MD6\Projects\eMD6\core，复制路径下的eMPL-hal、mllite、mpl，以及driver下的include、stm32L，一共这五个文件夹
代码移植
- 只需要修改stm32L下的log_stm32.c，其中fputc和头文件调用会报错，重命名一个函数fputcc，再将当前文件下的fputc替换为fputcc即可
// 重命名一个函数
int fputcc(int ch)
{
USART_SendData(USART1, (uint8_t) ch);
while (USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET);
return (ch);
}
-
storage_manager.h中，可能提示size_t没有定义，定义typedef unsigned int size_t;
-
进行编译，应该只有各种警告，MPL移植完成
初始化
- 初始化部分
result = mpu_init(&int_param);
if (result)
{
printf("one failed\n");
}
/*********MPL 配置参数************* */
// 只使用DMP可注释，初始化mpl
result = inv_init_mpl();
if (result)
{
printf("two failed\n");
}
// 只使用DMP可注释，启用四元数计算
inv_enable_quaternion();
// 只使用DMP可注释，静止校准偏差
inv_enable_fast_nomot();
// 只使用DMP可注释，温度改变校准偏差
inv_enable_gyro_tc();
// 只使用DMP可注释，启用 eMPL 的输出功能
inv_enable_eMPL_outputs();
// 只使用DMP可注释， MPL启动并准备处理传感器数据
result = inv_start_mpl();
if (result == INV_ERROR_NOT_AUTHORIZED)
{
printf("three failed\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);
/*********** MPL 配置参数 *********** */
// 只使用DMP可注释， 设置陀螺仪和加速度计的采样率。
inv_set_gyro_sample_rate(1000000L / gyro_rate);
inv_set_accel_sample_rate(1000000L / gyro_rate);
// 只使用DMP可注释， 用于设置陀螺仪的方向矩阵和比例因子（缩放）
inv_set_gyro_orientation_and_scale(
inv_orientation_matrix_to_scalar(gyro_orientation),
(long)gyro_fsr << 15);
inv_set_accel_orientation_and_scale(
inv_orientation_matrix_to_scalar(gyro_orientation),
(long)accel_fsr << 15);
/********************************* */
// 加载dmp固件
dmp_load_motion_driver_firmware();
调用
if (bsp_CheckTimer(KEY_TIMER))
{
dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more);
if (sensors & INV_WXYZ_QUAT) // DMP
{
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", pitch, roll, yaw);
}
if (sensors & INV_XYZ_GYRO)
{
Gyro[0] = gyro[0];
Gyro[1] = gyro[1];
Gyro[2] = gyro[2];
mpu_get_temperature(&temperature, &sensor_timestamp);
Temp = (float)temperature / (1 << 16);
/* Push the new data to the MPL. */
inv_build_gyro(gyro, sensor_timestamp);
}
if (sensors & INV_XYZ_ACCEL)
{
accel[0] = (long)accel_short[0];
accel[1] = (long)accel_short[1];
accel[2] = (long)accel_short[2];
Accel[0] = (long)accel_short[0];
Accel[1] = (long)accel_short[1];
Accel[2] = (long)accel_short[2];
/* Push the new data to the MPL. */
inv_build_accel(accel, 0, sensor_timestamp);
}
long data[9];
int8_t accuracy;
inv_execute_on_data();
if (inv_get_sensor_type_euler(data, &accuracy, (inv_time_t *)&sensor_timestamp))
{
printf("Pitch : %.4f ", data[0] * 1.0 / (1 << 16)); // inv_get_sensor_type_euler读出的数据是Q16格式，所以左移16位.
printf("Roll : %.4f ", data[1] * 1.0 / (1 << 16)); // inv_get_sensor_type_euler读出的数据是Q16格式，所以左移16位.
printf("Yaw : %.4f \n", data[2] * 1.0 / (1 << 16)); // inv_get_sensor_type_euler读出的数据是Q16格式，所以左移16位.
}
}
其他
-
mpl_key未定义，unsigned char *mpl_key = (unsigned char *)“eMPL 5.1”;
-
之前定义的inv_orientation_matrix_to_scalar可能会显示重复定义，删除定义部分即可
扫一扫
7090
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
