【嵌入式linux开发】SPI设备文件操作ICM-40609D传感器
前言
在本篇博客中，将从ICM-40609-D传感器的数据手册出发，简单了解之后，展示如何通过SPI接口与传感器进行通信。这里不是通过gpio直接操作，而是使用linux文件设备操作spi接口。开发板是旭日x3 module，底板是公司画的，上面带着icm40609d。
一、数据手册浅读
一打开就是对icm-40609-d传感器的一个介绍，包括：总体描述、特征、应用等。这里粗略看一看即可。
第一、二章也是介绍，跟前面的总体介绍差不多：介绍了加速度计和陀螺仪的量程和接口，以及一些特征。接口可以是IIC和SPI，本文使用SPI。
第三章是电气特性和通信接口的底层时序，对于外接传感器模块开发的，比如使使用stm32外接封装好的imu，就需要自己实现这种底层时序，对于linux应用开发就不需要了，在嵌入式Linux系统中，硬件操作和时序控制通常是由开发板的制造商或者硬件的驱动程序开发者来实现的，应用开发不需要深入了解硬件的底层实现，会通过设备文件和系统调用进行开发即可，应用开发者在某些情况下可能需要与驱动程序开发者合作，或者对驱动程序进行定制，以确保应用程序能够正确地与硬件设备通信。第三章对于使用设备文件开发的也是看看就可以：
第四章应用程序信息，介绍了芯片的引脚以及功能，但是没有详细的给出功能是怎么配置的，所以只是起一个介绍作用，看看就行。
第五章详细介绍了一些滤波器的寄存值应该怎么配置，加速度计、陀螺仪的量程等等，可以看。
第六章、第七章介绍fifo和中断，本文只是简单数据读取，没有用到fifo和中断。
第八章详细介绍通信接口：IIC与SPI。iic就不看了，spi需要看
注意如下
①如果spi总线设备上只有ICM-40609-D的话就用不到片选信号 CS 。
②数据先发送MSB，后发送LSB，地址格式如下：
从图中可以看出，发送寄存器地址时，最高位是读写位，也就是告诉icm现在是要对这个寄存器读还是写；发送数据时八位全是数据。
③支持单次或突发读写：单次的数据格式就是bits0-7是地址，bit8-15是数据；突发读写的格式：假设当前向0x11这个寄存器写数据，那么此时bits0-7是0x10010001，bits8-15是要写入的数据，如果此时想继续写数据到连续的下一个地址（0x12）中，那就直接将bit16-23填入要写的第二个数据即可，因为在突发模式下，地址会自增，就不需要再重新写寄存器的地址了。
第九章、第十章是封装和尺寸，不重要。
第十一章是生产商提供的使用笔记，是有参考价值的。
注意如下：
①设备启动时默认是spi通信接口。
②在传感器操作过程中，用户可以修改的寄存器设置只有ODR选择、FSR选择和传感器模式更改(寄存器参数GYRO_ODR、ACCEL_ODR、GYRO_FS_SEL、ACCEL_FS_SEL、GYRO_MODE、ACCEL_MODE)。
第十二章、十三章是寄存器映射，是需要关注的重点。
ICM-40609D一共有四个给用户的寄存器组，本文只会用到banks0，所以后面几个寄存器组，自己看，配置陷波滤波器、偏置时会用到。下面我把后续代码中会用到的寄存器截图出来，并给出一些配置的介绍：
①设备初始化（写入）：bit4表示配置传感器的spi模式，需要与代码中的一样，直接使用默认的 Mode 0 and Mode 3；bit0表示是否复位，一般操作之前复位一下传感器，所以这个寄存器的值：0x01。
②温度传感器数据寄存器（读取）：一共16位，注意区分高低字节。
读取出来的原始数据拼接成16位之后，经过以下公式换算成摄氏度：
③加速度计数据寄存器（读取）：一共6字节（48bit），需要注意X、Y、Z轴的高低位，这些寄存器就是在初始化完传感器之后，来读取的，原始数据需要经过转换才能是平常看到的数值，转换后面说。
④陀螺仪数据寄存器（读取）：一共6字节（48bit），需要注意X、Y、Z轴的高低位
⑤电源管理寄存器（写入）：bit5：是否使能温度传感器、bit4：当Accel和Gyro断电时RC振荡器断电与否、bit3:2：陀螺仪模式（关闭、待机、保留、低噪声四个模式）。bit1:0：加速度计模式（关闭、低功耗、低噪声三种模式）。注意：当从关闭模式转换到任何其他模式时，不要在200µs内发出任何寄存器写入。这一字节的填写示例：00011111
⑥陀螺仪初始化寄存器0（写入）：bit7:5：陀螺仪的量程选择（量程定义了传感器可以测量的最大和最小值的范围，在较低的量程设置下，传感器的灵敏度更高，可以减少噪声对测量结果的影响）；bit3:0：陀螺仪的数据输出速率选择（描述传感器在单位时间内能够提供多少个数据样本的指标）。
⑦加速度计初始化寄存器0（写入）：bit7:5：加速度计的量程选择；bit3:0：加速度计的数据输出速率选择。
⑧加速度计、陀螺仪UI滤波器带宽设置（写入）：bit7:4：描述涉及加速度计的两种模式（低功耗模式LP和低噪声模式LN）下的滤波器带宽和滤波设置；bit3:0：描述涉及陀螺仪的低噪声模式下的滤波器带宽和滤波设置。
这里具体选什么值还是需要更进一步的细分，如果应用需要在低功耗下运行，同时对噪声性能有较高要求，可以选择LN模式下的较高带宽设置。如果应用对数据更新速率要求不高，可以选择LP模式下的高倍平均滤波器设置来进一步降低功耗。有需求的可以网上看看有没有其他人仔细分析了这一字节，本文写入：
0x44进行设置。
⑨WHO AM I寄存器（读取）:此寄存器指示用户正在访问哪个设备在所有设置进行之前，先判断此寄存器的值是否是0x3B。
⑩寄存器组选择（写入）：写入不同的值表示当前操作哪个寄存器组。本文只操作banks0，所以初始化之前写入0x00即可，不写也行，默认就是0x00。
数据手册后续内容这里都没有用到。
二、linux系统下使用SPI设备文件操作ICM-40609-D
①首先确定传感器是在哪个spi总线设备下，本文是spi1.0：
②编写较为底层的spi.c与spi.h，封装读写一字节的函数、初始化函数等。后续icm40609d.cpp会调用这里的读写函数进行寄存器读写。
spi.c：
#include "IMU_spi.h"
#ifdef __cplusplus
extern "C" {
#endif
static uint8_t mode = 0;
static uint8_t bits = 8;
static uint8_t count = 2;
static uint32_t speed = 12000000;
static uint16_t delay = 0;
int SPIInit(int spi_num,int cs_num){
int ret = 0;
int fd;
char spi_dev[30];
sprintf(spi_dev,"/dev/spidev%d.%d",spi_num,cs_num);
fd = open(spi_dev, O_RDWR);
if (fd < 0) {
pabort("can't open device");
}
// SPI mode
ret = ioctl(fd, SPI_IOC_WR_MODE, &mode);
if (ret == -1) {
pabort("can't set spi mode");
}
ret = ioctl(fd, SPI_IOC_RD_MODE, &mode);
if (ret == -1) {
pabort("can't get spi mode");
}
// bits per word
ret = ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &bits);
if (ret == -1) {
pabort("can't set bits per word");
}
ret = ioctl(fd, SPI_IOC_RD_BITS_PER_WORD, &bits);
if (ret == -1) {
pabort("can't get bits per word");
}
// max speed Hz
ret = ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
if (ret == -1) {
pabort("can't set max speed hz");
}
ret = ioctl(fd, SPI_IOC_RD_MAX_SPEED_HZ, &speed);
if (ret == -1) {
pabort("can't get max speed hz");
}
printf("spi mode: %d\n", mode);
printf("bits per word: %d\n", bits);
printf("max speed: %d Hz (%d KHz)\n", speed, speed/1000);
return fd;
}
void SPIClose(int *fd_address){
close(*fd_address);
}
uint8_t SPI_ReadOneByte(int fd, uint8_t reg){
int8_t *tx_buffer;
int8_t rx_buffer[2]={0,0};
uint8_t readBuf;
int ret;
tx_buffer = (char *)calloc(count, sizeof(char));
if (tx_buffer == 0) {
pabort("failed to malloc tx_buffer");
}
tx_buffer[0] = 0x80 | reg; // setting msb to 1 makes this a "read" operation
struct spi_ioc_transfer tr = {
.tx_buf = (unsigned long)tx_buffer,
.rx_buf = (unsigned long)&rx_buffer,
.len = count,
.delay_usecs = delay,
.speed_hz = speed,
.bits_per_word = bits,
};
ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr);
if (ret < 1) {
pabort("can't send spi message");
}
free(tx_buffer);
readBuf = rx_buffer[1];//注意spi的特性，数据传输总是双向的，写入寄存器地址后返回的数据是无效的，第二个返回的字节才是有效数据
return readBuf;
}
//int fd：SPI设备的文件描述符，用于在Linux系统中表示打开的SPI设备
//uint8_t reg：要写入的寄存器地址
//uint8_t value：要写入寄存器的值
void SPI_WriteOneByte(int fd, uint8_t reg, uint8_t value){
int8_t *tx_buffer;
int8_t rx_buffer[2]={0,0};
int ret;
tx_buffer = (char *)calloc(count, sizeof(char));
if (tx_buffer == 0) {
pabort("failed to malloc tx_buffer");
}
tx_buffer[0] = reg; // 最高位设置为0表示接下来进行写操作
tx_buffer[1] = value;
//定义一个 spi_ioc_transfer 结构体 tr，用于设置SPI传输的具体参数
struct spi_ioc_transfer tr = {
.tx_buf = (unsigned long)tx_buffer,
.rx_buf = (unsigned long)&rx_buffer,
.len = count,
.delay_usecs = delay,
.speed_hz = speed,
.bits_per_word = bits,
};
//使用 ioctl 系统调用来执行SPI消息传输。SPI_IOC_MESSAGE(1) 指定要传输的消息数量
ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr);
if (ret < 1) {
pabort("can't write spi message");
}
free(tx_buffer);
return;
}
#ifdef __cplusplus
}
#endif
spi.h：
#include <stdint.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <getopt.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/types.h>
#include <linux/spi/spidev.h>
#include <stddef.h>
#include <math.h>
#include <sys/time.h>
typedef uint8_t state_check;
#define true 1
#define false 0
#ifdef __cplusplus
extern "C" {
#endif
int SPIInit(int spi_num,int cs_num);
void SPIClose(int *fd_address);
uint8_t SPI_ReadOneByte(int fd, uint8_t reg);
void SPI_WriteOneByte(int fd, uint8_t reg,uint8_t value);
#ifdef __cplusplus
}
#endif
代码重要的注释都有，需要注意的是：
spi_ioc_transfer 结构体来源于Linux内核对SPI（串行外设接口）设备通信的API定义。这个结构体用于描述单个SPI传输的特性，并且被用户空间程序用来通过ioctl系统调用来执行SPI数据传输 。
SPI_IOC_MESSAGE(1) 是一个用于Linux内核中SPI设备通信的ioctl命令。这个命令允许用户空间程序通过一个或多个spi_ioc_transfer结构体来执行SPI数据传输，这些传输操作会作为一个原子序列一起执行。
③编写icm40609d.cpp、icm40609d.hpp，封装传感器初始化、传感器数据读取等函数。
icm40609d.cpp:
#include "ICM40609D.hpp"
ICM40609D::ICM40609D(){
time_interval=0.005f;
gyroX_filtered = 0;
gyroY_filtered = 0;
gyroZ_filtered = 0;
ex_i = 0;
ey_i = 0;
ez_i = 0;
Kp=3.0;
Ki=0.5;
}
void ICM40609D::SPIinit(int spinum,int csnum, uint8_t aMode, uint8_t gMode, uint8_t Ascale,uint8_t Gscale,uint8_t AODR,uint8_t GODR)
{
fd = SPIInit(spinum,csnum);
fd_address=&fd;
gstGyroOffset ={0,0,0};
ICM40609D::Init(aMode,gMode, Ascale,Gscale,AODR,GODR);
}
state_check ICM40609D::Check(void)
{
state_check bRet = false;
if(VAL_WHO_AM_I == SPI_ReadOneByte(fd,ICM40609D_WHO_AM_I))
{
SPI_WriteOneByte(fd,ICM40609D_REG_BANK_SEL, 0x00);
bRet=true;
}
return bRet;
}
void ICM40609D::Init(uint8_t aMode, uint8_t gMode, uint8_t Ascale,uint8_t Gscale,uint8_t AODR,uint8_t GODR)
{
SPI_WriteOneByte(fd,ICM40609D_DEVICE_CONFIG, 0x01); // reset
usleep(1000);
state_check bRet = ICM40609D::Check();
if( true == bRet)
{
std::cout<< "Motion sersor is ICM-40609D\n"<<std::endl;
}
else
{
std::cout<<"Motion sersor NULL\n"<<std::endl;
}
SPI_WriteOneByte(fd, ICM40609D_PWR_MGMT0, gMode << 2 | aMode); // set accel and gyro modes
SPI_WriteOneByte(fd,ICM40609D_ACCEL_CONFIG0, Ascale << 5 | AODR); // set accel ODR and FS
SPI_WriteOneByte(fd,ICM40609D_GYRO_CONFIG0, Gscale << 5 | GODR); // set gyro ODR and FS
SPI_WriteOneByte(fd,ICM40609D_GYRO_ACCEL_CONFIG0, 0x44); // set gyro and accel bandwidth to ODR/10
switch (Gscale)
{
// Possible gyro scales (and their register bit settings) are:
case GFS_15_625DPS:
Gscale_factor=GFS_15_625DPS_SCALE_FACTOR;
break;
case GFS_31_25DPS:
Gscale_factor=GFS_31_25DPS_SCALE_FACTOR;
break;
case GFS_62_50DPS:
Gscale_factor=GFS_62_50DPS_SCALE_FACTOR;
break;
case GFS_125DPS:
Gscale_factor=GFS_125DPS_SCALE_FACTOR;
break;
case GFS_250DPS:
Gscale_factor=GFS_250DPS_SCALE_FACTOR;
break;
case GFS_500DPS:
Gscale_factor=GFS_500DPS_SCALE_FACTOR;
break;
case GFS_1000DPS:
Gscale_factor=GFS_1000DPS_SCALE_FACTOR;
break;
case GFS_2000DPS:
Gscale_factor=GFS_2000DPS_SCALE_FACTOR;
break;
}
switch (Ascale)
{
// Possible accelerometer scales (and their register bit settings) are:
case AFS_4G:
Ascale_factor= AFS_4G_SCALE_FACTOR;
break;
case AFS_8G:
Ascale_factor= AFS_8G_SCALE_FACTOR;
break;
case AFS_16G:
Ascale_factor= AFS_16G_SCALE_FACTOR;
break;
case AFS_32G:
Ascale_factor= AFS_32G_SCALE_FACTOR;
break;
}
usleep(5000);
return;
}
void ICM40609D::Close(void)
{
SPIClose(fd_address);
std::cout<< "imu sensor close"<<std::endl;
}
int16_t ICM40609D::ReadAxisData(uint8_t regLow, uint8_t regHigh) {
uint8_t lowByte = SPI_ReadOneByte(fd, regLow);
uint8_t highByte = SPI_ReadOneByte(fd, regHigh);
return (static_cast<int16_t>(highByte) << 8) | lowByte;
}
void ICM40609D::AccelRead(int16_t& s16X, int16_t& s16Y, int16_t& s16Z) {
s16X = ReadAxisData(ICM40609D_ACCEL_DATA_X0, ICM40609D_ACCEL_DATA_X1);
s16Y = ReadAxisData(ICM40609D_ACCEL_DATA_Y0, ICM40609D_ACCEL_DATA_Y1);
s16Z = ReadAxisData(ICM40609D_ACCEL_DATA_Z0, ICM40609D_ACCEL_DATA_Z1);
}
void ICM40609D::GyroRead(int16_t& s16X, int16_t& s16Y, int16_t& s16Z) {
s16X = ReadAxisData(ICM40609D_GYRO_DATA_X0, ICM40609D_GYRO_DATA_X1);
s16Y = ReadAxisData(ICM40609D_GYRO_DATA_Y0, ICM40609D_GYRO_DATA_Y1);
s16Z = ReadAxisData(ICM40609D_GYRO_DATA_Z0, ICM40609D_GYRO_DATA_Z1);
}
void ICM40609D::TempRead(int16_t& s16Temp)
{
s16Temp = ReadAxisData(ICM40609D_TEMP_DATA0, ICM40609D_TEMP_DATA1);
}
void ICM40609D::GyroOffset(void)
{
uint8_t i;
int16_t s16Gx = 0, s16Gy = 0, s16Gz = 0;
int32_t s32TempGx = 0, s32TempGy = 0, s32TempGz = 0;
for(i = 0; i < 1000; i ++)
{
ICM40609D::GyroRead(s16Gx, s16Gy, s16Gz);
s32TempGx += s16Gx;
s32TempGy += s16Gy;
s32TempGz += s16Gz;
usleep(5000);
}
gstGyroOffset.s16X = s32TempGx / 1000; //等价 s32TempGx / 64
gstGyroOffset.s16Y = s32TempGy / 1000;
gstGyroOffset.s16Z = s32TempGz / 1000;
int16_t regOffsetX = gstGyroOffset.s16X * 32;
int16_t regOffsetY = gstGyroOffset.s16Y * 32;
int16_t regOffsetZ = gstGyroOffset.s16Z * 32;
return;
}
void ICM40609D::imuDataGet()
{
static int count = 0;
float MotionVal[6];
int16_t s16Gyro[3], s16Accel[3], s16Temp[1];;
if (count < 1) {
GyroOffset();
count++;
}
ICM40609D::AccelRead(s16Accel[0], s16Accel[1], s16Accel[2]);
ICM40609D::GyroRead(s16Gyro[0], s16Gyro[1], s16Gyro[2]);
ICM40609D::TempRead(s16Temp[0]);
stAccelRawData.s16X = (s16Accel[0] * 9.801) / Ascale_factor;
stAccelRawData.s16Y = (s16Accel[1] * 9.801) / Ascale_factor;
stAccelRawData.s16Z = (s16Accel[2] * 9.801) / Ascale_factor;
gyroX_filtered = ALPHA * (s16Gyro[0] - gstGyroOffset.s16X) / Gscale_factor + (1 - ALPHA) * gyroX_filtered;
gyroY_filtered = ALPHA * (s16Gyro[1] - gstGyroOffset.s16Y) / Gscale_factor + (1 - ALPHA) * gyroY_filtered;
gyroZ_filtered = ALPHA * (s16Gyro[2] - gstGyroOffset.s16Z) / Gscale_factor + (1 - ALPHA) * gyroZ_filtered;
stGyroRawData.s16X = gyroX_filtered;
stGyroRawData.s16Y = gyroY_filtered;
stGyroRawData.s16Z = gyroZ_filtered;
stTempRawData.s16Temp = s16Temp[0] / 132.48 + 25;
}
icm40609d.hpp:
#include "IMU_spi.h"
#include <iostream>
#include <thread>
#define VAL_WHO_AM_I 0x3B
// User Bank 0
#define ICM40609D_DEVICE_CONFIG 0x11
#define ICM40609D_DRIVE_CONFIG 0x13
#define ICM40609D_INT_CONFIG 0x14
#define ICM40609D_FIFO_CONFIG 0x16
#define ICM40609D_TEMP_DATA1 0x1D
#define ICM40609D_TEMP_DATA0 0x1E
#define ICM40609D_ACCEL_DATA_X1 0x1F
#define ICM40609D_ACCEL_DATA_X0 0x20
#define ICM40609D_ACCEL_DATA_Y1 0x21
#define ICM40609D_ACCEL_DATA_Y0 0x22
#define ICM40609D_ACCEL_DATA_Z1 0x23
#define ICM40609D_ACCEL_DATA_Z0 0x24
#define ICM40609D_GYRO_DATA_X1 0x25
#define ICM40609D_GYRO_DATA_X0 0x26
#define ICM40609D_GYRO_DATA_Y1 0x27
#define ICM40609D_GYRO_DATA_Y0 0x28
#define ICM40609D_GYRO_DATA_Z1 0x29
#define ICM40609D_GYRO_DATA_Z0 0x2A
#define ICM40609D_TMST_FSYNCH 0x2B
#define ICM40609D_TMST_FSYNCL 0x2C
#define ICM40609D_INT_STATUS 0x2D
#define ICM40609D_FIFO_COUNTH 0x2E
#define ICM40609D_FIFO_COUNTL 0x2F
#define ICM40609D_FIFO_DATA 0x30
#define ICM40609D_APEX_DATA0 0x31
#define ICM40609D_APEX_DATA1 0x32
#define ICM40609D_APEX_DATA2 0x33
#define ICM40609D_APEX_DATA3 0x34
#define ICM40609D_APEX_DATA4 0x35
#define ICM40609D_APEX_DATA5 0x36
#define ICM40609D_INT_STATUS2 0x37
#define ICM40609D_INT_STATUS3 0x38
#define ICM40609D_SIGNAL_PATH_RESET 0x4B
#define ICM40609D_INTF_CONFIG0 0x4C
#define ICM40609D_INTF_CONFIG1 0x4D
#define ICM40609D_PWR_MGMT0 0x4E
#define ICM40609D_GYRO_CONFIG0 0x4F
#define ICM40609D_ACCEL_CONFIG0 0x50
#define ICM40609D_GYRO_CONFIG1 0x51
#define ICM40609D_GYRO_ACCEL_CONFIG0 0x52
#define ICM40609D_ACCEL_CONFIG1 0x53
#define ICM40609D_TMST_CONFIG 0x54
#define ICM40609D_APEX_CONFIG0 0x56
#define ICM40609D_SMD_CONFIG 0x57
#define ICM40609D_FIFO_CONFIG1 0x5F
#define ICM40609D_FIFO_CONFIG2 0x60
#define ICM40609D_FIFO_CONFIG3 0x61
#define ICM40609D_FSYNC_CONFIG 0x62
#define ICM40609D_INT_CONFIG0 0x63
#define ICM40609D_INT_CONFIG1 0x64
#define ICM40609D_INT_SOURCE0 0x65
#define ICM40609D_INT_SOURCE1 0x66
#define ICM40609D_INT_SOURCE3 0x68
#define ICM40609D_INT_SOURCE4 0x69
#define ICM40609D_FIFO_LOST_PKT0 0x6C
#define ICM40609D_FIFO_LOST_PKT1 0x6D
#define ICM40609D_SELF_TEST_CONFIG 0x70
#define ICM40609D_WHO_AM_I 0x75 // should return 0x47
#define ICM40609D_REG_BANK_SEL 0x76
#define AFS_4G 0x03 // 011: ±4g
#define AFS_8G 0x02 // 010: ±8g
#define AFS_16G 0x01 // 001: ±16g
#define AFS_32G 0x00 // 000: ±32g (default)
#define GFS_2000DPS 0x00 // default
#define GFS_1000DPS 0x01
#define GFS_500DPS 0x02
#define GFS_250DPS 0x03
#define GFS_125DPS 0x04
#define GFS_62_50DPS 0x05
#define GFS_31_25DPS 0x06
#define GFS_15_625DPS 0x07
// Low Noise mode
#define AODR_32kHz 0x01
#define AODR_16kHz 0x02
#define AODR_8kHz 0x03
#define AODR_4kHz 0x04
#define AODR_2kHz 0x05
#define AODR_1kHz 0x06 // default
//Low Noise or Low Power modes
#define AODR_500Hz 0x0F
#define AODR_200Hz 0x07
#define AODR_100Hz 0x08
#define AODR_50Hz 0x09
#define AODR_25Hz 0x0A
#define AODR_12_5Hz 0x0B
// Low Power mode
#define AODR_6_25Hz 0x0C
#define AODR_3_125Hz 0x0D
#define AODR_1_5625Hz 0x0E
#define GODR_32kHz 0x01
#define GODR_16kHz 0x02
#define GODR_8kHz 0x03
#define GODR_4kHz 0x04
#define GODR_2kHz 0x05
#define GODR_1kHz 0x06 // default
#define GODR_500Hz 0x0F
#define GODR_200Hz 0x07
#define GODR_100Hz 0x08
#define GODR_50Hz 0x09
#define GODR_25Hz 0x0A
#define GODR_12_5Hz 0x0B
#define aMode_OFF 0x01
#define aMode_LP 0x02
#define aMode_LN 0x03
#define gMode_OFF 0x00
#define gMode_SBY 0x01
#define gMode_LN 0x03
#define AFS_4G_SCALE_FACTOR 8192.0f
#define AFS_8G_SCALE_FACTOR 4096.0f
#define AFS_16G_SCALE_FACTOR 2048.0f
#define AFS_32G_SCALE_FACTOR 1024.0f
#define GFS_2000DPS_SCALE_FACTOR 16.4
#define GFS_1000DPS_SCALE_FACTOR 32.8
#define GFS_500DPS_SCALE_FACTOR 65.5
#define GFS_250DPS_SCALE_FACTOR 131.0f
#define GFS_125DPS_SCALE_FACTOR 262.0f
#define GFS_62_50DPS_SCALE_FACTOR 524.3
#define GFS_31_25DPS_SCALE_FACTOR 1048.6
#define GFS_15_625DPS_SCALE_FACTOR 2097.2
typedef struct imu_st_angles_data_tag
{
float fYaw;
float fPitch;
float fRoll;
}IMU_ST_ANGLES_DATA;
typedef struct imu_st_sensor_data_tag
{
float s16X;
float s16Y;
float s16Z;
}IMU_ST_SENSOR_DATA;
typedef struct imu_st_Temp_data_tag
{
float s16Temp;
}IMU_ST_Temp_DATA;
class ICM40609D
{
public:
ICM40609D();
void imuDataGet();
void SPIinit(int spinum,int csnum, uint8_t aMode, uint8_t gMode, uint8_t Ascale,uint8_t Gscale,uint8_t AODR,uint8_t GODR);
void Close(void);
std::thread imuDataGet_thread(void);
float time_interval;
float ex_i;
float ey_i;
float ez_i;
float Kp;
float Ki;
float gyroX_filtered;
float gyroY_filtered;
float gyroZ_filtered;
static constexpr float ALPHA = 0.1;
IMU_ST_QUATERNION_DATA stQuaternion;
IMU_ST_SENSOR_DATA stGyroRawData;
IMU_ST_SENSOR_DATA stAccelRawData;
IMU_ST_Temp_DATA stTempRawData;
private:
state_check Check(void);
void Init(uint8_t aMode, uint8_t gMode, uint8_t Ascale,uint8_t Gscale,uint8_t AODR,uint8_t GODR);
void GyroRead(int16_t& s16X, int16_t& s16Y, int16_t& s16Z);
void AccelRead(int16_t& s16X, int16_t& s16Y, int16_t& s16Z);
int16_t ReadAxisData(uint8_t regLow, uint8_t regHigh);
void TempRead(int16_t& s16Temp);
void GyroOffset(void);
void mahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az);
float Ascale_factor;
float Gscale_factor;
IMU_ST_SENSOR_DATA gstGyroOffset;
int fd;
int *fd_address;
};
上面的代码也比较简单，就实现了加速度和陀螺仪数据的读取和转换。只需要再写一个cpp文件，就可以调用这些封装了。
下面是大致流程：
三、ros1发布imu数据
在ros中测试上面的代码。
3.1、创建ros1工作空间
下面给出具体创建流程：
①mkdir catkin_ws
②cd catkin_ws
③mkdir src
④cd src
⑤catkin_init_workspace //在src中生成CmakeLists.txt
⑥cd …
⑦catkin_make //编译，会生成build和devel文件夹，编译源码生产的可执行文件会放入devel/lib中。
⑧Catkin_make install //生成install空间
至此工作空间创建完毕，这样就可以在工作空间中创建功能包了：必须创建在src中，并且在同一个工作空间中不允许同名的功能包：
下面就是功能包创建完成之后里面包含的内容：
3.2、数据发布节点代码
#include "ros/ros.h"
#include "ICM40609D.hpp"
#include <iostream>
#include <sensor_msgs/Imu.h>
int main(int argc, char **argv) {
ros::init(argc, argv, "imu_test_node");// 初始化ROS，设置节点名称为"imu_test_node"。
ros::NodeHandle nh;// 创建节点句柄。管理ros API资源，比如后面的nh.advertise
// 创建一个IMU数据发布者
ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("/data", 10);//设置了一个名为imu_pub的发布者，用于在/data主题上发布sensor_msgs::Imu类型的消息，消息队列大小为10。
// 设置循环频率
ros::Rate loop_rate(10);
ICM40609D imu1;
imu1.SPIinit(1,0,aMode_LN,gMode_LN,AFS_32G,GFS_31_25DPS,AODR_1kHz,GODR_1kHz);
while (ros::ok()) {
sensor_msgs::Imu imu_msg;
imu1.imuDataGet();
// 填充IMU数据
imu_msg.header.stamp = ros::Time::now();
imu_msg.header.frame_id = "imu_frame";
// 设置其他IMU数据字段
imu_msg.angular_velocity.x = imu1.stGyroRawData.s16X;
imu_msg.angular_velocity.y = imu1.stGyroRawData.s16Y;
imu_msg.angular_velocity.z = imu1.stGyroRawData.s16Z;
imu_msg.linear_acceleration.x = imu1.stAccelRawData.s16X;
imu_msg.linear_acceleration.y = imu1.stAccelRawData.s16Y;
imu_msg.linear_acceleration.z = imu1.stAccelRawData.s16Z;
// 发布IMU数据
imu_pub.publish(imu_msg);//在"/data"主题上发布IMU数据。
ros::spinOnce();
loop_rate.sleep();
}
imu1.Close();
return 0;
}
***在CmakeLists.txt文件中加入以下两行代码（cpp源代码需要放在功能包的src目录下）：
***运行之前先把环境变量设置一下·，以免找不到工作空间。或者直接添加进.bashrc的最后一行：source/home/sunrise/Documents/workspace/catkin_ws/devel/setup.bash
执行ros节点的命令：
①开启节点控制器：roscore
②开始运行发布节点：rosrun ICM40609D imu_test_node
至此测试和发布数据节点全部完成。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
