Use accel of the same instance for gyro instances that do not have va…

…lid temperature readings in temperature compensation, use primary baro for magnetometers without valid temperature in temperature compensation, use primary accel temperature for any barometers without a valid temperature reading.

platforms/common/include/px4_platform_common/defines.h

@@ -52,6 +52,8 @@
 #ifdef __cplusplus
 static inline constexpr bool PX4_ISFINITE(float x) { return __builtin_isfinite(x); }
 static inline constexpr bool PX4_ISFINITE(double x) { return __builtin_isfinite(x); }
+static inline constexpr bool PX4_ISNAN(float x) { return __builtin_isnan(x); }
+static inline constexpr bool PX4_ISNAN(double x) { return __builtin_isnan(x); }
 #else
 #define PX4_ISFINITE(x) __builtin_isfinite(x)
 #endif /* __cplusplus */

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

@@ -48,8 +48,6 @@
 #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;
 
@@ -96,10 +94,6 @@ class IST8308 : public device::I2C, public I2CSPIDriver<IST8308>
 
 	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")};

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

@@ -48,8 +48,6 @@
 #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;
 
@@ -97,10 +95,6 @@ class IST8310 : public device::I2C, public I2CSPIDriver<IST8310>
 
 	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")};

src/modules/logger/logged_topics.cpp

@@ -336,15 +336,15 @@ 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);
+	add_topic_multi("sensor_mag", 100, 3);
 }
 
 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_mag", 100, 4);
+	add_topic_multi("sensor_mag", 100, 3);
 }
 
 void LoggedTopics::add_vision_and_avoidance_topics()

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] = sensor_gyro.device_id;
+					_corrections.gyro_temperature[uorb_index] = sensor_gyro.temperature;
+					_corrections_changed = true;
+				}
+
+			} else if (PX4_ISNAN(sensor_gyro.temperature)) {
+
+				_corrections.gyro_device_ids[uorb_index] = sensor_gyro.device_id;
 
-					_corrections.gyro_device_ids[uorb_index] = report.device_id;
-					_corrections.gyro_temperature[uorb_index] = report.temperature;
+				// 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] = sensor_mag.device_id;
+					_corrections.mag_temperature[uorb_index] = sensor_mag.temperature;
+					_corrections_changed = true;
+				}
+
+			} else if (PX4_ISNAN(sensor_mag.temperature)) {
 
-					_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;
+
+				// 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] = sensor_baro.device_id;
+					_corrections.baro_temperature[uorb_index] = sensor_baro.temperature;
+					_corrections_changed = true;
+				}
+
+			} else if (PX4_ISNAN(sensor_baro.temperature)) {
+
+				_corrections.baro_device_ids[uorb_index] = sensor_baro.device_id;
+
+				// Use primary accelerometer instance if baro temperature was NAN.
+				sensor_accel_s sensor_accel;
 
-					_corrections.baro_device_ids[uorb_index] = report.device_id;
-					_corrections.baro_temperature[uorb_index] = report.temperature;
+				if (_accel_subs[0].update(&sensor_accel)) {
+					_corrections.baro_temperature[uorb_index] = sensor_accel.temperature;
 					_corrections_changed = true;
 				}
 			}

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();