旭日x3开发板测试IMU40609D（六轴传感器），包含ros节点发布

最新推荐文章于 2024-01-19 02:09:33 发布
最新推荐文章于 2024-01-19 02:09:33 发布
阅读量571 收藏 10
点赞数 11
文章标签： c++ 机器人 功能测试
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/m0_71523511/article/details/135644194
版权

旭日x3开发板测试IMU40609D（六轴传感器），包含ros节点发布

最终数据

在这里插入图片描述

1、环境安装

可以根据指导文档进行搭建：[https://developer.horizon.cc/documents_rdk/category/linux_development](https://developer.horizon.cc/documents_rdk/category/linux_development)

有一些地方需要注意：
在这里插入图片描述

2、测试代码

2.1、imu_test.cpp文件main函数：
#include "ICM40609D.hpp"
#include <iostream>



int main(int argc, char* argv[]){
    ICM40609D imu1;
    
    imu1.SPIinit(1,0,aMode_LN,gMode_LN,AFS_32G,GFS_31_25DPS,AODR_1kHz,GODR_1kHz);
    long start = micro_time();
    
    long end = micro_time();
    for (int i=0;i<1000;++i){
    imu1.imuDataGet();
    std::cout<<"Accel_X:"<<imu1.stAccelRawData.s16X<< "\tAccel_Y:"<<imu1.stAccelRawData.s16Y<<"\tAccel_Z:" << imu1.stAccelRawData.s16Z<<std::endl;
    std::cout<<"gyro_X:"<<imu1.stGyroRawData.s16X<< "\tgyro_Y:"<<imu1.stGyroRawData.s16Y<<"\tgyro_Z:" << imu1.stGyroRawData.s16Z<<std::endl;
    usleep(2000);
    }
    imu1.Close();

}

下面对上面的步骤进行简单描述：
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

2.2、ICM40609D.cpp
#include "ICM40609D.hpp"
uint64_t micro_time() {
    struct timeval tv;
    gettimeofday(&tv, NULL);
    return tv.tv_sec * (uint64_t)1000000 + tv.tv_usec;
}
ICM40609D::ICM40609D(){

    time_interval=0.005f;
    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}; 
    state_check bRet = ICM40609D::Check();
  
    if( true == bRet)
    {
        std::cout<< "Motion sersor is ICM-40609D\n"<<std::endl;
        ICM40609D::Init(aMode,gMode, Ascale,Gscale,AODR,GODR);
    }
    else
    {
     
        std::cout<<    "Motion sersor NULL\n"<<std::endl;
    }
    q0 = 1.0f;  
    q1 = 0.0f;
    q2 = 0.0f;
    q3 = 0.0f;

}
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_REG_BANK_SEL, 0x00); // select register bank 0
    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);

    /* offset */
    // bais 会随证白噪声的积分发生改变 因此这里计算的偏置感觉意义不大？
    /*
    ICM40609D::GyroOffset();
    */
    return;
}

void ICM40609D::Close(void)
{
    SPIClose(fd_address);
    std::cout<<    "imu sensor close"<<std::endl;
}

