events: abort calibration if starting temperature is too high

src/modules/events/temperature_calibration/accel.cpp

@@ -43,6 +43,7 @@
 #include "accel.h"
 #include <uORB/topics/sensor_accel.h>
 #include <mathlib/mathlib.h>
+#include <drivers/drv_hrt.h>
 
 TemperatureCalibrationAccel::TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature, float max_start_temperature)
 	: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
@@ -109,16 +110,27 @@ int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int
 	data.sensor_sample_filt[2] = accel_data.z;
 	data.sensor_sample_filt[3] = accel_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) {
-		data.cold_soaked = true;
-		data.low_temp = data.sensor_sample_filt[3];	//Record the low temperature
-		data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise;
+		// 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 -110;
+			} 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
@@ -143,10 +155,10 @@ int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int
 	}
 
 	//update linear fit matrices
-	data.sensor_sample_filt[3] -= data.ref_temp;
-	data.P[0].update((double)data.sensor_sample_filt[3], (double)data.sensor_sample_filt[0]);
-	data.P[1].update((double)data.sensor_sample_filt[3], (double)data.sensor_sample_filt[1]);
-	data.P[2].update((double)data.sensor_sample_filt[3], (double)data.sensor_sample_filt[2]);
+	double relative_temperature = data.sensor_sample_filt[3] - 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;
 }

src/modules/events/temperature_calibration/baro.cpp

@@ -43,6 +43,7 @@
 #include "baro.h"
 #include <uORB/topics/sensor_baro.h>
 #include <mathlib/mathlib.h>
+#include <drivers/drv_hrt.h>
 
 TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature, float max_start_temperature)
 	: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
@@ -103,9 +104,21 @@ int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int
 	}
 
 	if (!data.cold_soaked) {
-		data.cold_soaked = true;
-		data.low_temp = data.sensor_sample_filt[1];	//Record the low temperature
-		data.ref_temp = data.sensor_sample_filt[1] + 0.5f * _min_temperature_rise;
+		// 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[1] > _max_start_temperature) {
+				return -110;
+			} else {
+				data.cold_soaked = true;
+				data.low_temp = data.sensor_sample_filt[1]; // 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[1] + 0.5f * _min_temperature_rise;
+				return 1;
+			}
+		} else {
+			return 1;
+		}
 	}
 
 	// check if temperature increased
@@ -129,8 +142,8 @@ int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int
 	}
 
 	//update linear fit matrices
-	data.sensor_sample_filt[1] -= data.ref_temp;
-	data.P[0].update((double)data.sensor_sample_filt[1], (double)data.sensor_sample_filt[0]);
+	double relative_temperature = data.sensor_sample_filt[1] - data.ref_temp;
+	data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]);
 
 	return 1;
 }

src/modules/events/temperature_calibration/common.h

@@ -60,7 +60,7 @@ public:
 
 	/**
 	 * check & update new sensor data.
-	 * @return progress in range [0, 100], 110 when finished, <0 on error
+	 * @return progress in range [0, 100], 110 when finished, <0 on error, -110 if starting temperature is too hot
 	 */
 	virtual int update() = 0;
 
@@ -125,7 +125,13 @@ public:
 		int num_not_complete = 0;
 
 		for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) {
-			num_not_complete += update_sensor_instance(_data[uorb_index], _sensor_subs[uorb_index]);
+			int status = update_sensor_instance(_data[uorb_index], _sensor_subs[uorb_index]);
+			if (status == -1) {
+				return -1;
+			} else if (status == -110) {
+				return -110;
+			}
+			num_not_complete += status;
 		}
 
 		if (num_not_complete > 0) {
@@ -151,14 +157,14 @@ protected:
 	struct PerSensorData {
 		float sensor_sample_filt[Dim + 1]; ///< last value is the temperature
 		polyfitter < PolyfitOrder + 1 > P[Dim];
-		unsigned hot_soak_sat = 0;
-		uint32_t device_id = 0;
-		bool cold_soaked = false;
-		bool hot_soaked = false;
-		bool tempcal_complete = false;
-		float low_temp = 0.f;
-		float high_temp = 0.f;
-		float ref_temp = 0.f;
+		unsigned hot_soak_sat = 0; // counter that increments every time the sensor temperature reduces from the last reading
+		uint32_t device_id = 0; // ID for the sensor being calibrated
+		bool cold_soaked = false; // true when the sensor cold soak starting temperature condition had been verified and the starting temperature set
+		bool hot_soaked = false; // true when the sensor has achieved the specified temperature increase
+		bool tempcal_complete = false; // true when the calibration has been completed
+		float low_temp = 0.f; // low temperature recorded at start of calibration (deg C)
+		float high_temp = 0.f; // highest temperature recorded during calibration (deg C)
+		float ref_temp = 0.f; // calibration reference temperature, nominally in the middle of the calibration temperature range (deg C)
 	};
 
 	PerSensorData _data[SENSOR_COUNT_MAX];

src/modules/events/temperature_calibration/gyro.cpp

@@ -42,6 +42,7 @@
 #include <mathlib/mathlib.h>
 #include <uORB/topics/sensor_gyro.h>
 #include "gyro.h"
+#include <drivers/drv_hrt.h>
 
 TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, float max_start_temperature, int gyro_subs[], int num_gyros)
 	: TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature)
