Add magnetometer thermal compensation.

msg/SensorCorrection.msg

@@ -4,15 +4,6 @@
 
 uint64 timestamp		# time since system start (microseconds)
 
-# 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
-float32[4] gyro_temperature
-float32[3] gyro_offset_0	# gyro 0 XYZ offsets in the sensor frame in rad/s
-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
 # 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
@@ -22,6 +13,24 @@ float32[3] accel_offset_1	# accelerometer 1 offsets in the FRD board frame XYZ-a
 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
+float32[4] gyro_temperature
+float32[3] gyro_offset_0	# gyro 0 XYZ offsets in the sensor frame in rad/s
+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 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] 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
 uint32[4] baro_device_ids

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 @@ public:
 	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 @@ private:
 	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 @@ private:
 
 	hrt_abstime _reset_timestamp{0};
 	hrt_abstime _last_config_check_timestamp{0};
+	hrt_abstime _temperature_update_timestamp{0};
+
 	int _failure_count{0};
 
-	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		CONFIGURE,
-		READ,
-	} _state{STATE::RESET};
-
 	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 @@ public:
 	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 @@ private:
 	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 @@ private:
 
 	hrt_abstime _reset_timestamp{0};
 	hrt_abstime _last_config_check_timestamp{0};
+	hrt_abstime _temperature_update_timestamp{0};
+
 	int _failure_count{0};
 
-	enum class STATE : uint8_t {
-		RESET,
-		WAIT_FOR_RESET,
-		CONFIGURE,
-		MEASURE,
-		READ,
-	} _state{STATE::RESET};
-
 	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

@@ -39,6 +39,7 @@
 #include <px4_platform_common/log.h>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/sensor_correction.h>
 
 namespace calibration
 {
@@ -98,15 +99,21 @@ public:
 
 	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

@@ -347,6 +347,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

src/modules/temperature_compensation/TemperatureCompensation.cpp

@@ -52,6 +52,37 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
 	char nbuf[16] {};
 	int ret = PX4_ERROR;
 
+	/* accelerometer calibration parameters */
+	parameter_handles.accel_tc_enable = param_find("TC_A_ENABLE");
+	int32_t accel_tc_enabled = 0;
+	ret = param_get(parameter_handles.accel_tc_enable, &accel_tc_enabled);
+
+
+	if (ret == PX4_OK && accel_tc_enabled) {
+		for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) {
+			snprintf(nbuf, sizeof(nbuf), "TC_A%d_ID", j);
+			parameter_handles.accel_cal_handles[j].ID = param_find(nbuf);
+
+			for (unsigned i = 0; i < 3; i++) {
+				snprintf(nbuf, sizeof(nbuf), "TC_A%d_X3_%d", j, i);
+				parameter_handles.accel_cal_handles[j].x3[i] = param_find(nbuf);
+				snprintf(nbuf, sizeof(nbuf), "TC_A%d_X2_%d", j, i);
+				parameter_handles.accel_cal_handles[j].x2[i] = param_find(nbuf);
+				snprintf(nbuf, sizeof(nbuf), "TC_A%d_X1_%d", j, i);
+				parameter_handles.accel_cal_handles[j].x1[i] = param_find(nbuf);
+				snprintf(nbuf, sizeof(nbuf), "TC_A%d_X0_%d", j, i);
+				parameter_handles.accel_cal_handles[j].x0[i] = param_find(nbuf);
+			}
+
+			snprintf(nbuf, sizeof(nbuf), "TC_A%d_TREF", j);
+			parameter_handles.accel_cal_handles[j].ref_temp = param_find(nbuf);
+			snprintf(nbuf, sizeof(nbuf), "TC_A%d_TMIN", j);
+			parameter_handles.accel_cal_handles[j].min_temp = param_find(nbuf);
+			snprintf(nbuf, sizeof(nbuf), "TC_A%d_TMAX", j);
+			parameter_handles.accel_cal_handles[j].max_temp = param_find(nbuf);
+		}
+	}
+
 	/* rate gyro calibration parameters */
 	parameter_handles.gyro_tc_enable = param_find("TC_G_ENABLE");
 	int32_t gyro_tc_enabled = 0;
@@ -82,33 +113,33 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles &para
 		}
 	}
 
-	/* accelerometer calibration parameters */
+	/* magnetometer calibration parameters */
-	parameter_handles.accel_tc_enable = param_find("TC_A_ENABLE");
+	parameter_handles.mag_tc_enable = param_find("TC_M_ENABLE");
-	int32_t accel_tc_enabled = 0;
+	int32_t mag_tc_enabled = 0;
-	ret = param_get(parameter_handles.accel_tc_enable, &accel_tc_enabled);
+	ret = param_get(parameter_handles.mag_tc_enable, &mag_tc_enabled);
 