void ICM40609D::GyroRead(int16_t& s16X, int16_t& s16Y, int16_t& s16Z)
{
    uint8_t u8Buf[2];
    int16_t s16Buf[3] = {0};

    u8Buf[0]=SPI_ReadOneByte(fd,ICM40609D_GYRO_DATA_X0);
    u8Buf[1]=SPI_ReadOneByte(fd,ICM40609D_GYRO_DATA_X1);

    s16Buf[0]=  (u8Buf[1]<<8)|u8Buf[0];

    u8Buf[0]=SPI_ReadOneByte(fd,ICM40609D_GYRO_DATA_Y0);
    u8Buf[1]=SPI_ReadOneByte(fd,ICM40609D_GYRO_DATA_Y1);
    s16Buf[1]=  (u8Buf[1]<<8)|u8Buf[0];

    u8Buf[0]=SPI_ReadOneByte(fd,ICM40609D_GYRO_DATA_Z0);
    u8Buf[1]=SPI_ReadOneByte(fd,ICM40609D_GYRO_DATA_Z1);
    s16Buf[2]=  (u8Buf[1]<<8)|u8Buf[0];


    // s16X = s16Buf[0] - gstGyroOffset.s16X;
    // s16Y = s16Buf[1] - gstGyroOffset.s16Y;
    // s16Z = s16Buf[2] - gstGyroOffset.s16Z;

    s16X = s16Buf[0];
    s16Y = s16Buf[1];
    s16Z = s16Buf[2];

    return;
}
void ICM40609D::AccelRead(int16_t& s16X, int16_t& s16Y, int16_t& s16Z)
{
    uint8_t u8Buf[2];
    int16_t s16Buf[3] = {0};

    u8Buf[0]=SPI_ReadOneByte(fd,ICM40609D_ACCEL_DATA_X0);
    u8Buf[1]=SPI_ReadOneByte(fd,ICM40609D_ACCEL_DATA_X1);

    s16Buf[0]=  (u8Buf[1]<<8)|u8Buf[0];

    u8Buf[0]=SPI_ReadOneByte(fd,ICM40609D_ACCEL_DATA_Y0);
    u8Buf[1]=SPI_ReadOneByte(fd,ICM40609D_ACCEL_DATA_Y1);
    s16Buf[1]=  (u8Buf[1]<<8)|u8Buf[0];

    u8Buf[0]=SPI_ReadOneByte(fd,ICM40609D_ACCEL_DATA_Z0);
    u8Buf[1]=SPI_ReadOneByte(fd,ICM40609D_ACCEL_DATA_Z1);
    s16Buf[2]=  (u8Buf[1]<<8)|u8Buf[0];


    s16X = s16Buf[0];
    s16Y = s16Buf[1];
    s16Z = s16Buf[2];

    return;
}

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 < 64; i ++)
  {
    ICM40609D::GyroRead(s16Gx, s16Gy, s16Gz);
    s32TempGx += s16Gx;
    s32TempGy += s16Gy;
    s32TempGz += s16Gz;
    usleep(5000);
  }

  gstGyroOffset.s16X = s32TempGx >> 6; //等价 s32TempGx / 64
  gstGyroOffset.s16Y = s32TempGy >> 6;
  gstGyroOffset.s16Z = s32TempGz >> 6;

  return;
}

void ICM40609D::imuDataGet()
{

    float MotionVal[6];
    int16_t s16Gyro[3], s16Accel[3];
    ICM40609D::AccelRead(s16Accel[0], s16Accel[1], s16Accel[2]);

    ICM40609D::GyroRead(s16Gyro[0], s16Gyro[1], s16Gyro[2]);

    /*
    MotionVal[0]=(float)s16Gyro[0]/Gscale_factor;
    MotionVal[1]=(float)s16Gyro[1]/Gscale_factor;
    MotionVal[2]=(float)s16Gyro[2]/Gscale_factor;
    MotionVal[3]=s16Accel[0];
    MotionVal[4]=s16Accel[1];
    MotionVal[5]=s16Accel[2];
    */

    /* 计算旋转四元数 */
    /*
    mahonyAHRSupdate((float)MotionVal[0] * 0.01745, (float)MotionVal[1] * 0.01745, (float)MotionVal[2] * 0.01745,
                    (float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5]);
    stQuaternion.q0=q0;
    stQuaternion.q1=q1;
    stQuaternion.q2=q2;
    stQuaternion.q3=q3;
    */

    /* 计算姿态角 */
    /*
    pstAngles.fPitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.295; // pitch
    pstAngles.fRoll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.295; // roll
    pstAngles.fYaw = atan2(-2 * q1 * q2 - 2 * q0 * q3, 2 * q2 * q2 + 2 * q3 * q3 - 1) * 57.295;
    */
    stAccelRawData.s16X = (s16Accel[0]  * 9.801) / Ascale_factor;
    stAccelRawData.s16Y = (s16Accel[1]  * 9.801) / Ascale_factor;
    stAccelRawData.s16Z = (s16Accel[2]  * 9.801) / Ascale_factor;

    stGyroRawData.s16X = s16Gyro[0] / Gscale_factor;
    stGyroRawData.s16Y = s16Gyro[1] / Gscale_factor;
    stGyroRawData.s16Z = s16Gyro[2] / Gscale_factor;
}

