ist8310: add cross-axis compensation

src/drivers/magnetometer/isentek/ist8310/IST8310.cpp

@@ -35,6 +35,10 @@
 
 using namespace time_literals;
 
+#define IST8310_TEMP_RESOLUTION	(1/67.2)
+#define IST8310_TEMP_25C_OFFSET	27100
+#define IST8310_OTP_SENSITIVITY 330
+
 static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
 {
 	return (msb << 8u) | lsb;
@@ -81,6 +85,21 @@ void IST8310::print_status()
 	perf_print_counter(_reset_perf);
 	perf_print_counter(_bad_register_perf);
 	perf_print_counter(_bad_transfer_perf);
+	print_cross_axis_info();
+}
+
+void IST8310::print_cross_axis_info()
+{
+	PX4_INFO("cross axis cal: [%+3.2f,%+3.2f,%+3.2f|%+3.2f,%+3.2f,%+3.2f|%+3.2f,%+3.2f,%+3.2f]",
+		 (double)_crossaxis_inv[0],
+		 (double)_crossaxis_inv[1],
+		 (double)_crossaxis_inv[2],
+		 (double)_crossaxis_inv[3],
+		 (double)_crossaxis_inv[4],
+		 (double)_crossaxis_inv[5],
+		 (double)_crossaxis_inv[6],
+		 (double)_crossaxis_inv[7],
+		 (double)_crossaxis_inv[8]);
 }
 
 int IST8310::probe()
@@ -190,21 +209,34 @@ void IST8310::RunImpl()
 				uint8_t DATAZH;
 			} buffer{};
 
+			struct Temp {
+				uint8_t TEMPL;
+				uint8_t TEMPH;
+			} buffer_temp;
+
 			bool success = false;
 			uint8_t cmd = static_cast<uint8_t>(Register::STAT1);
+			uint8_t cmd_temp = static_cast<uint8_t>(Register::TEMPL);
 
-			if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) {
+			if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK
+			    && transfer(&cmd_temp, 1, (uint8_t *)&buffer_temp, sizeof(buffer_temp)) == PX4_OK) {
 
 				if (buffer.STAT1 & STAT1_BIT::DRDY) {
 					int16_t x = combine(buffer.DATAXH, buffer.DATAXL);
 					int16_t y = combine(buffer.DATAYH, buffer.DATAYL);
 					int16_t z = combine(buffer.DATAZH, buffer.DATAZL);
+					int16_t t = combine(buffer_temp.TEMPH, buffer_temp.TEMPL);
 
-					// sensor's frame is +x forward, +y right, +z up
-					z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z
+					_px4_mag.set_temperature((IST8310_TEMP_25C_OFFSET - t) * IST8310_TEMP_RESOLUTION + 25);
+
+					float xf = (float)x * _crossaxis_inv[0] + (float)y * _crossaxis_inv[1] + (float)z * _crossaxis_inv[2];
+					float yf = (float)x * _crossaxis_inv[3] + (float)y * _crossaxis_inv[4] + (float)z * _crossaxis_inv[5];
+					float zf = (float)x * _crossaxis_inv[6] + (float)y * _crossaxis_inv[7] + (float)z * _crossaxis_inv[8];
 
 					_px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf));
-					_px4_mag.update(now, x, y, z);
+
+					// sensor's frame is +x forward, +y right, +z up
+					_px4_mag.update(now, xf, yf, -zf);
 
 					success = true;
 
@@ -268,9 +300,158 @@ bool IST8310::Configure()
 
 	_px4_mag.set_scale(1.f / 1320.f); // 1320 LSB/Gauss
 
+	/* Pull & process cross-axis compensation */
+	cross_axis_comp();
+
 	return success;
 }
 
