Use accel of the same instance or primary baro for gyro instances that do not have valid temperature readings in temperature calibration data, use primary baro for magnetometers without valid temperature.

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

@@ -220,13 +220,6 @@ 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;
-				}
 			}
 		}
 
@@ -298,15 +291,3 @@ 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,10 +46,7 @@
 #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;
 
@@ -64,17 +61,9 @@ 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;
@@ -91,15 +80,9 @@ 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")};
@@ -108,14 +91,17 @@ 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,7 +188,6 @@ 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;
 
@@ -223,13 +222,6 @@ 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
@@ -304,15 +296,3 @@ 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,10 +46,7 @@
 #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;
 
@@ -64,18 +61,9 @@ 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;
@@ -92,15 +80,9 @@ 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")};
@@ -109,14 +91,18 @@ 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/modules/logger/logged_topics.cpp

@@ -344,17 +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,10 +215,12 @@ void VehicleAcceleration::Run()
 	// update corrections first to set _selected_sensor
 	bool selection_updated = SensorSelectionUpdate();
 
-	_calibration.SensorCorrectionsUpdate(selection_updated);
-	SensorBiasUpdate(selection_updated);
 	ParametersUpdate();
 
+	_calibration.SensorCorrectionsUpdate(selection_updated);
+
+	SensorBiasUpdate(selection_updated);
+
 	// require valid sensor sample rate to run
 	if (!PX4_ISFINITE(_filter_sample_rate)) {
 		CheckAndUpdateFilters();

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 @@ private:
 		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 @@ private:
 
 	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/TemperatureCompensationModule.cpp

@@ -144,16 +144,16 @@ void TemperatureCompensationModule::accelPoll()
 
 	// For each accel instance
 	for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) {
-		sensor_accel_s report;
+		sensor_accel_s sensor_accel;
 
-		// Grab temperature from report
-		if (_accel_subs[uorb_index].update(&report)) {
-			if (PX4_ISFINITE(report.temperature)) {
+		// Grab temperature from accel
+		if (_accel_subs[uorb_index].update(&sensor_accel)) {
+			if (PX4_ISFINITE(sensor_accel.temperature)) {
 				// Update the offsets and mark for publication if they've changed
-				if (_temperature_compensation.update_offsets_accel(uorb_index, report.temperature, offsets[uorb_index]) == 2) {
+				if (_temperature_compensation.update_offsets_accel(uorb_index, sensor_accel.temperature, offsets[uorb_index]) == 2) {
 
-					_corrections.accel_device_ids[uorb_index] = report.device_id;
-					_corrections.accel_temperature[uorb_index] = report.temperature;
+					_corrections.accel_device_ids[uorb_index] = sensor_accel.device_id;
+					_corrections.accel_temperature[uorb_index] = sensor_accel.temperature;
 					_corrections_changed = true;
 				}
 			}
@@ -167,16 +167,28 @@ void TemperatureCompensationModule::gyroPoll()
 
 	// For each gyro instance
 	for (uint8_t uorb_index = 0; uorb_index < GYRO_COUNT_MAX; uorb_index++) {
-		sensor_gyro_s report;
+		sensor_gyro_s sensor_gyro;
 
-		// Grab temperature from report
-		if (_gyro_subs[uorb_index].update(&report)) {
-			if (PX4_ISFINITE(report.temperature)) {
+		// Grab temperature from gyro
+		if (_gyro_subs[uorb_index].update(&sensor_gyro)) {
+			if (PX4_ISFINITE(sensor_gyro.temperature)) {
 				// Update the offsets and mark for publication if they've changed
-				if (_temperature_compensation.update_offsets_gyro(uorb_index, report.temperature, offsets[uorb_index]) == 2) {
+				if (_temperature_compensation.update_offsets_gyro(uorb_index, sensor_gyro.temperature, offsets[uorb_index]) == 2) {
 
-					_corrections.gyro_device_ids[uorb_index] = report.device_id;
-					_corrections.gyro_temperature[uorb_index] = report.temperature;
+					_corrections.gyro_device_ids[uorb_index] = sensor_gyro.device_id;
+					_corrections.gyro_temperature[uorb_index] = sensor_gyro.temperature;
+					_corrections_changed = true;
+				}
+
+			} else {
+
+				_corrections.gyro_device_ids[uorb_index] = sensor_gyro.device_id;
+
+				// Use accelerometer of the same instance if gyro temperature was NAN.
+				sensor_accel_s sensor_accel;
+
+				if (_accel_subs[uorb_index].update(&sensor_accel)) {
+					_corrections.gyro_temperature[uorb_index] = sensor_accel.temperature;
 					_corrections_changed = true;
 				}
 			}
@@ -190,16 +202,28 @@ void TemperatureCompensationModule::magPoll()
 
 	// For each mag instance
 	for (uint8_t uorb_index = 0; uorb_index < MAG_COUNT_MAX; uorb_index++) {
-		sensor_mag_s report;
+		sensor_mag_s sensor_mag;
 
 		// Grab temperature from report
-		if (_mag_subs[uorb_index].update(&report)) {
-			if (PX4_ISFINITE(report.temperature)) {
+		if (_mag_subs[uorb_index].update(&sensor_mag)) {
+			if (PX4_ISFINITE(sensor_mag.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) {
+				if (_temperature_compensation.update_offsets_mag(uorb_index, sensor_mag.temperature, offsets[uorb_index]) == 2) {
 
-					_corrections.mag_device_ids[uorb_index] = report.device_id;
-					_corrections.mag_temperature[uorb_index] = report.temperature;
+					_corrections.mag_device_ids[uorb_index] = sensor_mag.device_id;
+					_corrections.mag_temperature[uorb_index] = sensor_mag.temperature;
+					_corrections_changed = true;
+				}
+
+			} else {
+
+				_corrections.mag_device_ids[uorb_index] = sensor_mag.device_id;
+
+				// Use primary baro instance if mag temperature was NAN.
+				sensor_baro_s sensor_baro;
+
+				if (_accel_subs[0].update(&sensor_baro)) {
+					_corrections.mag_temperature[uorb_index] = sensor_baro.temperature;
 					_corrections_changed = true;
 				}
 			}
@@ -213,16 +237,28 @@ void TemperatureCompensationModule::baroPoll()
 
 	// For each baro instance
 	for (uint8_t uorb_index = 0; uorb_index < BARO_COUNT_MAX; uorb_index++) {
-		sensor_baro_s report;
+		sensor_baro_s sensor_baro;
 
 		// Grab temperature from report
-		if (_baro_subs[uorb_index].update(&report)) {
-			if (PX4_ISFINITE(report.temperature)) {
+		if (_baro_subs[uorb_index].update(&sensor_baro)) {
+			if (PX4_ISFINITE(sensor_baro.temperature)) {
 				// Update the offsets and mark for publication if they've changed
-				if (_temperature_compensation.update_offsets_baro(uorb_index, report.temperature, offsets[uorb_index]) == 2) {
+				if (_temperature_compensation.update_offsets_baro(uorb_index, sensor_baro.temperature, offsets[uorb_index]) == 2) {
 
-					_corrections.baro_device_ids[uorb_index] = report.device_id;
-					_corrections.baro_temperature[uorb_index] = report.temperature;
+					_corrections.baro_device_ids[uorb_index] = sensor_baro.device_id;
+					_corrections.baro_temperature[uorb_index] = sensor_baro.temperature;
+					_corrections_changed = true;
+				}
+
+			} else {
+
+				_corrections.baro_device_ids[uorb_index] = sensor_baro.device_id;
+
+				// Use primary accelerometer instance if baro temperature was NAN.
+				sensor_accel_s sensor_accel;
+
+				if (_accel_subs[0].update(&sensor_accel)) {
+					_corrections.baro_temperature[uorb_index] = sensor_accel.temperature;
 					_corrections_changed = true;
 				}
 			}

src/modules/temperature_compensation/temp_comp_params_mag.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 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

src/modules/temperature_compensation/temp_comp_params_mag_0.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 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

src/modules/temperature_compensation/temp_comp_params_mag_1.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 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

src/modules/temperature_compensation/temp_comp_params_mag_2.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 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

src/modules/temperature_compensation/temp_comp_params_mag_3.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 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

src/modules/temperature_compensation/temperature_calibration/mag.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 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

src/modules/temperature_compensation/temperature_calibration/mag.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2022-2023 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

src/modules/temperature_compensation/temperature_calibration/task.cpp

@@ -69,17 +69,13 @@ class TemperatureCalibration
 {
 public:
 
-	TemperatureCalibration(bool accel, bool gyro, bool mag, bool baro)
-		: _accel(accel)
-		, _gyro(gyro)
-		, _mag(mag)
-		, _baro(baro) {};
+	TemperatureCalibration(bool accel, bool gyro, bool mag, bool baro) :
+		_accel(accel), _gyro(gyro), _mag(mag), _baro(baro) {};
 
 	~TemperatureCalibration() = default;
 
 	/**
 	 * Start task.
-	 *
 	 * @return		OK on success.
 	 */
 	int		start();