void ICM40609D::mahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az){
    float Norm;
	float vx, vy, vz;
	float ex, ey, ez;
	float qa, qb, qc;
        
	// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
	if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

		// Normalise accelerometer measurement
		Norm = invSqrt(ax * ax + ay * ay + az * az);
		ax *= Norm;
		ay *= Norm;
		az *= Norm;        

		// Estimated direction of gravity and vector perpendicular to magnetic flux
		vx = 2*(q1 * q3 - q0 * q2);
		vy = 2*(q0 * q1 + q2 * q3);
		vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
	
		// Error is sum of cross product between estimated and measured direction of gravity
		ex = (ay * vz - az * vy);
		ey = (az * vx - ax * vz);
		ez = (ax * vy - ay * vx);
    ex_i += ex*time_interval;
    ey_i += ey*time_interval;
    ez_i += ez*time_interval;

    gx += Kp * ex + Ki * ex_i;
    gy += Kp * ey + Ki * ey_i;
    gz += Kp * ez + Ki * ez_i;

	}
	

	qa = q0;
	qb = q1;
	qc = q2;
 
	q0 += 0.5f*(-qb * gx - qc * gy - q3 * gz)*time_interval;
	q1 += 0.5f*(qa * gx + qc * gz - q3 * gy)*time_interval;
	q2 += 0.5f*(qa * gy - qb * gz + q3 * gx)*time_interval;
	q3 += 0.5f*(qa * gz + qb * gy - qc * gx)*time_interval; 
	
	// Normalise quaternion
	Norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
	q0 *= Norm;
	q1 *= Norm;
	q2 *= Norm;
	q3 *= Norm;

}
/*
std::thread ICM40609D::imuDataGet_thread(void){
    return std::thread([this]{imuDataGet();});
}
*/
2.3、ICM40609D.h
#include "IMU_spi.h"
#include <iostream>
#include <thread>
uint64_t micro_time();
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 q0,q1,q2,q3;
        IMU_ST_QUATERNION_DATA stQuaternion;
        IMU_ST_SENSOR_DATA stGyroRawData;
        IMU_ST_SENSOR_DATA stAccelRawData;
    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);
        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;

};
2.4、IMU_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;

static void pabort(const char *s) {
	perror(s);
	abort();
}
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];

	return readBuf;
}
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; // setting msb to 0 makes this a "write" operation
	tx_buffer[1] = value;
	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 write spi message");
	}

	free(tx_buffer);
	return;
}



float invSqrt(float x){
  float halfx = 0.5f * x;
  float y = x;
  
  long i = *(long*)&y;                //get bits for floating value
  i = 0x5f3759df - (i >> 1);          //gives initial guss you
  y = *(float*)&i;                    //convert bits back to float
  y = y * (1.5f - (halfx * y * y));   //newtop step, repeating increases accuracy
  
  return y;
}
#ifdef __cplusplus
}
#endif
2.5、IMU_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
#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

// User Bank 1
#define ICM40609D_SENSOR_CONFIG0            0x03
#define ICM40609D_GYRO_CONFIG_STATIC2       0x0B
#define ICM40609D_GYRO_CONFIG_STATIC3       0x0C
#define ICM40609D_GYRO_CONFIG_STATIC4       0x0D
#define ICM40609D_GYRO_CONFIG_STATIC5       0x0E
#define ICM40609D_GYRO_CONFIG_STATIC6       0x0F
#define ICM40609D_GYRO_CONFIG_STATIC7       0x10
#define ICM40609D_GYRO_CONFIG_STATIC8       0x11
#define ICM40609D_GYRO_CONFIG_STATIC9       0x12
#define ICM40609D_GYRO_CONFIG_STATIC10      0x13
#define ICM40609D_XG_ST_DATA                0x5F
#define ICM40609D_YG_ST_DATA                0x60
#define ICM40609D_ZG_ST_DATA                0x61
#define ICM40609D_TMSTAL0                   0x63
#define ICM40609D_TMSTAL1                   0x64
#define ICM40609D_TMSTAL2                   0x62
#define ICM40609D_INTF_CONFIG4              0x7A
#define ICM40609D_INTF_CONFIG5              0x7B
#define ICM40609D_INTF_CONFIG6              0x7C

// User Bank 2
#define ICM40609D_ACCEL_CONFIG_STATIC2      0x03
#define ICM40609D_ACCEL_CONFIG_STATIC3      0x04
#define ICM40609D_ACCEL_CONFIG_STATIC4      0x05
#define ICM40609D_XA_ST_DATA                0x3B
#define ICM40609D_YA_ST_DATA                0x3C
#define ICM40609D_ZA_ST_DATA                0x3D