@@ -102,9 +103,21 @@ int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int
 	}
 
 	if (!data.cold_soaked) {
-		data.cold_soaked = true;
-		data.low_temp = data.sensor_sample_filt[3];	//Record the low temperature
-		data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise;
+		// 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 -110;
+			} 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
@@ -129,10 +142,10 @@ int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int
 	}
 
 	//update linear fit matrices
-	data.sensor_sample_filt[3] -= data.ref_temp;
-	data.P[0].update((double)data.sensor_sample_filt[3], (double)data.sensor_sample_filt[0]);
-	data.P[1].update((double)data.sensor_sample_filt[3], (double)data.sensor_sample_filt[1]);
-	data.P[2].update((double)data.sensor_sample_filt[3], (double)data.sensor_sample_filt[2]);
+	double relative_temperature = data.sensor_sample_filt[3] - 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;
 }

src/modules/events/temperature_calibration/task.cpp

@@ -183,6 +183,7 @@ void TemperatureCalibration::task_main()
 
 	hrt_abstime next_progress_output = hrt_absolute_time() + 1e6;
 
+	bool abort_calibration = false;
 	while (!_force_task_exit) {
 		/* we poll on the gyro(s), since this is the sensor with the highest update rate.
 		 * Each individual sensor will then check on its own if there's new data.
@@ -212,19 +213,22 @@ void TemperatureCalibration::task_main()
 
 		for (int i = 0; i < num_calibrators; ++i) {
 			ret = calibrators[i]->update();
-
-			if (ret < 0) {
-				if (!error_reported[i]) {
-					PX4_ERR("Calibration update step failed (%i)", ret);
-					error_reported[i] = true;
-				}
-
+			if (ret == -110) {
+				abort_calibration = true;
+				PX4_ERR("Calibration won't start - sensor temperature too high");
+				_force_task_exit = true;
+				break;
+			} else if (ret < 0 && !error_reported[i]) {
+				// temperature has decreased so calibration is not being updated
+				error_reported[i] = true;
+				PX4_ERR("Calibration update step failed (%i)", ret);
 			} else if (ret < min_progress) {
+				// temperature is stable or increasing
 				min_progress = ret;
 			}
 		}
 
-		if (min_progress == 110) {
+		if (min_progress == 110 || abort_calibration) {
 			break; // we are done
 		}
 
@@ -237,25 +241,26 @@ void TemperatureCalibration::task_main()
 		}
 	}
 
-	PX4_INFO("Sensor Measurments completed");
+	if (!abort_calibration) {
+		PX4_INFO("Sensor Measurments completed");
 
-	// do final calculations & parameter storage
-	for (int i = 0; i < num_calibrators; ++i) {
-		int ret = calibrators[i]->finish();
+		// do final calculations & parameter storage
+		for (int i = 0; i < num_calibrators; ++i) {
+			int ret = calibrators[i]->finish();
 
-		if (ret < 0) {
-			PX4_ERR("Failed to finish calibration process (%i)", ret);
+			if (ret < 0) {
+				PX4_ERR("Failed to finish calibration process (%i)", ret);
+			}
+		}
+
+		param_notify_changes();
+		int ret = param_save_default();
+
+		if (ret != 0) {
+			PX4_ERR("Failed to save params (%i)", ret);
 		}
 	}
 
-	param_notify_changes();
-	int ret = param_save_default();
-
-	if (ret != 0) {
-		PX4_ERR("Failed to save params (%i)", ret);
-	}
-
-
 	for (int i = 0; i < num_calibrators; ++i) {
 		delete calibrators[i];
 	}