Add magnetometer thermal compensation.

msg/sensor_correction.msg

@@ -4,6 +4,15 @@
 
 uint64 timestamp		# time since system start (microseconds)
 
+# Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset
+# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
+uint32[4] accel_device_ids
+float32[4] accel_temperature
+float32[3] accel_offset_0	# accelerometer 0 offsets in the FRD board frame XYZ-axis in m/s^s
+float32[3] accel_offset_1	# accelerometer 1 offsets in the FRD board frame XYZ-axis in m/s^s
+float32[3] accel_offset_2	# accelerometer 2 offsets in the FRD board frame XYZ-axis in m/s^s
+float32[3] accel_offset_3	# accelerometer 3 offsets in the FRD board frame XYZ-axis in m/s^s
+
 # Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset
 # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
 uint32[4] gyro_device_ids
@@ -13,14 +22,14 @@ float32[3] gyro_offset_1	# gyro 1 XYZ offsets in the sensor frame in rad/s
 float32[3] gyro_offset_2	# gyro 2 XYZ offsets in the sensor frame in rad/s
 float32[3] gyro_offset_3	# gyro 3 XYZ offsets in the sensor frame in rad/s
 
-# Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset
+# Corrections for magnetometer measurement outputs where corrected_mag = raw_mag * mag_scale + mag_offset
 # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame
-uint32[4] accel_device_ids
-float32[4] accel_temperature
-float32[3] accel_offset_0	# accelerometer 0 offsets in the FRD board frame XYZ-axis in m/s^s
-float32[3] accel_offset_1	# accelerometer 1 offsets in the FRD board frame XYZ-axis in m/s^s
-float32[3] accel_offset_2	# accelerometer 2 offsets in the FRD board frame XYZ-axis in m/s^s
-float32[3] accel_offset_3	# accelerometer 3 offsets in the FRD board frame XYZ-axis in m/s^s
+uint32[4] mag_device_ids
+float32[4] mag_temperature
+float32[3] mag_offset_0	# magnetometer 0 offsets in the FRD board frame XYZ-axis in m/s^s
+float32[3] mag_offset_1	# magnetometer 1 offsets in the FRD board frame XYZ-axis in m/s^s
+float32[3] mag_offset_2	# magnetometer 2 offsets in the FRD board frame XYZ-axis in m/s^s
+float32[3] mag_offset_3	# magnetometer 3 offsets in the FRD board frame XYZ-axis in m/s^s
 
 # Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset
 # Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame

src/drivers/magnetometer/isentek/ist8308/IST8308.cpp

@@ -220,6 +220,13 @@ void IST8308::RunImpl()
 					perf_count(_bad_register_perf);
 					Reset();
 				}
+
+			} else {
+				// periodically update temperature (~1 Hz)
+				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
+					UpdateTemperature();
+					_temperature_update_timestamp = now;
+				}
 			}
 		}
 
@@ -291,3 +298,15 @@ void IST8308::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cle
 		RegisterWrite(reg, val);
 	}
 }
+
+void IST8308::UpdateTemperature()
+{
+	// Isentek magnetometers do not have a temperature sensor, so we will to use the baro's value.
+	sensor_baro_s sensor_baro;
+
+	if (_sensor_baro_sub[0].update(&sensor_baro)) {
+		if (PX4_ISFINITE(sensor_baro.temperature)) {
+			_px4_mag.set_temperature(sensor_baro.temperature);
+		}
+	}
+}

src/drivers/magnetometer/isentek/ist8308/IST8308.hpp

@@ -46,7 +46,10 @@
 #include <lib/drivers/device/i2c.h>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 #include <lib/perf/perf_counter.h>
+#include <lib/sensor_calibration/Barometer.hpp>
 #include <px4_platform_common/i2c_spi_buses.h>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/sensor_baro.h>
 
 using namespace iSentek_IST8308;
 
@@ -61,9 +64,17 @@ class IST8308 : public device::I2C, public I2CSPIDriver<IST8308>
 	void RunImpl();
 
 	int init() override;
+
 	void print_status() override;
 
 private:
+	enum class STATE : uint8_t {
+		RESET,
+		WAIT_FOR_RESET,
+		CONFIGURE,
+		READ,
+	} _state{STATE::RESET};
+
 	// Sensor Configuration
 	struct register_config_t {
 		Register reg;
@@ -80,9 +91,15 @@ class IST8308 : public device::I2C, public I2CSPIDriver<IST8308>
 	bool RegisterCheck(const register_config_t &reg_cfg);
 
 	uint8_t RegisterRead(Register reg);
+
 	void RegisterWrite(Register reg, uint8_t value);
+
 	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
 
+	void UpdateTemperature();
+
+	uORB::SubscriptionMultiArray<sensor_baro_s, calibration::Barometer::MAX_SENSOR_COUNT> _sensor_baro_sub{ORB_ID::sensor_baro};
+
 	PX4Magnetometer _px4_mag;
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
@@ -91,17 +108,14 @@ class IST8308 : public device::I2C, public I2CSPIDriver<IST8308>
 
 	hrt_abstime _reset_timestamp{0};
 	hrt_abstime _last_config_check_timestamp{0};
-	int _failure_count{0};
+	hrt_abstime _temperature_update_timestamp{0};
 
-	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		CONFIGURE,
-		READ,
-	} _state{STATE::RESET};
+	int _failure_count{0};
 
 	uint8_t _checked_register{0};
+
 	static constexpr uint8_t size_register_cfg{6};
+
 	register_config_t _register_cfg[size_register_cfg] {
 		// Register               | Set bits, Clear bits
 		{ Register::ACTR,         0, ACTR_BIT::SUSPEND_EN },

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

@@ -188,6 +188,7 @@ void IST8310::RunImpl()
 
 					_px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf));
 					_px4_mag.update(now, x, y, z);
+					UpdateTemperature();
 
 					success = true;
 
@@ -222,6 +223,13 @@ void IST8310::RunImpl()
 					Reset();
 					return;
 				}
+
+			} else {
+				// periodically update temperature (~1 Hz)
+				if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
+					UpdateTemperature();
+					_temperature_update_timestamp = now;
+				}
 			}
 
 			// initiate next measurement
@@ -296,3 +304,15 @@ void IST8310::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cle
 		RegisterWrite(reg, val);
 	}
 }
+
+void IST8310::UpdateTemperature()
+{
+	// Isentek magnetometers do not have a temperature sensor, so we will to use the baro's value.
+	sensor_baro_s sensor_baro;
+
+	if (_sensor_baro_sub[0].update(&sensor_baro)) {
+		if (PX4_ISFINITE(sensor_baro.temperature)) {
+			_px4_mag.set_temperature(sensor_baro.temperature);
+		}
+	}
+}

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

@@ -46,7 +46,10 @@
 #include <lib/drivers/device/i2c.h>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 #include <lib/perf/perf_counter.h>
+#include <lib/sensor_calibration/Barometer.hpp>
 #include <px4_platform_common/i2c_spi_buses.h>
+#include <uORB/SubscriptionMultiArray.hpp>
+#include <uORB/topics/sensor_baro.h>
 
 using namespace iSentek_IST8310;
 
@@ -61,9 +64,18 @@ class IST8310 : public device::I2C, public I2CSPIDriver<IST8310>
 	void RunImpl();
 
 	int init() override;
+
 	void print_status() override;
 
 private:
+	enum class STATE : uint8_t {
+		RESET,
+		WAIT_FOR_RESET,
+		CONFIGURE,
+		MEASURE,
+		READ,
+	} _state{STATE::RESET};
+
 	// Sensor Configuration
 	struct register_config_t {
 		Register reg;
@@ -80,9 +92,15 @@ class IST8310 : public device::I2C, public I2CSPIDriver<IST8310>
 	bool RegisterCheck(const register_config_t &reg_cfg);
 
 	uint8_t RegisterRead(Register reg);
+
 	void RegisterWrite(Register reg, uint8_t value);
+
 	void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
 
+	void UpdateTemperature();
+
+	uORB::SubscriptionMultiArray<sensor_baro_s, calibration::Barometer::MAX_SENSOR_COUNT> _sensor_baro_sub{ORB_ID::sensor_baro};
+
 	PX4Magnetometer _px4_mag;
 
 	perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
@@ -91,18 +109,14 @@ class IST8310 : public device::I2C, public I2CSPIDriver<IST8310>
 
 	hrt_abstime _reset_timestamp{0};
 	hrt_abstime _last_config_check_timestamp{0};
-	int _failure_count{0};
+	hrt_abstime _temperature_update_timestamp{0};
 
-	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		CONFIGURE,
-		MEASURE,
-		READ,
-	} _state{STATE::RESET};
+	int _failure_count{0};
 
 	uint8_t _checked_register{0};
+
 	static constexpr uint8_t size_register_cfg{4};
+
 	register_config_t _register_cfg[size_register_cfg] {
 		// Register               | Set bits, Clear bits
 		{ Register::CNTL2,        0, CNTL2_BIT::SRST },

src/lib/sensor_calibration/Magnetometer.cpp

@@ -65,6 +65,46 @@ void Magnetometer::set_device_id(uint32_t device_id)
 		Reset();
 
 		ParametersUpdate();
+		SensorCorrectionsUpdate(true);
+	}
+}
+
+void Magnetometer::SensorCorrectionsUpdate(bool force)
+{
+	// check if the selected sensor has updated
+	if (_sensor_correction_sub.updated() || force) {
+
+		// valid device id required
+		if (_device_id == 0) {
+			return;
+		}
+
+		sensor_correction_s corrections;
+
+		if (_sensor_correction_sub.copy(&corrections)) {
+			// find sensor_corrections index
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				if (corrections.mag_device_ids[i] == _device_id) {
+					switch (i) {
+					case 0:
+						_thermal_offset = Vector3f{corrections.mag_offset_0};
+						return;
+					case 1:
+						_thermal_offset = Vector3f{corrections.mag_offset_1};
+						return;
+					case 2:
+						_thermal_offset = Vector3f{corrections.mag_offset_2};
+						return;
+					case 3:
+						_thermal_offset = Vector3f{corrections.mag_offset_3};
+						return;
+					}
+				}
+			}
+		}
+
+		// zero thermal offset if not found
+		_thermal_offset.zero();
 	}
 }
 

src/lib/sensor_calibration/Magnetometer.hpp

@@ -38,8 +38,7 @@
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/log.h>
 #include <uORB/Subscription.hpp>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/battery_status.h>
+#include <uORB/topics/sensor_correction.h>
 
 namespace calibration
 {
@@ -98,15 +97,21 @@ class Magnetometer
 
 	void Reset();
 
+	void SensorCorrectionsUpdate(bool force = false);
+
 	void UpdatePower(float power) { _power = power; }
 
 private:
+	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
+
 	Rotation _rotation_enum{ROTATION_NONE};
 
 	matrix::Dcmf _rotation;
 	matrix::Vector3f _offset;
 	matrix::Matrix3f _scale;
+	matrix::Vector3f _thermal_offset;
 	matrix::Vector3f _power_compensation;
+
 	float _power{0.f};
 
 	int8_t _calibration_index{-1};

src/modules/logger/logged_topics.cpp

@@ -330,6 +330,7 @@ void LoggedTopics::add_thermal_calibration_topics()
 	add_topic_multi("sensor_accel", 100, 3);
 	add_topic_multi("sensor_baro", 100, 3);
 	add_topic_multi("sensor_gyro", 100, 3);
+	add_topic_multi("sensor_mag", 100, 4);
 }
 
 void LoggedTopics::add_sensor_comparison_topics()

src/modules/temperature_compensation/CMakeLists.txt

@@ -40,6 +40,7 @@ px4_add_module(
 		temperature_calibration/accel.cpp
 		temperature_calibration/baro.cpp
 		temperature_calibration/gyro.cpp
+		temperature_calibration/mag.cpp
 		temperature_calibration/task.cpp
 	DEPENDS
 		mathlib