That is kind of you to offer to look at the code. Here is the polling function I call from main which processes the accelerometer & magnetometer data as it becomes available (I pulled a lot of conditionally compiled debug and Unucleo-GUI code out to make it fit into a forum post. Hopefully I haven't removed any vital parts of the code in the doing so.):
void driver_lis2mdl_poll(void)
{
// Holds raw and compensated magnetometer x, y & z values.
float mag_raw_mG[3];
float mag_comp_mG[3];
// Holds the matrix to convert from Earth Centered Earth Fixed (XYZ or ECEF) to East North Up (ENU).
static float LSM6DSO_0_Matrix[3][3] = {{0.0, 1.0, 0.0}, {-1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}}; // ACC, GYRO
static float LIS2MDL_0_Matrix[3][3] = {{0.0, 1.0, 0.0}, {1.0, 0.0, 0.0}, {0.0, 0.0, 1.0}}; // MAG
// Rotate vector using quaternion where v_base are old values and v_head are rotated values.
float v_base[3] = {0.0, 1.0, 0.0};
float v_head[3];
// Holds heading and heading_valid (1 = good otherwise bad).
float heading;
int heading_valid;
dev_ctx_mag.write_reg = platform_write;
dev_ctx_mag.read_reg = platform_read;
dev_ctx_mag.handle = (void*)&i2c_add_mag;
dev_ctx_imu.write_reg = platform_write;
dev_ctx_imu.read_reg = platform_read;
dev_ctx_imu.handle = (void*)&i2c_add_imu;
/* Read samples in polling mode (no int) */
/* Read device status register */
lsm6dso_status_reg_get(&dev_ctx_imu, ®_lsm6dso);
lis2mdl_status_get(&dev_ctx_mag, ®_lis3mdl);
if ( reg_lsm6dso.xlda && reg_lsm6dso.gda )
{
/* Read imu data */
memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t));
memset(data_raw_angular_rate.u8bit, 0x00, 3 * sizeof(int16_t));
lsm6dso_acceleration_raw_get(&dev_ctx_imu, data_raw_acceleration.u8bit);
lsm6dso_angular_rate_raw_get(&dev_ctx_imu, data_raw_angular_rate.u8bit);
acceleration_mg[0] = lsm6dso_from_fs4_to_mg(data_raw_acceleration.i16bit[0]);
acceleration_mg[1] = lsm6dso_from_fs4_to_mg(data_raw_acceleration.i16bit[1]);
acceleration_mg[2] = lsm6dso_from_fs4_to_mg(data_raw_acceleration.i16bit[2]);
/// For the LSM9DS1 we had to flip X and Y. Now that the LSM6DSO flips Y for us by design, we only need to flip X.
/// acceleration_mg[0] *= -1;
/// acceleration_mg[1] *= -1;
/// acceleration_mg[2] *= -1;
angular_rate_mdps[0] = lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate.i16bit[0]);
angular_rate_mdps[1] = lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate.i16bit[1]);
angular_rate_mdps[2] = lsm6dso_from_fs2000_to_mdps(data_raw_angular_rate.i16bit[2]);
}
// Beginning of STMicro Middle Ware (happens to include sampling of magetometer).
if ( reg_lis3mdl.zyxda )
{
/* Read magnetometer data */
memset(data_raw_magnetic_field.u8bit, 0x00, 3 * sizeof(int16_t));
lis2mdl_magnetic_raw_get(&dev_ctx_mag, data_raw_magnetic_field.u8bit);
// Convert raw magnetometer value to milligauss.
magnetic_field_mgauss[0] = lsm6dso_from_fs4_to_mg(data_raw_magnetic_field.i16bit[0]);
magnetic_field_mgauss[1] = lsm6dso_from_fs4_to_mg(data_raw_magnetic_field.i16bit[1]);
magnetic_field_mgauss[2] = lsm6dso_from_fs4_to_mg(data_raw_magnetic_field.i16bit[2]);
/// For the LSM9DS1 we had to flip Y. Now that the LIS2MDL flips Y for us by design, we do not have to flip anything.
/// magnetic_field_mgauss[0] *= -1;
/// magnetic_field_mgauss[1] *= -1;
/// magnetic_field_mgauss[2] *= -1;
MMC_Input_t data_in;
MMC_Output_t data_out;
MEC_input_t data_mec_in;
MEC_output_t data_mec_out;
/* Convert magnetometer data from [mGauss] to [uT] */
data_in.Mag[0] = magnetic_field_mgauss[0] / 10.0f;
data_in.Mag[1] = magnetic_field_mgauss[1] / 10.0f;
data_in.Mag[2] = magnetic_field_mgauss[2] / 10.0f;
/* Time stamp [ms] */
data_in.TimeStamp = (int)otter_timer_ms;
/* Run Magnetometer Calibration algorithm */
MotionMC_Update(&data_in);
/* Get the magnetometer compensation for hard/soft iron */
MotionMC_GetCalParams(&data_out);
// Do hard & soft iron calibration
mag_raw_mG[0] = magnetic_field_mgauss[0];
mag_raw_mG[1] = magnetic_field_mgauss[1];
mag_raw_mG[2] = magnetic_field_mgauss[2];
/* Compensate magnetometer data */
/* NOTE: Convert hard iron coeficients [uT] to [mGauss] */
for (int i = 0; i < 3; i++)
{
mag_comp_mG[i] = 0.0f;
for (int j = 0; j < 3; j++)
{
mag_comp_mG[i] += (mag_raw_mG[j] - data_out.HI_Bias[j] * 10.0f) * data_out.SF_Matrix[i][j];
}
/* MISRA C-2012 rule 10.1_R3 violation for purpose */
mag_comp_mG[i] += (mag_comp_mG[i] >= 0.0f) ? 0.5f : -0.5f;
}
otter.magnetometer.x_normal = (int32_t)mag_comp_mG[0];
otter.magnetometer.y_normal = (int32_t)mag_comp_mG[1];
otter.magnetometer.z_normal = (int32_t)mag_comp_mG[2];
/* Do sensor orientation transformation */
IKS01A2_MOTION_SENSOR_Axes_t AccValue; /* Raw accelerometer data [mg] */
AccValue.x = acceleration_mg[0];
AccValue.y = acceleration_mg[1];
AccValue.z = acceleration_mg[2];
IKS01A2_MOTION_SENSOR_Axes_t MagValueComp; /* Compensated magnetometer data [mGauss] */
MagValueComp.x = (int32_t)mag_comp_mG[0];
MagValueComp.y = (int32_t)mag_comp_mG[1];
MagValueComp.z = (int32_t)mag_comp_mG[2];
// Transform from X, Y, Z (XYZ) frame to East, North, Up (ENU) frame.
// The arguments for transform_xyz_to_enu : INPUT, OUTPUT, MATRIX
transform_xyz_to_enu(&AccValue, data_mec_in.acc, LSM6DSO_0_Matrix);
transform_xyz_to_enu(&MagValueComp, data_mec_in.mag, LIS2MDL_0_Matrix);
/* Raw accelerometer data [g] */
data_mec_in.acc[0] = data_mec_in.acc[0] / 1000.0f; /* East */
data_mec_in.acc[1] = data_mec_in.acc[1] / 1000.0f; /* North */
data_mec_in.acc[2] = data_mec_in.acc[2] / 1000.0f; /* Up */
/* Compensated magnetometer data [uT / 50], [mGauss / 5] */
data_mec_in.mag[0] = data_mec_in.mag[0] / 5.0f; /* East */
data_mec_in.mag[1] = data_mec_in.mag[1] / 5.0f; /* North */
data_mec_in.mag[2] = data_mec_in.mag[2] / 5.0f; /* Up */
/* Delta time [s] */
data_mec_in.deltatime_s = (float)ALGO_PERIOD / 1000.0f;
/* Run E-Compass algorithm */
MotionEC_Run(&data_mec_in, &data_mec_out);
v_rotate(v_head, data_mec_out.quaternion, v_base);
heading_valid = calc_heading(&heading, v_head);
// Save heading and heading goodness into global structure.
otter.compass.bearing_smooth = heading;
otter.compass.bearing_valid = heading_valid;
}
}
