MPU6050基本读写和移植DMP读出三轴
文章目录
-
- 引言
- 简述一下mpu6050和stm32的一些坑吧
- MPU6050 I2C读写
- MPU6050 初始化
- 读取内部温度传感器
- 读取陀螺仪
- DMP的移植读取三轴角度
- 结语
引言
最近玩了IMU模块，看了很多的博客，从无到有。包含了一些我调试代码的过程，分享一些坑吧，
很多都是读不出来啥的，其实模块一般没啥问题，我会一步一步地教大家怎么排除常见问题。
我是参考正点原子的代码，因为他们没教怎么移植DMP
如果只需要移植DMP的，参考后面的移植过程就行了,前提是能读出原始数据
简述一下mpu6050和stm32的一些坑吧
TB上mpu6050也不贵，就那么几块钱，模块几乎都一模一样，贵的我不懂。模块内置2.2K上拉，最好别再外接上拉.
我用的是F407，我的是软件I2C驱动的，随便接两个纯净的IO就行了，IO口需要设置为开漏，NOPULL。.（设置推挽我的不行用）
正确读出来的地址是0X68，不是0XD0，虽然是左移一位，但是是错的，你读出这个，后面没法读出原始数据
F4的stm32开硬件浮点单元，在MDK设置就可以了
MPU6050 I2C读写
这个是按正点原子来的，正常的I2C读写流程。
发送设备地址，寄存器地址，要写入的数据，释放总线。
//MPU6050写寄存器 0正常 1异常
u8 MPU_Write_Byte(u8 reg,u8 data)
{
I2C_Start();
I2C_Send_Byte((MPU6050_ADDRESS<<1)|0);
if(I2C_Wait_Ack()) //等待应答
{
I2C_Stop();
return 1;
}
I2C_Send_Byte(reg);
I2C_Wait_Ack(); //等待ACK
I2C_Send_Byte(data);
if(I2C_Wait_Ack()) //等待ACK
{
I2C_Stop();
return 1;
}
I2C_Stop();
return 0;
}
虽然是一样的流程，但是有的出错了，赶紧回去检查一下I2C程序是否有问题，
一般原因就是在等待响应那里出问题，因为你发了I2C时序错了以后，
MPU识别不出你的数据，自然不会响应，没法拉低SDA线，程序就从if语句跳了出去
我检查是否失败的方法就是在if语句中加入prinrf,打印出来错误,x代表错误编号
printf("\r\nERRORx");
MPU写寄存器
//MPU6050读取寄存器
u8 MPU_Read_Byte(u8 reg)
{
u8 res;
I2C_Start();
I2C_Send_Byte((MPU6050_ADDRESS<<1)|0);//发送器件地址+写命令
I2C_Wait_Ack(); //等待应答
I2C_Send_Byte(reg); //写寄存器地址
I2C_Wait_Ack(); //等待应答
I2C_Start();
I2C_Send_Byte((MPU6050_ADDRESS<<1)|1);//发送器件地址+读命令
I2C_Wait_Ack(); //等待应答
res=I2C_Read_Byte(0);//读取数据,发送nACK
I2C_Stop(); //产生一个停止条件
return res;
}
还有两个多次读写函数，这里就不列出来了，但是形参一定要按DMP的格式来，不然后面没法移植，具体格式看正点原子的代码，这部分他们都有说的。
然后一步一步测试程序是否正确，这里直接读取ID来验证是否正确，因为读取ID不需要初始化MPU的内部寄存器，不用关心写时序是否出问题，是个验证你的I2C读写正确的很好手段
int main()
{
//前面就是串口，延时函数的初始化，自己匹配自己的程序
printf(“\r\n MPU ID:0x%X”,MPU_Read_Byte(MPU6050_WHO_AM_I));
//读内部ID寄存器，0X68正确，0XD0/0XD1就是有问题了
return 0;
}
前面写时序出问题，所以一直初始化不了内部寄存器，读出来的数据都是不变的000，后来发现是传感器内部有上拉，外部不要加上拉，还有stm32内部也不要上拉，输出为开漏。这保证了硬件部分至少是没问题的。
时序出问题就回环测试去，在函数内加printf，看程序断在哪里，读ID是最基本的，读不出来就是时序问题了。
MPU6050 初始化
要读原始数据的话，初始化代码很简单，不过要进行初始化才能读取下面三个传感器的数据。切记!
void MPU6050_Init(void)
{
MY_I2C_GPIO_Config(); //软件I2C IO口
delay_ms(100); //需要进行必要的延时
MPU_Write_Byte(MPU6050_RA_PWR_MGMT_1,0X00); //电源
MPU_Write_Byte(MPU6050_RA_SMPLRT_DIV,0x07); //采样率
MPU_Write_Byte(MPU6050_RA_CONFIG,0x06); //初始化
MPU_Write_Byte(MPU6050_RA_ACCEL_CONFIG,0x01); //传感器的量程
MPU_Write_Byte(MPU6050_RA_GYRO_CONFIG,0x18);
}
读取内部温度传感器
这个传感器据说是为了补偿两个主要传感器的温漂的
/*************************************************************************
* Function Name : MPU_ReadTemperature
* Description : 读取MPU内部温度传感器,并转化为摄氏度
* Input : none
* Output : temperature
* Return : none
**************************************************************************/
void MPU_ReadTemperature(float *temperature)
{
u8 buf[2];
short raw;
MPU_Read_Buf(MPU6050_ADDRESS,MPU6050_RA_TEMP_OUT_H,2,buf);
raw = ((u16)buf[0]<<8)|buf[1];
*temperature = 36.53+((double)raw)/340;
}
只需要传入高字节地址，内部地址指针自动偏移，读出来的数据就是高位和低位了，然后根据公式转化出来，测试方法就是用printf打印出来，然后用手指靠近，明显地温度会上升几度。
读取陀螺仪
陀螺仪和加速度计才是这模块主要的东西，读取这个东西就是用8位数组存数据，然后用16位的有符号整型读出来，但是这个是原始数据，没啥用，所以我说DMP程序移植才是最重要的，但是得保证这个能用。
void MPU6050_ReadGyroscope(short *gx,short *gy,short *gz)
{
u8 buf[6];
MPU_Read_Buf(MPU6050_ADDRESS,MPU6050_GYRO_OUT,6,buf);
*gx = ((u16)buf[0] << 8) | buf[1];
*gy = ((u16)buf[2] << 8) | buf[3];
*gz = ((u16)buf[4] << 8) | buf[5];
}
DMP的移植读取三轴角度
这是MPU6050最主要的东西了，因为原始数据只能拿来证明你的I2C没有错。
这里需要一个文件夹，官方给的，直接复制就好了，文件夹里有这些东西
然后在工程中添加这个文件夹/路径/C文件，编译一下，然后打开inv_mpu.c这个文件
修改成以下内容
#include "mpu6050.h"
#include "delay.h" //你的延时函数，最好用滴答定时器
#define MPU6050 //定义使用的IMU
#define MOTION_DRIVER_TARGET_MSP430 //虽然是MSP430的，但是没啥影响
//把下面几个注释掉
//#include "msp430.h"
//#include "msp430_i2c.h"
//#include "msp430_clock.h"
//#include "msp430_interrupt.h"
#define i2c_write MPU_Write_Buf //改成我们的I2C读写接口
#define i2c_read MPU_Read_Buf//注意，形参格式一定要一样
#define delay_ms delay_ms
#define get_ms mget_ms //没啥用，写个空函数给他避免报错
//static inline int reg_int_cb(struct int_param_s *int_param)
//{
// return msp430_reg_int_cb(int_param->cb, int_param->pin, int_param->lp_exit,
// int_param->active_low);
//} //注释掉
#define log_i(...) do {} while (0)
#define log_e(...) do {} while (0)
/* labs is already defined by TI's toolchain. */
/* fabs is for doubles. fabsf is for floats. */
#define fabs fabsf
#define min(a,b) ((a
void mget_ms(unsigned long *time){} //这个给个空函数就行
然后转到inv_mpu_dmp_motion_driver.c这个文件,做如下修改
#define MOTION_DRIVER_TARGET_MSP430
#if defined MOTION_DRIVER_TARGET_MSP430
//#include "msp430.h"
//#include "msp430_clock.h"
#define delay_ms delay_ms
#define get_ms mget_ms
#define log_i(...) do {} while (0)
#define log_e(...) do {} while (0)
此时再编译会报错reg_int_cb这个函数没定义，这个函数再inv_mpu.c这个文件里用到一次，我的在800行，直接注释就行了，大概是这个东西
// if (int_param)
// reg_int_cb(int_param);
最后还有个警告，setup_compass没用到，别去管他，这是磁场计用的，也就是mpu9250
到此，文件已经基本移植好了，接下来写初始化和调用就行了（参考正点原子）
//mpu6050,dmp初始化
//返回值:0,正常
// 其他,失败
u8 mpu_dmp_init(void)
{
u8 res=0;
struct int_param_s int_param;//这个没什么用，就是为了能给他实参调用起来
MY_I2C_GPIO_Config(); //初始化IIC总线
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;
}
编译首先是gyro_orientation这个是个数组没定义。
static signed char gyro_orientation[9] = { 1, 0, 0,
0, 1, 0,
0, 0, 1};
自检矫正代码，一般出在这里，先确保最先前说的，你的I2C移植没有问题，
可以读出ID和原始数据，如果没问题，就是前面移植DMP程序的时候漏了哪一步
为了使工程简洁，自己建了个驱动,包含前面两个和你的I2C就够了
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include "myiic.h" //I2C驱动，具体看你自己的
//MPU6050自测试
//返回值:0,正常
// 其他,失败
u8 run_self_test(void)
{
int result;
//char test_packet[4] = {0};
long gyro[3], accel[3];
result = mpu_run_self_test(gyro, accel);
if (result == 0x3)
{
/* 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);
return 0;
}else return 1;
}
先别急写别的代码，验证以下能否初始化，还是printf大法
uint8_t res;
res = mpu_dmp_init();
printf("\r\n%d",res);
正确返回值0，如果是0基本就大功告成
接下来把DMP结算后的数据用他给的驱动读出来即可。
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1° 范围:-90.0° <---> +90.0°
//roll:横滚角 精度:0.1° 范围:-180.0°<---> +180.0°
//yaw:航向角 精度:0.1° 范围:-180.0°<---> +180.0°
//返回值:0,正常
// 其他,失败
u8 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;
}
还是移植正点原子(正点原子的代码很严谨)
这里说q30没定义，在头文件加就好了，我也没了解过，但是我只是拿来应用
#define q30 1073741824.0f
接下来用prnintf读出数据就行了,移动MPU6050，三轴在变化，说明移植成功。
结语
本人也是刚接触这个东西不久，从小白的角度分享一下我的移植过程，这个过程不复杂，能照着步骤来基本都能读出来,也是卡在这个模块的这个移植好久了，花了几天时间才移植出来,分享出来给大家节约时间。