9D Orientation Calculation on STM32L4 using LSM6DSO16IS (IMU) + LIS2MDL (Magnetometer)

[Amith_lal]

I have implemented 9D orientation calculation on the STM32L4 using the LSM6DSO16IS and LIS2MDL sensors. Since there is an initial delay when flashing the program onto the ISPU, I chose to implement the algorithm on the STM32 itself, bypassing the ISPU.

The issue I'm facing is that the yaw axis value changes when I move the system and place it back in the same position—even after calibrating the magnetometer. I am using motionfx.h for both magnetometer and gyroscope calibration.

I believe the problem lies in the gyroscope calibration. When I set iKnobs.LMode = 1 (static mode), the gyroscope does not calibrate during motion, and the resulting drift affects the yaw angle. To address this, I also tried setting LMode = 2 (dynamic mode). However, in dynamic mode, calibration saturates after some time and stops updating. After that, the yaw value continues to drift even when the device is stationary.

Below is the code snippet I am using for orientation calculation and sensor calibration.

Question:
How can I achieve stable angle outputs—particularly for the yaw axis—without drift, even after motion?

/*****************************************************************************************************/

#include "motion_fx.h"

#define STATE_SIZE ((size_t)2450)

#define ENABLE_6X 1

#define FROM_MDPS_TO_DPS 0.001f

#define FROM_MG_TO_G 0.001f

#define SAMPLE_DISCARD 15

#define ALGO_FREQ  100U

#define ALGO_PERIOD  (1000U / ALGO_FREQ) 

static uint8_t mfxstate[STATE_SIZE];

static MFX_knobs_t iKnobs;

static float LastTime = 0.0f;

static uint8_t first_run = 1;

static volatile uint32_t TimeStamp = 0;

void MotionFX_Setup(void) {

__HAL_RCC_CRC_CLK_ENABLE();

if (MotionFX_GetStateSize() > STATE_SIZE) {

printf("Failure\n\r");

return FAILURE;

}

MotionFX_initialize((MFXState_t*) mfxstate);

MotionFX_MagCal_init(20, 1);

char lib_version[35];

MotionFX_GetLibVersion(lib_version);

printf("MotionFX Version: %s\n", lib_version);

MotionFX_getKnobs(mfxstate, &iKnobs);

strcpy(iKnobs.acc_orientation, "enu");

strcpy(iKnobs.gyro_orientation, "enu");

strcpy(iKnobs.mag_orientation, "esu");

iKnobs.gbias_acc_th_sc = 2.0f * 0.000765f;

iKnobs.gbias_gyro_th_sc = 2.0f * 0.002f;

iKnobs.output_type = MFX_ENGINE_OUTPUT_ENU;

iKnobs.LMode = 2;

iKnobs.modx = 1;

iKnobs.start_automatic_gbias_calculation = 1;

MotionFX_setKnobs(mfxstate, &iKnobs);

MotionFX_enable_6X(mfxstate, MFX_ENGINE_DISABLE);

MotionFX_enable_9X(mfxstate, MFX_ENGINE_DISABLE);

if (ENABLE_6X == 0) {

MotionFX_enable_6X(mfxstate, MFX_ENGINE_ENABLE);

} else {

MotionFX_enable_9X(mfxstate, MFX_ENGINE_ENABLE);

}

}

void MotionFX_Run(float *current_time, float *angle) {

MFX_input_t data_in;

MFX_output_t data_out;

MFX_MagCal_input_t mag_data_in;

MFX_MagCal_output_t mag_data_out;

float dT = *current_time - LastTime;

LastTime = *current_time;

float acc_raw[3], gyr_raw[3], mag_raw[3] = { 0 };

static uint8_t counter = 0;

lsm6dso16is_read_data_polling(ACCELEROMETER_U, acc_raw);//function to get acc raw data

lsm6dso16is_read_data_polling(GYROSCOPE_U, gyr_raw);//function to get gyro raw data

data_in.acc[0] = acc_raw[0] * FROM_MG_TO_G;

data_in.acc[1] = acc_raw[1] * FROM_MG_TO_G;

data_in.acc[2] = acc_raw[2] * FROM_MG_TO_G;

data_in.gyro[0] = gyr_raw[0] * FROM_MDPS_TO_DPS;

data_in.gyro[1] = gyr_raw[1] * FROM_MDPS_TO_DPS;

data_in.gyro[2] = gyr_raw[2] * FROM_MDPS_TO_DPS;

if (ENABLE_6X == 1) {

lsm6dso16is_sensor_hub(mag_raw);//function to get mag raw data in mGauss

mag_data_in.mag[0] = mag_raw[0] / 500.0f;

mag_data_in.mag[1] = mag_raw[1] / 500.0f;

mag_data_in.mag[2] = mag_raw[2] / 500.0f;

} else {

data_in.mag[0] = 0.0f;

data_in.mag[1] = 0.0f;

data_in.mag[2] = 0.0f;

}

mag_data_in.time_stamp = (int) TimeStamp;

TimeStamp += (uint32_t) ALGO_PERIOD;

if (++counter >= 2) {

MotionFX_MagCal_run(&mag_data_in);

counter = 0;

}

MotionFX_MagCal_getParams(&mag_data_out);

for (int i = 0; i < 3; i++) {

data_in.mag[i] = mag_data_in.mag[i] - mag_data_out.hi_bias[i];

}

printf("mag = [%.3f, %.3f, %.3f] q = %d dT = %.3f\n\r", data_in.mag[0],

data_in.mag[1], data_in.mag[2], mag_data_out.cal_quality, dT);

data_in.mag[0] = data_in.mag[0] / 50.0f;

data_in.mag[1] = data_in.mag[1] / 50.0f;

data_in.mag[2] = data_in.mag[2] / 50.0f;

MotionFX_propagate(mfxstate, &data_out, &data_in, &dT);

MotionFX_update(mfxstate, &data_out, &data_in, &dT, NULL);

angle[0] = data_out.rotation[1];

angle[1] = data_out.rotation[2];

angle[2] = data_out.rotation[0];

angle[3] = (float) mag_data_out.cal_quality;

}