FAQs
cancel
Turn on suggestions
Showing results for 
Search instead for 
Did you mean: 

Problem with Heading Calculation

gtk
Associate

‎2025-03-16 8:36 PM - edited ‎2025-03-16 11:29 PM

I'm using ST MotionFX v2.7.1 and have encountered an issue with heading calculation.

I'm following the sample code from the document:
um2220-getting-started-with-motionfx-sensor-fusion-library-in-xcubemems1-expansion-for-stm32cube-stmicroelectronics

 

#define STATE_SIZE (size_t)(2450)
static uint8_t mfxstate[STATE_SIZE];
MFX_knobs_t iKnobs;
MFX_input_t data_in_scale;

void main() {
    /* Sensor Fusion API initialization function */
    MotionFX_initialize((MFXState_t *)mfxstate);

    /* Modify knobs settings & set the knobs */
    MotionFX_getKnobs((MFXState_t *)mfxstate, &iKnobs);

    iKnobs.LMode = 2;
    iKnobs.modx = 1;
    iKnobs.output_type = 1;
    iKnobs.start_automatic_gbias_calculation = 1;
  
    //orientation

    MotionFX_setKnobs((MFXState_t *)mfxstate, &iKnobs);

    MotionFX_enable_6X((MFXState_t *)mfxstate, MFX_ENGINE_DISABLE);
    MotionFX_enable_9X((MFXState_t *)mfxstate, MFX_ENGINE_DISABLE);

    /* Enable 9-axis sensor fusion */
    MotionFX_enable_9X((MFXState_t *)mfxstate, MFX_ENGINE_ENABLE);

    // init last_time
    clock_gettime(CLOCK_MONOTONIC, &last_time);

    get_sensor_data_scale(&data_in_scale);

    MFX_input_t sensor_in;
    MFX_output_t sensor_out;

    for(int i =0; i< 9999999999; i++)
    {
        get_sensor_data(&sensor_in);

        float delta_time = get_delta_time();
        MotionFX_propagate((MFXState_t *)mfxstate, &sensor_out, &sensor_in, &delta_time);
        MotionFX_update((MFXState_t *)mfxstate, &sensor_out, &sensor_in, &delta_time, NULL);

        printf("Heading: %f\n", sensor_out.heading);
    }
}
View more

 

 

When I run the code, the heading value is incorrect. However, if I re-run MotionFX_initialize, the heading calculation becomes correct.

 

#define STATE_SIZE (size_t)(2450)
static uint8_t mfxstate[STATE_SIZE];
MFX_knobs_t iKnobs;
MFX_input_t data_in_scale;

void main() {
    for(int i =0; i< 9999999999; i++)
    {
        /* Sensor Fusion API initialization function */
        MotionFX_initialize((MFXState_t *)mfxstate);

        /* Modify knobs settings & set the knobs */
        MotionFX_getKnobs((MFXState_t *)mfxstate, &iKnobs);

        iKnobs.LMode = 2;
        iKnobs.modx = 1;
        iKnobs.output_type = 1;
        iKnobs.start_automatic_gbias_calculation = 1;

        //orientation

        MotionFX_setKnobs((MFXState_t *)mfxstate, &iKnobs);

        MotionFX_enable_6X((MFXState_t *)mfxstate, MFX_ENGINE_DISABLE);
        MotionFX_enable_9X((MFXState_t *)mfxstate, MFX_ENGINE_DISABLE);

        /* Enable 9-axis sensor fusion */
        MotionFX_enable_9X((MFXState_t *)mfxstate, MFX_ENGINE_ENABLE);

        // init last_time
        clock_gettime(CLOCK_MONOTONIC, &last_time);

        get_sensor_data_scale(&data_in_scale);

        MFX_input_t sensor_in;
        MFX_output_t sensor_out;

        get_sensor_data(&sensor_in);

        float delta_time = get_delta_time();
        MotionFX_propagate((MFXState_t *)mfxstate, &sensor_out, &sensor_in, &delta_time);
        MotionFX_update((MFXState_t *)mfxstate, &sensor_out, &sensor_in, &delta_time, NULL);

        printf("Heading: %f\n", sensor_out.heading);
    }
}
View more

 

 

Does anyone know why this happens? Any insights would be greatly appreciated! Let me know if you need more details.

0 Kudos
2 REPLIES 2
Federica Bossi
ST Employee

‎2025-03-21 1:52 AM

Hi @gtk ,

Can you share the outputs you get in both cases?

Thanks!

In order to give better visibility on the answered topics, please click on 'Accept as Solution' on the reply which solved your issue or answered your question.
gtk
Associate
In response to Federica Bossi

‎2025-03-25 1:50 AM 

//without 9x

