基于Linux微计算机BBB的飞控开发之IMU数据获取与预处理

最新推荐文章于 2024-04-17 20:06:15 发布

苦瓜不苦o

最新推荐文章于 2024-04-17 20:06:15 发布

阅读量1.3k

点赞数 5

分类专栏：四旋翼无人机开发

本文链接：https://blog.csdn.net/qq_36413840/article/details/105582690

版权

四旋翼无人机开发专栏收录该内容

2 篇文章 2 订阅

订阅专栏

基于Linux微计算机BBB的飞控开发之IMU数据获取与预处理

1. 介绍
2. Linux下调用I2C
- 2.1 Shell下的调用方法
- 2.2 使用C语言访问i2c设备
3. 配置IMU
4. 读取IMU

1. 介绍

最近在做Linux平台的飞控开发，过程中还是遇到了不少的问题，当然也学到很多东西。在我博客的四旋翼无人机开发专栏里我会分享我制作的每一个过程，希望能够帮到同样在制作飞控的小伙伴。硬件平台是Beaglebone Blue（BBB）。

2. Linux下调用I2C

BBB上的IMU是个已经停产的MPU9250，使用的是I2C-2。我将分别介绍一下与I2C有关的Shell命令和C语言调用方法。

2.1 Shell下的调用方法

i2cdetect -l可以列举I2C总线适配器。
图1

i2cdetect -y -r x可以探测i2c-x总线上的设备。图2的–表示i2c-x总线上这个地址没有设备。UU表示这个地址的设备已经有驱动器使用了。我使用的mpu9250的地址为0x68，显然mpu9250已经被某个驱动程序控制了。这里我没有研究过这个mpu9250驱动程序的使用方法。经过我的测试，只需要使用sudo rmmod inv_mpu6050_i2c就可以卸载掉驱动。卸掉之后再使用i2cdetect -y -r x，就是图3这样。可以看到0x68这里出现了68，这样就可以直接通过i2c访问mpu9250，否则会提示Device or resource busy。重启之后这个驱动程序会自动重装。
图2

图3
在这里插入图片描述
i2cdump -y x n可以读取i2c-x总线上地址为n的设备的所有寄存器。在后面我们使用C语言配置mpu9250寄存器的时候，可以使用这个命令检查是否配置成功。图4是获取mpu9250所有寄存器的值。
图4

i2cget -y x n a可以获取i2c-x总线上地址为n的设备的地址a的寄存器。可以看到mpu9250的0x00地址值为0xba，这跟图4中的一致。
图5
在这里插入图片描述
i2cset -y x n a v可以设置i2c-x总线上地址为n的设备的地址为a的寄存器的值为v。图6中就将mpu9250的0x6b处的寄存器修改成了0x00。读者自己尝试的时候一定要先查一下寄存器手册，看这个寄存器是只读还是可写的，只读是写入无效的。
图6 在这里插入图片描述

2.2 使用C语言访问i2c设备

Linux的一切皆文件的思想还是带来非常大方便的，访问一个设备就跟写文件一样简单。
下面是以mpu9250为例，简单介绍访问方法。程序经过实测。如果运行失败，请使用i2cdetect -y -r 2看一下0x68的位置是不是UU，如果是UU，则使用sudo rmmod inv_mpu6050_i2c卸载mpu9250驱动程序，原因在2.1提到了。

#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/i2c.h>
#include <linux/i2c-dev.h>

