树莓派使用C语言控制MPU6050

mpu6050是一个经济实用的imu传感器。本文介绍如何在树莓派中使用C语言控制MPU6050。为了减少代码移植难度,使用的函数均为Linux函数(read,write,ioctl等)而没有使用树莓派io库,所以其他已经安装好i2c驱动的Linux设备同样适用。

MPU6050接线

MPU6050的Data接在GPIO2,Clock 接在GPIO3。

树莓派使用C语言控制MPU6050_引脚图

linux调试工具

在编写程序控制前先使用Linux工具进行调试,测试传感器连接等是否正确。下面这些软件树莓派已经预装,没有安装的系统使用apt-get或pacman等软件管理器安装即可。

首先使用检验主机i2c驱动是否工作正常

i2cdetect -l

得到的结果为

i2c-1   i2c   bcm2835 I2C adapter    I2C adapter

因为树莓派默认状态只有一个i2c设备,结果中包含了设备号 i2c-1,设备类型 i2c,设备型号等。如果下面查看连接在i2c-1上有几个设备。

i2cdetect -y 1

其中的1为设备号,如果前面得到的设备号为i2c-2,那么则用-y 2。运行结果为

     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f
00:          -- -- -- -- -- -- -- -- -- -- -- -- -- 
10: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
20: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
30: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
40: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
50: -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- -- 
60: -- -- -- -- -- -- -- -- 68 -- -- -- -- -- -- -- 
70: -- -- -- -- -- -- -- --  

这里看到可以看到i2c-1总线上有一个设备,地址为0x68。下面使用i2cdump扫描设备的所有寄存器。

i2cdump -y 1 0x68

得到的结果为

No size specified (using byte-data access)
     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f    0123456789abcdef
00: fd 79 81 5c c1 5e f4 ea f6 0f 03 34 28 6a 4d ac    ?y?\?^?????4(jM?
10: 4a 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00    J...............
20: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00    ................
30: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00    ................
40: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00    ................
50: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00    ................
60: 00 00 00 00 00 00 00 00 00 00 00 40 00 00 00 00    ...........@....
70: 00 00 00 00 00 68 00 00 00 00 00 00 00 00 00 00    .....h..........
80: fd 79 81 5c c1 5e f4 ea f6 0f 03 34 28 6a 4d ac    ?y?\?^?????4(jM?
90: 4a 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00    J...............
a0: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00    ................
b0: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00    ................
c0: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00    ................
d0: 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00    ................
e0: 00 00 00 00 00 00 00 00 00 00 00 40 00 00 00 00    ...........@....
f0: 00 00 00 00 00 68 00 00 00 00 00 00 00 00 00 00    .....h..........

此外还可以使用i2cget和i2cset对单个寄存器进行读写。下面使用i2cget读取地址为0x75的寄存器

i2cget -y 1 0x68 0x75

得到0x68,说明0x75寄存器内存放的是0x68,这与前面寄存器扫描得到的结果一致。

使用C语言控制MPU6050

首先编写i2c的适配程序。除了初始化外,MPU6050读写寄存器都是先发送地址再读取或者发送寄存器数据的。因为读写寄存器使用很频繁,所以将其封装起来

#include "i2c.h"

//******************************************
//Name:		i2c_init
//Parameter:	addr	uint8_t	i2c slave address
//Return:	fd	int	i2c handle
//Description:	i2c initization
//******************************************
int i2c_init(uint8_t addr)
{
	int fd;
	fd = open(I2C_PATH, O_RDWR);
	return fd; 
}

//******************************************
//Name:		i2c_readreg
//Parameter:	fd	int	i2c handle
//		addr	uint8_t	register address
//Return:	buff	uint8_t	received data
//Description:	read register
//******************************************
uint8_t i2c_readreg(int fd,uint8_t addr)
{
	uint8_t buff;

	write(fd,&addr,1);
	read(fd,&buff,1);

	return buff;
}

//******************************************
//Name:		i2c_writereg
//Parameter:	fd	int	i2c handle
//		addr	uint8_t	register address
//		data	uint8_t	send data
//Return:	void
//Description:	write register
//******************************************
void i2c_writereg(int fd,uint8_t addr,uint8_t data)
{
	uint8_t buff[2];

	buff[0]=addr;
	buff[1]=data;

	write(fd,buff,2);
}

MPU6050上层包含了三个函数:mpu6050_init内包含了传感器的一些寄存器配置。mpu6050_get为读取寄存器数据,因为mpu6050中很多寄存器都是使用两个寄存器储存一个数据的,所以这里单独封装了一个函数。对于单独寄存器中存储的就是一个数据时,用i2c_readreg函数就可以。mpu6050_rd是读取传感器数据,并进行了单位换算和方向矫正。其中使用的宏定义在mpu6050.h中定义。

#include "mpu6050.h"

//******************************************
//Name:		mpu6050_init
//Parameter:	void	
//Return:	fd	int	mpu6050 handle
//Description:	mpu6050 initization
//******************************************
int mpu6050_init(void)
{
	int fd;
	
	fd=i2c_init(0x68);

	i2c_writereg(fd,ADDR_PWR_MGMT_1,0x00);
	i2c_writereg(fd,ADDR_SMPLRT_DIV,0x07);
	i2c_writereg(fd,ADDR_CONFIG,0x06);
	i2c_writereg(fd,ADDR_GYRO_CONFIG,0x00);
	i2c_writereg(fd,ADDR_ACCEL_CONFIG,0x00);

	return fd;
}
//******************************************
//Name:		mpu6050_get
//Parameter:	fd	int	i2c handle
//		addr	uint8_t	register address
//Return:	data	uint16_treceived data
//Description:	read mpu6050 register
//******************************************
int16_t mpu6050_get(int fd,uint8_t addr)
{
	uint8_t data1,data2;
	uint16_t data;

	data1=i2c_readreg(fd,addr);
	data2=i2c_readreg(fd,addr+1);

	data=data1;
	data=(data<<8)+data2;

	return data;
}

//******************************************
//Name:		mpu6050_rd
//Parameter:	fd	int	i2c handle
//		data	double	imu data
//Return:	void
//Description:	read mpu6050 imu data
//******************************************
void mpu6050_rd(int fd,double data[3])
{
	double accx,accz;

	//unit convert
	data[1]=mpu6050_get(fd,ADDR_GYRO_YOUT)*250.0/0x7fff*PI/180;
	accx=mpu6050_get(fd,ADDR_ACCEL_XOUT)*2.0/0x7fff;
	accz=mpu6050_get(fd,ADDR_ACCEL_ZOUT)*2.0/0x7fff;

	//peak value
	data[2]=accx;
	if(accx>1)	accx=1;
	if(accx<-1)	accx=-1;

	//Use accelerometer yz axis data to calculate angle in full range.
	if(accz>0)			
		data[0]=-asin(accx);
	else if(accx<0)
		data[0]=-PI+asin(accx);
	else
		data[0]=PI+asin(accx);

}