Problem with the Pitch and Yaw data from IMU ICM20948

Question

I'm having a problem on the yaw angle. The Roll and the Pitch are behaving correctly with values between -180° and 180° but the Yaw give me values between -25° and 30° and usually if i stop the rotation, instead of remain constant,the value decreases toward 0. i suppose the problem could be related to the initialization and calibration of the magnetometer because when i try to use the DMP also the yaw behave correctly. I will also add that the Pitch answer at a slower speed but really strangely just with positive angles while with negative is as good and precise as the Roll. I'm using Zephyr and I have imported the SparkFun library for ICM20948 converting the .cpp file to .c (complitely rewriting a fwe functions and adapting others). This is the main:

#include <stdio.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/printk.h>
#include "ICM20948/ICM_20948.h"
#include "config.h"
#include "log.h"

#define M_PI   3.14159265358979323846264338327950288
#define degToRad(angleInDegrees) ((angleInDegrees) * M_PI / 180.0)
#define radToDeg(angleInRadians) ((angleInRadians) * 180.0 / M_PI)
#define DATA_SAMPLES (50)
#define SLEEP_TIME_MS 10

// Now declare the structure that represents the ICM.
ICM_20948_Device_t myICM;
ICM_20948_AGMT_t agmt = {{{0, 0, 0}}, {{0, 0, 0}}, {{0, 0, 0}}, {0}};

float max_m[3], min_m[3];
float scale_x, scale_y, scale_z;
int prt;

int main(void) {
    // init_usb_device(WAIT_FOR_USB_DTR);
    prt = 0;
    printf("Start\n");
    bool success = false;
    float ax = 0.0f, ay = 0.0f, az = 0.0f, gx = 0.0f, gy = 0.0f, gz = 0.0f, mx = 0.0f, my = 0.0f, mz = 0.0f;

    if((ICM_20948_init(&myICM, I2C_PORT, ICM20948_I2C_ADDR))== ICM_20948_Stat_Ok) {
        printf("ICM_20948_init success\n");
    }
    
    ICM_20948_fss_t myfss;
    myfss.a = gpm2;   // (ICM_20948_ACCEL_CONFIG_FS_SEL_e)
    myfss.g = dps250; // (ICM_20948_GYRO_CONFIG_1_FS_SEL_e)
    ICM_20948_set_FSS(myfss);
    icm_20948_DMP_data_t data;
    
    while (1) {
 
        // int i, j = 0;
        if (ICM_20948_get_agmt(&myICM, &agmt) == ICM_20948_Stat_Ok)
        {
            ax = getAccMG(agmt.acc.axes.x, myfss.a);
            ay = getAccMG(agmt.acc.axes.y, myfss.a);
            az = getAccMG(agmt.acc.axes.z, myfss.a);
            float acc_scale = 1.0f / 16320.0f;
            ax *= acc_scale;
            ay *= acc_scale;
            az *= acc_scale;

            gx = getGyrDPS(agmt.gyr.axes.x, myfss.g);
            gy = getGyrDPS(agmt.gyr.axes.y, myfss.g);
            gz = getGyrDPS(agmt.gyr.axes.z, myfss.g);
            gx = gx * (M_PI / 180.0f);
            gy = gy * (M_PI / 180.0f);
            gz = gz * (M_PI / 180.0f);

            mx = (getMagUT(agmt.mag.axes.x) + 205);
            my = (getMagUT(agmt.mag.axes.y) - 26);
            mz = (getMagUT(agmt.mag.axes.z) + 65.5);
            
            MahonyAHRSupdate(ax, ay, az, mx, my, mz, gx, gy, gz);
        }
        else
        {
            printf("Error reading data\n");
        }
        k_msleep(SLEEP_TIME_MS);
    }
    return 0;
}

This is the filter i'm using:

