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;
}
}