int main(void)
{
	/*
	/dev/i2c-2其实是i2c总线驱动程序的接口，我们打开这个文件的时候
	并不是打开了一个真正的文件，而是触发了系统调用，open()会触发软
	中断，cpu执行权移交给操作系统，操作系统最终会调用i2c驱动程序里
	面的open()函数，进行一些初始化操作。ioctl(),write(),read()也
	是这样的原理。
	*/
	int file = open("/dev/i2c-2", O_RDWR);
	if(-1 == file)
		return 1;
	//这里是设置我们要访问的设备的i2c地址，mpu9250是0x68。
	if(-1 == ioctl(file, I2C_SLAVE, 0x68))
	{
		close(file);
		return 1;
	}
	/*
	这里就是写入了,了解i2c协议的读者应该知道写入和读取之前都要先写
	入需要写或读的寄存器的地址。这里的0x6b就是我们测试时写的寄存器
	的地址。我们把它写入0x01，再把它读出来看看是不是我们写的值。
	*/
	char writebuf[] = {0x6b, 0x01};
	if(-1 == write(file, writebuf, 2))	//写入两个字节
	{
		close(file);
		return 1;
	}
	char addr = 0x6b;
	char readbuf;
	if(-1 == write(file, addr, 1))	//仅写入地址
	{
		close(file);
		return 1;
	}
	if(-1 == read(file, &readbuf, 1))
	{
		close(file);
		return 1;
	}
	printf("0x68 register = 0x%02x", readbuf);
	close(file);
	return 0;
}

3. 配置IMU

先提一下mpu9250是mpu6050与磁力计ak8963的杂交种，mpu9250我们可以直接通过i2c-2的0x68地址访问，而访问ak8963有两种方式，一种是使用mpu9250的i2c主机去访问ak8963，我们再从mpu9250获取数据，还有一种是使用i2c-2的0x0c地址直接访问。第一种我没有尝试过，所以我们来看第二种如何实现。首先我们看图2，其中0x0c的位置并不存在设备，那时因为mpu9250默认使用了第一种方式，因而我们要先配置mpu9250关闭i2c主机，使能旁通，即将mpu9250的0x6a寄存器的bit5配置为0，将0x37的bit1配置为1。读者可以使用i2cset -y 2 0x68 0x6a 0x00 && i2cset -y 2 0x68 0x37 0x02来实现，并且使用i2cdetect -y -r 2看是否0x0c的位置显示为0c，若显示0c则成功。下面就可以直接用0x0c的地址访问ak8963了。
使用C语言访问i2c前面已经简单说了。下面我们使用i2c配置IMU。直接上程序。
这里的read_data和write_data的第一个参数是设备的i2c地址，第二个参数是寄存器地址**（下面的寄存器地址是用的宏定义，限于篇幅无法给出，可以查手册，或者联系我）**，第三个参数是寄存器的值，第四个参数是写入或读取的字节数。函数定义这里暂不方便给出，其实也就是对write和read函数做了简单的封装，根据前面的介绍和i2c协议，读者应该可以写出这样的函数。下面就是主要的寄存器配置内容。注意初始化ak8963之前一定要先设置mpu9050。

#define MPU9250_DEV	0x68
#define AK8963_DEV	0x0c
static int mpu9250_init(void)
{
	unsigned char reg_val;

	/*读取设备ID*/
	if(-1 == read_data(MPU9250_DEV, WHO_AM_I, &reg_val, 1))
		return -1;
	else
		printf("The ID of MPU9250 is 0x%02x\n\r", reg_val);

	/*复位*/
	reg_val = (1 << 7);
	if(-1 == write_data(MPU9250_DEV, PWR_MGMT_1, &reg_val, 1))
		return -1;
	udelay(50000);	//等待复位完成

	/*调整时钟*/
	reg_val = 0;
	if(-1 == write_data(MPU9250_DEV, PWR_MGMT_1, &reg_val, 1))
		return -1;

	/*设置采样率为1000Hz*/
#define INTERNAL_SAMPLE_RATE 1000
	reg_val = INTERNAL_SAMPLE_RATE / 1000 - 1;
	if(-1 == write_data(MPU9250_DEV, SMPLRT_DIV, &reg_val, 1))
		return -1;

	/*设置加速度计量程为+\-8g*/
	reg_val = (2 << 3);
	if(-1 == write_data(MPU9250_DEV, ACCEL_CONFIG, &reg_val, 1))
		return -1;

	/*设置陀螺仪量程为+2000dps*/
	reg_val = (3 << 3);
	if(-1 == write_data(MPU9250_DEV, GYRO_CONFIG, &reg_val, 1))
		return -1;

	/*设置低通滤波为20Hz*/
	reg_val = (4 << 0);
	if(-1 == write_data(MPU9250_DEV, CONFIG, &reg_val, 1))
		return -1;

	/*关闭MPU9250的I2C主机*/
	reg_val = (0 << 5);
	if(-1 == write_data(MPU9250_DEV, USER_CTRL, &reg_val, 1))
		return -1;

	/*使用直通方式访问AK8963*/
	reg_val = (1 << 1);
	if(-1 == write_data(MPU9250_DEV, INT_PIN_CFG, &reg_val, 1))
		return -1;
	
	return 0;
}