void MahonyAHRSupdate(float ax, float ay, float az, float mx, float my, float mz, float gx, float gy, float gz) {
    float recipNorm;
    float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;  
    float hx, hy, bx, bz;
    float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
    float halfex, halfey, halfez;
    float qa, qb, qc;

    // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
    if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
        MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az);
        return;
    }

    // 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
        recipNorm = invSqrt(ax * ax + ay * ay + az * az);
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;     

        // Normalise magnetometer measurement
        recipNorm = invSqrt(mx * mx + my * my + mz * mz);
        mx *= recipNorm;
        my *= recipNorm;
        mz *= recipNorm;   

        // Auxiliary variables to avoid repeated arithmetic
        q0q0 = q0 * q0;
        q0q1 = q0 * q1;
        q0q2 = q0 * q2;
        q0q3 = q0 * q3;
        q1q1 = q1 * q1;
        q1q2 = q1 * q2;
        q1q3 = q1 * q3;
        q2q2 = q2 * q2;
        q2q3 = q2 * q3;
        q3q3 = q3 * q3;   

        // Reference direction of Earth's magnetic field
        hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
        hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
        bx = sqrt(hx * hx + hy * hy);
        bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
        
        // printf("bx: %f, bz: %f\n", bx, bz);

        // Estimated direction of gravity and magnetic field
        halfvx = q1q3 - q0q2;
        halfvy = q0q1 + q2q3;
        halfvz = q0q0 - 0.5f + q3q3;
        halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
        halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
        halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);  
    
        // Error is sum of cross product between estimated direction and measured direction of field vectors
        halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
        halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
        halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);

        // Compute and apply integral feedback if enabled
        if(twoKi > 0.0f) {
            integralFBx += twoKi * halfex * (1.0f / sampleFreq);    // integral error scaled by Ki
            integralFBy += twoKi * halfey * (1.0f / sampleFreq);
            integralFBz += twoKi * halfez * (1.0f / sampleFreq);
            gx += integralFBx;  // apply integral feedback
            gy += integralFBy;
            gz += integralFBz;
        }
        else {
            integralFBx = 0.0f; // prevent integral windup
            integralFBy = 0.0f;
            integralFBz = 0.0f;
        }

        // Apply proportional feedback
        gx += twoKp * halfex;
        gy += twoKp * halfey;
        gz += twoKp * halfez;
    }
    
    // Integrate rate of change of quaternion
    gx *= (0.5f * (1.0f / sampleFreq));     // pre-multiply common factors
    gy *= (0.5f * (1.0f / sampleFreq));
    gz *= (0.5f * (1.0f / sampleFreq));
    qa = q0;
    qb = q1;
    qc = q2;
    q0 += (-qb * gx - qc * gy - q3 * gz);
    q1 += (qa * gx + qc * gz - q3 * gy);
    q2 += (qa * gy - qb * gz + q3 * gx);
    q3 += (qa * gz + qb * gy - qc * gx); 
    
    // Normalise quaternion
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;

    double sinr_cosp = 2 * (q0 * q1 + q2 * q3);
    double cosr_cosp = 1 - 2 * (q1 * q1 + q2 * q2);
    double roll = atan2(sinr_cosp, cosr_cosp)*180/M_PI;

    // pitch (y-axis rotation)
    double sinp = sqrt(1 + 2 * (q0 * q2 - q1 * q3));
    double cosp = sqrt(1 - 2 * (q0 * q2 - q1 * q3));
    double pitch = (2 * atan2(sinp, cosp) - M_PI / 2)*180/M_PI;

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q0 * q3 + q1 * q2);
    double cosy_cosp = 1 - 2 * (q2 * q2 + q3 * q3);
    double yaw = atan2(siny_cosp, cosy_cosp)*180/M_PI;
    if (yaw > 180.0) yaw -= 360.0;
    if (yaw < -180.0) yaw += 360.0;

    printf("Roll: %f, Pitch: %f, Yaw: %f\n", roll, pitch, yaw);
}

This is the prj.conf:

CONFIG_CBPRINTF_FP_SUPPORT=y
CONFIG_NEWLIB_LIBC=y
CONFIG_NEWLIB_LIBC_FLOAT_PRINTF=y

CONFIG_ZSL=y
CONFIG_FPU=y

CONFIG_HEAP_MEM_POOL_SIZE=32768
CONFIG_SYSTEM_WORKQUEUE_STACK_SIZE=8192
CONFIG_MAIN_STACK_SIZE=32768
CONFIG_LOG_BUFFER_SIZE=4000

CONFIG_SERIAL=y
CONFIG_CONSOLE=y
CONFIG_UART_CONSOLE=y
CONFIG_UART_LINE_CTRL=y

CONFIG_THREAD_NAME=y
CONFIG_THREAD_STACK_INFO=y

CONFIG_DEBUG=y

CONFIG_I2C=y
CONFIG_LOG=y

CONFIG_LOG_DEFAULT_LEVEL=3
CONFIG_LOG_MODE_OVERFLOW=y

I will still add few useful functions from my ICM20948.c file:

