复制 motion_driver_6.12\arm\STM32F4_MD6\Projects\eMD6\core 下的所有文件到工程：
工程使用keil，在motion_driver_6.12\mpl libraries\arm\Keil等下找到libmpllib_keil_m4，替换mpl文件夹里面平台库文件libmpllib.a：
打开工程设置MPL_LOG_NDEBUG=1,EMPL,MPU6050,EMPL_TARGET_STM32F4，和添加头文件：
添加目录和添加C文件：
修改inv_mpu.c文件：
#if defined EMPL_TARGET_STM32F4
#include "i2c.h"
#include "main.h"
#include "log.h"
#include
#define i2c_write Sensors_I2C_WriteRegister
#define i2c_read Sensors_I2C_ReadRegister
#define delay_ms HAL_Delay //mdelay
#define get_ms get_tick_count
#define log_i printf //MPL_LOGI
#define log_e printf //MPL_LOGE
#define min(a,b) ((a 编写驱动接口代码和初始化代码：
#ifndef _MY_MPU6050_H_
#define _MY_MPU6050_H_
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx.h"
#include
#include
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* Exported macro ------------------------------------------------------------*/
/* Exported functions ------------------------------------------------------- */
void gyro_data_ready_cb(void);
int Sensors_I2C_WriteRegister(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data);
int Sensors_I2C_ReadRegister(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data);
int get_tick_count(unsigned long *count);
void mpu6050_init(void);
uint8_t mpu_dmp_init(void);
uint8_t mpu_dmp_get_data(float *pitch,float *roll,float *yaw);
int fputcc(int ch);
/******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/
#endif #include "usart.h"
#include "i2c.h"
#include "gpio.h"
#include "main.h"
#include "my_mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "invensense.h"
#include "invensense_adv.h"
#include "eMPL_outputs.h"
#include "mltypes.h"
#include "mpu.h"
#include "log.h"
#include "packet.h"
/* Private typedef -----------------------------------------------------------*/
/* Data read from MPL. */
#define PRINT_ACCEL (0x01)
#define PRINT_GYRO (0x02)
#define PRINT_QUAT (0x04)
#define PRINT_COMPASS (0x08)
#define PRINT_EULER (0x10)
#define PRINT_ROT_MAT (0x20)
#define PRINT_HEADING (0x40)
#define PRINT_PEDO (0x80)
#define PRINT_LINEAR_ACCEL (0x100)
#define PRINT_GRAVITY_VECTOR (0x200)
volatile uint32_t hal_timestamp = 0;
#define ACCEL_ON (0x01)
#define GYRO_ON (0x02)
#define COMPASS_ON (0x04)
#define MOTION (0)
#define NO_MOTION (1)
/* Starting sampling rate. */
#define DEFAULT_MPU_HZ (20)
#define FLASH_SIZE (512)
#define FLASH_MEM_START ((void*)0x1800)
#define PEDO_READ_MS (1000)
#define TEMP_READ_MS (500)
#define COMPASS_READ_MS (100)
#define q30 1073741824.0f
static signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1};
int Sensors_I2C_WriteRegister(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data)
{
HAL_StatusTypeDef status = HAL_OK;
status = HAL_I2C_Mem_Write(&hi2c1, slave_addr, reg_addr, I2C_MEMADD_SIZE_8BIT, data, length, 0xFF);
if (status != HAL_OK)
{
printf("I2C write wrong!\n");
return HAL_ERROR;
}
while (HAL_I2C_GetState(&hi2c1) != HAL_I2C_STATE_READY);
while (HAL_I2C_IsDeviceReady(&hi2c1, slave_addr, 50, 0xFF) == HAL_TIMEOUT);
while (HAL_I2C_GetState(&hi2c1) != HAL_I2C_STATE_READY);
return status;
}
int Sensors_I2C_ReadRegister(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char *data)
{
HAL_StatusTypeDef status = HAL_OK;
status = HAL_I2C_Mem_Read(&hi2c1,slave_addr, reg_addr, I2C_MEMADD_SIZE_8BIT, data, length, 0xFF);
while (HAL_I2C_GetState(&hi2c1) != HAL_I2C_STATE_READY);
while (HAL_I2C_IsDeviceReady(&hi2c1, slave_addr, 50, 0xFF) != HAL_TIMEOUT);
while (HAL_I2C_GetState(&hi2c1) != HAL_I2C_STATE_READY);
return status;
}
int get_tick_count(unsigned long *count)
{
*count = HAL_GetTick();
return 0;
}
int fputcc(int ch)
{
HAL_UART_Transmit(&huart1, (uint8_t*)&ch, 1, 0xFF);
return 0;
}
uint8_t run_self_test(void)
{
int result;
long gyro[3], accel[3];
result = mpu_run_self_test(gyro, accel);
if (result == 0x07) { //返回0x03为MPU6050六轴，只要通过该if语句，就可以实现零偏自动校准
/* Test passed. We can trust the gyro data here, so let's push it down
* to the DMP.
*/
float sens;
unsigned short accel_sens;
mpu_get_gyro_sens(&sens); //读取当前陀螺仪的状态
gyro[0] = (long)(gyro[0] * sens);
gyro[1] = (long)(gyro[1] * sens);
gyro[2] = (long)(gyro[2] * sens);
dmp_set_gyro_bias(gyro); //根据读取的状态进行校准
mpu_get_accel_sens(&accel_sens); //读取当前加速度计的状态
accel[0] *= accel_sens;
accel[1] *= accel_sens;
accel[2] *= accel_sens;
dmp_set_accel_bias(accel); //根据读取的状态进行校准
printf("setting bias succesfully ......\r\n");
return 0;
}
else
return 1;
}
//mpu6050,dmp初始化
//返回值:0,正常
// 其他,失败
uint8_t mpu_dmp_init(void)
{
uint8_t res=0;
struct int_param_s int_param;//这个没什么用，就是为了能给他实参调用起来
if(mpu_init(&int_param)==0) //初始化MPU6050
{
res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
if(res)return 1;
res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO
if(res)return 2;
res=mpu_set_sample_rate(100); //设置采样率
if(res)return 3;
res=dmp_load_motion_driver_firmware(); //加载dmp固件
if(res)return 4;
res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
if(res)return 5;
res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能
DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
DMP_FEATURE_GYRO_CAL);
if(res)return 6;
res=dmp_set_fifo_rate(100); //设置DMP输出速率(最大不超过200Hz)
if(res)return 7;
res = run_self_test(); //自检
if(res)return 8;
res=mpu_set_dmp_state(1); //使能DMP
if(res)return 9;
}
return 0;
}
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
//返回值:0,正常
// 其他,失败
uint8_t mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
unsigned long sensor_timestamp;
short gyro[3], accel[3], sensors;
unsigned char more;
long quat[4];
if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;
/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
* This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
**/
/*if (sensors & INV_XYZ_GYRO )
send_packet(PACKET_TYPE_GYRO, gyro);
if (sensors & INV_XYZ_ACCEL)
send_packet(PACKET_TYPE_ACCEL, accel); */
/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
* The orientation is set by the scalar passed to dmp_set_orientation during initialization.
**/
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
}else return 2;
return 0;
}
修改通讯地址：
完成后编译未报错，在main函数调用mpu_dmp_get_data(&pitch, &roll, &yaw)，就可以读取欧拉角。
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）