cancel
Showing results for 
Search instead for 
Did you mean: 

MotionFX gives wrong results

Sergey Sokol
Associate II
Posted on February 14, 2018 at 20:16

Hello to all,

I'm trying to get run the MotionFX library using STM32F4 Discovery board and STEVAL-MKI159V1 board (which consists of 9 axes LSM9DS1 inertial module). I represent this module as a combination of LSM6DS0 accelerometer+gyroscope and LIS3MDL magnetometer which seems to be correct according to the data sheets of these modules.

I got the library run but the output values are very weird.

Firstly, I can't find the units of them anywhere. Are they the same as input units?

Secondly, the gravity data just roll from -1 to 1 for all axes.

The acceleration and rotation seem to be random numbers.

The heading has some consistency but sill jumps as well.

Also I never can get the magnetometer calibration done.

I've checked the input data and it seems to be correct for all sensors.

Can anybody help me and explain what I'm doing wrong?

Kind regards,

Sergey

#lsm9ds1 #motionfx #lis3mdl #lsm6ds0

Note: this post was migrated and contained many threaded conversations, some content may be missing.
9 REPLIES 9
Jaime Tortosa_O
Associate
Posted on February 15, 2018 at 15:04

Hi Sergey,

I suggest to start with the example, DataLogFusion is using MotionFX library.

Have you read already the

http://www.st.com/content/ccc/resource/technical/document/design_tip/group0/4e/f7/06/e0/34/0f/44/ca/DM00358267/files/DM00358267.pdf/jcr:content/translations/en.DM00358267.pdf

?
Sergey Sokol
Associate II
Posted on February 15, 2018 at 15:49

Hi Jaime,

I am using exactly this example which I've changed a bit to run it with the available hardware.

I read the Quick guide but haven't found there anything that can help me.

Is it possible that this weird behavior is caused by incorrect knobs?

Kind regards,

Sergey

Posted on February 15, 2018 at 17:00

Yes, knobs have a big impact on the behavior.

The input values are:

  • mag in uT/50
  • acc in g
  • gyro in dps

Make sure you're passing the data with that units.

The output data is:

  • rotation in degrees
  • quaternion has not units
  • gravity in g
  • linear acceleration in m/s2
Posted on February 15, 2018 at 18:02

Thank you very much for clarifications.

I left the knobs as the were in the original DataLogFusion example, just changed the orientation (maybe incorrectly though, but I tried different variants, the result is the same).

And I wonder why magnetometer calibration never gets successful...

Posted on February 16, 2018 at 10:11

Please share you configuration, in particular the orientation settings.

There could be several reasons why the magnetometer calibration never gets successful. Do you have the right unit of the input? Did you perform '8' shape movement with the board?

Posted on February 16, 2018 at 11:13

Hello Miroslav,

Thanks for your reply.

Here's my code of configuration the MotionFx module:

void MotionFX_manager_init(void *handle)

{

  uint8_t instance;

  MotionFX_initialize();

  MotionFX_getKnobs(ipKnobs);

  ipKnobs->gbias_acc_th_sc_6X = GBIAS_ACC_TH_SC_6X;

  ipKnobs->gbias_gyro_th_sc_6X = GBIAS_GYRO_TH_SC_6X;

  ipKnobs->gbias_mag_th_sc_6X = GBIAS_MAG_TH_SC_6X;

  ipKnobs->gbias_acc_th_sc_9X = GBIAS_ACC_TH_SC_9X;

  ipKnobs->gbias_gyro_th_sc_9X = GBIAS_GYRO_TH_SC_9X;

  ipKnobs->gbias_mag_th_sc_9X = GBIAS_MAG_TH_SC_9X;

  BSP_GYRO_Get_Instance(handle, &instance);

  switch (instance)

  {

    case LSM6DS0_G_0:

      ipKnobs->acc_orientation[0] = 'e';

      ipKnobs->acc_orientation[1] = 'n';

      ipKnobs->acc_orientation[2] = 'u';

      ipKnobs->gyro_orientation[0] = 'e';

      ipKnobs->gyro_orientation[1] = 'n';

      ipKnobs->gyro_orientation[2] = 'u';

      break;

    default:

      return;

 }

  ipKnobs->mag_orientation[0] = 's';

  ipKnobs->mag_orientation[1] = 'e';

  ipKnobs->mag_orientation[2] = 'u';

  ipKnobs->output_type = MFX_ENGINE_OUTPUT_ENU;

  ipKnobs->LMode = 1;

  ipKnobs->modx = 1;

  MotionFX_setKnobs(ipKnobs);

  MotionFX_enable_6X(MFX_ENGINE_DISABLE);

  MotionFX_enable_9X(MFX_ENGINE_DISABLE);

}

The bias values are defined as followed:

&sharpdefine GBIAS_ACC_TH_SC_6X              (2.0f*0.000765f)

&sharpdefine GBIAS_GYRO_TH_SC_6X             (2.0f*0.002f)

&sharpdefine GBIAS_MAG_TH_SC_6X              (2.0f*0.001500f)

&sharpdefine GBIAS_ACC_TH_SC_9X              (2.0f*0.000765f)

&sharpdefine GBIAS_GYRO_TH_SC_9X             (2.0f*0.002f)

&sharpdefine GBIAS_MAG_TH_SC_9X              (2.0f*0.001500f)

