magnetometer Calibration Library HardFault

[MRaff.1]

Hello, 

   I am working on a custom board based on STM32F407 CPU with the LSM6DSV16XTR accelerometer+gyroscope and LIS2MDLTR magnetometer.

The aim is to get board inclination and compass information with the maximum accuracy.

The MotionFX library seems to work fine but the data from magnetometer are not correct since it requires calibration. 

On CubeMX even if I select the MotionMC software package, the MotionMC library is not added to the output.  I supposed this happen because the Magnetometer calibration functions are already available on the MotionFX library. 

The MX_MEMS_Process() is called from a FreeRTOS task, SensorReadRequest is set to 1 from a timer as done in the application demo. 

The problem is that

MotionFX_MagCal_getParams(&mag_cal_data_out);

does not update the mag_cal_data_out variable. All the hi_bias output values are 0, cal_quality always 0. During calibration I move the board with the typical "8" pattern but nothing happen. 

static MOTION_SENSOR_Axes_t AccValue;
static MOTION_SENSOR_Axes_t GyrValue;
static MOTION_SENSOR_AxesRaw_t GyrValueRaw;
static MOTION_SENSOR_Axes_t MagValue;
static MFX_MagCal_output_t mag_cal_data_out;
static MFX_MagCal_input_t mag_cal_data_in;

void MX_MEMS_Init(TIM_HandleTypeDef* timer)
{

#ifdef BSP_IP_MEMS_INT1_PIN_NUM
  /* Force MEMS INT1 pin of the sensor low during startup in order to disable I3C and enable I2C. This function needs
   * to be called only if user wants to disable I3C / enable I2C and didn't put the pull-down resistor to MEMS INT1 pin
   * on his HW setup. This is also the case of usage X-NUCLEO-IKS4A1 or X-NUCLEO-IKS01A3 expansion board together with
   * sensor in DIL24 adapter board where the LDO with internal pull-up is used.
   */
  MEMS_INT1_Force_Low();
#endif



  /* Initialize Timer */
  BSP_IP_TIM_INIT();

  /* Configure Timer to run with desired algorithm frequency */
  TIM_Config(timer, ALGO_FREQ);

  /* Initialize (disabled) sensors */
  Init_Sensors();

#ifdef BSP_IP_MEMS_INT1_PIN_NUM
  /* Initialize MEMS INT1 pin back to it's default state after I3C disable / I2C enable */
  MEMS_INT1_Init();
#endif
#ifdef MOTION_TL
  /* TiltSensing API initialization function */
  MotionTL_manager_init();

  /* OPTIONAL */
  /* Get library version */
  MotionTL_manager_get_version(MotionTlLibVersion, &MotionTlLibVersionLen);

  /* Set the angle computation mode */
  MotionTL_GetKnobs(&knobs);
  knobs.mode = AngleMode;
  MotionTL_SetKnobs(&knobs);
#endif

  //Sensor Fusion INIT
  MotionFX_manager_init();

  //Init Motion MC
  MotionFX_MagCal_init(MOTION_MC_SAMPLE_TIME_MS, 1);

  /* Optional: Get version */
  MotionFX_GetLibVersion(MotionFxLibVersion);

  BSP_SENSOR_GYR_GetSensitivity(&m_gyroSensitivity);

  //Enable 9X sensor fusion
  MotionFX_manager_start_9X();

  DWT_Init();

  (void)HAL_TIM_Base_Start_IT(timer);
  
  timestamp_prev_ms = HAL_GetTick();
}

/**
  * @brief  Process of the application
  * PLL version
  * @retval None
  */
