Three qestions about MotionFX CM0+

[WrenchInTheWorks]

Hello wonderfull STM community,

Really enjoying the experience with MotionFX so far, really easy to implement and get working, well done ST! Even on a G0 in CM0+ mode its working well! I'm just woudring three things:

1. What precaution(s), if any, should be taken when pausing and later resuming the RTOS task that runs MotionFX_CM0P_update()?
2. What is the refrence point for the quatenion in 6x mode? (to be fair I have not worked much with quatenions before so that might be an uneducated question. They are quite complex after all! (pun intended)) 
3. Is it normal for data_out.rotation_6x to be near 0 (e-16) during oppration in 6 axis mode? (other values, inlcuding inputs, are changing as expected)

Note that mag is just uninitialized, the values are those of what was already in memory, I should fix that, through this is the first I've noticed it.

Thanks and happy coding! 

[Pantera_MXJ]

Hello WrenchInTheWorks

Have you manage to get fusion values that are correct using the MotionFX library? like heading, Roll, Pitch, Yaw?

If yes how did you define this part:?

strcpy((char *)MotionFX_knobs.acc_orientation, "swu");
strcpy((char *)MotionFX_knobs.gyro_orientation, "swu");
strcpy((char *)MotionFX_knobs.mag_orientation, "enu");

and what sensor/board you are using?

Kind regards

JD

[WrenchInTheWorks]

Hello @Pantera_MXJ,

I'm getting values for everything but Roll, Pitch, Yaw, unsure why, maybe because I have not set my orientation yet or because you may not get them in 6x mode, not sure . I've got bigger problems in my code base to deal with now that I know I can get values in out of the FX engine.

So to answer your first question I ignored that part (for now), I'll let you know once I get that sorted.

I'm using a custom board with the LSM6DSMTR.

Cheers,

Michael 

[Pantera_MXJ]

Thank you! will wait for your feedback as I can't figure out how to set up my board correctly.

[WrenchInTheWorks]

What board do you have? Can you share your code? I may be able to help.

Cheers, 

Michael 

[Pantera_MXJ]

Hello WrenchInTheWorks

Thanks for the help! I have a IKS02A1 I had some progress but still some values are not ok, I think I'm close but something is wrong, I just added the code in the post I have related to this problem:

X-NUCLEO-IKS02A1 sensor orientation (axis) on PCB ... - STMicroelectronics Community

Kind regards.

JD

[Federica Bossi · Accepted solution]

Hi @Pantera_MXJ ,

I share also here the corrected code you posted on the other topic, so that if someone else in the future encounters the same issue can solve:

/**
 ******************************************************************************
 * File Name          : app_mems.c
 * Description        : This file provides code for the configuration
 * of the STMicroelectronics.X-CUBE-MEMS1.11.3.0 instances.
 ******************************************************************************
 * @attention
 *
 * JMX Fusion Terminal JUL 17 2025
 * 
 *
 * This software is licensed under terms that can be found in the LICENSE file
 * in the root directory of this software component.
 * If no LICENSE file comes with this software, it is provided AS-IS.
 *
 ******************************************************************************
 */