+void
+IST8310::cross_axis_comp()
+{
+	int ret;
+	uint8_t bOTPDataFlags;
+	uint8_t tempBuff[2];
+	bool crossaxis_enable = 0;
+	float inv[9] = {0};
+	short OTPcrossaxis[9] = {0};
+
+	uint8_t crossxbuf[6];
+	uint8_t crossybuf[6];
+	uint8_t crosszbuf[6];
+
+	update_crossaxis();
+
+	//check otp_date (Sensors before some date appears to have the matrix stored transposed)
+	bOTPDataFlags = 0;
+
+	ret = read(Register::OTP, (uint8_t *)&tempBuff, sizeof(tempBuff));
+
+	if (OK != ret) {
+		perf_count(_bad_transfer_perf);
+		return;
+	}
+
+
+	if (tempBuff[0] == 0xff) {
+		bOTPDataFlags = 0;
+
+	} else if (tempBuff[0] <= 0x12) {
+		if (tempBuff[1] <= 0x07) {
+			bOTPDataFlags = 1;
+		}
+	}
+
+	ret = read(Register::XX_CROSS_L, (uint8_t *)&tempBuff, sizeof(tempBuff));
+
+	if (OK != ret) {
+		perf_count(_bad_transfer_perf);
+		return;
+	}
+
+	if ((tempBuff[0] == 0xFF) && (tempBuff[1] == 0xFF)) {
+		crossaxis_enable = 0;
+
+	} else {
+		crossaxis_enable = 1;
+	}
+
+	if (crossaxis_enable == 0) {
+		update_crossaxis();
+		return;
+
+	} else {
+		ret = read(Register::XX_CROSS_L, (uint8_t *)&crossxbuf, sizeof(crossxbuf));
+
+		if (OK != ret) {
+			perf_count(_bad_transfer_perf);
+			return;
+		}
+
+		ret = read(Register::YX_CROSS_L, (uint8_t *)&crossybuf, sizeof(crossybuf));
+
+		if (OK != ret) {
+			perf_count(_bad_transfer_perf);
+			return;
+		}
+
+		ret = read(Register::ZX_CROSS_L, (uint8_t *)&crosszbuf, sizeof(crosszbuf));
+
+		if (OK != ret) {
+			perf_count(_bad_transfer_perf);
+			return;
+		}
+
+		if (bOTPDataFlags) {
+			//before
+			OTPcrossaxis[0] = ((int16_t) crossxbuf[1]) << 8 | crossxbuf[0];
+			OTPcrossaxis[1] = ((int16_t) crossxbuf[3]) << 8 | crossxbuf[2];
+			OTPcrossaxis[2] = ((int16_t) crossxbuf[5]) << 8 | crossxbuf[4];
+			OTPcrossaxis[3] = ((int16_t) crossybuf[1]) << 8 | crossybuf[0];
+			OTPcrossaxis[4] = ((int16_t) crossybuf[3]) << 8 | crossybuf[2];
+			OTPcrossaxis[5] = ((int16_t) crossybuf[5]) << 8 | crossybuf[4];
+			OTPcrossaxis[6] = ((int16_t) crosszbuf[1]) << 8 | crosszbuf[0];
+			OTPcrossaxis[7] = ((int16_t) crosszbuf[3]) << 8 | crosszbuf[2];
+			OTPcrossaxis[8] = ((int16_t) crosszbuf[5]) << 8 | crosszbuf[4];
+
+		} else {
+			//after
+			OTPcrossaxis[0] = ((int16_t) crossxbuf[1]) << 8 | crossxbuf[0];
+			OTPcrossaxis[3] = ((int16_t) crossxbuf[3]) << 8 | crossxbuf[2];
+			OTPcrossaxis[6] = ((int16_t) crossxbuf[5]) << 8 | crossxbuf[4];
+			OTPcrossaxis[1] = ((int16_t) crossybuf[1]) << 8 | crossybuf[0];
+			OTPcrossaxis[4] = ((int16_t) crossybuf[3]) << 8 | crossybuf[2];
+			OTPcrossaxis[7] = ((int16_t) crossybuf[5]) << 8 | crossybuf[4];
+			OTPcrossaxis[2] = ((int16_t) crosszbuf[1]) << 8 | crosszbuf[0];
+			OTPcrossaxis[5] = ((int16_t) crosszbuf[3]) << 8 | crosszbuf[2];
+			OTPcrossaxis[8] = ((int16_t) crosszbuf[5]) << 8 | crosszbuf[4];
+		}
+
+		_crossaxis_det = ((int32_t)OTPcrossaxis[0]) * ((int32_t)OTPcrossaxis[4]) * ((int32_t)OTPcrossaxis[8]) +
+				 ((int32_t)OTPcrossaxis[1]) * ((int32_t)OTPcrossaxis[5]) * ((int32_t)OTPcrossaxis[6]) +
+				 ((int32_t)OTPcrossaxis[2]) * ((int32_t)OTPcrossaxis[3]) * ((int32_t)OTPcrossaxis[7]) -
+				 ((int32_t)OTPcrossaxis[0]) * ((int32_t)OTPcrossaxis[5]) * ((int32_t)OTPcrossaxis[7]) -
+				 ((int32_t)OTPcrossaxis[2]) * ((int32_t)OTPcrossaxis[4]) * ((int32_t)OTPcrossaxis[6]) -
+				 ((int32_t)OTPcrossaxis[1]) * ((int32_t)OTPcrossaxis[3]) * ((int32_t)OTPcrossaxis[8]);
+
+		if (_crossaxis_det == 0) {
+			update_crossaxis();
+		}
+
+		inv[0] = (float)OTPcrossaxis[4] * (float)OTPcrossaxis[8] - (float)OTPcrossaxis[5] * (float)OTPcrossaxis[7];
+		inv[1] = (float)OTPcrossaxis[2] * (float)OTPcrossaxis[7] - (float)OTPcrossaxis[1] * (float)OTPcrossaxis[8];
+		inv[2] = (float)OTPcrossaxis[1] * (float)OTPcrossaxis[5] - (float)OTPcrossaxis[2] * (float)OTPcrossaxis[4];
+		inv[3] = (float)OTPcrossaxis[5] * (float)OTPcrossaxis[6] - (float)OTPcrossaxis[3] * (float)OTPcrossaxis[8];
+		inv[4] = (float)OTPcrossaxis[0] * (float)OTPcrossaxis[8] - (float)OTPcrossaxis[2] * (float)OTPcrossaxis[6];
+		inv[5] = (float)OTPcrossaxis[2] * (float)OTPcrossaxis[3] - (float)OTPcrossaxis[0] * (float)OTPcrossaxis[5];
+		inv[6] = (float)OTPcrossaxis[3] * (float)OTPcrossaxis[7] - (float)OTPcrossaxis[4] * (float)OTPcrossaxis[6];
+		inv[7] = (float)OTPcrossaxis[1] * (float)OTPcrossaxis[6] - (float)OTPcrossaxis[0] * (float)OTPcrossaxis[7];
+		inv[8] = (float)OTPcrossaxis[0] * (float)OTPcrossaxis[4] - (float)OTPcrossaxis[1] * (float)OTPcrossaxis[3];
+
+		for (int i = 0; i < 9; i++) {
+			_crossaxis_inv[i] = inv[i] * ((float)IST8310_OTP_SENSITIVITY) / ((float)_crossaxis_det);
+		}
+
+	}
+
+	return;
+}
+
+void
+IST8310::update_crossaxis()
+{
+	*_crossaxis_inv = 1;
+	*(_crossaxis_inv + 1) = 0;
+	*(_crossaxis_inv + 2) = 0;
+	*(_crossaxis_inv + 3) = 0;
+	*(_crossaxis_inv + 4) = 1;
+	*(_crossaxis_inv + 5) = 0;
+	*(_crossaxis_inv + 6) = 0;
+	*(_crossaxis_inv + 7) = 0;
+	*(_crossaxis_inv + 8) = 1;
+	_crossaxis_det = 1;
+}
+
 bool IST8310::RegisterCheck(const register_config_t &reg_cfg)
 {
 	bool success = true;
@@ -298,6 +479,12 @@ uint8_t IST8310::RegisterRead(Register reg)
 	return buffer;
 }
 
