Question
Problem with Heading Calculation
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);
}
}
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);
}
}
Does anyone know why this happens? Any insights would be greatly appreciated! Let me know if you need more details.