void MX_MEMS_Process(void)
{
	//MotionTl ------------------
	MTL_input_t tl_data_in;
	MTL_output_t tl_data_out;
	uint64_t timestamp_ms;
	int32_t localshock = 0;
	MOTION_SENSOR_Axes_t AccValuePrev;
	//MotionFx ------------------
	MFX_input_t 	fx_data_in;
	MFX_output_t 	fx_data_out;
	float dT;

  
  if (SensorReadRequest == 1U)
  {
    SensorReadRequest = 0;

    /* Acquire data from enabled sensors and fill Msg stream */
    memcpy(&AccValuePrev, &AccValue, sizeof(MOTION_SENSOR_Axes_t));

    BSP_SENSOR_ACC_GetAxes(&AccValue);

    //Gyro_Sensor_Handler(&msg_dat);
    // dps = value*sensitivity/1000;
    //BSP_SENSOR_GYR_GetAxes(&GyrValue);
    BSP_SENSOR_GYR_GetAxesRaw(&GyrValueRaw);
    GyrValue.x = (int32_t)((float)GyrValueRaw.x * m_gyroSensitivity);
    GyrValue.y = (int32_t)((float)GyrValueRaw.y * m_gyroSensitivity);
    GyrValue.z = (int32_t)((float)GyrValueRaw.z * m_gyroSensitivity);

    //Magneto_Sensor_Handler(&msg_dat);
    //in mGauss   (1T = 10000 Gauss)
    BSP_SENSOR_MAG_GetAxes(&MagValue);

    /* TiltSensing specific part */
    tl_data_in.acc_x = (float)AccValue.x/1000;
    tl_data_in.acc_y = (float)AccValue.y/1000;
    tl_data_in.acc_z = (float)AccValue.z/1000;

    timestamp_ms = HAL_GetTick();
    dT = ((float)timestamp_ms - (float)timestamp_prev_ms)/1000.0; //dt in seconds

#ifdef MOTION_TL
    MotionTL_manager_run(&tl_data_in, timestamp_ms, &tl_data_out);

    if (tl_data_out.roll_3x > 90)
       	m_inclination = 180 - tl_data_out.pitch_3x;
       else if (tl_data_out.roll_3x < -90)
       	m_inclination = -180 - tl_data_out.pitch_3x;
       else
       	m_inclination = tl_data_out.pitch_3x;
#endif

    //FX SENSOR FUSION
    fx_data_in.acc[0] = tl_data_in.acc_x;
    fx_data_in.acc[1] = tl_data_in.acc_y;
    fx_data_in.acc[2] = tl_data_in.acc_z;
    fx_data_in.gyro[0] = ((float)GyrValue.x)/1000.0;
    fx_data_in.gyro[1] = ((float)GyrValue.y)/1000.0;
    fx_data_in.gyro[2] = ((float)GyrValue.z)/1000.0;

    //it wants uT/50
    //1mG -> 0.1uT
    fx_data_in.mag[0] = ((float)MagValue.x)/500.0;
    fx_data_in.mag[1] = ((float)MagValue.y)/500.0;
    fx_data_in.mag[2] = ((float)MagValue.z)/500.0;

    //MC Calibration
    if (m_bMCEnabled)
    {
    	memcpy(&magCalibration.mag, &fx_data_in.mag, MFX_NUM_AXES*sizeof(float));
    	magCalibration.time_stamp = (int)(timestamp_ms - timestamp_MC_start_ms);
    	MotionFX_MagCal_run(&magCalibration);

    	/* Get the magnetometer calibration coefficients */
    	MotionFX_MagCal_getParams(&mag_cal_data_out);
    	m_nMCQuality = mag_cal_data_out.cal_quality;
    	if ((MFX_MAGCALGOOD == mag_cal_data_out.cal_quality) /*|| (MFX_CM0P_CALQSTATUSBEST== mag_cal_data_out.cal_quality)*/ )
    	{
    		m_bMCDone = true;
    	}
    }

	MotionFX_manager_run(&fx_data_in, &fx_data_out, dT);

	m_eCompass = fx_data_out.heading;
	m_eCompass_err = fx_data_out.headingErr;

    timestamp_prev_ms = timestamp_ms;
  }
}

[liaifat85]

Check that the magCalibration.time_stamp is being updated correctly each time the MotionFX_MagCal_run() function is called.