Posted on February 16, 2018 at 11:13Hello Miroslav,
Thanks for your reply.
Here's my code of configuration the MotionFx module:
void MotionFX_manager_init(void *handle)
{
uint8_t instance;
MotionFX_initialize();
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;
BSP_GYRO_Get_Instance(handle, &instance);
switch (instance)
{
case LSM6DS0_G_0:
ipKnobs->acc_orientation[0] = 'e';
ipKnobs->acc_orientation[1] = 'n';
ipKnobs->acc_orientation[2] = 'u';
ipKnobs->gyro_orientation[0] = 'e';
ipKnobs->gyro_orientation[1] = 'n';
ipKnobs->gyro_orientation[2] = 'u';
break;
default:
return;
}
ipKnobs->mag_orientation[0] = 's';
ipKnobs->mag_orientation[1] = 'e';
ipKnobs->mag_orientation[2] = 'u';
ipKnobs->output_type = MFX_ENGINE_OUTPUT_ENU;
ipKnobs->LMode = 1;
ipKnobs->modx = 1;
MotionFX_setKnobs(ipKnobs);
MotionFX_enable_6X(MFX_ENGINE_DISABLE);
MotionFX_enable_9X(MFX_ENGINE_DISABLE);
}
The bias values are defined as followed:
&sharpdefine GBIAS_ACC_TH_SC_6X (2.0f*0.000765f)
&sharpdefine GBIAS_GYRO_TH_SC_6X (2.0f*0.002f)
&sharpdefine GBIAS_MAG_TH_SC_6X (2.0f*0.001500f)
&sharpdefine GBIAS_ACC_TH_SC_9X (2.0f*0.000765f)
&sharpdefine GBIAS_GYRO_TH_SC_9X (2.0f*0.002f)
&sharpdefine GBIAS_MAG_TH_SC_9X (2.0f*0.001500f)
And here is my code of using MotionFX library
static void FX_Data_Handler(TMsg *Msg)
{
MFX_input_t data_in;
MFX_input_t *pdata_in = &data_in;
MFX_output_t data_out;
MFX_output_t *pdata_out = &data_out;
char str[21];
if ((Sensors_Enabled & ACCELEROMETER_SENSOR) && (Sensors_Enabled & GYROSCOPE_SENSOR) && (Sensors_Enabled & MAGNETIC_SENSOR))
{
data_in.gyro[0] = GYR_Value.AXIS_X * FROM_MDPS_TO_DPS;
data_in.gyro[1] = GYR_Value.AXIS_Y * FROM_MDPS_TO_DPS;
data_in.gyro[2] = GYR_Value.AXIS_Z * FROM_MDPS_TO_DPS;
data_in.acc[0] = ACC_Value.AXIS_X * FROM_MG_TO_G;
data_in.acc[1] = ACC_Value.AXIS_Y * FROM_MG_TO_G;
data_in.acc[2] = ACC_Value.AXIS_Z * FROM_MG_TO_G;
data_in.mag[0] = MAG_Value.AXIS_X * FROM_MGAUSS_TO_UT50;
data_in.mag[1] = MAG_Value.AXIS_Y * FROM_MGAUSS_TO_UT50;
data_in.mag[2] = MAG_Value.AXIS_Z * FROM_MGAUSS_TO_UT50;
/* Run Sensor Fusion algorithm */
MotionFX_manager_run(pdata_in, pdata_out, MOTIONFX_ENGINE_DELTATIME);
if (SF_6X_Enabled == 1)
{
memcpy(&Msg->Data[55], (void *)pdata_out->quaternion_6X, 4 * sizeof(float));
memcpy(&Msg->Data[71], (void *)pdata_out->rotation_6X, 3 * sizeof(float));
memcpy(&Msg->Data[83], (void *)pdata_out->gravity_6X, 3 * sizeof(float));
memcpy(&Msg->Data[95], (void *)pdata_out->linear_acceleration_6X, 3 * sizeof(float));
memcpy(&Msg->Data[107], (void *)&(pdata_out->heading_6X), sizeof(float));
}
else
{
memcpy(&Msg->Data[55], (void *)pdata_out->quaternion_9X, 4 * sizeof(float));
memcpy(&Msg->Data[71], (void *)pdata_out->rotation_9X, 3 * sizeof(float));
memcpy(&Msg->Data[83], (void *)pdata_out->gravity_9X, 3 * sizeof(float));
memcpy(&Msg->Data[95], (void *)pdata_out->linear_acceleration_9X, 3 * sizeof(float));
memcpy(&Msg->Data[107], (void *)&(pdata_out->heading_9X), sizeof(float));
}
}
}
I've checked the inputs and they seem to be correct. At least accelerometer and gyroscope data. The magnetometer raw data seem to have strong hard iron shift.
As for calibration I performed different shapes and none of thew was successful. Should I just perform 8 shape around X axle?
Kind regards,
Sergey.
