Add magnetometer thermal calibration/compensation.

boards/px4/fmu-v5/test.px4board

@@ -4,6 +4,7 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_DRIVERS_PCA9685_PWM_OUT=n
 CONFIG_EXAMPLES_FAKE_GPS=n
 CONFIG_MODULES_ATTITUDE_ESTIMATOR_Q=n
+CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_LOCAL_POSITION_ESTIMATOR=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
 CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n

boards/px4/fmu-v5x/test.px4board

@@ -7,6 +7,7 @@ CONFIG_DRIVERS_IRLOCK=n
 CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
 CONFIG_MODULES_ROVER_POS_CONTROL=n
+CONFIG_MODULES_SIMULATION_SIMULATOR_SIH=n
 CONFIG_BOARD_TESTING=y
 CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
 CONFIG_DRIVERS_TEST_PPM=y

msg/SensorCorrection.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/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 @@ 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

@@ -344,16 +344,17 @@ void LoggedTopics::add_estimator_replay_topics()
 
 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_accel", 100, 4);
+	add_topic_multi("sensor_baro", 100, 4);
+	add_topic_multi("sensor_gyro", 100, 4);
+	add_topic_multi("sensor_mag", 100, 4);
 }
 
 void LoggedTopics::add_sensor_comparison_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_accel", 100, 4);
+	add_topic_multi("sensor_baro", 100, 4);
+	add_topic_multi("sensor_gyro", 100, 4);
 	add_topic_multi("sensor_mag", 100, 4);
 }
 

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -215,9 +215,11 @@ void VehicleAcceleration::Run()
 	// update corrections first to set _selected_sensor
 	bool selection_updated = SensorSelectionUpdate();
 
+	ParametersUpdate();
+
 	_calibration.SensorCorrectionsUpdate(selection_updated);
+
 	SensorBiasUpdate(selection_updated);
-	ParametersUpdate();
 
 	// require valid sensor sample rate to run
 	if (!PX4_ISFINITE(_filter_sample_rate)) {

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -790,6 +790,8 @@ void VehicleAngularVelocity::Run()
 
 	const hrt_abstime time_now_us = hrt_absolute_time();
 
+	ParametersUpdate();
+
 	// update corrections first to set _selected_sensor
 	const bool selection_updated = SensorSelectionUpdate(time_now_us);
 
@@ -801,9 +803,8 @@ void VehicleAngularVelocity::Run()
 		}
 	}
 
-	ParametersUpdate();
-
 	_calibration.SensorCorrectionsUpdate(selection_updated);
+
 	SensorBiasUpdate(selection_updated);
 
 	if (_reset_filters) {

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -369,8 +369,8 @@ void VehicleMagnetometer::UpdatePowerCompensation()
 			if (_vehicle_thrust_setpoint_0_sub.update(&vehicle_thrust_setpoint)) {
 				const matrix::Vector3f thrust_setpoint = matrix::Vector3f(vehicle_thrust_setpoint.xyz);
 
-				for (auto &cal : _calibration) {
-					cal.UpdatePower(thrust_setpoint.length());
+				for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+					_calibration[i].UpdatePower(thrust_setpoint.length());
 				}
 			}
 
@@ -382,14 +382,14 @@ void VehicleMagnetometer::UpdatePowerCompensation()
 			if (_battery_status_sub.update(&bat_stat)) {
 				float power = bat_stat.current_a * 0.001f; // current in [kA]
 
-				for (auto &cal : _calibration) {
-					cal.UpdatePower(power);
+				for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+					_calibration[i].UpdatePower(power);
 				}
 			}
 
 		} else {
-			for (auto &cal : _calibration) {
-				cal.UpdatePower(0.f);
+			for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+				_calibration[i].UpdatePower(0.f);
 			}
 		}
 	}
@@ -412,6 +412,10 @@ void VehicleMagnetometer::Run()
 		}
 	}
 
+	for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
+		_calibration[i].SensorCorrectionsUpdate();
+	}
+
 	UpdatePowerCompensation();
 
 	UpdateMagBiasEstimate();

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp

@@ -146,6 +146,7 @@ class VehicleMagnetometer : public ModuleParams, public px4::ScheduledWorkItem
 		Current_inst0,
 		Current_inst1
 	};
+
 	MagCompensationType _mag_comp_type{MagCompensationType::Disabled};
 
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
@@ -158,6 +159,7 @@ class VehicleMagnetometer : public ModuleParams, public px4::ScheduledWorkItem
 
 	uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {};
 	matrix::Vector3f _data_sum[MAX_SENSOR_COUNT] {};
+
 	int _data_sum_count[MAX_SENSOR_COUNT] {};
 	hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {};
 

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