树莓派IIC通讯获取BMI08x IMU数据进行姿态解算，并通过UART/TCP在rviz上显示
Posted lwlv
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了树莓派IIC通讯获取BMI08x IMU数据进行姿态解算，并通过UART/TCP在rviz上显示相关的知识，希望对你有一定的参考价值。
截至2021年，树莓派出的最新款应该是Raspberry Pi 400，设计得跟键盘一样，很难想象到这是个树莓派，尤其是它的标语写的很好“你的下一个电脑，何必是电脑”，不言而喻。反正我看到后就有很想买的冲动，它对于很多硬件通讯方面的测试以及开发一些小玩意还是很有帮助的，如下图所示。
（图片来源自Buy a Raspberry Pi 400 Personal Computer Kit – Raspberry Pi）
最近刚好玩了一个小项目，通过树莓派IIC采集Bosch BMI08x IMU数据并进行姿态解算，然后通过串口或TCP的方式传输至PC端进行可视化。具体过程如下：
目录
1. 树莓派的IIC通讯
树莓派运行的是MPU级别的处理器，因此可以运行桌面级系统，也就是RaspberryPi-OS (previously called Raspbian)，应该是基于Linux内核又封装的一层UI。最新的树莓派系统可以在https://www.raspberrypi.com/software/下载到，然后通过Raspberry Pi Imager软件将系统烧录至一个SD卡上。SD再插在树莓派上就可以开始用了。
树莓派预留有40个引脚的GPIO PIN，可以进行IIC，SPI，UART等通讯的开发，还支持WIFI，蓝牙，网卡等。在终端输入
sudo raspi-config可设置将哪些接口使能。尤其SSH使能还是挺方便的，因为默认树莓派默认是装有VNC server。PC端再安装VNC viewer便可以远程至树莓派，而不需要额外添加显示器。
在Linux下的i2c-tools是一个很方便的i2c调试工具（通过sudo apt install i2c-tools安装）。可以查看接入了几个IIC设备，设备地址，读写寄存器等。
比如上图，我通过sudo i2cdetect -l可以查看到总线1接入了设备。再通过sudo i2cdetect -y 1可查询到总线1上又连有两个IIC slave，地址分别为0x19和0x69。用i2cdump指令可获取当前设备地址的寄存器0x0~0xFF的值。这个工具还是挺方便，尤其是获取IIC的设备地址。
- 如果是python开发，用smbus库可实现对IIC硬件的读写。
import smbus
bus = smbus.SMBus(1)
address = 0x68
bus.write_byte_data(address, 0x6B, 0x00) #写寄存器
bus.read_byte_data(address, 0x0) #读寄存器
- 如果是C语言开发，目前主要wiringpi库，也非常容易。主要操作如下。
wiringPiSetup();
int dev_handle = wiringPiI2CSetup(0x19); //0x19为设备地址
uint8_t register = 0x0;
uint8_t value = wiringPiI2CReadReg8(dev_handle, register); //读寄存器0，value为读取到的值
uint8_t value_write = 0xF;
value = wiringPiI2CReadReg8(dev_handle, register, value_write); //写寄存器0的值为0xF- 当然我也看到网上有大神通过GPIO操作手写IIC协议， 详见
https://blog.csdn.net/weixin_36181191/article/details/114020192树莓派的IIC通讯方法和调试就总结到这。
2. 博世BMI08x IMU数据获取及其姿态解算
2.1 IMU数据获取
以前比较火的IMU是InvenSense公司出的MPU系列，后来Bosch也开始做。这次用BMI08x系列采集数据（BMI088和bmi085引脚和寄存器定义几乎一样，只是量程等方面有差别），它的数据手册可以在Inertial Measurement Unit BMI088 | Bosch Sensortec下载到。官方还给出了其示例程序（https://github.com/BoschSensortec/BMI08x-Sensor-API），但需要配合他们所开发的Application board 3.0开发板才能用。不过正如它的README所说，在自己的工程中添加bmi08a.c, bmi08g.c,bmi08x_defs.h and bmi08x.h就好了。此外，我还看到另外一个大神用STM32基于SPI通讯方式实现的例程GitHub - SEASKY-Master/BMI088_Master，及其演示效果。开源BMI088六轴姿态传感器_哔哩哔哩_bilibili-#BMI088# #六轴姿态传感器# #STM32f405RGT6#开源一款BMI088六轴姿态传感器，相比普通姿态传感器有何优势大家自己百度一下，就会知道了开源仓库如下：https://github.com/SEASKY-Master/BMI088_Master
https://www.bilibili.com/video/BV1Dp4y1C7rC
这里我参考官方给的read_sensor_data例程。在common.c文件下对IIC寄存器的读写进行了修改，如下。
common.h
/**\\
* Copyright (c) 2021 Bosch Sensortec GmbH. All rights reserved.
*
* SPDX-License-Identifier: BSD-3-Clause
**/
#ifndef COMMON_H
#define COMMON_H
/*! CPP guard */
#ifdef __cplusplus
extern "C" {
#endif
#include <stdio.h>
#include <wiringPi.h>
#include <wiringPiI2C.h>
#include "bmi08x_defs.h"
/*!
* @brief Function for reading the sensor's registers through I2C bus.
*
* @param[in] reg_addr : Register address.
* @param[out] reg_data : Pointer to the data buffer to store the read data.
* @param[in] length : No of bytes to read.
* @param[in] intf_ptr : Interface pointer
*
* @return Status of execution
* @retval = BMI08X_INTF_RET_SUCCESS -> Success
* @retval != BMI08X_INTF_RET_SUCCESS -> Failure Info
*
*/
BMI08X_INTF_RET_TYPE bmi08x_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr);
/*!
* @brief Function for writing the sensor's registers through I2C bus.
*
* @param[in] reg_addr : Register address.
* @param[in] reg_data : Pointer to the data buffer whose value is to be written.
* @param[in] length : No of bytes to write.
* @param[in] intf_ptr : Interface pointer
*
* @return Status of execution
* @retval = BMI08X_INTF_RET_SUCCESS -> Success
* @retval != BMI08X_INTF_RET_SUCCESS -> Failure Info
*
*/
BMI08X_INTF_RET_TYPE bmi08x_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr);
/*!
* @brief This function provides the delay for required time (Microsecond) as per the input provided in some of the
* APIs.
*
* @param[in] period_us : The required wait time in microsecond.
* @param[in] intf_ptr : Interface pointer
*
* @return void.
*
*/
void bmi08x_delay_us(uint32_t period, void *intf_ptr);
/*!
* @brief Function to select the interface between SPI and I2C.
*
* @param[in] bma : Structure instance of bmi08x_dev
* @param[in] intf : Interface selection parameter
* For I2C : BMI08X_I2C_INTF
* For SPI : BMI08X_SPI_INTF
* @param[in] variant : Sensor variant parameter
* For BMI085 : BMI085_VARIANT
* For BMI088 : BMI088_VARIANT
*
* @return Status of execution
* @retval 0 -> Success
* @retval < 0 -> Failure Info
*/
int8_t bmi08x_interface_init(struct bmi08x_dev *bma, uint8_t intf, uint8_t variant);
/*!
* @brief Prints the execution status of the APIs.
*
* @param[in] api_name : Name of the API whose execution status has to be printed.
* @param[in] rslt : Error code returned by the API whose execution status has to be printed.
*
* @return void.
*/
void bmi08x_error_codes_print_result(const char api_name[], int8_t rslt);
#ifdef __cplusplus
}
#endif /* End of CPP guard */
#endif /* COMMON_H */common.c
/**
* Copyright (C) 2021 Bosch Sensortec GmbH. All rights reserved.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include "common.h"
/******************************************************************************/
/*! Macro definitions */
#define BMI08X_READ_WRITE_LEN UINT8_C(46)
int dev_acc;
int dev_gyro;
/******************************************************************************/
/*! Static variable definition */
/*! Variable that holds the I2C device address or SPI chip selection for accel */
uint8_t acc_dev_add;
/*! Variable that holds the I2C device address or SPI chip selection for gyro */
uint8_t gyro_dev_add;
/******************************************************************************/
/*! User interface functions */
/*!
* I2C read function map to COINES platform
*/
BMI08X_INTF_RET_TYPE bmi08x_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t len, void *intf_ptr)
{
uint8_t dev_addr = *(uint8_t*)intf_ptr;
int fd;
if(dev_addr == 0x18 || dev_addr == 0x69)
fd = dev_acc;
else if (dev_addr == 0x68 || dev_addr == 0x69)
fd = dev_gyro;
else
return -1;
if(len == 1)
{
int tmp = wiringPiI2CReadReg8(fd, reg_addr);
printf("Device %d read register 0x%x value = 0x%x\\n", fd, reg_addr, tmp);
reg_data[0] = ((uint8_t)tmp);
return 0;
}
printf("The reading length is %d \\n", len);
int i = 0;
for(i = 0; i < len; i++)
{
int tmp = wiringPiI2CReadReg8(fd, reg_addr+i);
printf("Device %d read register 0x%x value = 0x%x\\n", fd, reg_addr+i, tmp);
reg_data[i] = ((uint8_t)tmp);
}
return 0;
}
/*!
* I2C write function map to COINES platform
*/
BMI08X_INTF_RET_TYPE bmi08x_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t len, void *intf_ptr)
{
uint8_t dev_addr = *(uint8_t*)intf_ptr;
int fd;
if(dev_addr == 0x18 || dev_addr == 0x69)
fd = dev_acc;
else if (dev_addr == 0x68 || dev_addr == 0x69)
fd = dev_gyro;
else
return -1;
if(len == 1)
{
int tmp = wiringPiI2CWriteReg8(fd, reg_addr, reg_data[0]);
printf("Device %d write register 0x%x value = 0x%x\\n", fd, reg_addr, reg_data[0]);
return 0;
}
printf("The writing length is %d \\n", len);
int i = 0;
for(i = 0; i < len; i++)
{
int tmp = wiringPiI2CWriteReg8(fd, reg_addr+i, reg_data[i]);
printf("Device %d write register 0x%x value = 0x%x\\n", fd, reg_addr+i, reg_data[i]);
}
return 0;
}
/*!
* Delay function map to COINES platform
*/
void bmi08x_delay_us(uint32_t period, void *intf_ptr)
{
usleep(period*10);
}
/*!
* @brief Function to select the interface between SPI and I2C.
* Also to initialize coines platform
*/
int8_t bmi08x_interface_init(struct bmi08x_dev *bmi08x, uint8_t intf, uint8_t variant)
{
int8_t rslt = BMI08X_OK;
if (bmi08x != NULL)
{
/* Bus configuration : I2C */
if (intf == BMI08X_I2C_INTF)
{
printf("I2C Interface \\n");
/* To initialize the user I2C function */
acc_dev_add = BMI08X_ACCEL_I2C_ADDR_PRIMARY;
gyro_dev_add = BMI08X_GYRO_I2C_ADDR_PRIMARY;
bmi08x->intf = BMI08X_I2C_INTF;
bmi08x->read = bmi08x_i2c_read;
bmi08x->write = bmi08x_i2c_write;
}
/* Selection of bmi085 or bmi088 sensor variant */
bmi08x->variant = variant;
/* Assign accel device address to accel interface pointer */
bmi08x->intf_ptr_accel = &acc_dev_add;
/* Assign gyro device address to gyro interface pointer */
bmi08x->intf_ptr_gyro = &gyro_dev_add;
/* Configure delay in microseconds */
bmi08x->delay_us = bmi08x_delay_us;
/* Configure max read/write length (in bytes) ( Supported length depends on target machine) */
bmi08x->read_write_len = BMI08X_READ_WRITE_LEN;
}
else
{
rslt = BMI08X_E_NULL_PTR;
}
return rslt;
}
/*!
* @brief Prints the execution status of the APIs.
*/
void bmi08x_error_codes_print_result(const char api_name[], int8_t rslt)
{
if(rslt == BMI08X_OK)
printf("%s --- successful\\r\\n", api_name);
if (rslt != BMI08X_OK)
{
printf("%s\\t", api_name);
if (rslt == BMI08X_E_NULL_PTR)
{
printf("Error [%d] : Null pointer\\r\\n", rslt);
}
else if (rslt == BMI08X_E_COM_FAIL)
{
printf("Error [%d] : Communication failure\\r\\n", rslt);
}
else if (rslt == BMI08X_E_DEV_NOT_FOUND)
{
printf("Error [%d] : Device not found\\r\\n", rslt);
}
else if (rslt == BMI08X_E_OUT_OF_RANGE)
{
printf("Error [%d] : Out of Range\\r\\n", rslt);
}
else if (rslt == BMI08X_E_INVALID_INPUT)
{
printf("Error [%d] : Invalid input\\r\\n", rslt);
}
else if (rslt == BMI08X_E_CONFIG_STREAM_ERROR)
{
printf("Error [%d] : Config stream error\\r\\n", rslt);
}
else if (rslt == BMI08X_E_RD_WR_LENGTH_INVALID)
{
printf("Error [%d] : Invalid Read write length\\r\\n", rslt);
}
else if (rslt == BMI08X_E_INVALID_CONFIG)
{
printf("Error [%d] : Invalid config\\r\\n", rslt);
}
else if (rslt == BMI08X_E_FEATURE_NOT_SUPPORTED)
{
printf("Error [%d] : Feature not supported\\r\\n", rslt);
}
else if (rslt == BMI08X_W_FIFO_EMPTY)
{
printf("Warning [%d] : FIFO empty\\r\\n", rslt);
}
else
{
printf("Error [%d] : Unknown error code\\r\\n", rslt);
}
}
}
当然还要加入树莓派初始化和板子初始化（设置一些高低电平，带宽，量程等）的部分，这个可以放在一个文件下定义。
bool rasp_init()
{
// raspberry pi setting
if(-1 == wiringPiSetup())
{
std::cout << "setup error\\n";
return false;
}
// Handle of acc
dev_acc = wiringPiI2CSetup(0x19);
if (dev_acc == -1)
{
std::cout << "no accel(BMI088) i2c device found \\n";
return false;
}
else
{
std::cout << "accel(BMI088) i2c device found: " << dev_acc << "\\n";
}
// Handle of gyro
dev_gyro = wiringPiI2CSetup(0x69);
if (dev_gyro == -1)
{
std::cout << "no gyro(BMI088) i2c device found \\n";
return false;
}
else
{
std::cout << "gyro(BMI088) i2c device found: " << dev_gyro << "\\n";
}
return true;
}
/*!
* @brief This internal API is used to initializes the bmi08x sensor
* settings like power mode and OSRS settings.
*
* @param[in] void
*
* @return void
*
*/
static int8_t init_bmi08x(void)
{
int8_t rslt;
rslt = bmi08a_init(&bmi08xdev);
bmi08x_error_codes_print_result("bmi08a_init", rslt);
if (rslt == BMI08X_OK)
{
rslt = bmi08g_init(&bmi08xdev);
bmi08x_error_codes_print_result("bmi08g_init", rslt);
}
if (rslt == BMI08X_OK)
{
printf("Uploading config file !\\n");
rslt = bmi08a_load_config_file(&bmi08xdev);
bmi08x_error_codes_print_result("bmi08a_load_config_file", rslt);
}
if (rslt == BMI08X_OK)
{
bmi08xdev.accel_cfg.odr = BMI08X_ACCEL_ODR_1600_HZ;
if (bmi08xdev.variant == BMI085_VARIANT)
{
bmi08xdev.accel_cfg.range = BMI085_ACCEL_RANGE_16G;
}
else if (bmi08xdev.variant == BMI088_VARIANT)
{
bmi08xdev.accel_cfg.range = BMI088_ACCEL_RANGE_24G;
}
bmi08xdev.accel_cfg.power = BMI08X_ACCEL_PM_ACTIVE; /*user_accel_power_modes[user_bmi088_accel_low_power]; */
bmi08xdev.accel_cfg.bw = BMI08X_ACCEL_BW_NORMAL; /* Bandwidth and OSR are same */
rslt = bmi08a_set_power_mode(&bmi08xdev);
bmi08x_error_codes_print_result("bmi08a_set_power_mode", rslt);
rslt = bmi08a_set_meas_conf(&bmi08xdev);
bmi08x_error_codes_print_result("bmi08a_set_meas_conf", rslt);
bmi08xdev.gyro_cfg.odr = BMI08X_GYRO_BW_230_ODR_2000_HZ;
bmi08xdev.gyro_cfg.range = BMI08X_GYRO_RANGE_250_DPS;
bmi08xdev.gyro_cfg.bw = BMI08X_GYRO_BW_230_ODR_2000_HZ;
bmi08xdev.gyro_cfg.power = BMI08X_GYRO_PM_NORMAL;
rslt = bmi08g_set_power_mode(&bmi08xdev);
bmi08x_error_codes_print_result("bmi08g_set_power_mode", rslt);
rslt = bmi08g_set_meas_conf(&bmi08xdev);
bmi08x_error_codes_print_result("bmi08g_set_meas_conf", rslt);
}
return rslt;
}初始化完成后便可直接调用bmi08a.c和bmi08g.c里面的bmi08a_get_data和bmi08g_get_data函数分别获取加速度和角速度数据了。我仔细看了官方的例程，不得不说写得真是严谨和规范。
不过比较坑的是，上传配置文件和获取加速度计和陀螺仪状态是有问题，我查了下手册，压根就没有定义上传配置操作的寄存器，以至于我还在官方论坛咨询，也有可能是保留以后用吧。
总之，IIC数据可以通过树莓派获取到了。
2.2 姿态解算算法
姿态解算是指通过6轴或者9轴数据通过融合等方式得到三维空间中刚体的姿态。描述三维空间中刚体姿态的方法有欧拉角，旋转矩阵和四元数，这三者之间都能相互转换。不过欧拉角存在奇异值问题（万向锁），旋转矩阵存在不是正交阵的问题且运算量大，一般在计算机中采用四元数的方式去描述。
关于姿态解算，常用的算法有以下几种：
- 卡尔曼滤波器EKF
- Mahony的方向余弦矩阵算法
- Madgwick的梯度下降姿态解算算法
https://zhuanlan.zhihu.com/p/82973264这些算法网上都可以找到现成的，无论是python，matlab还是C。我目前暂时只试验了complementary filter方法，可在https://github.com/ccny-ros-pkg/imu_tools上找到，仅一个ComplementaryFilter类就实现了，也很方便移植，输出得到的是四元数四个参数，以及角速度修正之后的值。
3. UART或TCP数据传输并在rviz上显示
3.1 UART通讯
前面经过树莓派的数据采集以及姿态解算，我们需要将数据再次上传至PC端进行原始数据分析和可视化。树莓派可进行的方法有TTL串口通讯，有线或者无线wifi的形式通过tcp/ip协议进行传输。无论是串口还是TCP都可采用标准的modbus serial/tcp协议，如python下的modbus_tk库，C语言实现的libmodbus库等，后面再专门花时间说这两个库。
我直接使用的自由协议，也是很多IMU模块公司采用的，比如以0x55开头，然后计算长度和校验位。
树莓派有两个串口，默认将 serial0 映射到外接GPIO的15、16脚，将 serial1 映射到BT蓝牙上。
网上很多说需要将这两个映射互换一下，也就是通过更改/boot/config.txt文件，在后面加入dtoverlay=pi3-miniuart-bt。
不过请先看官方在/boot/overlays/README里面的一句话。也就是如果使用树莓派系统我们可以直接通过打开/dev/serial0的方式打开串口即可。
https://blog.csdn.net/Mark_md/article/details/107181151?utm_medium=distribute.pc_aggpage_search_result.none-task-blog-2~aggregatepage~first_rank_ecpm_v1~rank_v31_ecpm-1-107181151.pc_agg_new_rank&utm_term=ttyama0+%E6%A0%91%E8%8E%93%E6%B4%BE&spm=1000.2123.3001.4430结论是：
- /dev/ttyS0 更像是单片机中的一个一个字节去查询串口中断。
- /dev/ttyAMA0 更像是串口开启了硬件的 FIFO/DMA 。
也就是即便不互换也可以直接用。更何况官方说直接使用/dev/serial0是推荐的用法。
同样打开串口和发送数据也可以用wiringPI库。
int fd;
if(wiringPiSetup() < 0)
return NULL;
if((fd = serialOpen("/dev/serial0", 115200))<0)
{
printf("Serial Open Failed!\\n");
return NULL;
}
else
printf("Serial Open Succeed!\\n");不过用wiringPI库写指令的时候，会默认将0当作终止符。好在Linux下基本都是文件操作，
可直接使用ssize_t write (int fd, const void * buf, size_t count)和ssize_t read(int fd, void * buf, size_t count)进行读写操作。
3.2 rviz显示
rviz是ROS里面的可视化工具，还是非常强大了，基于了轻量级的游戏引擎OGRE。如果是使用虚拟机安装的ROS。可设置串口将PC端的物理串口地址映射到虚拟机的Linux系统，这里的COM1就对应Linux下的ttyS0。当然我们还需要打开它的可执行权限，sudo chmod 777 /dev/ttyUSB0。
ROS的serial包仅在kinetic和melodic下有，不过可以下载源码编译后放在自己的工程下用。
至于显示，则继续使用imut_tools下的rviz_imu_plugin工具，编译后打开rviz的add plugin可以找到rviz_imu_plugin。
最终效果如下：
Enjoy!
以上是关于树莓派IIC通讯获取BMI08x IMU数据进行姿态解算，并通过UART/TCP在rviz上显示的主要内容，如果未能解决你的问题，请参考以下文章
树莓派IIC通讯获取BMI08x IMU数据进行姿态解算，并通过UART/TCP在rviz上显示