Почему датчик IMU ICM20948 Waveshare отправляет неточные данные? ⇐ C++

[Anonymous]

У меня есть Raspberry Pi 4 с Bookworm и датчик Waveshare 10-Dof IMU, подключенный через i2c. Я уверен, что подключил правильно. Я получил тестовое приложение (wget https://files.waveshare.com/upload/c/c1 ... _D_Code.7z) и изменил его для libi2c-dev.
Когда я получу шаг , значения крена и рыскания от датчика. Я получаю неточные данные, даже не прикасаясь к датчику:
[img]https://i. sstatic.net/nuXe4wVP.png[/img]

Нужно ли мне его калибровать? Что с ним не так?
main.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 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] 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


Подробнее здесь: https://stackoverflow.com/questions/786 ... urate-data