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没有错。

这里需要一个文件夹,官方给的,直接复制就好了,文件夹里有这些东西

MPU6050基本读写和移植DMP读出三轴_第1张图片

然后在工程中添加这个文件夹/路径/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,三轴在变化,说明移植成功。

结语

本人也是刚接触这个东西不久,从小白的角度分享一下我的移植过程,这个过程不复杂,能照着步骤来基本都能读出来,也是卡在这个模块的这个移植好久了,花了几天时间才移植出来,分享出来给大家节约时间。

你可能感兴趣的:(stm32,单片机)