为什么ICM20948 Waveshare IMU传感器发送的数据不准确？

问题描述 投票：0回答：1

我有带有 Bookworm 的 Raspberry Pi 4，以及通过 i2c 连接的 Waveshare 10-Dof IMU 传感器。我确信我连接正确。我得到了测试应用程序（wget https://files.waveshare.com/upload/c/c1/10_DOF_IMU_Sensor_D_Code.7z）并针对 libi2c-dev 进行了修改。

当我从传感器获取俯仰、横滚和偏航值时，即使我不触摸传感器，我也会得到不准确的数据：

enter image description here

我需要校准它吗？有什么问题吗？

主.cpp：

#include "Waveshare_10Dof-D.h"

int main(int argc, char *argv[])
{
    IMU_EN_SENSOR_TYPE enMotionSensorType;
        IMU_ST_ANGLES_DATA stAngles;
        IMU_ST_SENSOR_DATA stGyroRawData;
        IMU_ST_SENSOR_DATA stAccelRawData;
        IMU_ST_SENSOR_DATA stMagnRawData;

        imuInit(&enMotionSensorType);
        if (IMU_EN_SENSOR_TYPE_ICM20948 != enMotionSensorType) {
            exit(1);
        }

        while (1) {
            imuDataGet(&stAngles, &stGyroRawData, &stAccelRawData, &stMagnRawData);

            std::cout << "ROLL = " << stAngles.fRoll
                      << ", PITCH = " << stAngles.fPitch
                      << ", YAW = " << stAngles.fYaw
                      << std::endl;
        }

    return 0;
}

Waveshare_10Dof-D.h：

#ifndef __Waveshare_10DOF_D_H__
#define __Waveshare_10DOF_D_H__
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <fcntl.h>
#include <stdint.h>
#include <stddef.h>
#include <math.h>
#include <chrono>
#include <thread> 

#define true 1
#define false 0
/* define ICM-20948 Device I2C address*/
#define I2C_ADD_ICM20948 0x68
#define I2C_ADD_ICM20948_AK09916 0x0C
#define I2C_ADD_ICM20948_AK09916_READ 0x80
#define I2C_ADD_ICM20948_AK09916_WRITE 0x00
/* define ICM-20948 Register */
/* user bank 0 register */
#define REG_ADD_WIA 0x00
#define REG_VAL_WIA 0xEA
#define REG_ADD_USER_CTRL 0x03
#define REG_VAL_BIT_DMP_EN 0x80
#define REG_VAL_BIT_FIFO_EN 0x40
#define REG_VAL_BIT_I2C_MST_EN 0x20
#define REG_VAL_BIT_I2C_IF_DIS 0x10
#define REG_VAL_BIT_DMP_RST 0x08
#define REG_VAL_BIT_DIAMOND_DMP_RST 0x04
#define REG_ADD_PWR_MIGMT_1 0x06
#define REG_VAL_ALL_RGE_RESET 0x80
#define REG_VAL_RUN_MODE 0x01 //Non low-power mode
#define REG_ADD_LP_CONFIG 0x05
#define REG_ADD_PWR_MGMT_1 0x06
#define REG_ADD_PWR_MGMT_2 0x07
#define REG_ADD_ACCEL_XOUT_H 0x2D
#define REG_ADD_ACCEL_XOUT_L 0x2E
#define REG_ADD_ACCEL_YOUT_H 0x2F
#define REG_ADD_ACCEL_YOUT_L 0x30
#define REG_ADD_ACCEL_ZOUT_H 0x31
#define REG_ADD_ACCEL_ZOUT_L 0x32
#define REG_ADD_GYRO_XOUT_H 0x33
#define REG_ADD_GYRO_XOUT_L 0x34
#define REG_ADD_GYRO_YOUT_H 0x35
#define REG_ADD_GYRO_YOUT_L 0x36
#define REG_ADD_GYRO_ZOUT_H 0x37
#define REG_ADD_GYRO_ZOUT_L 0x38
#define REG_ADD_EXT_SENS_DATA_00 0x3B
#define REG_ADD_REG_BANK_SEL 0x7F
#define REG_VAL_REG_BANK_0 0x00
#define REG_VAL_REG_BANK_1 0x10
#define REG_VAL_REG_BANK_2 0x20
#define REG_VAL_REG_BANK_3 0x30