static int ak8963_init(void)
{
	unsigned char reg_val;

	/*读取设备ID*/
	if(-1 == read_data(AK8963_DEV, WIA, &reg_val, 1))
		return -1;
	else
		printf("The ID of AK8963 is 0x%02x\n\r", reg_val);

	/*复位*/
	reg_val = (1 << 0);
	if(-1 == write_data(AK8963_DEV, CNTL2, &reg_val, 1))
		return -1;
	udelay(50000);	//等待复位完成

	/*设定为100Hz、16bit连续采样模式*/
	reg_val = (6 << 0) | (1 << 4);
	if(-1 == write_data(AK8963_DEV, CNTL1, &reg_val, 1))
		return -1;
}

4. 读取IMU

首先我们读出原始数据：
主要ak8963有读取保护，读取完数据之后，一定要读取ST2（0x09）寄存器，否则ak8963会陷入数据保护状态，读出来值就一直是原来的值。

unsigned char i2c_data[14];
if(-1 == read_data(MPU9250_DEV, ACCEL_XOUT_H, i2c_data, 14))
		return -1;

	mpu_raw_data.acc.x = (short)((i2c_data[0] << 8) | i2c_data[1]);
	mpu_raw_data.acc.y = (short)((i2c_data[2] << 8) | i2c_data[3]);
	mpu_raw_data.acc.z = (short)((i2c_data[4] << 8) | i2c_data[5]);
	mpu_raw_data.gyro.x = (short)((i2c_data[8] << 8) | i2c_data[9]);
	mpu_raw_data.gyro.y = (short)((i2c_data[10] << 8) | i2c_data[11]);
	mpu_raw_data.gyro.z = (short)((i2c_data[12] << 8) | i2c_data[13]);
	mpu_raw_data.temperature = (short) ((i2c_data[6] << 8) | i2c_data[7]);
	
if(-1 == read_data(AK8963_DEV, HXL, i2c_data, 6))
		return -1;
/*解除读取保护*/
if(-1 == read_data(AK8963_DEV, ST2, &ak8963_status_2, 1))
		return -1;
	
	mag_raw_data.x = (i2c_data[0] | (i2c_data[1] << 8));
	mag_raw_data.y = (i2c_data[2] | (i2c_data[3] << 8));
	mag_raw_data.z = (i2c_data[4] | (i2c_data[5] << 8));

然后对数据进行纠偏：
这里的acc_offset、gyro_offset与mag_offset是在IMU静止的条件下读取1000次数据，并取平均值计算出来的，也就是加速度计、陀螺仪、磁力计的零点漂移。

filter_buf[filter_cnt][ACC_FILTER].x = mpu_raw_data.acc.x - acc_offset.x;
filter_buf[filter_cnt][ACC_FILTER].y = mpu_raw_data.acc.y - acc_offset.y;
filter_buf[filter_cnt][ACC_FILTER].z = mpu_raw_data.acc.z - acc_offset.z;
filter_buf[filter_cnt][GYRO_FILTER].x = mpu_raw_data.gyro.x - gyro_offset.x;
filter_buf[filter_cnt][GYRO_FILTER].y = mpu_raw_data.gyro.y - gyro_offset.y;
filter_buf[filter_cnt][GYRO_FILTER].z = mpu_raw_data.gyro.z - gyro_offset.z;
mag_handled_data.x = (mag_raw_data.x - mag_offset.x);
mag_handled_data.y = (mag_raw_data.y - mag_offset.y);
mag_handled_data.z = (mag_raw_data.z - mag_offset.z);
//这里对磁力计的数据另外进行了坐标转换，磁力计原始数据的坐标轴与加速度计、陀螺仪不一致。

接着对数据进行滑动均值滤波：

