MPU6050基本读写和移植DMP读出三轴

本文主要是介绍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();	//等待ACKI2C_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<b)?a:b)
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);//设置FIFOif(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);	//使能DMPif(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;
}