/* user bank 1 register */
/* user bank 2 register */
#define REG_ADD_GYRO_SMPLRT_DIV 0x00
#define REG_ADD_GYRO_CONFIG_1 0x01
#define REG_VAL_BIT_GYRO_DLPCFG_2 0x10   /* bit[5:3] */
#define REG_VAL_BIT_GYRO_DLPCFG_4 0x20   /* bit[5:3] */
#define REG_VAL_BIT_GYRO_DLPCFG_6 0x30   /* bit[5:3] */
#define REG_VAL_BIT_GYRO_FS_250DPS 0x00  /* bit[2:1] */
#define REG_VAL_BIT_GYRO_FS_500DPS 0x02  /* bit[2:1] */
#define REG_VAL_BIT_GYRO_FS_1000DPS 0x04 /* bit[2:1] */
#define REG_VAL_BIT_GYRO_FS_2000DPS 0x06 /* bit[2:1] */
#define REG_VAL_BIT_GYRO_DLPF 0x01       /* bit[0]   */
#define REG_ADD_ACCEL_SMPLRT_DIV_2 0x11
#define REG_ADD_ACCEL_CONFIG 0x14
#define REG_VAL_BIT_ACCEL_DLPCFG_2 0x10 /* bit[5:3] */
#define REG_VAL_BIT_ACCEL_DLPCFG_4 0x20 /* bit[5:3] */
#define REG_VAL_BIT_ACCEL_DLPCFG_6 0x30 /* bit[5:3] */
#define REG_VAL_BIT_ACCEL_FS_2g 0x00    /* bit[2:1] */
#define REG_VAL_BIT_ACCEL_FS_4g 0x02    /* bit[2:1] */
#define REG_VAL_BIT_ACCEL_FS_8g 0x04    /* bit[2:1] */
#define REG_VAL_BIT_ACCEL_FS_16g 0x06   /* bit[2:1] */
#define REG_VAL_BIT_ACCEL_DLPF 0x01     /* bit[0]   */

/* user bank 3 register */
#define REG_ADD_I2C_SLV0_ADDR 0x03
#define REG_ADD_I2C_SLV0_REG 0x04
#define REG_ADD_I2C_SLV0_CTRL 0x05
#define REG_VAL_BIT_SLV0_EN 0x80
#define REG_VAL_BIT_MASK_LEN 0x07
#define REG_ADD_I2C_SLV0_DO 0x06
#define REG_ADD_I2C_SLV1_ADDR 0x07
#define REG_ADD_I2C_SLV1_REG 0x08
#define REG_ADD_I2C_SLV1_CTRL 0x09
#define REG_ADD_I2C_SLV1_DO 0x0A

/* define ICM-20948 Register  end */

/* define ICM-20948 MAG Register  */
#define REG_ADD_MAG_WIA1 0x00
#define REG_VAL_MAG_WIA1 0x48
#define REG_ADD_MAG_WIA2 0x01
#define REG_VAL_MAG_WIA2 0x09
#define REG_ADD_MAG_ST2 0x10
#define REG_ADD_MAG_DATA 0x11
#define REG_ADD_MAG_CNTL2 0x31
#define REG_VAL_MAG_MODE_PD 0x00
#define REG_VAL_MAG_MODE_SM 0x01
#define REG_VAL_MAG_MODE_10HZ 0x02
#define REG_VAL_MAG_MODE_20HZ 0x04
#define REG_VAL_MAG_MODE_50HZ 0x05
#define REG_VAL_MAG_MODE_100HZ 0x08
#define REG_VAL_MAG_MODE_ST 0x10
/* define ICM-20948 MAG Register  end */

#define MAG_DATA_LEN 6

