From c4b91c855889a229ac9632e9cc8c9fcd0eeb4c43 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 19 Aug 2021 17:29:26 +0200
Subject: [PATCH] calibration params: avoid using NaN as default

 - NaN is not supported by JSON, and leads to parsing failure in QGC.
 - fixes https://github.com/PX4/PX4-Autopilot/issues/18095
---
 src/lib/sensor_calibration/Accelerometer.cpp | 16 ++++++++++++++--
 src/lib/sensor_calibration/Accelerometer.hpp |  2 ++
 src/lib/sensor_calibration/Gyroscope.cpp     | 16 ++++++++++++++--
 src/lib/sensor_calibration/Gyroscope.hpp     |  2 ++
 src/lib/sensor_calibration/Magnetometer.cpp  | 16 ++++++++++++++--
 src/lib/sensor_calibration/Magnetometer.hpp  |  2 ++
 src/modules/sensors/sensor_params_acc0.c     |  3 ++-
 src/modules/sensors/sensor_params_acc1.c     |  3 ++-
 src/modules/sensors/sensor_params_acc2.c     |  3 ++-
 src/modules/sensors/sensor_params_acc3.c     |  3 ++-
 src/modules/sensors/sensor_params_gyro0.c    |  3 ++-
 src/modules/sensors/sensor_params_gyro1.c    |  3 ++-
 src/modules/sensors/sensor_params_gyro2.c    |  3 ++-
 src/modules/sensors/sensor_params_gyro3.c    |  3 ++-
 src/modules/sensors/sensor_params_mag0.c     |  3 ++-
 src/modules/sensors/sensor_params_mag1.c     |  3 ++-
 src/modules/sensors/sensor_params_mag2.c     |  3 ++-
 src/modules/sensors/sensor_params_mag3.c     |  3 ++-
 18 files changed, 72 insertions(+), 18 deletions(-)

diff --git a/src/lib/sensor_calibration/Accelerometer.cpp b/src/lib/sensor_calibration/Accelerometer.cpp
index d67f6372c5..a33494fb1d 100644
--- a/src/lib/sensor_calibration/Accelerometer.cpp
+++ b/src/lib/sensor_calibration/Accelerometer.cpp
@@ -210,7 +210,14 @@ void Accelerometer::ParametersUpdate()
 		}
 
 		// CAL_ACCx_TEMP
-		set_temperature(GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index));
+		float cal_temp = GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index);
+
+		if (cal_temp > TEMPERATURE_INVALID) {
+			set_temperature(cal_temp);
+
+		} else {
+			set_temperature(NAN);
+		}
 
 		// CAL_ACCx_OFF{X,Y,Z}
 		set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
@@ -263,7 +270,12 @@ bool Accelerometer::ParametersSave()
 			success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
 		}
 
-		success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature);
+		if (PX4_ISFINITE(_temperature)) {
+			success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature);
+
+		} else {
+			success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, TEMPERATURE_INVALID);
+		}
 
 		return success;
 	}
diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp
index 01fa97a9aa..020eb6d33d 100644
--- a/src/lib/sensor_calibration/Accelerometer.hpp
+++ b/src/lib/sensor_calibration/Accelerometer.hpp
@@ -92,6 +92,8 @@ public:
 	void SensorCorrectionsUpdate(bool force = false);
 
 private:
+	static constexpr float TEMPERATURE_INVALID = -1000.f;
+
 	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
 	Rotation _rotation_enum{ROTATION_NONE};
diff --git a/src/lib/sensor_calibration/Gyroscope.cpp b/src/lib/sensor_calibration/Gyroscope.cpp
index d7d436a098..8a0eca4466 100644
--- a/src/lib/sensor_calibration/Gyroscope.cpp
+++ b/src/lib/sensor_calibration/Gyroscope.cpp
@@ -195,7 +195,14 @@ void Gyroscope::ParametersUpdate()
 		}
 
 		// CAL_GYROx_TEMP
-		set_temperature(GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index));
+		float cal_temp = GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index);
+
+		if (cal_temp > TEMPERATURE_INVALID) {
+			set_temperature(cal_temp);
+
+		} else {
+			set_temperature(NAN);
+		}
 
 		// CAL_GYROx_OFF{X,Y,Z}
 		set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