-	if (ret == PX4_OK && accel_tc_enabled) {
+	if (ret == PX4_OK && mag_tc_enabled) {
-		for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) {
+		for (unsigned j = 0; j < MAG_COUNT_MAX; j++) {
-			snprintf(nbuf, sizeof(nbuf), "TC_A%d_ID", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_M%d_ID", j);
-			parameter_handles.accel_cal_handles[j].ID = param_find(nbuf);
+			parameter_handles.mag_cal_handles[j].ID = param_find(nbuf);
 
 			for (unsigned i = 0; i < 3; i++) {
-				snprintf(nbuf, sizeof(nbuf), "TC_A%d_X3_%d", j, i);
+				snprintf(nbuf, sizeof(nbuf), "TC_M%d_X3_%d", j, i);
-				parameter_handles.accel_cal_handles[j].x3[i] = param_find(nbuf);
+				parameter_handles.mag_cal_handles[j].x3[i] = param_find(nbuf);
-				snprintf(nbuf, sizeof(nbuf), "TC_A%d_X2_%d", j, i);
+				snprintf(nbuf, sizeof(nbuf), "TC_M%d_X2_%d", j, i);
-				parameter_handles.accel_cal_handles[j].x2[i] = param_find(nbuf);
+				parameter_handles.mag_cal_handles[j].x2[i] = param_find(nbuf);
-				snprintf(nbuf, sizeof(nbuf), "TC_A%d_X1_%d", j, i);
+				snprintf(nbuf, sizeof(nbuf), "TC_M%d_X1_%d", j, i);
-				parameter_handles.accel_cal_handles[j].x1[i] = param_find(nbuf);
+				parameter_handles.mag_cal_handles[j].x1[i] = param_find(nbuf);
-				snprintf(nbuf, sizeof(nbuf), "TC_A%d_X0_%d", j, i);
+				snprintf(nbuf, sizeof(nbuf), "TC_M%d_X0_%d", j, i);
-				parameter_handles.accel_cal_handles[j].x0[i] = param_find(nbuf);
+				parameter_handles.mag_cal_handles[j].x0[i] = param_find(nbuf);
 			}
 
-			snprintf(nbuf, sizeof(nbuf), "TC_A%d_TREF", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_M%d_TREF", j);
-			parameter_handles.accel_cal_handles[j].ref_temp = param_find(nbuf);
+			parameter_handles.mag_cal_handles[j].ref_temp = param_find(nbuf);
-			snprintf(nbuf, sizeof(nbuf), "TC_A%d_TMIN", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_M%d_TMIN", j);
-			parameter_handles.accel_cal_handles[j].min_temp = param_find(nbuf);
+			parameter_handles.mag_cal_handles[j].min_temp = param_find(nbuf);
-			snprintf(nbuf, sizeof(nbuf), "TC_A%d_TMAX", j);
+			snprintf(nbuf, sizeof(nbuf), "TC_M%d_TMAX", j);
-			parameter_handles.accel_cal_handles[j].max_temp = param_find(nbuf);
+			parameter_handles.mag_cal_handles[j].max_temp = param_find(nbuf);
 		}
 	}
 
@@ -154,6 +185,33 @@ int TemperatureCompensation::parameters_update()
 		return ret;
 	}
 
+	/* accelerometer calibration parameters */
+	param_get(parameter_handles.accel_tc_enable, &_parameters.accel_tc_enable);
+
+	if (_parameters.accel_tc_enable == 1) {
+		for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) {
+			if (param_get(parameter_handles.accel_cal_handles[j].ID, &(_parameters.accel_cal_data[j].ID)) == PX4_OK) {
+				param_get(parameter_handles.accel_cal_handles[j].ref_temp, &(_parameters.accel_cal_data[j].ref_temp));
+				param_get(parameter_handles.accel_cal_handles[j].min_temp, &(_parameters.accel_cal_data[j].min_temp));
+				param_get(parameter_handles.accel_cal_handles[j].max_temp, &(_parameters.accel_cal_data[j].max_temp));
+
+				for (unsigned int i = 0; i < 3; i++) {
+					param_get(parameter_handles.accel_cal_handles[j].x3[i], &(_parameters.accel_cal_data[j].x3[i]));
+					param_get(parameter_handles.accel_cal_handles[j].x2[i], &(_parameters.accel_cal_data[j].x2[i]));
+					param_get(parameter_handles.accel_cal_handles[j].x1[i], &(_parameters.accel_cal_data[j].x1[i]));
+					param_get(parameter_handles.accel_cal_handles[j].x0[i], &(_parameters.accel_cal_data[j].x0[i]));
+				}
+
+			} else {
+				// Set all cal values to zero
+				memset(&_parameters.accel_cal_data[j], 0, sizeof(_parameters.accel_cal_data[j]));
+
+				PX4_WARN("FAIL ACCEL %d CAL PARAM LOAD - USING DEFAULTS", j);
+				ret = PX4_ERROR;
+			}
+		}
+	}
+
 	/* rate gyro calibration parameters */
 	param_get(parameter_handles.gyro_tc_enable, &_parameters.gyro_tc_enable);
 
@@ -181,28 +239,28 @@ int TemperatureCompensation::parameters_update()
 		}
 	}
 
-	/* accelerometer calibration parameters */
+	/* magnetometer calibration parameters */
-	param_get(parameter_handles.accel_tc_enable, &_parameters.accel_tc_enable);
+	param_get(parameter_handles.mag_tc_enable, &_parameters.mag_tc_enable);
 