ICM_20948_Status_e ICM_20948_init(ICM_20948_Device_t *_ICM, struct device *_i2c_dev, uint8_t _imu_i2c_addr)
{
    ICM_20948_Status_e ret = ICM_20948_Stat_Err;

    ICM = _ICM;

    if (NULL != _i2c_dev)
    {
        i2c_dev = _i2c_dev;
    }

    i2c_configure(i2c_dev, I2C_SPEED_SET(I2C_SPEED_STANDARD));

    if (!i2c_dev) {
        LOG_ERR("I2C device not valid.");
        return ICM_20948_Stat_Err;
    }

    if (0 != _imu_i2c_addr)
    {
        imu_i2c_addr = _imu_i2c_addr;
    }

    // Initialize ICM
    if(ICM_20948_init_struct(ICM) == ICM_20948_Stat_Ok)
      LOG_INF("ICM_20948_init_struct success\n");

    // Link the serif
    if(ICM_20948_link_serif(ICM, &_Serif)== ICM_20948_Stat_Ok)
      LOG_INF("ICM_20948_link_serif success\n");

    while (ICM_20948_check_id(ICM) != ICM_20948_Stat_Ok)
    {
        //TODO add a timeout or max retries
        LOG_INF("whoami does not match. Halting... ");
        k_sleep(K_SECONDS(1));
    }

    // Here we are doing a SW reset to make sure the device starts in a known state
    ICM_20948_sw_reset(ICM);   

    k_sleep(K_MSEC(500));

    // Now wake the sensor up
    ICM_20948_sleep(ICM, false);
    ICM_20948_low_power(ICM, false);

    // Start the magnetometer
    startupMagnetometer(false);

    // Set Gyro and Accelerometer to a particular sample mode
    ICM_20948_set_sample_mode(ICM, (ICM_20948_InternalSensorID_bm)(ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_Sample_Mode_Continuous); // optiona: ICM_20948_Sample_Mode_Continuous. ICM_20948_Sample_Mode_Cycled

    ICM_20948_set_full_scale(ICM, (ICM_20948_InternalSensorID_bm)(ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), ICM_20948_fss);

    // Set up DLPF configuration
    ICM_20948_dlpcfg_t myDLPcfg;
    myDLPcfg.a = acc_d473bw_n499bw;
    myDLPcfg.g = gyr_d361bw4_n376bw5;
    ICM_20948_set_dlpf_cfg(ICM, (ICM_20948_InternalSensorID_bm)(ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), myDLPcfg);

  ICM_20948_smplrt_t mySMPLRTcfg;
  mySMPLRTcfg.a = 1;
  mySMPLRTcfg.g = 1;
  ICM_20948_set_sample_rate(ICM, (ICM_20948_InternalSensorID_bm)(ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr), mySMPLRTcfg);
  
    // Choose whether or not to use DLPF
    ICM_20948_enable_dlpf(ICM, ICM_20948_Internal_Acc, false);
    ICM_20948_enable_dlpf(ICM, ICM_20948_Internal_Gyr, false);

  #if defined(ICM_20948_USE_DMP)
    ICM->_dmp_firmware_available = true; 
    printf("Initialized dmp_firmware_available = true\n");
#else
    ICM->_dmp_firmware_available = false; // Initialize dmp_firmware_available
    printf("Initialized dmp_firmware_available = false\n");
#endif

  ret = ICM_20948_Stat_Ok;

  return ret;
}

...

ICM_20948_Status_e startupMagnetometer(bool minimal)
{
  ICM_20948_Status_e retval = ICM_20948_Stat_Ok;

  i2cMasterPassthrough(false); //Do not connect the SDA/SCL pins to AUX_DA/AUX_CL
  i2cMasterEnable(true);

  resetMag();

  //After a ICM reset the Mag sensor may stop responding over the I2C master
  //Reset the Master I2C until it responds
  uint8_t tries = 0;
  while (tries < MAX_MAGNETOMETER_STARTS)
  {
    tries++;

    //See if we can read the WhoIAm register correctly
    retval = magWhoIAm();
    if (retval == ICM_20948_Stat_Ok)
      break; //WIA matched!

    i2cMasterReset(); //Otherwise, reset the master I2C and try again

    k_msleep(10);
  }

  if (tries == MAX_MAGNETOMETER_STARTS)
  {
    printf("Magnetomiter max start reached\n");
    status = ICM_20948_Stat_WrongID;
    return status;
  }
  else
  {
    printf("Magnetomiter success\n");
  }

  //Return now if minimal is true. The mag will be configured manually for the DMP
  if (minimal) // Return now if minimal is true
  {
    printf("Magnetomiter minal success\n");
    return status;
  }

  //Set up magnetometer
  AK09916_CNTL2_Reg_t reg;
  reg.MODE = AK09916_mode_cont_100hz;
  reg.reserved_0 = 0; // Make sure the unused bits are clear. Probably redundant, but prevents confusion when looking at the I2C traffic
  retval = writeMag(AK09916_REG_CNTL2, (uint8_t *)&reg);
  if (retval != ICM_20948_Stat_Ok)
  {
    status = retval;
    return status;
  }

  retval = i2cControllerConfigurePeripheral(0, MAG_AK09916_I2C_ADDR, AK09916_REG_ST1, 9, true, true, false, false, false, 0);
  if (retval != ICM_20948_Stat_Ok)
  {
    status = retval;
    return status;
  }

  return status;
}

...


ICM_20948_Status_e i2cMasterPassthrough(bool passthrough)
{
  status = ICM_20948_i2c_master_passthrough(ICM, passthrough);
  return status;
}

ICM_20948_Status_e i2cMasterEnable(bool enable)
{
  status = ICM_20948_i2c_master_enable(ICM, enable);
  return status;
}

ICM_20948_Status_e i2cMasterReset()
{
  status = ICM_20948_i2c_master_reset(ICM);
  return status;
}

ICM_20948_Status_e magWhoIAm()
{
  ICM_20948_Status_e retval = ICM_20948_Stat_Ok;

  uint8_t whoiam1, whoiam2;
  whoiam1 = readMag(AK09916_REG_WIA1);
  retval = status;
  if (retval != ICM_20948_Stat_Ok)
  {
    return retval;
  }
  whoiam2 = readMag(AK09916_REG_WIA2);
  // readMag calls i2cMasterSingleR which calls ICM_20948_i2c_master_single_r
  // i2cMasterSingleR updates status so it is OK to set retval to status here
  retval = status;
  if (retval != ICM_20948_Stat_Ok)
  {
    // debugPrint(F("ICM_20948::magWhoIAm: whoiam1: "));
    // debugPrintf((int)whoiam1);
    // debugPrint(F(" (should be 72) whoiam2: "));
    // debugPrintf((int)whoiam2);
    // debugPrint(F(" (should be 9) readMag set status to: "));
    // debugPrintStatus(status);
    // debugPrintln(F(""));
    return retval;
  }

  if ((whoiam1 == (MAG_AK09916_WHO_AM_I >> 8)) && (whoiam2 == (MAG_AK09916_WHO_AM_I & 0xFF)))
  {
    retval = ICM_20948_Stat_Ok;
    status = retval;
    return status;
  }
  retval = ICM_20948_Stat_WrongID;
  status = retval;
  return status;
}

uint8_t readMag(AK09916_Reg_Addr_e reg)
{
  uint8_t data = i2cMasterSingleR(MAG_AK09916_I2C_ADDR, reg); // i2cMasterSingleR updates status too
  return data;
}

uint8_t i2cMasterSingleR(uint8_t addr, uint8_t reg)
{
  uint8_t data = 0;
  status = ICM_20948_i2c_master_single_r(ICM, addr, reg, &data);
  if (status != ICM_20948_Stat_Ok)
  {
    // debugPrint(F("ICM_20948::i2cMasterSingleR: ICM_20948_i2c_master_single_r returned: "));
    // debugPrintStatus(status);
    // debugPrintln(F(""));
  }
  return data;
}

ICM_20948_Status_e i2cControllerConfigurePeripheral(uint8_t peripheral, uint8_t addr, uint8_t reg, uint8_t len, bool Rw, bool enable, bool data_only, bool grp, bool swap, uint8_t dataOut)
{
  status = ICM_20948_i2c_controller_configure_peripheral(ICM, peripheral, addr, reg, len, Rw, enable, data_only, grp, swap, dataOut);
  return status;
}

ICM_20948_Status_e writeMag(AK09916_Reg_Addr_e reg, uint8_t *pdata)
{
  status = i2cMasterSingleW(MAG_AK09916_I2C_ADDR, reg, *pdata);
  return status;
}

ICM_20948_Status_e i2cMasterSingleW(uint8_t addr, uint8_t reg, uint8_t data)
{
  status = ICM_20948_i2c_master_single_w(ICM, addr, reg, &data);
  return status;
}

ICM_20948_Status_e resetMag()
{
  uint8_t SRST = 1;
  // SRST: Soft reset
  // “0”: Normal
  // “1”: Reset
  // When “1” is set, all registers are initialized. After reset, SRST bit turns to “0” automatically.
  status = i2cMasterSingleW(MAG_AK09916_I2C_ADDR, AK09916_REG_CNTL3, SRST);
  return status;
}

I tried to review the entire code. i added the hard-iron calibration (I hope correctly) and know the yaw start correctly around 0° and also try to add a soft iron calibration that didn't change the results. I try to make the filter print the bx and bz and the value i obtained where between 0.6 and 0.85 for bx and between 0.4 and 0.7 for bz trying to rotate the sensor in various direction. The Pitch for some reason is slower than the Roll. More precisly it works fine in one direction but in the other it does a step before reaching the correct value. For example, when rotating to 90 instead of a fast and steadly grow to that value it stop at 65°-67° and then it goes up to 84°-87°. while going back to 0° it stop at 25°-26° and then reaches the 0. I try to review the magnetometer startup o the original library but i didn't see any potential error in what i reported (the other file in the library where already .c so i didn't touch them). I hope to have sent you all the code required to understand the situation without too many unnecessary parts.