The Angle(Quaternion) moves more (2 times or 3 times) when using MotionFX

[Min-Kyoung Kim]

When I use MotionFX, Quaternion seems to be moving more.

So when I multiple 2 or 3 on Gyro output, It moves well.

But when I move fast, It moves more and it returns to its correct place.

I attach my code. (multiple 2 on gyro output)

Gyro, Accel, Mag output

void Sensor_GetRawData(Sensor_StateTypeDef *handle)
{
	Sensor_StateTypeDef *ctx = (Sensor_StateTypeDef *)handle;
	Type3Axis16bit_U raw_data_tmp;
 
	Sensor_IO_SPI_CS_Enable(ctx);
	Sensor_IO_SPI_Read(ctx,ctx->RegRX, raw_data_tmp.u8bit,6);
	Sensor_IO_SPI_CS_Disable(ctx);
 
/* int32_t save*/
	ctx->pDataRaw->AXIS_X = ( int32_t )(raw_data_tmp.i16bit[0] * ctx->sensitivity);
	ctx->pDataRaw->AXIS_Y = ( int32_t )(raw_data_tmp.i16bit[1] * ctx->sensitivity);
	ctx->pDataRaw->AXIS_Z = ( int32_t )(raw_data_tmp.i16bit[2] * ctx->sensitivity);
}

Gyro Sensitivity : 70

Accel Sensitivity : 0.244

Mag Sensitivity : 1.5

I checked correct setting value.

Below, MotionFX function

deltatime : 0.01 (So I ran MotionFX Run function every 10ms.

void MotionFX_manager_init()
{
  char LibVersion[36];
  MotionFX_initialize();
 
  MotionFX_GetLibVersion(LibVersion);
 
  //ST MotionFX Engine Initializations
  ipKnobs = &iKnobs;
 
  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;
 
  ipKnobs->acc_orientation[0] ='w';
  ipKnobs->acc_orientation[1] ='s';
  ipKnobs->acc_orientation[2] ='u';
 
  ipKnobs->gyro_orientation[0] = 'w';
  ipKnobs->gyro_orientation[1] = 's';
  ipKnobs->gyro_orientation[2] = 'u';
 
  ipKnobs->mag_orientation[0] = 's';
  ipKnobs->mag_orientation[1] = 'w';
  ipKnobs->mag_orientation[2] = 'u';
 
  ipKnobs->output_type = MFX_ENGINE_OUTPUT_ENU;
  ipKnobs->LMode = 2;
  ipKnobs->modx  = 1;
 
  MotionFX_setKnobs(ipKnobs);
  
  MotionFX_enable_6X(MFX_ENGINE_DISABLE);
  MotionFX_enable_9X(MFX_ENGINE_ENABLE);
 
  discardedCount = 0;
}
 
void MotionFX_manager_run(T_SensorsData SensorData)
{
  uint8_t Rbuffer[100];
 
  iDataIN.gyro[0] = SensorData.gyro.AXIS_X * FROM_MDPS_TO_DPS;
  iDataIN.gyro[1] = SensorData.gyro.AXIS_Y * FROM_MDPS_TO_DPS;
  iDataIN.gyro[2] = SensorData.gyro.AXIS_Z * FROM_MDPS_TO_DPS;
 
  iDataIN.acc[0] = SensorData.acc.AXIS_X * FROM_MGASS_TO_GAUSS;
  iDataIN.acc[1] = SensorData.acc.AXIS_Y * FROM_MGASS_TO_GAUSS;
  iDataIN.acc[2] = SensorData.acc.AXIS_Z * FROM_MGASS_TO_GAUSS;
 
  iDataIN.mag[0] = (SensorData.mag.AXIS_X - MAG_Offset.AXIS_X) * FROM_MGAUSS_TO_UT50;
  iDataIN.mag[1] = (SensorData.mag.AXIS_Y - MAG_Offset.AXIS_Y) * FROM_MGAUSS_TO_UT50;
  iDataIN.mag[2] = (SensorData.mag.AXIS_Z - MAG_Offset.AXIS_Z) * FROM_MGAUSS_TO_UT50;
 
   MotionFX_propagate(&iDataOUT, &iDataIN, MOTIONFX_ENGINE_DELTATIME);
   MotionFX_update(&iDataOUT, &iDataIN, MOTIONFX_ENGINE_DELTATIME, NULL);
}