/*滑动均值滤波*/
	for(i = 0; i < SLIDING_FILTER_CNT_MAX; i++)
	{
		filter_tmp[ACC_FILTER].x += filter_buf[i][ACC_FILTER].x;
		filter_tmp[ACC_FILTER].y += filter_buf[i][ACC_FILTER].y;
		filter_tmp[ACC_FILTER].z += filter_buf[i][ACC_FILTER].z;
		filter_tmp[GYRO_FILTER].x += filter_buf[i][GYRO_FILTER].x;
		filter_tmp[GYRO_FILTER].y += filter_buf[i][GYRO_FILTER].y;
		filter_tmp[GYRO_FILTER].z += filter_buf[i][GYRO_FILTER].z;
	}
	if(filter_buf_full)
	{
		mpu_handled_data.acc.x = filter_tmp[ACC_FILTER].x / SLIDING_FILTER_CNT_MAX;
		mpu_handled_data.acc.y = filter_tmp[ACC_FILTER].y / SLIDING_FILTER_CNT_MAX;
		mpu_handled_data.acc.z = filter_tmp[ACC_FILTER].z / SLIDING_FILTER_CNT_MAX;
		mpu_handled_data.gyro.x = filter_tmp[GYRO_FILTER].x / SLIDING_FILTER_CNT_MAX;
		mpu_handled_data.gyro.y = filter_tmp[GYRO_FILTER].y / SLIDING_FILTER_CNT_MAX;
		mpu_handled_data.gyro.z = filter_tmp[GYRO_FILTER].z / SLIDING_FILTER_CNT_MAX;
	}
	else
	{
		mpu_handled_data.acc.x = filter_tmp[ACC_FILTER].x / filter_cnt;
		mpu_handled_data.acc.y = filter_tmp[ACC_FILTER].y / filter_cnt;
		mpu_handled_data.acc.z = filter_tmp[ACC_FILTER].z / filter_cnt;
		mpu_handled_data.gyro.x = filter_tmp[GYRO_FILTER].x / filter_cnt;
		mpu_handled_data.gyro.y = filter_tmp[GYRO_FILTER].y / filter_cnt;
		mpu_handled_data.gyro.z = filter_tmp[GYRO_FILTER].z / filter_cnt;
	}

最后对数据进行转换：

#define TO_ANGLE 	0.061037	//陀螺仪数据转换为角度制角速度的转换系数
#define ANG_TO_RAD	0.01745329	//角度制转为弧度制
imu->acc.x = mpu_handled_data.acc.x;
imu->acc.y = mpu_handled_data.acc.y;
imu->acc.z = mpu_handled_data.acc.z;
imu->gyro.x = mpu_handled_data.gyro.x * TO_ANGLE * ANG_TO_RAD;
imu->gyro.y = mpu_handled_data.gyro.y * TO_ANGLE * ANG_TO_RAD;
imu->gyro.z = mpu_handled_data.gyro.z * TO_ANGLE * ANG_TO_RAD;
imu->temperature = mpu_handled_data.temperature;
imu->mag.x = mag_handled_data.y;
imu->mag.y = mag_handled_data.x;
imu->mag.z = -mag_handled_data.z;

这里对磁力计的数据进行了坐标转换，是因为磁力计的坐标轴与加速度计与陀螺仪的坐标轴不一致。
图7
在这里插入图片描述
在后面的文章中我会继续介绍IMU的姿态解算。

苦瓜不苦o

关注

5
点赞
踩
8

收藏

觉得还不错? 一键收藏
1
评论
基于Linux微计算机BBB的飞控开发之IMU数据获取与预处理

基于Linux微计算机BBB的飞控开发之IMU数据获取与预处理1. 介绍2. Linux下调用I2C2.1 Shell下的调用方法1. 介绍最近在做Linux平台的飞控开发，过程中还是遇到了不少的问题，当然也学到很多东西。在我博客的四旋翼无人机开发专栏里我会分享我制作的每一个过程，希望能够帮到同样在制作飞控的小伙伴。硬件平台是Beaglebone Blue（BBB）。2. Linux下调用I...
复制链接

扫一扫