#ifdef __cplusplus
extern "C"
{
#endif

  typedef enum
  {
    IMU_EN_SENSOR_TYPE_NULL = 0,
    IMU_EN_SENSOR_TYPE_ICM20948,
    IMU_EN_SENSOR_TYPE_BMP280,
    IMU_EN_SENSOR_TYPE_MAX
  } IMU_EN_SENSOR_TYPE;

  typedef struct imu_st_angles_data_tag
  {
    float fYaw;
    float fPitch;
    float fRoll;
  } IMU_ST_ANGLES_DATA;

  typedef struct imu_st_sensor_data_tag
  {
    int16_t s16X;
    int16_t s16Y;
    int16_t s16Z;
  } IMU_ST_SENSOR_DATA;

  typedef struct icm20948_st_avg_data_tag
  {
    uint8_t u8Index;
    int16_t s16AvgBuffer[8];
  } ICM20948_ST_AVG_DATA;

  void imuInit(IMU_EN_SENSOR_TYPE *penMotionSensorType);
  void imuDataGet(IMU_ST_ANGLES_DATA *pstAngles,
                  IMU_ST_SENSOR_DATA *pstGyroRawData,
                  IMU_ST_SENSOR_DATA *pstAccelRawData,
                  IMU_ST_SENSOR_DATA *pstMagnRawData);

  uint8_t I2C_ReadOneByte(uint8_t DevAddr, uint8_t RegAddr);
  void I2C_WriteOneByte(uint8_t DevAddr, uint8_t RegAddr, uint8_t value);

#ifdef __cplusplus
}
#endif

#endif //__Waveshare_10DOF_D_H__

Waveshare_10Dof-D.cpp：

#include "Waveshare_10Dof-D.h"

#include <iostream>

extern "C"
{
    #include<linux/i2c-dev.h>
    #include <i2c/smbus.h>
}
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <fcntl.h>

IMU_ST_SENSOR_DATA gstGyroOffset = {0, 0, 0};
#ifdef __cplusplus
extern "C"
{
#endif

  void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
  float invSqrt(float x);

  void icm20948init(void);
  bool icm20948Check(void);
  void icm20948GyroRead(int16_t *ps16X, int16_t *ps16Y, int16_t *ps16Z);
  void icm20948AccelRead(int16_t *ps16X, int16_t *ps16Y, int16_t *ps16Z);
  void icm20948MagRead(int16_t *ps16X, int16_t *ps16Y, int16_t *ps16Z);
  bool icm20948MagCheck(void);
  void icm20948CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal);
  void icm20948GyroOffset(void);
  void icm20948ReadSecondary(uint8_t u8I2CAddr, uint8_t u8RegAddr, uint8_t u8Len, uint8_t *pu8data);
  void icm20948WriteSecondary(uint8_t u8I2CAddr, uint8_t u8RegAddr, uint8_t u8data);
  bool icm20948Check(void);

  int file = -1;

  void i2cInit(void)
  {
    int adapter_nr = 1;
    char filename[20];
    snprintf(filename, 19, "/dev/i2c-%d", adapter_nr);
    file = open(filename, O_RDWR);
    if (file < 0) {
      exit(1);
    }
    return;
  }

  uint8_t I2C_ReadOneByte(uint8_t DevAddr, uint8_t RegAddr)
  {
      if (ioctl(file, I2C_SLAVE, DevAddr) < 0) {
          exit(1);
      }

      uint8_t res;
      res = i2c_smbus_read_byte_data(file, RegAddr);
      if (res < 0) {
          std::cout << "Read error" << std::endl;
      }
      return res;
  }

  void I2C_WriteOneByte(uint8_t DevAddr, uint8_t RegAddr, uint8_t value)
  {
    if (ioctl(file, I2C_SLAVE, DevAddr) < 0) {
        exit(1);
    }

    if (i2c_smbus_write_byte_data(file, RegAddr, value) < 0) {
        std::cout << "Write error" << std::endl;
    }
  }