@@ -243,7 +250,12 @@ bool Gyroscope::ParametersSave()
 			success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
 		}
 
-		success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature);
+		if (PX4_ISFINITE(_temperature)) {
+			success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature);
+
+		} else {
+			success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, TEMPERATURE_INVALID);
+		}
 
 		return success;
 	}
diff --git a/src/lib/sensor_calibration/Gyroscope.hpp b/src/lib/sensor_calibration/Gyroscope.hpp
index 1c386449a1..aaa8e96df7 100644
--- a/src/lib/sensor_calibration/Gyroscope.hpp
+++ b/src/lib/sensor_calibration/Gyroscope.hpp
@@ -96,6 +96,8 @@ public:
 	void SensorCorrectionsUpdate(bool force = false);
 
 private:
+	static constexpr float TEMPERATURE_INVALID = -1000.f;
+
 	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};
 
 	Rotation _rotation_enum{ROTATION_NONE};
diff --git a/src/lib/sensor_calibration/Magnetometer.cpp b/src/lib/sensor_calibration/Magnetometer.cpp
index 524ee86717..d54c2ce7e5 100644
--- a/src/lib/sensor_calibration/Magnetometer.cpp
+++ b/src/lib/sensor_calibration/Magnetometer.cpp
@@ -195,7 +195,14 @@ void Magnetometer::ParametersUpdate()
 		}
 
 		// CAL_MAGx_TEMP
-		set_temperature(GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index));
+		float cal_temp = GetCalibrationParamFloat(SensorString(), "TEMP", _calibration_index);
+
+		if (cal_temp > TEMPERATURE_INVALID) {
+			set_temperature(cal_temp);
+
+		} else {
+			set_temperature(NAN);
+		}
 
 		// CAL_MAGx_OFF{X,Y,Z}
 		set_offset(GetCalibrationParamsVector3f(SensorString(), "OFF", _calibration_index));
@@ -263,7 +270,12 @@ bool Magnetometer::ParametersSave()
 			success &= SetCalibrationParam(SensorString(), "ROT", _calibration_index, -1);
 		}
 
-		success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature);
+		if (PX4_ISFINITE(_temperature)) {
+			success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, _temperature);
+
+		} else {
+			success &= SetCalibrationParam(SensorString(), "TEMP", _calibration_index, TEMPERATURE_INVALID);
+		}
 
 		return success;
 	}
diff --git a/src/lib/sensor_calibration/Magnetometer.hpp b/src/lib/sensor_calibration/Magnetometer.hpp
index 677afe7e40..b4781675dc 100644
--- a/src/lib/sensor_calibration/Magnetometer.hpp
+++ b/src/lib/sensor_calibration/Magnetometer.hpp
@@ -100,6 +100,8 @@ public:
 	void UpdatePower(float power) { _power = power; }
 
 private:
+	static constexpr float TEMPERATURE_INVALID = -1000.f;
+
 	Rotation _rotation_enum{ROTATION_NONE};
 
 	matrix::Dcmf _rotation;
diff --git a/src/modules/sensors/sensor_params_acc0.c b/src/modules/sensors/sensor_params_acc0.c
index 773e2d57fc..b7f3d5d162 100644
--- a/src/modules/sensors/sensor_params_acc0.c
+++ b/src/modules/sensors/sensor_params_acc0.c
@@ -172,8 +172,9 @@ PARAM_DEFINE_FLOAT(CAL_ACC0_ZSCALE, 1.0f);
  *
  * Temperature during last calibration.
  *
+ * @unit celcius
  * @category system
  * @group Sensor Calibration
  * @volatile
  */
