基于Linux微计算机BBB的飞控开发之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, ®_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, ®_val, 1))
return -1;
udelay(50000); //等待复位完成
/*调整时钟*/
reg_val = 0;
if(-1 == write_data(MPU9250_DEV, PWR_MGMT_1, ®_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, ®_val, 1))
return -1;
/*设置加速度计量程为+\-8g*/
reg_val = (2 << 3);
if(-1 == write_data(MPU9250_DEV, ACCEL_CONFIG, ®_val, 1))
return -1;
/*设置陀螺仪量程为+2000dps*/
reg_val = (3 << 3);
if(-1 == write_data(MPU9250_DEV, GYRO_CONFIG, ®_val, 1))
return -1;
/*设置低通滤波为20Hz*/
reg_val = (4 << 0);
if(-1 == write_data(MPU9250_DEV, CONFIG, ®_val, 1))
return -1;
/*关闭MPU9250的I2C主机*/
reg_val = (0 << 5);
if(-1 == write_data(MPU9250_DEV, USER_CTRL, ®_val, 1))
return -1;
/*使用直通方式访问AK8963*/
reg_val = (1 << 1);
if(-1 == write_data(MPU9250_DEV, INT_PIN_CFG, ®_val, 1))
return -1;
return 0;
}
static int ak8963_init(void)
{
unsigned char reg_val;
/*读取设备ID*/
if(-1 == read_data(AK8963_DEV, WIA, ®_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, ®_val, 1))
return -1;
udelay(50000); //等待复位完成
/*设定为100Hz、16bit连续采样模式*/
reg_val = (6 << 0) | (1 << 4);
if(-1 == write_data(AK8963_DEV, CNTL1, ®_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的姿态解算。
