MotionFX "nan" values after a while on Discovery kit IoT node

[Walter Lucetti]

Hi,

I'm testing the MotionFX library with the Discovery Kit IoT node [B-L4S5I-IOT01-A] and I have a problem: randomly the sensor fusion algorithm "explodes" and I can only get only "nan" values for all the `MFX_MagCal_output_t` values, Gyro Biases included.

What can be the cause of this behavior?

Following my initialization code:

int32_t initMotionFX()
{
  char lib_version[MFX_STR_LENG];
  MFX_knobs_t iKnobs;
  /* Sensor Fusion API initialization function */
  MotionFX_initialize();
 
  MotionFX_GetLibVersion(lib_version);
 
  MotionFX_getKnobs(&iKnobs);
  iKnobs.LMode = 1;
  iKnobs.gyro_orientation[0] = 'n';
  iKnobs.gyro_orientation[1] = 'w';
  iKnobs.gyro_orientation[2] = 'u';
  iKnobs.acc_orientation[0] = 'n';
  iKnobs.acc_orientation[1] = 'w';
  iKnobs.acc_orientation[2] = 'u';
  // Note: the magnetometer is rotated by 180° respect to the IMU
  iKnobs.mag_orientation[0] = 's';
  iKnobs.mag_orientation[1] = 'e';
  iKnobs.mag_orientation[2] = 'u';
 
  MotionFX_setKnobs(&iKnobs);
 
  // ----> Experimental init bias value
  // Note: to be replaced by flash memory reading
  init_gyro_calib[0] = 0.08; 
  init_gyro_calib[1] = 0.02;
  init_gyro_calib[2] = 1.02;
  MotionFX_setGbias(init_gyro_calib);
  // <---- Experimental init bias value
 
  MotionFX_enable_6X(MFX_ENGINE_ENABLE);
  MotionFX_enable_9X(MFX_ENGINE_ENABLE);
 
  return 0;
}

[Eleon BORLINI]

Hi @Community member​ ,

can you please be more specific with the "explosion" of the algorithm? is it an issue due to a wrong calculation (I mean, something like and overflow issue coming from cumulated errors), or data in input to the algorithm are corrupted (did you checked streaming data directly from the sensors?)

Since the X-MEMS-CUBE1 have been developed on the NUCLEO-L476RG (that is the Nucleo board with the micro closer to the B-L4S5I-IOT01-A), I would suggest you to check the integrity of the signals first, adding periodical calibration phases before running the MotionFX, such as the MotionAC accelerometer calibration and the MotionGC gyroscope calibration libraries.

This would prevent cumulating offset errors coming from integrations.

-Eleon

[Walter Lucetti]

Hi Eleon,

thank you for your reply.

I also suppose ​that is something related to the input data, so I checked them and they seem correct (unless I'm using wrong units).

I'm providing the gyroscope biases estimated by MotionFX during initialization, but I have not calibrated the accelerometer.

I have a doubt about the units because it is not well specified in the documentation, but I suppose that raw accelerations ​from are in mg, raw angular rates are in mdps and raw magnetic field are in gauss (10^-4 uT).

I'm going to try to use MotionAC to calibrate the accelerometer.

​I​ have a doubt: the gyro biases are estimated by MotionFX while running, but are they applied or must I subtract them from the raw data as for the magnetometer biases?

Thank you

Walter

[Miroslav BATEK]

Hello Walter,

I think the problem is in wrong units.

Please check the motion.h file.

typedef struct
{
  float mag[MFX_NUM_AXES];                 /* Calibrated mag [uT]/50 */
  float acc[MFX_NUM_AXES];                 /* Acceleration in [g] */
  float gyro[MFX_NUM_AXES];                /* Angular rate [dps] */
} MFX_input_t;
 
typedef struct {
  float mag[MFX_NUM_AXES];                 /* Uncalibrated mag [uT]/50 */
  int time_stamp;                          /* Timestamp [ms] */
} MFX_MagCal_input_t;

The accelerations must be in g, angular rates in dps and magnetic field in uT/50.

In LMode = 1 the gyro bias is estimated by the library so you don't have to correct the data before passing to the library

You can check the user manual for MotionFX for more details.

https://www.st.com/content/ccc/resource/technical/document/user_manual/group0/31/0e/66/39/cb/f7/4e/cd/DM00394369/files/DM00394369.pdf/jcr:content/translations/en.DM00394369.pdf

[Walter Lucetti]

Hi @Miroslav BATEK​ 

thank you for the reply.

The validity of the measure units has been my first doubt. I know what are the required input units, but I'm not sure about the units of the outputs of the LSM6DSL and of the LIS3MDL.

In my code, I'm adopting this:

#define FROM_MGAUSS_TO_UT50  (0.1f/50.0f)
[...]
    mag_cal_in.mag[0] = ((float) sensor_data.magRaw[0]) * FROM_MGAUSS_TO_UT50; // uT/50
    mag_cal_in.mag[1] = ((float) sensor_data.magRaw[1]) * FROM_MGAUSS_TO_UT50; // uT/50
    mag_cal_in.mag[2] = ((float) sensor_data.magRaw[2]) * FROM_MGAUSS_TO_UT50; // uT/50
    mag_cal_in.time_stamp = (int) HAL_GetTick();
 
[...]
      data_in.acc[0] = ((float) sensor_data.accRaw[0]) / 1e3; // g
      data_in.acc[1] = ((float) sensor_data.accRaw[1]) / 1e3; // g
      data_in.acc[2] = ((float) sensor_data.accRaw[2]) / 1e3; // g
      data_in.gyro[0] = ((float) sensor_data.gyroRaw[0]) / 1e3; // dps
      data_in.gyro[1] = ((float) sensor_data.gyroRaw[1]) / 1e3; // dps
      data_in.gyro[2] = ((float) sensor_data.gyroRaw[2]) / 1e3; // dps

and I'm using the STM drivers to get the data: `LSM6DSL_ACC_GetAxes`, `LSM6DSL_GYRO_GetAxes` and `LIS3MDL_MAG_GetAxes`.

While I'm pretty sure that the returned accelerations are in [mg], I cannot say the same about the angular rates [mdps?] and magnetic fields [mGauss?].

What do you think?

[Miroslav BATEK]

Hi Walter,

function LSM6DSL_ACC_GetAxes returns angular rate in mdps and LIS3MDL_MAG_GetAxes returns magnetic field in mGauss.

[Walter Lucetti]

So I'm using correct data units. At this point, the only problem can come from accelerometer or magnetometer calibration.

Is there a way to provide an initial guess of magnetometer biases to the algorithm?

[Miroslav BATEK]

Can you share more of your code? (Where do you call MotionFX_MagCal_init and MotionFX_MagCal_run).

Can you share data which you are passing to the MotionFX_MagCal_run function?

[Walter Lucetti]

@Miroslav BATEK​ I shared the code of IMU, Magnetometer, and MotionFX tasks on Dropbox.

Let me know if you can correctly get it.

[Miroslav BATEK]

I was able to download the files and I checked them. Unfortunately I don't see anything wrong.

Would it be possible to collect a few seconds of data which are passed to the MotionFX_MagCal_run and the output?