-PARAM_DEFINE_FLOAT(CAL_ACC0_TEMP, NAN);
+PARAM_DEFINE_FLOAT(CAL_ACC0_TEMP, -1000.f);
diff --git a/src/modules/sensors/sensor_params_acc1.c b/src/modules/sensors/sensor_params_acc1.c
index 97953d6057..eb5351a37d 100644
--- a/src/modules/sensors/sensor_params_acc1.c
+++ b/src/modules/sensors/sensor_params_acc1.c
@@ -172,8 +172,9 @@ PARAM_DEFINE_FLOAT(CAL_ACC1_ZSCALE, 1.0f);
  *
  * Temperature during last calibration.
  *
+ * @unit celcius
  * @category system
  * @group Sensor Calibration
  * @volatile
  */
-PARAM_DEFINE_FLOAT(CAL_ACC1_TEMP, NAN);
+PARAM_DEFINE_FLOAT(CAL_ACC1_TEMP, -1000.f);
diff --git a/src/modules/sensors/sensor_params_acc2.c b/src/modules/sensors/sensor_params_acc2.c
index e309bd8d6e..98eaa14312 100644
--- a/src/modules/sensors/sensor_params_acc2.c
+++ b/src/modules/sensors/sensor_params_acc2.c
@@ -172,8 +172,9 @@ PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f);
  *
  * Temperature during last calibration.
  *
+ * @unit celcius
  * @category system
  * @group Sensor Calibration
  * @volatile
  */
-PARAM_DEFINE_FLOAT(CAL_ACC2_TEMP, NAN);
+PARAM_DEFINE_FLOAT(CAL_ACC2_TEMP, -1000.f);
diff --git a/src/modules/sensors/sensor_params_acc3.c b/src/modules/sensors/sensor_params_acc3.c
index 723545779d..972d554e37 100644
--- a/src/modules/sensors/sensor_params_acc3.c
+++ b/src/modules/sensors/sensor_params_acc3.c
@@ -172,8 +172,9 @@ PARAM_DEFINE_FLOAT(CAL_ACC3_ZSCALE, 1.0f);
  *
  * Temperature during last calibration.
  *
+ * @unit celcius
  * @category system
  * @group Sensor Calibration
  * @volatile
  */
-PARAM_DEFINE_FLOAT(CAL_ACC3_TEMP, NAN);
+PARAM_DEFINE_FLOAT(CAL_ACC3_TEMP, -1000.f);
diff --git a/src/modules/sensors/sensor_params_gyro0.c b/src/modules/sensors/sensor_params_gyro0.c
index a1cd917454..1f3f03b2fa 100644
--- a/src/modules/sensors/sensor_params_gyro0.c
+++ b/src/modules/sensors/sensor_params_gyro0.c
@@ -145,8 +145,9 @@ PARAM_DEFINE_FLOAT(CAL_GYRO0_ZOFF, 0.0f);
  *
  * Temperature during last calibration.
  *
+ * @unit celcius
  * @category system
  * @group Sensor Calibration
  * @volatile
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO0_TEMP, NAN);
+PARAM_DEFINE_FLOAT(CAL_GYRO0_TEMP, -1000.f);
diff --git a/src/modules/sensors/sensor_params_gyro1.c b/src/modules/sensors/sensor_params_gyro1.c
index ac939c2237..6ef782ef8c 100644
--- a/src/modules/sensors/sensor_params_gyro1.c
+++ b/src/modules/sensors/sensor_params_gyro1.c
@@ -145,8 +145,9 @@ PARAM_DEFINE_FLOAT(CAL_GYRO1_ZOFF, 0.0f);
  *
  * Temperature during last calibration.
  *
+ * @unit celcius
  * @category system
  * @group Sensor Calibration
  * @volatile
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO1_TEMP, NAN);
+PARAM_DEFINE_FLOAT(CAL_GYRO1_TEMP, -1000.f);
diff --git a/src/modules/sensors/sensor_params_gyro2.c b/src/modules/sensors/sensor_params_gyro2.c
index f6c97df594..2f5e91e35f 100644
--- a/src/modules/sensors/sensor_params_gyro2.c
+++ b/src/modules/sensors/sensor_params_gyro2.c
@@ -145,8 +145,9 @@ PARAM_DEFINE_FLOAT(CAL_GYRO2_ZOFF, 0.0f);
  *
  * Temperature during last calibration.
  *