// User Bank 4
#define ICM40609D_APEX_CONFIG1              0x40
#define ICM40609D_APEX_CONFIG2              0x41
#define ICM40609D_APEX_CONFIG3              0x42
#define ICM40609D_APEX_CONFIG4              0x43
#define ICM40609D_APEX_CONFIG5              0x44
#define ICM40609D_APEX_CONFIG6              0x45
#define ICM40609D_APEX_CONFIG7              0x46
#define ICM40609D_APEX_CONFIG8              0x47
#define ICM40609D_APEX_CONFIG9              0x48
#define ICM40609D_ACCEL_WOM_X_THR           0x4A
#define ICM40609D_ACCEL_WOM_Y_THR           0x4B
#define ICM40609D_ACCEL_WOM_Z_THR           0x4C
#define ICM40609D_INT_SOURCE6               0x4D
#define ICM40609D_INT_SOURCE7               0x4E
#define ICM40609D_INT_SOURCE8               0x4F
#define ICM40609D_INT_SOURCE9               0x50
#define ICM40609D_INT_SOURCE10              0x51
#define ICM40609D_OFFSET_USER0              0x77
#define ICM40609D_OFFSET_USER1              0x78
#define ICM40609D_OFFSET_USER2              0x79
#define ICM40609D_OFFSET_USER3              0x7A
#define ICM40609D_OFFSET_USER4              0x7B
#define ICM40609D_OFFSET_USER5              0x7C
#define ICM40609D_OFFSET_USER6              0x7D
#define ICM40609D_OFFSET_USER7              0x7E
#define ICM40609D_OFFSET_USER8              0x7F

#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




#ifdef __cplusplus
extern "C" {
#endif



typedef struct imu_st_quaternion_data_tag{
    float q0;
    float q1;
    float q2;
    float q3;
}IMU_ST_QUATERNION_DATA;

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;


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);
float invSqrt(float x);
#ifdef __cplusplus
}
#endif

3、ros发布数据节点

将上面的imu_test.cpp进行更改，使其成为ros中的一个节点，将读取到的传感器数据发布至话题：/data，供其它节点使用。

3.1、创建ros工作空间

在这里插入图片描述
下面给出具体创建流程：
①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;
}

***在cpp文件写好之后要在CmakeLists.txt文件中加入以下两行代码（cpp源代码需要放在功能包的src目录下）：
在这里插入图片描述
***运行之前先把环境变量设置一下·，以免找不到工作空间。或者直接添加进.bashrc的最后一行：source/home/sunrise/Documents/workspace/catkin_ws/devel/setup.bash
在这里插入图片描述
执行ros节点的命令：
①开启节点控制器：roscore
②开始运行发布节点：rosrun ICM40609D imu_test_node

至此测试和发布数据节点全部完成。