#define Kp 4.50f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 1.0f  // integral gain governs rate of convergence of gyroscope biases

  float angles[3];
  float q0, q1, q2, q3;

  void imuInit(IMU_EN_SENSOR_TYPE *penMotionSensorType)
  {
    bool bRet = false;

    i2cInit();
    bRet = icm20948Check();
    if (true == bRet)
    {
      *penMotionSensorType = IMU_EN_SENSOR_TYPE_ICM20948;
      icm20948init();
    }
    else
    {
      *penMotionSensorType = IMU_EN_SENSOR_TYPE_NULL;
    }

    q0 = 1.0f;
    q1 = 0.0f;
    q2 = 0.0f;
    q3 = 0.0f;

    return;
  }

  void imuDataGet(IMU_ST_ANGLES_DATA *pstAngles,
                  IMU_ST_SENSOR_DATA *pstGyroRawData,
                  IMU_ST_SENSOR_DATA *pstAccelRawData,
                  IMU_ST_SENSOR_DATA *pstMagnRawData)
  {
    float MotionVal[9];
    int16_t s16Gyro[3], s16Accel[3], s16Magn[3];

    icm20948AccelRead(&s16Accel[0], &s16Accel[1], &s16Accel[2]);
    icm20948GyroRead(&s16Gyro[0], &s16Gyro[1], &s16Gyro[2]);
    icm20948MagRead(&s16Magn[0], &s16Magn[1], &s16Magn[2]);

    MotionVal[0] = s16Gyro[0] / 32.8;
    MotionVal[1] = s16Gyro[1] / 32.8;
    MotionVal[2] = s16Gyro[2] / 32.8;
    MotionVal[3] = s16Accel[0];
    MotionVal[4] = s16Accel[1];
    MotionVal[5] = s16Accel[2];
    MotionVal[6] = s16Magn[0];
    MotionVal[7] = s16Magn[1];
    MotionVal[8] = s16Magn[2];
    imuAHRSupdate((float)MotionVal[0] * 0.0175, (float)MotionVal[1] * 0.0175, (float)MotionVal[2] * 0.0175,
                  (float)MotionVal[3], (float)MotionVal[4], (float)MotionVal[5],
                  (float)MotionVal[6], (float)MotionVal[7], MotionVal[8]);

    pstAngles->fPitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3;                                // pitch
    pstAngles->fRoll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // roll
    pstAngles->fYaw = atan2(-2 * q1 * q2 - 2 * q0 * q3, 2 * q2 * q2 + 2 * q3 * q3 - 1) * 57.3;

    pstGyroRawData->s16X = s16Gyro[0];
    pstGyroRawData->s16Y = s16Gyro[1];
    pstGyroRawData->s16Z = s16Gyro[2];

    pstAccelRawData->s16X = s16Accel[0];
    pstAccelRawData->s16Y = s16Accel[1];
    pstAccelRawData->s16Z = s16Accel[2];

    pstMagnRawData->s16X = s16Magn[0];
    pstMagnRawData->s16Y = s16Magn[1];
    pstMagnRawData->s16Z = s16Magn[2];

    return;
  }

  void imuAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
  {
    float norm;
    float hx, hy, hz, bx, bz;
    float vx, vy, vz, wx, wy, wz;
    float exInt = 0.0, eyInt = 0.0, ezInt = 0.0;
    float ex, ey, ez, halfT = 0.024f;

    float q0q0 = q0 * q0;
    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    float q0q3 = q0 * q3;
    float q1q1 = q1 * q1;
    float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q2 = q2 * q2;
    float q2q3 = q2 * q3;
    float q3q3 = q3 * q3;

    norm = invSqrt(ax * ax + ay * ay + az * az);
    ax = ax * norm;
    ay = ay * norm;
    az = az * norm;

    norm = invSqrt(mx * mx + my * my + mz * mz);
    mx = mx * norm;
    my = my * norm;
    mz = mz * norm;

    // compute reference direction of flux
    hx = 2 * mx * (0.5f - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2);
    hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5f - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1);
    hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5f - q1q1 - q2q2);
    bx = sqrt((hx * hx) + (hy * hy));
    bz = hz;

    // estimated direction of gravity and flux (v and w)
    vx = 2 * (q1q3 - q0q2);
    vy = 2 * (q0q1 + q2q3);
    vz = q0q0 - q1q1 - q2q2 + q3q3;
    wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2);
    wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);
    wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2);

    // error is sum of cross product between reference direction of fields and direction measured by sensors
    ex = (ay * vz - az * vy) + (my * wz - mz * wy);
    ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
    ez = (ax * vy - ay * vx) + (mx * wy - my * wx);

    if (ex != 0.0f && ey != 0.0f && ez != 0.0f)
    {
      exInt = exInt + ex * Ki * halfT;
      eyInt = eyInt + ey * Ki * halfT;
      ezInt = ezInt + ez * Ki * halfT;

      gx = gx + Kp * ex + exInt;
      gy = gy + Kp * ey + eyInt;
      gz = gz + Kp * ez + ezInt;
    }

    q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
    q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
    q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
    q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;

    norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 = q0 * norm;
    q1 = q1 * norm;
    q2 = q2 * norm;
    q3 = q3 * norm;
  }

  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;
  }

  void icm20948init(void)
  {
    /* user bank 0 register */
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0);
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_PWR_MIGMT_1, REG_VAL_ALL_RGE_RESET);
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_PWR_MIGMT_1, REG_VAL_RUN_MODE);

    /* user bank 2 register */
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_2);
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_GYRO_SMPLRT_DIV, 0x07);
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_GYRO_CONFIG_1,
                     REG_VAL_BIT_GYRO_DLPCFG_6 | REG_VAL_BIT_GYRO_FS_1000DPS | REG_VAL_BIT_GYRO_DLPF);
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_ACCEL_SMPLRT_DIV_2, 0x07);
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_ACCEL_CONFIG,
                     REG_VAL_BIT_ACCEL_DLPCFG_6 | REG_VAL_BIT_ACCEL_FS_2g | REG_VAL_BIT_ACCEL_DLPF);

    /* user bank 0 register */
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0);

    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    /* offset */
    icm20948GyroOffset();

    icm20948MagCheck();

    icm20948WriteSecondary(I2C_ADD_ICM20948_AK09916 | I2C_ADD_ICM20948_AK09916_WRITE,
                           REG_ADD_MAG_CNTL2, REG_VAL_MAG_MODE_20HZ);
    return;
  }

  bool icm20948Check(void)
  {
    bool bRet = false;
    if (REG_VAL_WIA == I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_WIA))
    {
      bRet = true;
    }
    return bRet;
  }
  void icm20948GyroRead(int16_t *ps16X, int16_t *ps16Y, int16_t *ps16Z)
  {
    uint8_t u8Buf[6];
    int16_t s16Buf[3] = {0};
    uint8_t i;
    int32_t s32OutBuf[3] = {0};
    static ICM20948_ST_AVG_DATA sstAvgBuf[3];
    static int16_t ss16c = 0;
    ss16c++;

    u8Buf[0] = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_GYRO_XOUT_L);
    u8Buf[1] = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_GYRO_XOUT_H);
    s16Buf[0] = (u8Buf[1] << 8) | u8Buf[0];

    u8Buf[0] = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_GYRO_YOUT_L);
    u8Buf[1] = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_GYRO_YOUT_H);
    s16Buf[1] = (u8Buf[1] << 8) | u8Buf[0];

    u8Buf[0] = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_GYRO_ZOUT_L);
    u8Buf[1] = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_GYRO_ZOUT_H);
    s16Buf[2] = (u8Buf[1] << 8) | u8Buf[0];

    for (i = 0; i < 3; i++)
    {
      icm20948CalAvgValue(&sstAvgBuf[i].u8Index, sstAvgBuf[i].s16AvgBuffer, s16Buf[i], s32OutBuf + i);
    }
    *ps16X = s32OutBuf[0] - gstGyroOffset.s16X;
    *ps16Y = s32OutBuf[1] - gstGyroOffset.s16Y;
    *ps16Z = s32OutBuf[2] - gstGyroOffset.s16Z;

    return;
  }
  void icm20948AccelRead(int16_t *ps16X, int16_t *ps16Y, int16_t *ps16Z)
  {
    uint8_t u8Buf[2];
    int16_t s16Buf[3] = {0};
    uint8_t i;
    int32_t s32OutBuf[3] = {0};
    static ICM20948_ST_AVG_DATA sstAvgBuf[3];

    u8Buf[0] = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_ACCEL_XOUT_L);
    u8Buf[1] = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_ACCEL_XOUT_H);
    s16Buf[0] = (u8Buf[1] << 8) | u8Buf[0];

    u8Buf[0] = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_ACCEL_YOUT_L);
    u8Buf[1] = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_ACCEL_YOUT_H);
    s16Buf[1] = (u8Buf[1] << 8) | u8Buf[0];

    u8Buf[0] = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_ACCEL_ZOUT_L);
    u8Buf[1] = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_ACCEL_ZOUT_H);
    s16Buf[2] = (u8Buf[1] << 8) | u8Buf[0];

    for (i = 0; i < 3; i++)
    {
      icm20948CalAvgValue(&sstAvgBuf[i].u8Index, sstAvgBuf[i].s16AvgBuffer, s16Buf[i], s32OutBuf + i);
    }
    *ps16X = s32OutBuf[0];
    *ps16Y = s32OutBuf[1];
    *ps16Z = s32OutBuf[2];

    return;
  }
  void icm20948MagRead(int16_t *ps16X, int16_t *ps16Y, int16_t *ps16Z)
  {
    uint8_t counter = 20;
    uint8_t u8Data[MAG_DATA_LEN];
    int16_t s16Buf[3] = {0};
    uint8_t i;
    int32_t s32OutBuf[3] = {0};
    static ICM20948_ST_AVG_DATA sstAvgBuf[3];
    while (counter > 0)
    {
      std::this_thread::sleep_for(std::chrono::milliseconds(10));
      icm20948ReadSecondary(I2C_ADD_ICM20948_AK09916 | I2C_ADD_ICM20948_AK09916_READ,
                            REG_ADD_MAG_ST2, 1, u8Data);

      if ((u8Data[0] & 0x01) != 0)
        break;

      counter--;
    }

    if (counter != 0)
    {
      icm20948ReadSecondary(I2C_ADD_ICM20948_AK09916 | I2C_ADD_ICM20948_AK09916_READ,
                            REG_ADD_MAG_DATA,
                            MAG_DATA_LEN,
                            u8Data);
      s16Buf[0] = ((int16_t)u8Data[1] << 8) | u8Data[0];
      s16Buf[1] = ((int16_t)u8Data[3] << 8) | u8Data[2];
      s16Buf[2] = ((int16_t)u8Data[5] << 8) | u8Data[4];
    }

    for (i = 0; i < 3; i++)
    {
      icm20948CalAvgValue(&sstAvgBuf[i].u8Index, sstAvgBuf[i].s16AvgBuffer, s16Buf[i], s32OutBuf + i);
    }

    *ps16X = s32OutBuf[0];
    *ps16Y = -s32OutBuf[1];
    *ps16Z = -s32OutBuf[2];
    return;
  }

  void icm20948ReadSecondary(uint8_t u8I2CAddr, uint8_t u8RegAddr, uint8_t u8Len, uint8_t *pu8data)
  {
    uint8_t i;
    uint8_t u8Temp;

    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3); //swtich bank3
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_ADDR, u8I2CAddr);
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_REG, u8RegAddr);
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_CTRL, REG_VAL_BIT_SLV0_EN | u8Len);

    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0); //swtich bank0

    u8Temp = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_USER_CTRL);
    u8Temp |= REG_VAL_BIT_I2C_MST_EN;
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_USER_CTRL, u8Temp);
    std::this_thread::sleep_for(std::chrono::milliseconds(5));
    u8Temp &= ~REG_VAL_BIT_I2C_MST_EN;
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_USER_CTRL, u8Temp);

    for (i = 0; i < u8Len; i++)
    {
      *(pu8data + i) = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_EXT_SENS_DATA_00 + i);
    }
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3); //swtich bank3

    u8Temp = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_CTRL);
    u8Temp &= ~((REG_VAL_BIT_I2C_MST_EN) & (REG_VAL_BIT_MASK_LEN));
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_CTRL, u8Temp);

    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0); //swtich bank0
  }

  void icm20948WriteSecondary(uint8_t u8I2CAddr, uint8_t u8RegAddr, uint8_t u8data)
  {
    uint8_t u8Temp;
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3); //swtich bank3
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV1_ADDR, u8I2CAddr);
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV1_REG, u8RegAddr);
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV1_DO, u8data);
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV1_CTRL, REG_VAL_BIT_SLV0_EN | 1);

    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0); //swtich bank0

    u8Temp = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_USER_CTRL);
    u8Temp |= REG_VAL_BIT_I2C_MST_EN;
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_USER_CTRL, u8Temp);
    std::this_thread::sleep_for(std::chrono::milliseconds(5));
    u8Temp &= ~REG_VAL_BIT_I2C_MST_EN;
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_USER_CTRL, u8Temp);

    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_3); //swtich bank3

    u8Temp = I2C_ReadOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_CTRL);
    u8Temp &= ~((REG_VAL_BIT_I2C_MST_EN) & (REG_VAL_BIT_MASK_LEN));
    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_I2C_SLV0_CTRL, u8Temp);

    I2C_WriteOneByte(I2C_ADD_ICM20948, REG_ADD_REG_BANK_SEL, REG_VAL_REG_BANK_0); //swtich bank0

    return;
  }

  void icm20948CalAvgValue(uint8_t *pIndex, int16_t *pAvgBuffer, int16_t InVal, int32_t *pOutVal)
  {
    uint8_t i;

    *(pAvgBuffer + ((*pIndex)++)) = InVal;
    *pIndex &= 0x07;

    *pOutVal = 0;
    for (i = 0; i < 8; i++)
    {
      *pOutVal += *(pAvgBuffer + i);
    }
    *pOutVal >>= 3;
  }

  void icm20948GyroOffset(void)
  {
    uint8_t i;
    int16_t s16Gx = 0, s16Gy = 0, s16Gz = 0;
    int32_t s32TempGx = 0, s32TempGy = 0, s32TempGz = 0;
    for (i = 0; i < 32; i++)
    {
      icm20948GyroRead(&s16Gx, &s16Gy, &s16Gz);
      s32TempGx += s16Gx;
      s32TempGy += s16Gy;
      s32TempGz += s16Gz;
      std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
    gstGyroOffset.s16X = s32TempGx >> 5;
    gstGyroOffset.s16Y = s32TempGy >> 5;
    gstGyroOffset.s16Z = s32TempGz >> 5;
    return;
  }

  bool icm20948MagCheck(void)
  {
    bool bRet = false;
    uint8_t u8Ret[2];

    icm20948ReadSecondary(I2C_ADD_ICM20948_AK09916 | I2C_ADD_ICM20948_AK09916_READ,
                          REG_ADD_MAG_WIA1, 2, u8Ret);
    if ((u8Ret[0] == REG_VAL_MAG_WIA1) && (u8Ret[1] == REG_VAL_MAG_WIA2))
    {
      bRet = true;
    }

    return bRet;
  }

#ifdef __cplusplus
}
#endif
c++ raspberry-pi i2c imu
1个回答
0
投票

看起来Waveshare的示例代码有错误。我在其他项目中找到了 ICM20948 的计算。所以我改变了Waveshare示例中的一些代码：

  float invSqrt(float x)
  {
    float y = 1 / std::sqrt(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;
  }

现在效果完美！您可以在下图中看到结果（有轻微漂移）：

enter image description here

最新问题
© www.soinside.com 2019 - 2024. All rights reserved.