程序员经验分享-hwhale

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

2023-05-16

基于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的姿态解算。

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

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

随机推荐

  • IMX6ULL学习笔记（21）——MMDC接口使用(DDR3测试)

    一 MMDC简介 MMDC 接口与 STM32 的 FSMC 接口类似 xff0c 只不过 MMDC 接口专用于外接 DDR xff0c 并且 MMDC 外部引脚不复用 MMDC 是一个多模的 DDR 控制器 xff0c 可以连接 16 位

  • IMX6ULL学习笔记（22）——eLCDIF接口使用（TFT-LCD屏显示）

    一 TFT LCD简介 TFT LCD xff08 Thin Film Transistor Liquid Crystal Display xff09 即薄膜晶体管液晶显示器 TFT LCD 与无源 TN LCD STN LCD 的简单矩阵

  • STM32 ROS控制器底层代码讲解

    本文主要对控制器底层代码的整天架构进行讲解 控制器由两部分组成一部分是BootLoader 另一部分是APP xff1b BootLoader主要用于固件升级 xff0c APP则作为应用程序 BootLoader的地址为 0x800000

  • STM32 使用microros与ROS2通信

    本文主要介绍如何在STM32中使用microros与ROS2进行通信 xff0c 在ROS1中标准的库是rosserial 在ROS2中则是microros 目前网上的资料也有一部分了 xff0c 但是都没有提供完整可验证的demo xff

  • 用ROS自带的gazebo仿真器搭建自己的环境

    近期需要搭建一个室内仿真环境 xff0c 用于实验调试 xff0c 所以想把相关技巧记录下来 xff0c 如有错误 xff0c 还请批评指正 xff0c 谢谢 参考网页 xff1a 使用gazebo中的building editor创建一个

  • docker如何删除容器里的文件

    问题起因 为什么会有这个问题呢 xff1f 起因是要从一个es搜索引擎从另一个es搜索引擎中拷贝数据 图方便没用软件导致重启es的时候拷贝的数据 xff0c 引发了es的启动异常 解决方案 docker inspect docker ins

  • 从程序中学习EKF-SLAM（一）

    在一次课程的结课作业上 xff0c 作业要求复写一个EKF SLAM系统 xff0c 我从中学到了好多知识 作为一个典型轻量级slam系统 xff0c 这个小项目应该特别适合于slam系统入门 xff0c 可以了解到经典卡尔曼滤波器在sla

  • numpy和tensorflow的一些用法与联系

    tensorflow和numpy值的差别 在numpy中生成的np array可以直接在 debug中看到产生的具体数字 xff1b 而在tensorflow中却只是一个tensor类型 xff0c 需要调用tf Session run X

  • ubuntu18.04 安装librealsense并验证

    安装环境 OS Ubuntu 18 04 bionic Kernel x86 64 Linux 4 15 0 20 generic 安装Realsense SDK 参考https github com IntelRealSense libr

  • YOLOV3只显示一类检测结果，并输出位置信息

    YOLOV3批量检测图片 xff0c 只显示一类检测结果 xff0c 并输出位置信息保存到txt 第一步 xff1a 首先修改YOLOV3中src imge c中的void draw detections函数 这里的修改实现了保存检测类别的

  • 搭建PX4开发环境

    搭建PX4开发环境 官方网站PX4 IO xff0c 我使用的是ubuntu20 04 一 官方环境搭建 1 下载PX4固件 span class token function git span clone https github com

  • PX4二次开发——程序运行过程

    PX4二次开发 程序运行过程 一 写在前面 px4固件程序与最开始我们所学习的对单片机外设开发不同 xff0c 是因为飞行器控制系统是一个复杂的系统 xff0c 要求实时性好 xff0c 完成复杂的控制任务 xff0c 简简单单的按照之前所

  • PX4二次开发——编译与启动脚本的修改

    PX4二次开发 编译和启动脚本的修改 一 在修改之前我们先了解一下目录结构 1 1 总目录结构 上图 xff0c 是源码目录 Src xff1a 目录是源码目录存放所有的源码 xff0c 源码的查看都应该在这里 Mavlink xff1a

  • PX4二次开发——uorb订阅

    PX4二次开发 uorb订阅 一 写在前面 我们写了一个一个功能的模块 xff0c 这些模块并不是独立的 模块之间是有数据传递的 xff0c 这样才能组合到一起实现飞行控制的目的 那么解决模块之间的数据传递的方式就是通过uorb订阅的方式

  • PX4二次开发——PX4程序架构

    PX4程序架构 一 从RCS启动脚本可以看出哪些东西 启动脚本是一个神奇的东西 xff0c 它能够识别出你对应的飞机类型 xff0c 加载对应的混控器 xff0c 选择对应的姿态 位置估计程序以及控制程序 xff0c 初始化你需要的驱动程序

  • ROS

    ROS 1 ROS测试 启动ros master 打开一个终端roscore 启动小海龟仿真器 新打开一个终端rosrun turtlesim turtlesim node 启动海龟控制节点 新打开一个终端rosrun turtlesim

  • c++之vector 及 二维容器vector＜vector＜int＞＞初始化方法 及 三维数组初始化

    C 43 43 二维容器vector lt vector gt 初始化方法解析 遇到的问题 xff1a 在解决 求最大字串 问题时想到了用二位数组vector lt vector lt int gt gt table xff0c 但是不知道

  • react 中 setState( ) 的两种写法

    react 想要更新视图只能用 setState 方法 关于 setState 这里有三件事情需要知道 1 不要直接更新状态 xff0c 而是使用 setState 2 状态更新可能是异步的 React 可以将多个setState 调用合并

  • 在32位机器上实现64位数的除法

    概述 在32位机器上不能直接进行64位数据的除法 xff0c 比如a或b是64位的数据的时候 xff0c 要计算a b xff0c 不能直接data 61 a b 这样的计算 xff0c 编译器会报错 xff0c 缺少相关的指令 这就需要我

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

    基于Linux微计算机BBB的飞控开发之IMU数据获取与预处理 1 介绍2 Linux下调用I2C2 1 Shell下的调用方法2 2 使用C语言访问i2c设备 3 配置IMU4 读取IMU 1 介绍 最近在做Linux平台的飞控开发 xf

热门标签