#ifdef __cplusplus
extern "C" {
#endif

/* Includes ------------------------------------------------------------------*/
#include "app_mems.h"
#include "main.h"
#include <stdio.h>
#include <string.h>

#define _USE_MATH_DEFINES
#include <math.h>

#include "iks02a1_motion_sensors.h"
#include "stm32u5xx_nucleo.h"
#include "motion_fx.h"
#include "iks02a1_mems_control_ex.h"

/* Private typedef -----------------------------------------------------------*/
typedef enum {
  APP_STATE_CALIBRATING,
  APP_STATE_RUNNING
} AppState_t;

/* Private define ------------------------------------------------------------*/
#define SAMPLING_FREQ_HZ           100
#define SAMPLING_PERIOD_MS         (1000 / SAMPLING_FREQ_HZ)
#define STATE_SIZE                 (2450)
#define PRINT_FREQUENCY_HZ         5
#define PRINT_INTERVAL_CYCLES      (SAMPLING_FREQ_HZ / PRINT_FREQUENCY_HZ)
#define GBIAS_ACC_TH_SC            (2.0f * 0.000765f)
#define GBIAS_GYRO_TH_SC           (2.0f * 0.002f)
#define GBIAS_MAG_TH_SC            (2.0f * 0.001500f)
#define DECIMATION                 1U
#define MAG_CAL_DIVISOR            500.0f

/* Private variables ---------------------------------------------------------*/
static MFX_knobs_t MotionFX_knobs;
static MFX_input_t  data_in;
static MFX_output_t data_out;
static uint8_t      mfxstate[STATE_SIZE];
static MFX_MagCal_input_t mag_data_in_cal;
static MFX_MagCal_output_t mag_cal_out;

static AppState_t   AppState = APP_STATE_CALIBRATING;
static uint32_t     print_counter = 0;
static IKS02A1_MOTION_SENSOR_Axes_t mag_axes;

/* Private function prototypes -----------------------------------------------*/
static void MX_IKS02A1_DataLogTerminal_Init(void);
static void MX_IKS02A1_DataLogTerminal_Process(void);
static void initialize_motion_fx(void);
static void print_full_output(void);
static void print_calibration_status(void);

void MX_MEMS_Init(void)
{
  MX_IKS02A1_DataLogTerminal_Init();
}

void MX_MEMS_Process(void)
{
  MX_IKS02A1_DataLogTerminal_Process();
}

/**
  * @brief  Initialize the application.
  */
void MX_IKS02A1_DataLogTerminal_Init(void)
{
  BSP_LED_Init(LED2);
  BSP_PB_Init(BUTTON_KEY, BUTTON_MODE_EXTI);
  BSP_COM_Init(COM1);

  printf("-- STMicroelectronics JMX MotionFX Fusion DEMO --\r\n");

  IKS02A1_MOTION_SENSOR_Init(IKS02A1_ISM330DHCX_0, MOTION_ACCELERO | MOTION_GYRO);
  IKS02A1_MOTION_SENSOR_Init(IKS02A1_IIS2MDC_0, MOTION_MAGNETO);
  IKS02A1_MOTION_SENSOR_Enable(IKS02A1_ISM330DHCX_0, MOTION_ACCELERO);
  IKS02A1_MOTION_SENSOR_Enable(IKS02A1_ISM330DHCX_0, MOTION_GYRO);
  IKS02A1_MOTION_SENSOR_Enable(IKS02A1_IIS2MDC_0, MOTION_MAGNETO);
  printf("Programando valores de Rango para ISM330DHCX Gyro/Acc...\r\n");
  IKS02A1_MOTION_SENSOR_SetFullScale(IKS02A1_ISM330DHCX_0, MOTION_ACCELERO, 4);
  IKS02A1_MOTION_SENSOR_SetFullScale(IKS02A1_ISM330DHCX_0, MOTION_GYRO, 500);
  int32_t fs_acc;
  int32_t fs_gyro;
  IKS02A1_MOTION_SENSOR_GetFullScale(IKS02A1_ISM330DHCX_0, MOTION_ACCELERO, &fs_acc);
  IKS02A1_MOTION_SENSOR_GetFullScale(IKS02A1_ISM330DHCX_0, MOTION_GYRO, &fs_gyro);
  printf("Accelerometer FSR set to: %ld g\r\n", fs_acc);
  printf("Gyroscope FSR set to: %ld dps\r\n", fs_gyro);


  IKS02A1_MOTION_SENSOR_SetOutputDataRate(IKS02A1_ISM330DHCX_0, MOTION_ACCELERO, SAMPLING_FREQ_HZ);
  IKS02A1_MOTION_SENSOR_SetOutputDataRate(IKS02A1_ISM330DHCX_0, MOTION_GYRO, SAMPLING_FREQ_HZ);
  IKS02A1_MOTION_SENSOR_SetOutputDataRate(IKS02A1_IIS2MDC_0, MOTION_MAGNETO, SAMPLING_FREQ_HZ);

  initialize_motion_fx();
}

/**
  * @brief  Main application process.
  */
void MX_IKS02A1_DataLogTerminal_Process(void)
{
    IKS02A1_MOTION_SENSOR_Axes_t acc_axes, gyro_axes;
    float delta_time = (float)SAMPLING_PERIOD_MS / 1000.0f;

    IKS02A1_MOTION_SENSOR_GetAxes(IKS02A1_ISM330DHCX_0, MOTION_ACCELERO, &acc_axes);
    IKS02A1_MOTION_SENSOR_GetAxes(IKS02A1_ISM330DHCX_0, MOTION_GYRO,    &gyro_axes);
    IKS02A1_MOTION_SENSOR_GetAxes(IKS02A1_IIS2MDC_0, MOTION_MAGNETO,    &mag_axes);

    data_in.acc[0]  = acc_axes.x / 1000.0f;
    data_in.acc[1]  = acc_axes.y / 1000.0f;
    data_in.acc[2]  = acc_axes.z / 1000.0f;

    data_in.gyro[0] = gyro_axes.x / 1000.0f;
    data_in.gyro[1] = gyro_axes.y / 1000.0f;
    data_in.gyro[2] = gyro_axes.z / 1000.0f;

    if (AppState == APP_STATE_CALIBRATING)
    {
        // Durante la calibración, alimentar la utilidad MagCal con datos en uT/50
        mag_data_in_cal.mag[0] = mag_axes.x / MAG_CAL_DIVISOR;
        mag_data_in_cal.mag[1] = mag_axes.y / MAG_CAL_DIVISOR;
        mag_data_in_cal.mag[2] = mag_axes.z / MAG_CAL_DIVISOR;
        mag_data_in_cal.time_stamp = HAL_GetTick();
        MotionFX_MagCal_run(&mag_data_in_cal);

        print_calibration_status();
        MotionFX_MagCal_getParams(&mag_cal_out);

        if (mag_cal_out.cal_quality == MFX_MAGCALGOOD)
        {
            printf("\r\n--- CALIBRACION COMPLETA! ---\r\n");
            printf("Iniciando salida de datos de fusion...\r\n\r\n");
            AppState = APP_STATE_RUNNING;
        }
    }
    else if (AppState == APP_STATE_RUNNING)
    {

        data_in.mag[0] = (mag_axes.x / MAG_CAL_DIVISOR) - mag_cal_out.hi_bias[0];
        data_in.mag[1] = (mag_axes.y / MAG_CAL_DIVISOR) - mag_cal_out.hi_bias[1];
        data_in.mag[2] = (mag_axes.z / MAG_CAL_DIVISOR) - mag_cal_out.hi_bias[2];
        MotionFX_propagate((MFXState_t *)mfxstate, &data_out, &data_in, &delta_time);
        MotionFX_update((MFXState_t *)mfxstate, &data_out, &data_in, &delta_time, NULL);

        print_counter++;
        if (print_counter >= PRINT_INTERVAL_CYCLES)
        {
            print_full_output();
            print_counter = 0;
        }
    }

    HAL_Delay(SAMPLING_PERIOD_MS);
}

/**
  * @brief  Initialize the MotionFX library and configure knobs.
  */
static void initialize_motion_fx(void)
{
    __HAL_RCC_CRC_CLK_ENABLE();
    MotionFX_initialize((MFXState_t *)mfxstate);

    MotionFX_getKnobs((MFXState_t *)mfxstate, &MotionFX_knobs);

    BSP_SENSOR_ACC_GetOrientation(MotionFX_knobs.acc_orientation);
    BSP_SENSOR_GYR_GetOrientation(MotionFX_knobs.gyro_orientation);
    BSP_SENSOR_MAG_GetOrientation(MotionFX_knobs.mag_orientation);

    MotionFX_knobs.gbias_acc_th_sc  = GBIAS_ACC_TH_SC;
    MotionFX_knobs.gbias_gyro_th_sc = GBIAS_GYRO_TH_SC;
    MotionFX_knobs.gbias_mag_th_sc  = GBIAS_MAG_TH_SC;
    MotionFX_knobs.LMode            = 1;
    MotionFX_knobs.modx             = DECIMATION;
    MotionFX_knobs.output_type      = MFX_ENGINE_OUTPUT_ENU;
    MotionFX_knobs.ATime = 0.2f;
    MotionFX_knobs.MTime = 0.1f;



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

    printf("Orientations -> ACC:%s, GYR:%s, MAG:%s\r\n",
           MotionFX_knobs.acc_orientation,
           MotionFX_knobs.gyro_orientation,
           MotionFX_knobs.mag_orientation);
    printf("Fusion output type -> %s\r\n",
           (MotionFX_knobs.output_type == MFX_ENGINE_OUTPUT_ENU) ? "ENU" : "NED");

    MotionFX_enable_9X((MFXState_t *)mfxstate, MFX_ENGINE_ENABLE);

    MotionFX_MagCal_init(SAMPLING_PERIOD_MS, 1);

    printf("Slowly rotate the board in a figure-8 pattern to calibrate the magnetometer.\r\n\r\n");
}

/**
  * @brief  Print the full fused data output and diagnostic info.
  */
static void print_full_output(void)
{
    float quat_q0 = data_out.quaternion[0];
    float quat_q1 = data_out.quaternion[1];
    float quat_q2 = data_out.quaternion[2];
    float quat_q3 = data_out.quaternion[3];

    float rot_yaw   = data_out.rotation[0];
    float rot_pitch = data_out.rotation[1];
    float rot_roll  = data_out.rotation[2];

    float g_x = data_out.gravity[0];
    float g_y = data_out.gravity[1];
    float g_z = data_out.gravity[2];

    float la_x = data_out.linear_acceleration[0];
    float la_y = data_out.linear_acceleration[1];
    float la_z = data_out.linear_acceleration[2];

    float fused_heading = data_out.heading;
    float heading_error = data_out.headingErr;

    printf("Q(%.4f,%.4f,%.4f,%.4f) R(Y:%.4f,P:%.4f,R:%.4f) G_g(x:%.4f,y:%.4f,z:%.4f) LA_g(x:%.4f,y:%.4f,z:%.4f) H_error:%.3f Heading_Fused:%.2f\r\n",
           quat_q0, quat_q1, quat_q2, quat_q3,
           rot_yaw, rot_pitch, rot_roll,
           g_x, g_y, g_z,
           la_x, la_y, la_z,
		   heading_error, fused_heading);
}

/**
  * @brief  Print only the magnetometer calibration status.
  */
static void print_calibration_status(void)
{
    MFX_MagCal_output_t mag_cal_out_local;
    MotionFX_MagCal_getParams(&mag_cal_out_local);

    MFX_MagCal_quality_t cal_quality = mag_cal_out_local.cal_quality;
    char *quality_str;
    switch(cal_quality)
    {
      case MFX_MAGCALUNKNOWN: quality_str = "UNKNOWN"; break;
      case MFX_MAGCALPOOR:    quality_str = "POOR";    break;
      case MFX_MAGCALOK:      quality_str = "OK";      break;
      case MFX_MAGCALGOOD:    quality_str = "GOOD";    break;
      default:                quality_str = "ERROR";   break;
    }

    printf("Calibrating... Quality: %s         \r", quality_str);
}

#ifdef __cplusplus
}
#endif