ISM330DHCx Angle Reading Error

[Amal Babu_]

Hi , STM Community 

i was using the ISM330DHCx 3D accelerometer and gyroscope Sensor for Angle Measurement purpose.

And decided to try with MOTION_FX libraryA  because i found an example related to this library.

i was able to get the Accelerometer and gyroscope values correctly but why i use the MOTION_FX library functions 

MotionFX_propagate(mfxstate, &data_out, &data_in, &dT)   ,  MotionFX_update(mfxstate, &data_out, &data_in, &dT, NULL) to get the angle data  through data_out.rotation[0] , data_out.rotation[1], data_out.rotation[2]

i was getting almost constant data like

data_out.rotation[0] = 356.23 ,

data_out.rotation[1]  = 7.88 ,

data_out.rotation[2]  = 2.34 

how to get the accurate angle from this ISM330DHCx sensor , can anyone suggest any way to read angle from the Sensor using MOTION_FX library or any error in the implementation....

Details about my test :

1.Controller used : STM32F103T8U6  64KB

2. used in Custom Board 

3. Verified both acceleration and Gyro Values both are changing correctly

4 platform used CUBE IDE

5 CLOCK used 72 MHz

6 connected using i2c at 400KHz

int main(void)

{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */

  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */

  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */

  MX_GPIO_Init();

  MX_CRC_Init();

  /* USER CODE BEGIN 2 */

  BSP_I2C1_Init();

  CUSTOM_MOTION_SENSOR_Init( 0, MOTION_GYRO);

  CUSTOM_MOTION_SENSOR_Init( 0, MOTION_ACCELERO);

  CUSTOM_MOTION_SENSOR_SetOutputDataRate(0, MOTION_ACCELERO, (float)ALGO_FREQ);

  CUSTOM_MOTION_SENSOR_SetFullScale(0, MOTION_ACCELERO, 4);

  CUSTOM_MOTION_SENSOR_SetOutputDataRate(0, MOTION_GYRO, (float)ALGO_FREQ);

  CUSTOM_MOTION_SENSOR_SetFullScale(0, MOTION_GYRO, 2000);

//  MotionFX_CM0P_initialize(MFX_CM0P_MCU_STM32);

//  MotionFX_CM0P_setOrientation("nwu", "nwu", NULL);

//  MotionFX_CM0P_enable_gbias(MFX_CM0P_ENGINE_ENABLE);

//  MotionFX_CM0P_enable_euler(MFX_CM0P_ENGINE_ENABLE);

//  MotionFX_CM0P_enable_6X(MFX_CM0P_ENGINE_ENABLE);

//  MotionFX_CM0P_enable_9X(MFX_CM0P_ENGINE_DISABLE);

  MotionFX_initialize((MFXState_t *)mfxstate);

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

  ipKnobs.acc_orientation[0] = 'n';

  ipKnobs.acc_orientation[1] = 'w';

  ipKnobs.acc_orientation[2] = 'u';

  ipKnobs.gyro_orientation[0] = 'n';

  ipKnobs.gyro_orientation[1] = 'w';

  ipKnobs.gyro_orientation[2] = 'u';

  ipKnobs.gbias_acc_th_sc = GBIAS_ACC_TH_SC;

  ipKnobs.gbias_gyro_th_sc = GBIAS_GYRO_TH_SC;

  ipKnobs.output_type = MFX_ENGINE_OUTPUT_ENU;

  ipKnobs.LMode = 1;

  ipKnobs.modx = DECIMATION;

//  ipKnobs->acc_orientation[0] = 'n';

//  ipKnobs->acc_orientation[1] = 'w';

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

//  ipKnobs->gyro_orientation[0] = 'n';

//  ipKnobs->gyro_orientation[1] = 'w';

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

//

//  ipKnobs->gbias_acc_th_sc = GBIAS_ACC_TH_SC;

//  ipKnobs->gbias_gyro_th_sc = GBIAS_GYRO_TH_SC;

//

//  ipKnobs->output_type = MFX_ENGINE_OUTPUT_ENU;

//  ipKnobs->LMode = 1;

//  ipKnobs->modx = DECIMATION;

  MotionFX_setKnobs(mfxstate, &ipKnobs);

  MotionFX_enable_6X(mfxstate, MFX_ENGINE_DISABLE);

  MotionFX_enable_9X(mfxstate, MFX_ENGINE_DISABLE);

  MotionFX_enable_6X(mfxstate, MFX_ENGINE_ENABLE);

  MotionFX_enable_9X(mfxstate, MFX_ENGINE_DISABLE);

    /* OPTIONAL */

    /* Get library version */

  LibVersionLen = (int)MotionFX_GetLibVersion(LibVersion)

  /* USER CODE END 2 */

  /* Infinite loop */

  /* USER CODE BEGIN WHILE */

  while (1)

  {

if( Read_Timer >= 100)

   {

    float dT;

  fusion_flag = 0;

  CUSTOM_MOTION_SENSOR_GetAxesRaw(0, MOTION_ACCELERO, &accr);

  CUSTOM_MOTION_SENSOR_GetAxesRaw(0, MOTION_GYRO, &gyro);

  /* Convert angular velocity from [mdps] to [dps] */

  data_in.gyro[0] = (float)gyro.x * FROM_MDPS_TO_DPS;

  data_in.gyro[1] = (float)gyro.y * FROM_MDPS_TO_DPS;

  data_in.gyro[2] = (float)gyro.z * FROM_MDPS_TO_DPS;

  /* Convert acceleration from [mg] to [g] */

  data_in.acc[0] = (float)accr.x * FROM_MG_TO_G;

  data_in.acc[1] = (float)accr.y * FROM_MG_TO_G;

  data_in.acc[2] = (float)accr.z * FROM_MG_TO_G;

  /* Don't set mag values because we use only acc and gyro */

  data_in.mag[0] = 0.0f;

  data_in.mag[1] = 0.0f;

  data_in.mag[2] = 0.0f;

  dT = (float) (CurrentTime/1000.0)- (LastTime);

  LastTime = CurrentTime/1000.0;

  MotionFX_propagate(mfxstate, &data_out, &data_in, &dT);

  MotionFX_update(mfxstate, &data_out, &data_in, &dT, NULL);

  }

}

}

Regards 

Amal