mag: [-0.208000, -0.996000, 0.565000]
acc: [9.514451, 1.042671, 0.016750]
gyro: [0.009163, -0.059559, 0.056047]
Yaw: 350.114685, Pitch: -0.101226, Roll: 6.254028
Quaternion: [W=0.005579, X=-0.054271, Y=0.086079, Z=0.994794]
Gravity: [X=-0.108937, Y=-0.001756, Z=-0.994047]
Linear Acceleration: [X=0.933735, Y=0.014994, Z=8.520404]
Heading: 350.114685
Heading Error: 0.417009
==============================
mag: [-0.214000, 0.873000, 0.604000]
acc: [9.762706, 1.114456, -0.442074]
gyro: [-0.070555, -0.111177, 0.226325]
Yaw: 350.849396, Pitch: 2.392230, Roll: 6.477042
Quaternion: [W=-0.016269, X=-0.057963, Y=0.078449, Z=0.995099]
Gravity: [X=-0.112805, Y=0.041474, Z=-0.992751]
Linear Acceleration: [X=1.001651, Y=-0.400600, Z=8.769955]
Heading: 350.849396
Heading Error: 0.300941
==============================
mag: [-0.214000, 0.858000, 0.616000]
acc: [9.881749, 0.461216, -0.324825]
gyro: [-0.071166, -0.021686, -0.625525]
Yaw: 5.967731, Pitch: 2.589645, Roll: 4.119998
Quaternion: [W=-0.024422, X=-0.034713, Y=-0.052819, Z=0.997702]
Gravity: [X=-0.071846, Y=0.045066, Z=-0.996397]
Linear Acceleration: [X=0.389371, Y=-0.279760, Z=8.885352]
Heading: 5.967731
Heading Error: 0.106927
==============================
mag: [-0.115000, 0.807000, 0.649000]
acc: [9.217143, 0.588634, 0.117248]
gyro: [-0.707075, -0.092699, -0.029016]
Yaw: 6.942193, Pitch: 0.362013, Roll: 3.827046
Quaternion: [W=-0.005173, X=-0.033138, Y=-0.060616, Z=0.997598]
Gravity: [X=-0.066745, Y=0.006304, Z=-0.997750]
Linear Acceleration: [X=0.521889, Y=0.123552, Z=8.219393]
Heading: 6.942193
Heading Error: 0.091870
==============================
mag: [0.053000, 0.789000, 0.685000]
acc: [9.414551, 0.581455, -0.057428]
gyro: [-1.808310, -0.272140, 0.388510]
Yaw: 7.401766, Pitch: 0.368290, Roll: 3.591856
Quaternion: [W=-0.005229, X=-0.031067, Y=-0.064616, Z=0.997413]
Gravity: [X=-0.062649, Y=0.006415, Z=-0.998015]
Linear Acceleration: [X=0.518807, Y=-0.051012, Z=8.416535]
Heading: 7.401766
Heading Error: 0.089419
==============================
View more

 

//with 9x

mag: [0.056000, 0.906000, 0.580000]
acc: [9.843464, 0.562313, 0.478564]
gyro: [0.030390, 0.082467, 0.527176]
Yaw: 0.548519, Pitch: -2.783378, Roll: 3.265410
Quaternion: [W=0.024141, X=-0.028600, Y=-0.004091, Z=0.999291]
Gravity: [X=-0.056961, Y=-0.048481, Z=-0.997199]
Linear Acceleration: [X=0.505351, Y=0.430083, Z=8.846266]
Heading: 0.548519
Heading Error: 0.100627
==============================
mag: [0.026000, 0.921000, 0.583000]
acc: [9.811160, -0.222532, 1.222133]
gyro: [0.006414, 1.013576, 0.492967]
Yaw: 3.713483, Pitch: -7.100546, Roll: -1.289394
Quaternion: [W=0.062252, X=0.009218, Y=-0.033033, Z=0.997471]
Gravity: [X=0.022502, Y=-0.123580, Z=-0.992079]
Linear Acceleration: [X=-0.200030, Y=1.098553, Z=8.819080]
Heading: 3.713483
Heading Error: 0.049027
==============================
mag: [0.059000, 0.945000, 0.547000]
acc: [9.217143, -0.576670, 0.567098]
gyro: [-0.872772, 0.035125, 0.729219]
Yaw: 10.832839, Pitch: -3.520864, Roll: -3.573319
Quaternion: [W=0.033510, X=0.028126, Y=-0.095257, Z=0.994491]
Gravity: [X=0.062326, Y=-0.061293, Z=-0.996172]
Linear Acceleration: [X=-0.514344, Y=0.505806, Z=8.220971]
Heading: 10.832839
Heading Error: 0.064499
==============================
mag: [0.191000, 0.963000, 0.472000]
acc: [10.156923, -0.013759, 3.787236]
gyro: [-1.945602, 0.439211, -0.312762]
Yaw: 13.846349, Pitch: -20.449083, Roll: -0.072748
Quaternion: [W=0.176287, X=-0.020776, Y=-0.118736, Z=0.976930]
Gravity: [X=0.001270, Y=-0.349375, Z=-0.936982]
Linear Acceleration: [X=-0.012489, Y=3.437861, Z=9.219941]
Heading: 13.846349
Heading Error: 0.046600
==============================
mag: [0.083000, 0.939000, 0.535000]
acc: [9.631699, -0.590428, 0.399003]
gyro: [-0.426230, 0.458759, 0.186466]
Yaw: 13.611735, Pitch: -2.372174, Roll: -3.504874
Quaternion: [W=0.024167, X=0.027907, Y=-0.119053, Z=0.992201]
Gravity: [X=0.061133, Y=-0.041313, Z=-0.997274]
Linear Acceleration: [X=-0.529295, Y=0.357690, Z=8.634424]
Heading: 13.611735
Heading Error: 0.064753
==============================
View more
0 Kudos
Top