-	if (_parameters.accel_tc_enable == 1) {
+	if (_parameters.mag_tc_enable == 1) {
-		for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) {
+		for (unsigned j = 0; j < MAG_COUNT_MAX; j++) {
-			if (param_get(parameter_handles.accel_cal_handles[j].ID, &(_parameters.accel_cal_data[j].ID)) == PX4_OK) {
+			if (param_get(parameter_handles.mag_cal_handles[j].ID, &(_parameters.mag_cal_data[j].ID)) == PX4_OK) {
-				param_get(parameter_handles.accel_cal_handles[j].ref_temp, &(_parameters.accel_cal_data[j].ref_temp));
+				param_get(parameter_handles.mag_cal_handles[j].ref_temp, &(_parameters.mag_cal_data[j].ref_temp));
-				param_get(parameter_handles.accel_cal_handles[j].min_temp, &(_parameters.accel_cal_data[j].min_temp));
+				param_get(parameter_handles.mag_cal_handles[j].min_temp, &(_parameters.mag_cal_data[j].min_temp));
-				param_get(parameter_handles.accel_cal_handles[j].max_temp, &(_parameters.accel_cal_data[j].max_temp));
+				param_get(parameter_handles.mag_cal_handles[j].max_temp, &(_parameters.mag_cal_data[j].max_temp));
 
 				for (unsigned int i = 0; i < 3; i++) {
-					param_get(parameter_handles.accel_cal_handles[j].x3[i], &(_parameters.accel_cal_data[j].x3[i]));
+					param_get(parameter_handles.mag_cal_handles[j].x3[i], &(_parameters.mag_cal_data[j].x3[i]));
-					param_get(parameter_handles.accel_cal_handles[j].x2[i], &(_parameters.accel_cal_data[j].x2[i]));
+					param_get(parameter_handles.mag_cal_handles[j].x2[i], &(_parameters.mag_cal_data[j].x2[i]));
-					param_get(parameter_handles.accel_cal_handles[j].x1[i], &(_parameters.accel_cal_data[j].x1[i]));
+					param_get(parameter_handles.mag_cal_handles[j].x1[i], &(_parameters.mag_cal_data[j].x1[i]));
-					param_get(parameter_handles.accel_cal_handles[j].x0[i], &(_parameters.accel_cal_data[j].x0[i]));
+					param_get(parameter_handles.mag_cal_handles[j].x0[i], &(_parameters.mag_cal_data[j].x0[i]));
 				}
 
 			} else {
 				// Set all cal values to zero
-				memset(&_parameters.accel_cal_data[j], 0, sizeof(_parameters.accel_cal_data[j]));
+				memset(&_parameters.mag_cal_data[j], 0, sizeof(_parameters.mag_cal_data[j]));
 
-				PX4_WARN("FAIL ACCEL %d CAL PARAM LOAD - USING DEFAULTS", j);
+				PX4_WARN("FAIL MAG %d CAL PARAM LOAD - USING DEFAULTS", j);
 				ret = PX4_ERROR;
 			}
 		}
@@ -311,6 +369,15 @@ bool TemperatureCompensation::calc_thermal_offsets_3D(const SensorCalData3D &coe
 	return ret;
 }
 
+int TemperatureCompensation::set_sensor_id_accel(uint32_t device_id, int topic_instance)
+{
+	if (_parameters.accel_tc_enable != 1) {
+		return 0;
+	}
+
+	return set_sensor_id(device_id, topic_instance, _accel_data, _parameters.accel_cal_data, ACCEL_COUNT_MAX);
+}
+
 int TemperatureCompensation::set_sensor_id_gyro(uint32_t device_id, int topic_instance)
 {
 	if (_parameters.gyro_tc_enable != 1) {
@@ -320,13 +387,13 @@ int TemperatureCompensation::set_sensor_id_gyro(uint32_t device_id, int topic_in
 	return set_sensor_id(device_id, topic_instance, _gyro_data, _parameters.gyro_cal_data, GYRO_COUNT_MAX);
 }
 
-int TemperatureCompensation::set_sensor_id_accel(uint32_t device_id, int topic_instance)
+int TemperatureCompensation::set_sensor_id_mag(uint32_t device_id, int topic_instance)
 {
-	if (_parameters.accel_tc_enable != 1) {
+	if (_parameters.mag_tc_enable != 1) {
 		return 0;
 	}
 
-	return set_sensor_id(device_id, topic_instance, _accel_data, _parameters.accel_cal_data, ACCEL_COUNT_MAX);
+	return set_sensor_id(device_id, topic_instance, _mag_data, _parameters.mag_cal_data, MAG_COUNT_MAX);
 }
 
 int TemperatureCompensation::set_sensor_id_baro(uint32_t device_id, int topic_instance)
@@ -352,6 +419,32 @@ int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instanc
 	return -1;
 }
 
+int TemperatureCompensation::update_offsets_accel(int topic_instance, float temperature, float *offsets)
+{
+	// Check if temperature compensation is enabled
+	if (_parameters.accel_tc_enable != 1) {
+		return 0;
+	}
+
+	// Map device ID to uORB topic instance
+	uint8_t mapping = _accel_data.device_mapping[topic_instance];
+
+	if (mapping == 255) {
+		return -1;
+	}
+
+	// Calculate and update the offsets
+	calc_thermal_offsets_3D(_parameters.accel_cal_data[mapping], temperature, offsets);
+
+	// Check if temperature delta is large enough to warrant a new publication
+	if (fabsf(temperature - _accel_data.last_temperature[topic_instance]) > 1.0f) {
+		_accel_data.last_temperature[topic_instance] = temperature;
+		return 2;
+	}
+
+	return 1;
+}
+
 int TemperatureCompensation::update_offsets_gyro(int topic_instance, float temperature, float *offsets)
 {
 	// Check if temperature compensation is enabled
@@ -378,26 +471,26 @@ int TemperatureCompensation::update_offsets_gyro(int topic_instance, float tempe
 	return 1;
 }
 
-int TemperatureCompensation::update_offsets_accel(int topic_instance, float temperature, float *offsets)
+int TemperatureCompensation::update_offsets_mag(int topic_instance, float temperature, float *offsets)
 {
 	// Check if temperature compensation is enabled
-	if (_parameters.accel_tc_enable != 1) {
+	if (_parameters.mag_tc_enable != 1) {
 		return 0;
 	}
 
 	// Map device ID to uORB topic instance
-	uint8_t mapping = _accel_data.device_mapping[topic_instance];
+	uint8_t mapping = _mag_data.device_mapping[topic_instance];
 
 	if (mapping == 255) {
 		return -1;
 	}
 
 	// Calculate and update the offsets
-	calc_thermal_offsets_3D(_parameters.accel_cal_data[mapping], temperature, offsets);
+	calc_thermal_offsets_3D(_parameters.mag_cal_data[mapping], temperature, offsets);
 
 	// Check if temperature delta is large enough to warrant a new publication
-	if (fabsf(temperature - _accel_data.last_temperature[topic_instance]) > 1.0f) {
+	if (fabsf(temperature - _mag_data.last_temperature[topic_instance]) > 1.0f) {
-		_accel_data.last_temperature[topic_instance] = temperature;
+		_mag_data.last_temperature[topic_instance] = temperature;
 		return 2;
 	}
 
@@ -433,6 +526,19 @@ int TemperatureCompensation::update_offsets_baro(int topic_instance, float tempe
 void TemperatureCompensation::print_status()
 {
 	PX4_INFO("Temperature Compensation:");
+
+	PX4_INFO(" accel: enabled: %" PRId32, _parameters.accel_tc_enable);
+
+	if (_parameters.accel_tc_enable == 1) {
+		for (int i = 0; i < ACCEL_COUNT_MAX; ++i) {
+			uint8_t mapping = _accel_data.device_mapping[i];
+
+			if (_accel_data.device_mapping[i] != 255) {
+				PX4_INFO("  using device ID %" PRId32 " for topic instance %i", _parameters.accel_cal_data[mapping].ID, i);
+			}
+		}
+	}
+
 	PX4_INFO(" gyro: enabled: %" PRId32, _parameters.gyro_tc_enable);
 
 	if (_parameters.gyro_tc_enable == 1) {
@@ -445,14 +551,14 @@ void TemperatureCompensation::print_status()
 		}
 	}
 
-	PX4_INFO(" accel: enabled: %" PRId32, _parameters.accel_tc_enable);
+	PX4_INFO(" mag: enabled: %" PRId32, _parameters.mag_tc_enable);
 
-	if (_parameters.accel_tc_enable == 1) {
+	if (_parameters.mag_tc_enable == 1) {
-		for (int i = 0; i < ACCEL_COUNT_MAX; ++i) {
+		for (int i = 0; i < MAG_COUNT_MAX; ++i) {
-			uint8_t mapping = _accel_data.device_mapping[i];
+			uint8_t mapping = _mag_data.device_mapping[i];
 
-			if (_accel_data.device_mapping[i] != 255) {
+			if (_mag_data.device_mapping[i] != 255) {
-				PX4_INFO("  using device ID %" PRId32 " for topic instance %i", _parameters.accel_cal_data[mapping].ID, i);
+				PX4_INFO("  using device ID %" PRId32 " for topic instance %i", _parameters.mag_cal_data[mapping].ID, i);
 			}
 		}
 	}

src/modules/temperature_compensation/TemperatureCompensation.h

@@ -49,13 +49,15 @@
 namespace temperature_compensation
 {
 
-static constexpr uint8_t GYRO_COUNT_MAX = 4;
 static constexpr uint8_t ACCEL_COUNT_MAX = 4;
+static constexpr uint8_t GYRO_COUNT_MAX  = 4;
+static constexpr uint8_t MAG_COUNT_MAX   = 4;
 static constexpr uint8_t BARO_COUNT_MAX  = 4;
 
-static_assert(GYRO_COUNT_MAX == 4, "GYRO_COUNT_MAX must be 4 (if changed, add/remove TC_* params to match the count)");
 static_assert(ACCEL_COUNT_MAX == 4,
 	      "ACCEL_COUNT_MAX must be 4 (if changed, add/remove TC_* params to match the count)");
+static_assert(GYRO_COUNT_MAX == 4, "GYRO_COUNT_MAX must be 4 (if changed, add/remove TC_* params to match the count)");
+static_assert(MAG_COUNT_MAX == 4,  "MAG_COUNT_MAX must be 4 (if changed, add/remove TC_* params to match the count)");
 static_assert(BARO_COUNT_MAX == 4, "BARO_COUNT_MAX must be 4 (if changed, add/remove TC_* params to match the count)");
 
 static constexpr uint8_t SENSOR_COUNT_MAX = 4;
@@ -74,12 +76,13 @@ public:
 	/** supply information which device_id matches a specific uORB topic_instance
 	 *  (needed if a system has multiple sensors of the same type)
 	 *  @return index for compensation parameter entry containing matching device ID on success, <0 otherwise */
-	int set_sensor_id_gyro(uint32_t device_id, int topic_instance);
 	int set_sensor_id_accel(uint32_t device_id, int topic_instance);
+	int set_sensor_id_gyro(uint32_t device_id, int topic_instance);
+	int set_sensor_id_mag(uint32_t device_id, int topic_instance);
 	int set_sensor_id_baro(uint32_t device_id, int topic_instance);
 
 	/**
-	 * Apply Thermal corrections to gyro (& other) sensor data.
+	 * Apply Thermal corrections to accel, gyro, mag, and baro sensor data.
 	 * @param topic_instance uORB topic instance
 	 * @param sensor_data input sensor data, output sensor data with applied corrections
 	 * @param temperature measured current temperature
@@ -89,8 +92,9 @@ public:
 	 *         1: corrections applied but no changes to offsets,
 	 *         2: corrections applied and offsets updated
 	 */
-	int update_offsets_gyro(int topic_instance, float temperature, float *offsets);
 	int update_offsets_accel(int topic_instance, float temperature, float *offsets);
+	int update_offsets_gyro(int topic_instance, float temperature, float *offsets);
+	int update_offsets_mag(int topic_instance, float temperature, float *offsets);
 	int update_offsets_baro(int topic_instance, float temperature, float *offsets);
 
 	/** output current configuration status to console */
@@ -178,11 +182,14 @@ private:
 
 	// create a struct containing all thermal calibration parameters
 	struct Parameters {
+		int32_t accel_tc_enable{0};
+		SensorCalData3D accel_cal_data[ACCEL_COUNT_MAX] {};
+
 		int32_t gyro_tc_enable{0};
 		SensorCalData3D gyro_cal_data[GYRO_COUNT_MAX] {};
 
-		int32_t accel_tc_enable{0};
+		int32_t mag_tc_enable{0};
-		SensorCalData3D accel_cal_data[ACCEL_COUNT_MAX] {};
+		SensorCalData3D mag_cal_data[MAG_COUNT_MAX] {};
 
 		int32_t baro_tc_enable{0};
 		SensorCalData1D baro_cal_data[BARO_COUNT_MAX] {};
@@ -190,11 +197,14 @@ private:
 
 	// create a struct containing the handles required to access all calibration parameters
 	struct ParameterHandles {
+		param_t accel_tc_enable{PARAM_INVALID};
+		SensorCalHandles3D accel_cal_handles[ACCEL_COUNT_MAX] {};
+
 		param_t gyro_tc_enable{PARAM_INVALID};
 		SensorCalHandles3D gyro_cal_handles[GYRO_COUNT_MAX] {};
 
-		param_t accel_tc_enable{PARAM_INVALID};
+		param_t mag_tc_enable{PARAM_INVALID};
-		SensorCalHandles3D accel_cal_handles[ACCEL_COUNT_MAX] {};
+		SensorCalHandles3D mag_cal_handles[MAG_COUNT_MAX] {};
 
 		param_t baro_tc_enable{PARAM_INVALID};
 		SensorCalHandles1D baro_cal_handles[BARO_COUNT_MAX] {};
@@ -271,8 +281,9 @@ private:
 		float last_temperature[SENSOR_COUNT_MAX] {};
 	};
 
-	PerSensorData _gyro_data;
 	PerSensorData _accel_data;
+	PerSensorData _gyro_data;
+	PerSensorData _mag_data;
 	PerSensorData _baro_data;
 
 	template<typename T>

src/modules/temperature_compensation/TemperatureCompensationModule.cpp

@@ -65,6 +65,24 @@ void TemperatureCompensationModule::parameters_update()
 {
 	_temperature_compensation.parameters_update();
 
+	// Accel
+	for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) {
+		sensor_accel_s report;
+
+		if (_accel_subs[uorb_index].copy(&report)) {
+			int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, uorb_index);
+
+			if (temp < 0) {
+				PX4_INFO("No temperature calibration available for accel %" PRIu8 " (device id %" PRIu32 ")", uorb_index,
+					 report.device_id);
+				_corrections.accel_device_ids[uorb_index] = 0;
+
+			} else {
+				_corrections.accel_device_ids[uorb_index] = report.device_id;
+			}
+		}
+	}
+
 	// Gyro
 	for (uint8_t uorb_index = 0; uorb_index < GYRO_COUNT_MAX; uorb_index++) {
 		sensor_gyro_s report;
@@ -83,20 +101,20 @@ void TemperatureCompensationModule::parameters_update()
 		}
 	}
 
-	// Accel
+	// Mag
-	for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) {
+	for (uint8_t uorb_index = 0; uorb_index < MAG_COUNT_MAX; uorb_index++) {
-		sensor_accel_s report;
+		sensor_mag_s report;
 
-		if (_accel_subs[uorb_index].copy(&report)) {
+		if (_mag_subs[uorb_index].copy(&report)) {
 			int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, uorb_index);
 
 			if (temp < 0) {
-				PX4_INFO("No temperature calibration available for accel %" PRIu8 " (device id %" PRIu32 ")", uorb_index,
+				PX4_INFO("No temperature calibration available for mag %" PRIu8 " (device id %" PRIu32 ")", uorb_index,
 					 report.device_id);
-				_corrections.accel_device_ids[uorb_index] = 0;
+				_corrections.mag_device_ids[uorb_index] = 0;
 
 			} else {
-				_corrections.accel_device_ids[uorb_index] = report.device_id;
+				_corrections.mag_device_ids[uorb_index] = report.device_id;
 			}
 		}
 	}
@@ -166,6 +184,29 @@ void TemperatureCompensationModule::gyroPoll()
 	}
 }
 
+void TemperatureCompensationModule::magPoll()
+{
+	float *offsets[] = {_corrections.mag_offset_0, _corrections.mag_offset_1, _corrections.mag_offset_2, _corrections.mag_offset_3 };
+
+	// For each mag instance
+	for (uint8_t uorb_index = 0; uorb_index < MAG_COUNT_MAX; uorb_index++) {
+		sensor_mag_s report;
+
+		// Grab temperature from report
+		if (_mag_subs[uorb_index].update(&report)) {
+			if (PX4_ISFINITE(report.temperature)) {
+				// Update the offsets and mark for publication if they've changed
+				if (_temperature_compensation.update_offsets_mag(uorb_index, report.temperature, offsets[uorb_index]) == 2) {
+
+					_corrections.mag_device_ids[uorb_index] = report.device_id;
+					_corrections.mag_temperature[uorb_index] = report.temperature;
+					_corrections_changed = true;
+				}
+			}
+		}
+	}
+}
+
 void TemperatureCompensationModule::baroPoll()
 {
 	float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2, &_corrections.baro_offset_3 };
@@ -204,16 +245,22 @@ void TemperatureCompensationModule::Run()
 			if (cmd.command == vehicle_command_s::VEHICLE_CMD_PREFLIGHT_CALIBRATION) {
 				bool got_temperature_calibration_command = false;
 				bool accel = false;
-				bool baro = false;
 				bool gyro = false;
+				bool mag = false;
+				bool baro = false;
+
+				if ((int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
+					accel = true;
+					got_temperature_calibration_command = true;
+				}
 
 				if ((int)(cmd.param1) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
 					gyro = true;
 					got_temperature_calibration_command = true;
 				}
 
-				if ((int)(cmd.param5) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
+				if ((int)(cmd.param2) == vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION) {
-					accel = true;
+					mag = true;
 					got_temperature_calibration_command = true;
 				}
 
@@ -223,7 +270,7 @@ void TemperatureCompensationModule::Run()
 				}
 
 				if (got_temperature_calibration_command) {
-					int ret = run_temperature_calibration(accel, baro, gyro);
+					int ret = run_temperature_calibration(accel, baro, gyro, mag);
 
 					// publish ACK
 					vehicle_command_ack_s command_ack{};
@@ -258,6 +305,7 @@ void TemperatureCompensationModule::Run()
 
 	accelPoll();
 	gyroPoll();
+	magPoll();
 	baroPoll();
 
 	// publish sensor corrections if necessary
@@ -306,27 +354,34 @@ int TemperatureCompensationModule::custom_command(int argc, char *argv[])
 	if (!strcmp(argv[0], "calibrate")) {
 
 		bool accel_calib = false;
-		bool baro_calib = false;
 		bool gyro_calib = false;
+		bool mag_calib = false;
+		bool baro_calib = false;
 		bool calib_all = true;
 		int myoptind = 1;
 		int ch;
+
 		const char *myoptarg = nullptr;
 
-		while ((ch = px4_getopt(argc, argv, "abg", &myoptind, &myoptarg)) != EOF) {
+		while ((ch = px4_getopt(argc, argv, "agmb", &myoptind, &myoptarg)) != EOF) {
 			switch (ch) {
 			case 'a':
 				accel_calib = true;
 				calib_all = false;
 				break;
 
-			case 'b':
+			case 'g':
-				baro_calib = true;
+				gyro_calib = true;
 				calib_all = false;
 				break;
 
-			case 'g':
+			case 'm':
-				gyro_calib = true;
+				mag_calib = true;
+				calib_all = false;
+				break;
+
+			case 'b':
+				baro_calib = true;
 				calib_all = false;
 				break;
 
@@ -349,7 +404,8 @@ int TemperatureCompensationModule::custom_command(int argc, char *argv[])
 		vcmd.timestamp = hrt_absolute_time();
 		vcmd.param1 = (float)((gyro_calib
 				       || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
-		vcmd.param2 = NAN;
+		vcmd.param2 = (float)((mag_calib
+				       || calib_all) ? vehicle_command_s::PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION : NAN);
 		vcmd.param3 = NAN;
 		vcmd.param4 = NAN;
 		vcmd.param5 = ((accel_calib
@@ -398,8 +454,9 @@ a temperature cycle.
 	PRINT_MODULE_USAGE_NAME("temperature_compensation", "system");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start the module, which monitors the sensors and updates the sensor_correction topic");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("calibrate", "Run temperature calibration process");
-	PRINT_MODULE_USAGE_PARAM_FLAG('g', "calibrate the gyro", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('a', "calibrate the accel", true);
+	PRINT_MODULE_USAGE_PARAM_FLAG('g', "calibrate the gyro", true);
+	PRINT_MODULE_USAGE_PARAM_FLAG('m', "calibrate the mag", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('b', "calibrate the baro (if none of these is given, all will be calibrated)", true);
 	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 

src/modules/temperature_compensation/TemperatureCompensationModule.h

@@ -54,6 +54,7 @@
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_correction.h>
 #include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_command_ack.h>
 
@@ -97,6 +98,7 @@ private:
 
 	void accelPoll();
 	void gyroPoll();
+	void magPoll();
 	void baroPoll();
 
 	/**
@@ -119,6 +121,13 @@ private:
 		{ORB_ID(sensor_gyro), 3},
 	};
 
+	uORB::Subscription _mag_subs[MAG_COUNT_MAX] {
+		{ORB_ID(sensor_mag), 0},
+		{ORB_ID(sensor_mag), 1},
+		{ORB_ID(sensor_mag), 2},
+		{ORB_ID(sensor_mag), 3},
+	};
+
 	uORB::Subscription _baro_subs[BARO_COUNT_MAX] {
 		{ORB_ID(sensor_baro), 0},
 		{ORB_ID(sensor_baro), 1},

src/modules/temperature_compensation/temp_comp_params_mag.c

@@ -0,0 +1,41 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * Thermal compensation for magnetometer sensors.
+ *
+ * @group Thermal Compensation
+ * @reboot_required true
+ * @boolean
+ */
+PARAM_DEFINE_INT32(TC_M_ENABLE, 0);

src/modules/temperature_compensation/temp_comp_params_mag_0.c

@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* Magnetometer 0 */
+
+/**
+ * ID of Magnetometer that the calibration is for.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_INT32(TC_M0_ID, 0);
+
+/**
+ * Magnetometer offset temperature ^3 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_X3_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^3 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_X3_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^3 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_X3_2, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^2 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_X2_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^2 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_X2_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^2 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_X2_2, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^1 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_X1_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^1 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_X1_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^1 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_X1_2, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^0 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_X0_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^0 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_X0_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^0 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_X0_2, 0.0f);
+
+/**
+ * Magnetometer calibration reference temperature.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_TREF, 25.0f);
+
+/**
+ * Magnetometer calibration minimum temperature.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_TMIN, 0.0f);
+
+/**
+ * Magnetometer calibration maximum temperature.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M0_TMAX, 100.0f);

src/modules/temperature_compensation/temp_comp_params_mag_1.c

@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* Magnetometer 1 */
+
+/**
+ * ID of Magnetometer that the calibration is for.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_INT32(TC_M1_ID, 0);
+
+/**
+ * Magnetometer offset temperature ^3 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_X3_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^3 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_X3_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^3 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_X3_2, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^2 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_X2_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^2 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_X2_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^2 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_X2_2, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^1 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_X1_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^1 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_X1_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^1 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_X1_2, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^0 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_X0_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^0 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_X0_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^0 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_X0_2, 0.0f);
+
+/**
+ * Magnetometer calibration reference temperature.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_TREF, 25.0f);
+
+/**
+ * Magnetometer calibration minimum temperature.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_TMIN, 0.0f);
+
+/**
+ * Magnetometer calibration maximum temperature.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M1_TMAX, 100.0f);

src/modules/temperature_compensation/temp_comp_params_mag_2.c

@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* Magnetometer 2 */
+
+/**
+ * ID of Magnetometer that the calibration is for.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_INT32(TC_M2_ID, 0);
+
+/**
+ * Magnetometer offset temperature ^3 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_X3_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^3 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_X3_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^3 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_X3_2, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^2 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_X2_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^2 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_X2_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^2 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_X2_2, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^1 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_X1_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^1 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_X1_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^1 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_X1_2, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^0 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_X0_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^0 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_X0_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^0 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_X0_2, 0.0f);
+
+/**
+ * Magnetometer calibration reference temperature.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_TREF, 25.0f);
+
+/**
+ * Magnetometer calibration minimum temperature.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_TMIN, 0.0f);
+
+/**
+ * Magnetometer calibration maximum temperature.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M2_TMAX, 100.0f);

src/modules/temperature_compensation/temp_comp_params_mag_3.c

@@ -0,0 +1,162 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/* Magnetometer 3 */
+
+/**
+ * ID of Magnetometer that the calibration is for.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_INT32(TC_M3_ID, 0);
+
+/**
+ * Magnetometer offset temperature ^3 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_X3_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^3 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_X3_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^3 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_X3_2, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^2 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_X2_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^2 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_X2_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^2 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_X2_2, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^1 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_X1_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^1 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_X1_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^1 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_X1_2, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^0 polynomial coefficient - X axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_X0_0, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^0 polynomial coefficient - Y axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_X0_1, 0.0f);
+
+/**
+ * Magnetometer offset temperature ^0 polynomial coefficient - Z axis.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_X0_2, 0.0f);
+
+/**
+ * Magnetometer calibration reference temperature.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_TREF, 25.0f);
+
+/**
+ * Magnetometer calibration minimum temperature.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_TMIN, 0.0f);
+
+/**
+ * Magnetometer calibration maximum temperature.
+ *
+ * @group Thermal Compensation
+ * @category system
+ */
+PARAM_DEFINE_FLOAT(TC_M3_TMAX, 100.0f);

src/modules/temperature_compensation/temperature_calibration/mag.cpp

@@ -0,0 +1,229 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mag.cpp
+ * Implementation of the Mag Temperature Calibration for onboard sensors.
+ *
+ * @author Siddharth Bharat Purohit
+ * @author Paul Riseborough
+ * @author Beat Küng <beat-kueng@gmx.net>
+ */
+
+#include "mag.h"
+#include <uORB/topics/sensor_mag.h>
+#include <mathlib/mathlib.h>
+#include <drivers/drv_hrt.h>
+
+TemperatureCalibrationMag::TemperatureCalibrationMag(float min_temperature_rise, float min_start_temperature,
+		float max_start_temperature)
+	: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
+{
+	// init subscriptions
+	_num_sensor_instances = orb_group_count(ORB_ID(sensor_mag));
+
+	if (_num_sensor_instances > SENSOR_COUNT_MAX) {
+		_num_sensor_instances = SENSOR_COUNT_MAX;
+	}
+
+	for (unsigned i = 0; i < _num_sensor_instances; i++) {
+		_sensor_subs[i] = orb_subscribe_multi(ORB_ID(sensor_mag), i);
+	}
+}
+
+TemperatureCalibrationMag::~TemperatureCalibrationMag()
+{
+	for (unsigned i = 0; i < _num_sensor_instances; i++) {
+		orb_unsubscribe(_sensor_subs[i]);
+	}
+}
+
+int TemperatureCalibrationMag::update_sensor_instance(PerSensorData &data, int sensor_sub)
+{
+	bool finished = data.hot_soaked;
+
+	bool updated;
+	orb_check(sensor_sub, &updated);
+
+	if (!updated) {
+		return finished ? 0 : 1;
+	}
+
+	sensor_mag_s mag_data;
+	orb_copy(ORB_ID(sensor_mag), sensor_sub, &mag_data);
+
+	if (finished) {
+		// if we're done, return, but we need to return after orb_copy because of poll()
+		return 0;
+	}
+
+	if (PX4_ISFINITE(mag_data.temperature)) {
+		data.has_valid_temperature = true;
+
+	} else {
+		return 0;
+	}
+
+	data.device_id = mag_data.device_id;
+
+	data.sensor_sample_filt[0] = mag_data.x;
+	data.sensor_sample_filt[1] = mag_data.y;
+	data.sensor_sample_filt[2] = mag_data.z;
+	data.sensor_sample_filt[3] = mag_data.temperature;
+
+	// wait for min start temp to be reached before starting calibration
+	if (data.sensor_sample_filt[3] < _min_start_temperature) {
+		return 1;
+	}
+
+	if (!data.cold_soaked) {
+		// allow time for sensors and filters to settle
+		if (hrt_absolute_time() > 10E6) {
+			// If intial temperature exceeds maximum declare an error condition and exit
+			if (data.sensor_sample_filt[3] > _max_start_temperature) {
+				return -TC_ERROR_INITIAL_TEMP_TOO_HIGH;
+
+			} else {
+				data.cold_soaked = true;
+				data.low_temp = data.sensor_sample_filt[3]; // Record the low temperature
+				data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature
+				data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise;
+				return 1;
+			}
+
+		} else {
+			return 1;
+		}
+	}
+
+	// check if temperature increased
+	if (data.sensor_sample_filt[3] > data.high_temp) {
+		data.high_temp = data.sensor_sample_filt[3];
+		data.hot_soak_sat = 0;
+
+	} else {
+		return 1;
+	}
+
+	//TODO: Detect when temperature has stopped rising for more than TBD seconds
+	if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
+		data.hot_soaked = true;
+	}
+
+	if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
+		TC_DEBUG("\nMag: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
+			 (double)data.sensor_sample_filt[1],
+			 (double)data.sensor_sample_filt[2], (double)data.sensor_sample_filt[3], (double)data.low_temp, (double)data.high_temp,
+			 (double)(data.high_temp - data.low_temp));
+	}
+
+	//update linear fit matrices
+	double relative_temperature = (double)data.sensor_sample_filt[3] - (double)data.ref_temp;
+	data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]);
+	data.P[1].update(relative_temperature, (double)data.sensor_sample_filt[1]);
+	data.P[2].update(relative_temperature, (double)data.sensor_sample_filt[2]);
+
+	return 1;
+}
+
+int TemperatureCalibrationMag::finish()
+{
+	for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
+		finish_sensor_instance(_data[uorb_index], uorb_index);
+	}
+
+	int32_t enabled = 1;
+	int result = param_set_no_notification(param_find("TC_A_ENABLE"), &enabled);
+
+	if (result != PX4_OK) {
+		PX4_ERR("unable to reset TC_A_ENABLE (%i)", result);
+	}
+
+	return result;
+}
+
+int TemperatureCalibrationMag::finish_sensor_instance(PerSensorData &data, int sensor_index)
+{
+	if (!data.has_valid_temperature) {
+		PX4_WARN("Result Mag %d does not have a valid temperature sensor", sensor_index);
+
+		uint32_t param = 0;
+		set_parameter("TC_M%d_ID", sensor_index, &param);
+		return 0;
+	}
+
+	if (!data.hot_soaked || data.tempcal_complete) {
+		return 0;
+	}
+
+	double res[3][4] = {};
+	data.P[0].fit(res[0]);
+	res[0][3] = 0.0; // normalise the correction to be zero at the reference temperature
+	PX4_INFO("Result Mag %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (double)res[0][0], (double)res[0][1],
+		 (double)res[0][2],
+		 (double)res[0][3]);
+	data.P[1].fit(res[1]);
+	res[1][3] = 0.0; // normalise the correction to be zero at the reference temperature
+	PX4_INFO("Result Mag %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (double)res[1][0], (double)res[1][1],
+		 (double)res[1][2],
+		 (double)res[1][3]);
+	data.P[2].fit(res[2]);
+	res[2][3] = 0.0; // normalise the correction to be zero at the reference temperature
+	PX4_INFO("Result Mag %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (double)res[2][0], (double)res[2][1],
+		 (double)res[2][2],
+		 (double)res[2][3]);
+	data.tempcal_complete = true;
+
+	char str[30];
+	float param = 0.0f;
+	int result = PX4_OK;
+
+	set_parameter("TC_M%d_ID", sensor_index, &data.device_id);
+
+	for (unsigned axis_index = 0; axis_index < 3; axis_index++) {
+		for (unsigned coef_index = 0; coef_index <= 3; coef_index++) {
+			sprintf(str, "TC_M%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
+			param = (float)res[axis_index][coef_index];
+			result = param_set_no_notification(param_find(str), &param);
+
+			if (result != PX4_OK) {
+				PX4_ERR("unable to reset %s", str);
+			}
+		}
+	}
+
+	set_parameter("TC_M%d_TMAX", sensor_index, &data.high_temp);
+	set_parameter("TC_M%d_TMIN", sensor_index, &data.low_temp);
+	set_parameter("TC_M%d_TREF", sensor_index, &data.ref_temp);
+	return 0;
+}

src/modules/temperature_compensation/temperature_calibration/mag.h

@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include "common.h"
+#include "polyfit.hpp"
+
+class TemperatureCalibrationMag : public TemperatureCalibrationCommon<3, 3>
+{
+public:
+	TemperatureCalibrationMag(float min_temperature_rise, float min_start_temperature, float max_start_temperature);
+	virtual ~TemperatureCalibrationMag();
+
+	/**
+	 * @see TemperatureCalibrationBase::finish()
+	 */
+	int finish();
+
+private:
+
+	virtual inline int update_sensor_instance(PerSensorData &data, int sensor_sub);
+
+	inline int finish_sensor_instance(PerSensorData &data, int sensor_index);
+};

src/modules/temperature_compensation/temperature_calibration/task.cpp

@@ -56,6 +56,7 @@
 #include "accel.h"
 #include "baro.h"
 #include "gyro.h"
+#include "mag.h"
 
 class TemperatureCalibration;
 
@@ -68,7 +69,12 @@ class TemperatureCalibration
 {
 public:
 
-	TemperatureCalibration(bool accel, bool baro, bool gyro) : _accel(accel), _baro(baro), _gyro(gyro) {}
+	TemperatureCalibration(bool accel, bool gyro, bool mag, bool baro)
+		: _accel(accel)
+		, _gyro(gyro)
+		, _mag(mag)
+		, _baro(baro) {};
+
 	~TemperatureCalibration() = default;
 
 	/**
@@ -93,8 +99,9 @@ private:
 	int	_control_task = -1;		// task handle for task
 
 	const bool _accel; ///< enable accel calibration?
-	const bool _baro; ///< enable baro calibration?
 	const bool _gyro;  ///< enable gyro calibration?
+	const bool _mag;   ///< enable mag calibration?
+	const bool _baro;  ///< enable baro calibration?
 };
 
 void TemperatureCalibration::task_main()
@@ -163,6 +170,17 @@ void TemperatureCalibration::task_main()
 		}
 	}
 
+	if (_mag) {
+		calibrators[num_calibrators] = new TemperatureCalibrationMag(min_temp_rise, min_start_temp, max_start_temp);
+
+		if (calibrators[num_calibrators]) {
+			++num_calibrators;
+
+		} else {
+			PX4_ERR("alloc failed");
+		}
+	}
+
 	hrt_abstime next_progress_output = hrt_absolute_time() + 1e6;
 
 	// control LED's: blink, then turn solid according to progress
@@ -331,11 +349,12 @@ void TemperatureCalibration::publish_led_control(led_control_s &led_control)
 	_led_control_pub.publish(led_control);
 }
 
-int run_temperature_calibration(bool accel, bool baro, bool gyro)
+int run_temperature_calibration(bool accel, bool gyro, bool mag, bool baro)
 {
 	if (temperature_calibration::instance.load() == nullptr) {
-		PX4_INFO("Starting temperature calibration task (accel=%i, baro=%i, gyro=%i)", (int)accel, (int)baro, (int)gyro);
+		PX4_INFO("Starting temperature calibration task (accel=%i, gyro=%i, mag=%i, baro=%i)", (int)accel, (int)gyro, (int)mag,
-		temperature_calibration::instance.store(new TemperatureCalibration(accel, baro, gyro));
+			 (int)baro);
+		temperature_calibration::instance.store(new TemperatureCalibration(accel, gyro, mag, baro));
 
 		if (temperature_calibration::instance.load() == nullptr) {
 			PX4_ERR("alloc failed");

src/modules/temperature_compensation/temperature_calibration/temperature_calibration.h

@@ -36,4 +36,4 @@
 
 /** start temperature calibration in a new task for one or multiple sensors
  * @return 0 on success, <0 error otherwise  */
-int run_temperature_calibration(bool accel, bool baro, bool gyro);
+int run_temperature_calibration(bool accel, bool gyro, bool mag, bool baro);