geewonii️
关注
  • 11
    点赞
  • 踩
  • 10
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
陀螺仪ICM-40607
08-02
Invensene的mems陀螺仪。可用于手机，手表等产品。 The ICM-40607-P is a high performance 6-axis MEMS MotionTracking device that combines a 3-axis gyroscope and a 3-axis accelerometer. It has a configurable host interface that supports I3CSM, I2C, and SPI serial communication, features up to 2.25 Kbytes FIFO and 2 programmable interrupts with ultra-low-power wake-on-motion support to minimize system power consumption.
icm---条件迭代算法
10-24
----icm---条件迭代算法，条件迭代算法基于MRF----ICM----
2012美赛数学建模ICM-C题附件
01-27
如果你想找有没找到2012 ICM C题的附件内容，你可以下载这个来试试，会有意外惊喜的
icm-42605 v0.3.pdf
10-12
ICM42605寄存器手册，icm42605 datasheet，icm42605开发文档，完整版，i2c时序
ICM-42605.pdf
07-24
TDK的ICM-42605传感器的datasheet, 帮助学习这个IC。
ICM-40609D DS-000272 v0.8.pdf
10-10
ICM-40609D DS-000272 v0.8中文完整技术手册，ICM-40609D DS-000272 v0.8中文完整技术手册
ArduPilot开源代码之AP_InertialSensor_Backend
lida2003的专栏
06-18 777
初始化：probe/init/start (被AP_Vehicle::setup调用)数据更新：read_fifo (被start中定时函数调用)API接口：update (被AP_InertialSensor::update调用)注：关于SPI的DMA优化在ChibiOS操作系统中进行BSP的支持。换句话说AP中更多的是High Level Design。
K210官方例程ICM陀螺仪驱动中一处错误
qq_39562344的博客
01-26 395
k210裸机，官方例程ICM驱动中，快速求平方根倒数函数中，i定义为long，在64位系统中8字节，又定义float x2，y，此时y是4字节对齐，导致下面类型强转（long*）y出现不对齐取址，core dump。定义中的long改为int即可。 ...
ICM20602.zipIMU官方手册
06-02
IMU官方手册，对加速度计和陀螺仪的所有寄存器进行详细描述，资料齐全。
ICM-20689-v2.2-002.pdf
11-14
icm20689 是一款惯导芯片，用在无人机飞控上，上面包含1个三轴加速计，1个三轴陀螺仪。。。。。。。。。。。。。。。。。
ICM-glitch:一个很棒的项目。 呜呜呜
07-11
ICM-毛刺 ##GLITCH ICM 2014 最终项目汤米佩恩 & Pat Shiu | ###优先事项： 将用户鼠标映射到法师大小而不是画布大小 延迟加载图像 2 秒，让一个过滤器仅作为初始视图播放 - 然后用户开始使用按钮播放 为 UI 加载 PNG 文件，摆脱 controlP5 清楚/简洁地命名 UI 按钮，也许在轨道上使用图标 简化与 UI 的交互，仅隔离与按钮的交互，而不是跨画布的 mouseX/Y
ICM-42607-P-DS-000417.pdf
09-18
ICM-42607-P-DS-000417.pdf
ICM20648驱动 C++.zip
03-12
icm-20648底层驱动，包含所有寄存器声明，简单的参数配置，读取加速度、陀螺仪、内部温度等函数。
DS-000189-ICM-20948-v1.3 (1).pdf
11-08
ICM20948完成数据手册，包括寄存器介绍
官方手册MPU_9250 与 ICM-20948 的区别.pdf
12-13
最新的TDK9轴陀螺仪芯片ICM-20948与原本9轴陀螺仪MPU-9250/55芯片的全方位对比。包括异同点，兼容性，特性，怎么修改应用。
地平线旭日 X3 开发板上手体验
最新发布
哦豁灬
01-19 1143
最近嫖到一块旭日X3开发板，借此熟悉地平线 AI 芯片旭日 X3 模型部署流程，以及算法工具链。这里基本是跟着进行操作，其中也遇到一些奇怪的问题。
01-旭日X3派测评——开箱测试&系统烧写&性能初测
路拾遗的博客
08-12 1354
由于我之后使用想用VScode ssh登录，但是我需要提前获取开发板IP地址，此时使用串口登录开发板并进行网络参数确认，登录用户名：sunrise 密码：sunrise。但是由于我的以太网是校园网，连接没办法直接通网，所以我在SSH登陆时遇到了一些麻烦，有点后悔没装桌面版本 😦。打开balenaEtcher软件，第一步选择镜像文件，第二步选择TF卡，第三步Flash。通电正常，不过还未开始烧写代码工作，芯片发烫挺严重（很担心会不会烧了）。此外用SSH登录，ip地址配置，推荐参考下面这个帖子。...
【STM32Cube】学习笔记（三）：六轴传感器
weixin_52988524的博客
04-02 5848
文章目录摘要一、简介1.I2C原理2.MPU6050介绍3.MPU6050寄存器介绍4.DMP使用二、硬件电路设计三、软件设计1.CubeMX配置2.CubeIDE代码3.结果显示总结附录 摘要 本篇文章用STM32CubeMX和STM32CubeIDE软件编程，主控芯片为STM32F407ZGT6驱动六轴陀螺仪传感器。本章我们将来驱动MPU6050，通过I2C通信总线读取其原始数据，并利用其自带的DMP实现姿态解算，最终将测得的数据通过UART串口发送到上位机。通过本文可以初步学习I2C原理以及MPU6
icm20608 icm20602 区别
12-08
ICM20608和ICM20602都是因特尔公司设计和生产的惯性测量单元（IMU）。它们都是经过重新调校和改进的，旨在提供更高精度和性能的传感解决方案。 区别之一是ICM20608和ICM20602的测量范围不同。ICM20608的加速度计和陀螺仪测量范围分别达到16g和2000度/秒，而ICM20602的测量范围分别为8g和1000度/秒。对于某些应用而言，较大的测量范围可以提供更广泛的应用场景和更高的灵敏度。 另一个区别是ICM20608和ICM20602的噪声水平不同。ICM20608的噪声水平较低，可以提供更准确的测量结果。而ICM20602的噪声水平相对较高，可能导致一些误差和不精确的测量。 此外，ICM20608和ICM20602的功耗也有所不同。ICM20608的功耗更低，可能在一些低功率应用中更适用。ICM20602的功耗稍高，可能对电池寿命和能源管理造成一些影响。 总体而言，ICM20608和ICM20602在测量范围、噪声水平和功耗等方面存在一些差异。选择哪个取决于具体的应用需求，以及对测量精度和功耗的要求。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值