+ * @unit celcius
  * @category system
  * @group Sensor Calibration
  * @volatile
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO2_TEMP, NAN);
+PARAM_DEFINE_FLOAT(CAL_GYRO2_TEMP, -1000.f);
diff --git a/src/modules/sensors/sensor_params_gyro3.c b/src/modules/sensors/sensor_params_gyro3.c
index 09265faf9e..74a4dcd509 100644
--- a/src/modules/sensors/sensor_params_gyro3.c
+++ b/src/modules/sensors/sensor_params_gyro3.c
@@ -145,8 +145,9 @@ PARAM_DEFINE_FLOAT(CAL_GYRO3_ZOFF, 0.0f);
  *
  * Temperature during last calibration.
  *
+ * @unit celcius
  * @category system
  * @group Sensor Calibration
  * @volatile
  */
-PARAM_DEFINE_FLOAT(CAL_GYRO3_TEMP, NAN);
+PARAM_DEFINE_FLOAT(CAL_GYRO3_TEMP, -1000.f);
diff --git a/src/modules/sensors/sensor_params_mag0.c b/src/modules/sensors/sensor_params_mag0.c
index 91e5dd34ab..0d56250969 100644
--- a/src/modules/sensors/sensor_params_mag0.c
+++ b/src/modules/sensors/sensor_params_mag0.c
@@ -241,8 +241,9 @@ PARAM_DEFINE_FLOAT(CAL_MAG0_ZCOMP, 0.0f);
  *
  * Temperature during last calibration.
  *
+ * @unit celcius
  * @category system
  * @group Sensor Calibration
  * @volatile
  */
-PARAM_DEFINE_FLOAT(CAL_MAG0_TEMP, NAN);
+PARAM_DEFINE_FLOAT(CAL_MAG0_TEMP, -1000.f);
diff --git a/src/modules/sensors/sensor_params_mag1.c b/src/modules/sensors/sensor_params_mag1.c
index f161b0d1b8..0c952d4d9c 100644
--- a/src/modules/sensors/sensor_params_mag1.c
+++ b/src/modules/sensors/sensor_params_mag1.c
@@ -241,8 +241,9 @@ PARAM_DEFINE_FLOAT(CAL_MAG1_ZCOMP, 0.0f);
  *
  * Temperature during last calibration.
  *
+ * @unit celcius
  * @category system
  * @group Sensor Calibration
  * @volatile
  */
-PARAM_DEFINE_FLOAT(CAL_MAG1_TEMP, NAN);
+PARAM_DEFINE_FLOAT(CAL_MAG1_TEMP, -1000.f);
diff --git a/src/modules/sensors/sensor_params_mag2.c b/src/modules/sensors/sensor_params_mag2.c
index cb0b697fc5..9dd828571c 100644
--- a/src/modules/sensors/sensor_params_mag2.c
+++ b/src/modules/sensors/sensor_params_mag2.c
@@ -241,8 +241,9 @@ PARAM_DEFINE_FLOAT(CAL_MAG2_ZCOMP, 0.0f);
  *
  * Temperature during last calibration.
  *
+ * @unit celcius
  * @category system
  * @group Sensor Calibration
  * @volatile
  */
-PARAM_DEFINE_FLOAT(CAL_MAG2_TEMP, NAN);
+PARAM_DEFINE_FLOAT(CAL_MAG2_TEMP, -1000.f);
diff --git a/src/modules/sensors/sensor_params_mag3.c b/src/modules/sensors/sensor_params_mag3.c
index 42bd6c943e..129775c560 100644
--- a/src/modules/sensors/sensor_params_mag3.c
+++ b/src/modules/sensors/sensor_params_mag3.c
@@ -241,8 +241,9 @@ PARAM_DEFINE_FLOAT(CAL_MAG3_ZCOMP, 0.0f);
  *
  * Temperature during last calibration.
  *
+ * @unit celcius
  * @category system
  * @group Sensor Calibration
  * @volatile
  */
-PARAM_DEFINE_FLOAT(CAL_MAG3_TEMP, NAN);
+PARAM_DEFINE_FLOAT(CAL_MAG3_TEMP, -1000.f);