Strange yaw behavior in using MotionFx Library?

[MChen.10]

I am trying to feed the sensor readings (gyro in dps and acceleration in G) to the MotionFx library, correctly specify the orientation (verified by ST local FAE) for 6X calculation.

At beginning I put the PCB on table, after power on and initialization/calibration, when I rotate the PCB around the roll axis (pure roll) by 45 degrees I can see both roll and yaw output change (roll angle is about 45 deg and yaw may be 30 deg).

This situation won't happen when I use the Madgwick filter with same sensor readings and procedure, is this behavior correct for MotionFx library or anything I missed?

[Miroslav BATEK]

Can you please share your MotionFX knobs settings and sensor(s) you use?

[MChen.10]

The MCU / sensor I used is STM32F401 / ISM330DLC and the mark of first pin of sensor is on the right bottom corner of PCB.

#define MFX_GBIAS_GYO_TH_SC_6X (2.0f*0.002f)

#define MFX_GBIAS_ACC_TH_SC_6X (2.0f*0.000765f)

#define MFX_GBIAS_MAG_TH_SC_6X (2.0f*0.001500f)

Mfx_init() {

MotionFX_initialize();

MotionFX_MagCal_init(1.0, MFX_ENGINE_DISABLE); // disable magnetic calibration library

MotionFX_GetLibVersion(Mfx_lib_version);

MotionFX_getKnobs( &Mfx_parms );

Mfx_parms.LMode = 1;

Mfx_parms.modx  = 2;

Mfx_parms.output_type = MFX_ENGINE_OUTPUT_NED;

Mfx_parms.gbias_gyro_th_sc_6X = MFX_GBIAS_GYO_TH_SC_6X;

Mfx_parms.gbias_acc_th_sc_6X = MFX_GBIAS_ACC_TH_SC_6X;

Mfx_parms.gbias_mag_th_sc_6X = MFX_GBIAS_MAG_TH_SC_6X;

Mfx_parms.acc_orientation[0] = 'w';

Mfx_parms.acc_orientation[1] = 's';

Mfx_parms.acc_orientation[2] = 'u';

Mfx_parms.gyro_orientation[0] = 'w';

Mfx_parms.gyro_orientation[1] = 's';

Mfx_parms.gyro_orientation[2] = 'u';

MotionFX_setKnobs( &Mfx_parms );

MotionFX_enable_6X( MFX_ENGINE_DISABLE );

MotionFX_enable_9X( MFX_ENGINE_DISABLE );

// delay few ms

MotionFX_enable_6X( MFX_ENGINE_ENABLE );

}

[Miroslav BATEK]

It seems OK to me.

What is the frequency you read the data from the sensor and you call MotionFX_propagate, MotionFX_update?

I would be helpful if you can share a datalog with the input and output data.

[MChen.10]

Hi Miroslav,

The datalog with input/output data are stored in cloud storage

https://drive.google.com/file/d/18bH_K9HOR5xdT9ELLL8XlVeuWXOcjYCG/view?usp=sharing

https://drive.google.com/file/d/1WBW35xbjrFfI7hICrrGDV0ecQeVUlQZW/view?usp=sharing

Thanks for your help in advance.

[Eleon BORLINI]

Hi @MChen.10​ ,

can you please give a feedback in case you got further insights on your issue?

Tks

-Eleon

[MChen.10]

Yes I will give a log as suggested by Miroslav ASAP (sorry for late because I am occupied in solving custom's problem these days)

[Miroslav BATEK]

Hi @MChen.10​,

I have analyzed your datalog and checked the video. Where did you get the attitude values? Is it directly output from the MotionFX or did you make some calculation?

I have extracted the Acc and Gyro data from you log and passed them through MotionFX library. The output looks good, I can see rotation only in one axis, which is in line with your movement. You can see my log in attachment.

[MChen.10]

Hi Miroslav,

Below is the code for calling MotionFx library. I read the raw data from registers directly (not FIFO) and then subtract by the bias which is calculated in calibration phase (put on stationary table) after power on and well configured, translate them to correct unit, say, dps and g, before saving to Mfx_din.

========================================================================

while (1)

 {

  /* USER CODE END WHILE */

  // --------------------------------------------------------------

  currTick_100us = HAL_GetTickIn100us();

  dTick = TIME_DIFF(currTick_100us, prevTick_100us);

  // IMU_FUSION_RATE_TICK = 200 for 50Hz

  if ( dTick >= IMU_FUSION_RATE_TICK ) 

  {

   prevTick_100us = currTick_100us;

   if ( firstFusion ) {

    firstFusion = 0;

    // read GYO/ACC from registers directly and translate the raw

    // data to Mfx_din structure

    imu_xsm330_nondma_read( &hi2c1, 1, 1 );

   }

   else {

    imu_xsm330_ahrs_update( dTick * IMU_FUSION_DELTA_T );

    imu_xsm330_nondma_read( &hi2c1, 1, 1 );

   }

  } // compare dTick to IMU_FUSION_RATE_TICK

  // --------------------------------------------------------------

 }

  

void imu_xsm330_ahrs_update( float dt )

{

 float dT = dt;

 MotionFX_propagate( &Mfx_dout, &Mfx_din, &dT );

 MotionFX_update( &Mfx_dout, &Mfx_din, &dT, NULL );

}

========================================================================

Anyway, I will check your log file and see what is the difference between our result.

BRs,

MJ

[MChen.10]

Hi Miroslav,

May I know what is the version of library you used? My library is ST MotionFX v2.4.0.

BRs,

MJ