+int IST8310::read(Register reg, void *data, unsigned count)
+{
+	const uint8_t cmd = static_cast<uint8_t>(reg);
+	return transfer(&cmd, 1, (uint8_t *)data, count);
+}
+
 void IST8310::RegisterWrite(Register reg, uint8_t value)
 {
 	uint8_t buffer[2] { (uint8_t)reg, value };

src/drivers/magnetometer/isentek/ist8310/IST8310.hpp

@@ -83,6 +83,23 @@ private:
 	void RegisterWrite(Register reg, uint8_t value);
 	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
 
+	int read(Register reg, void *data, unsigned count);
+
+	/**
+	* Pull cross axis compensation from device and calculate compensation matrix
+	*/
+	void cross_axis_comp();
+
+	/**
+	* Update cross axis matrix to identity
+	*/
+	void update_crossaxis();
+
+	/**
+	* Print cross axis matrix
+	*/
+	void print_cross_axis_info();
+
 	PX4Magnetometer _px4_mag;
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
@@ -110,4 +127,7 @@ private:
 		{ Register::AVGCNTL,      AVGCNTL_BIT::Y_16TIMES_SET | AVGCNTL_BIT::XZ_16TIMES_SET, AVGCNTL_BIT::Y_16TIMES_CLEAR | AVGCNTL_BIT::XZ_16TIMES_CLEAR },
 		{ Register::PDCNTL,       PDCNTL_BIT::PULSE_NORMAL, 0 },
 	};
+
+	float _crossaxis_inv[9];
+	int32_t _crossaxis_det;
 };

src/drivers/magnetometer/isentek/ist8310/iSentek_IST8310_registers.hpp

@@ -82,6 +82,8 @@ enum class Register : uint8_t {
 	AVGCNTL    = 0x41, // Average Control Register
 	PDCNTL     = 0x42, // Pulse Duration Control Register
 
+	OTP        = 0x99,
+
 	XX_CROSS_L = 0x9C, // cross axis xx low byte
 	XX_CROSS_H = 0x9D, // cross axis xx high byte
 	XY_CROSS_L = 0x9E, // cross axis xy low byte