And here is my code of using MotionFX library

static void FX_Data_Handler(TMsg *Msg)

{

  MFX_input_t data_in;

  MFX_input_t *pdata_in = &data_in;

  MFX_output_t data_out;

  MFX_output_t *pdata_out = &data_out;

    char str[21];

  if ((Sensors_Enabled & ACCELEROMETER_SENSOR) && (Sensors_Enabled & GYROSCOPE_SENSOR) && (Sensors_Enabled & MAGNETIC_SENSOR))

  {

    data_in.gyro[0] = GYR_Value.AXIS_X  * FROM_MDPS_TO_DPS;

    data_in.gyro[1] = GYR_Value.AXIS_Y  * FROM_MDPS_TO_DPS;

    data_in.gyro[2] = GYR_Value.AXIS_Z  * FROM_MDPS_TO_DPS;

    data_in.acc[0] = ACC_Value.AXIS_X * FROM_MG_TO_G;

    data_in.acc[1] = ACC_Value.AXIS_Y * FROM_MG_TO_G;

    data_in.acc[2] = ACC_Value.AXIS_Z * FROM_MG_TO_G;

    data_in.mag[0] = MAG_Value.AXIS_X * FROM_MGAUSS_TO_UT50;

    data_in.mag[1] = MAG_Value.AXIS_Y * FROM_MGAUSS_TO_UT50;

    data_in.mag[2] = MAG_Value.AXIS_Z * FROM_MGAUSS_TO_UT50;

       

    /* Run Sensor Fusion algorithm */

    MotionFX_manager_run(pdata_in, pdata_out, MOTIONFX_ENGINE_DELTATIME);

    if (SF_6X_Enabled == 1)

    {

       memcpy(&Msg->Data[55], (void *)pdata_out->quaternion_6X, 4 * sizeof(float));

       memcpy(&Msg->Data[71], (void *)pdata_out->rotation_6X, 3 * sizeof(float));

       memcpy(&Msg->Data[83], (void *)pdata_out->gravity_6X, 3 * sizeof(float));

       memcpy(&Msg->Data[95], (void *)pdata_out->linear_acceleration_6X, 3 * sizeof(float));

       memcpy(&Msg->Data[107], (void *)&(pdata_out->heading_6X), sizeof(float));

    }

    else

    {

       memcpy(&Msg->Data[55], (void *)pdata_out->quaternion_9X, 4 * sizeof(float));

       memcpy(&Msg->Data[71], (void *)pdata_out->rotation_9X, 3 * sizeof(float));

       memcpy(&Msg->Data[83], (void *)pdata_out->gravity_9X, 3 * sizeof(float));

       memcpy(&Msg->Data[95], (void *)pdata_out->linear_acceleration_9X, 3 * sizeof(float));

       memcpy(&Msg->Data[107], (void *)&(pdata_out->heading_9X), sizeof(float));

    }

  }

}

I've checked the inputs and they seem to be correct. At least accelerometer and gyroscope data. The magnetometer raw data seem to have strong hard iron shift.

As for calibration I performed different shapes and none of thew was successful. Should I just perform 8 shape around X axle?

Kind regards,

Sergey.

Posted on February 16, 2018 at 11:52

I think the orientation for the LSM9DS1 is 'NEU' for acc and gyro and 'SEU' for the magnetometer.

The 8 shape should be done in 3D space, but you can also do circle along each axis.

If you are able to draw mag data in X-Y, X-Z, Y-Z 2D charts it would be helpful. 

I expect you enable the sensor fusion and mag calibration somewhere in your program.

Posted on February 16, 2018 at 13:54

Miroslav, changing the orientation to NEU changed nothing. It still behaves in the same way.

I was performing rotation along each 3 axis to calibrate magnetometer but it was unsuccessful yet.

The MotionFX initialization is performed as follows:

/* Sensor Fusion API initialization function */

  MotionFX_manager_init(GYRO_handle);

  /* OPTIONAL */

  /* Get library version */

  MotionFX_manager_get_version(lib_version, &lib_version_len);

    

    /* Enable magnetometer calibration */

  MotionFX_manager_MagCal_start(SAMPLE_PERIOD);

  /* Test if calibration data are available */

  MFX_MagCal_output_t mag_cal_test;

  MotionFX_MagCal_getParams(&mag_cal_test);

  MotionFX_manager_start_9X();

Here are my charts:

0690X00000609ikQAA.png0690X00000609hcQAA.png0690X00000609hhQAA.png

Looks like there is a big soft iron distortion because the dependence isn't spherical but elliptical.

Posted on February 17, 2018 at 11:40

I was trying to use 6X algorithm instead of 9X and it just hangs after the first iteration

Also the application with 9X just hangs in a random moment and I need to reset the MCU to run it again. And this happens every time sooner or later.

One more note. I use the SSD1306 OLED display to indicate the data which shares the I2C interface with motion sensor. Can it affect somehow on the MotionFX algorithm work?

I attach my whole project, maybe it can help to understand what is wrong...

________________

Attachments :

SensorFusion.zip : https://st--c.eu10.content.force.com/sfc/dist/version/download/?oid=00Db0000000YtG6&ids=0680X000006Hxxl&d=%2Fa%2F0X0000000b2r%2F9shpzAzh9zyCv.0Db7K4CNJDCAv03SHAglTgYUmiklU&asPdf=false