Raw data out of STMicro's LISM2MDL so bad that it breaks STMicro's CUBE-MEMS Compass Library? [SOLVED]

[Rstua.1]

I have a compass program using an LSM9DS1 chip and the STMicro's CUBE-MEMS compass library. This is working.

I switched from the LSM9DS1 to the LISM2MDL. I am using the same program. This does not work. It appears the STMicro CUBE-MEMS library breaks (never creates good compensation values) because the LISM2MDL magnetometer values are so far out of alignment.

I captured the raw maximum & minimum magnetometer values from the LSM9DS1:

Max X: 62

Min X: -38

Max Y: 42

Min Y: -57

Max Z: 37

Min Z: -60

These are not very far from having zero average value. The offsets to make the maximums equal distant from zero:

X: -12

Y: 8

Z: 12

I adapted the program to use the LISM2MDL (I removed the STMicro HAL for the LSM9DS1 and replaced it with the STMicro HAL for the LISM2MDL. Now my raw minimum and maximum magnetometer values look like this:

Max X: -5

Min X: -44

Max Y: 21

Min Y: -16

Max Z: 30

Min Z: -9

The X values are terrible and very far from having zero average value. The others are not too bad. The offsets to make the maximums equal distant from zero:

X: 25

Y: 3

Z: -11

But when I pass these values to the STMicro CUBE-MEMS Library "MotionMC_Update()" I do not get good correction values back like I did when using the LSM8DS1.

[Rstua.1]

To further make my point, I decided to try and break my working LSM9DS1 compass program. Specifically, I am going to subtract 25 from the LSM9DS1 raw X value to simulate the bad LIS2MDL raw X value.

This change does appear to break the STMicro CUBE-MEMS Compass Library! I am testing the code using STMicro's Unucleo-GUI's Scatter Plot and Compass features. I can no longer get the compass to work. I turn the LSM9DS1 360 degrees and the simulated compass on the screen only wiggles between ~150 to ~210 degrees. And the Scatter Plot is clearly creating 3 distinctly separate balls. Ideally, all 3 colors (x-y, x-z & y-z) would form a single ball after enough data is collected to do a proper calibration of the magnetometer.

Do all STMicro LIS2MDL magnetometers behave this badly?

[Miroslav BATEK]

Hello,

Are you passing the magnetometer data to the MotionMC library in correct unit which is micro tesla [uT]?

In X-CUBE-MEMS1 is application for LSM6DSO and LIS2MDL whihc are populated on X-NUCLEO-IKS01A3, you can use it for reference,

[Rstua.1]

Hi Miroslav,

Thank you for your response.

I have re-written my LIS2MDL code to "pre calibrate" the raw data before submitting it to the STMicro's CUBE-MEMS function "MotionMC_Update()".

I have captured the minimum and maximum values I am creating from the raw LIS2MDL magnetometer values:

Max X: 48

Min X: -54

Max Y: 46

Min Y: -51

Max Z: 49

Min Z: -51

Note, my "normalization" code is tuned for this particular LIS2MDL chip and testing location. (That is, my "normalizatoin" code is not adapting as data is read from the LIS2MDL. The offset and magnitude adjustment values are fixed so as not to confuse the Middle Ware or cloud testing results.). The Middle Ware still does not appear to be calibrating properly. The (Unuclio-GUI) scatter plot initially appears as 3 separate balls not centered on the origin of the graph - then jumped (assume this was the Middle Ware making the calibration) creating 3 new separate balls not centered on the origin. If I could make any generalization, I would say the 3 new balls were simply arrived at by rotating the center of each ball around the origin of the plot 180 degrees. (That is, their positions were flipped left to right and up to down.) The Unuclio-GUI virtual compass was still not working. Only moving between about 150 to 210 degrees fro a full rotation of the magnetometer.

-thanks

[Miroslav BATEK]

My recommendation is following:

• Don't do any pre-calibration the MotionMC library is able to make the calibration.
• Make the MotionMC functional before the to use MotionEC
• You can share the data which you are passing to the algorithm. Use uT unit as I mentioned before.
• Refer to ECompass or MagnetometerCalibration application in X-CUBE-MEMS1 package
• You can also share part of your code so we can check it.

[Rstua.1]

I have removed any pre-calibratoin except the conversions I found in the original STMicro example code. A quick check and it appears I make all the MotionMC calls before the MotionEC calls:

$ grep Motion Src/*.c
Src/driver_lis2mdl_lsm6dso.c:char MotionMC_LoadCalFromNVM(unsigned short int datasize, unsigned int *data)
Src/driver_lis2mdl_lsm6dso.c:char MotionMC_SaveCalInNVM(unsigned short int datasize, unsigned int *data)
Src/driver_lis2mdl_lsm6dso.c: // Holds Electric Compass (MotionEC) middle ware frequency.
Src/driver_lis2mdl_lsm6dso.c: // Initialize the STMicro's Motion Middle Ware.
Src/driver_lis2mdl_lsm6dso.c: MotionMC_Initialize((int)ALGO_PERIOD, (unsigned short int)1);
Src/driver_lis2mdl_lsm6dso.c: MotionEC_Initialize(&otter_freq);
Src/driver_lis2mdl_lsm6dso.c: MotionEC_SetOrientationEnable(MEC_ENABLE);
Src/driver_lis2mdl_lsm6dso.c: MotionEC_SetVirtualGyroEnable(MEC_ENABLE);
Src/driver_lis2mdl_lsm6dso.c: MotionEC_SetGravityEnable(MEC_ENABLE);
Src/driver_lis2mdl_lsm6dso.c: MotionEC_SetLinearAccEnable(MEC_ENABLE);
Src/driver_lis2mdl_lsm6dso.c: MotionMC_Update(&data_in);
Src/driver_lis2mdl_lsm6dso.c: MotionMC_GetCalParams(&data_out);
Src/driver_lis2mdl_lsm6dso.c: MotionEC_Run(&data_mec_in, &data_mec_out);
Src/main.c:/// MotionMC_Initialize((int)ALGO_PERIOD, (unsigned short int)1);
Src/main.c:/// MotionMC_Initialize((int)ALGO_PERIOD, (unsigned short int)0);

[Rstua.1]

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, &reg_lsm6dso);
	lis2mdl_status_get(&dev_ctx_mag, &reg_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;
	}
}
 
 

[Rstua.1]

I noticed there was an error in the code. I used an accelerometer HAL function to normalize the raw magnetometer value to Gauss. However, the LIS2MDL is fixed at 1.5 mG per unit of measure. So instead I now divide by 1.5 to get mG:

		// Convert raw magnetometer value to milligauss.
		// Sensitivity of magnetometer for LIS2MDL is 1.5mgauss/LSB, sensitivity .
//		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]);
		magnetic_field_mgauss[0] = (data_raw_magnetic_field.i16bit[0] / 1.5);
		magnetic_field_mgauss[1] = (data_raw_magnetic_field.i16bit[1] / 1.5);
		magnetic_field_mgauss[2] = (data_raw_magnetic_field.i16bit[2] / 1.5);
 

...Unfortunately, this did not fix the problem.

[Miroslav BATEK]

You can use function lis2mdl_from_lsb_to_mgauss to convert raw data to mgauss.

This function multiply the value by 1.5 so I think your code is wrong.

float_t lis2mdl_from_lsb_to_mgauss(int16_t lsb)
{
 return ((float_t)lsb * 1.5f);
}

[Rstua.1]

Ah! That Worked!

...can't believe I wasn't able to get here empirically (print out the values and adjust them until they are all in an expected range).

-thanks!