diff --git a/src/modules/events/temperature_calibration/accel.cpp b/src/modules/events/temperature_calibration/accel.cpp index 601c10aea189..022429ff6262 100644 --- a/src/modules/events/temperature_calibration/accel.cpp +++ b/src/modules/events/temperature_calibration/accel.cpp @@ -45,7 +45,8 @@ #include #include -TemperatureCalibrationAccel::TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature, float max_start_temperature) +TemperatureCalibrationAccel::TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature, + float max_start_temperature) : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature) { @@ -121,6 +122,7 @@ int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int // 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 @@ -128,6 +130,7 @@ int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise; return 1; } + } else { return 1; } diff --git a/src/modules/events/temperature_calibration/baro.cpp b/src/modules/events/temperature_calibration/baro.cpp index 1ab97399fe9d..7830f81ac2d7 100644 --- a/src/modules/events/temperature_calibration/baro.cpp +++ b/src/modules/events/temperature_calibration/baro.cpp @@ -45,7 +45,8 @@ #include #include -TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature, float max_start_temperature) +TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature, + float max_start_temperature) : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature, max_start_temperature) { @@ -109,6 +110,7 @@ int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int // 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 @@ -116,6 +118,7 @@ int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int data.ref_temp = data.sensor_sample_filt[1] + 0.5f * _min_temperature_rise; return 1; } + } else { return 1; } diff --git a/src/modules/events/temperature_calibration/common.h b/src/modules/events/temperature_calibration/common.h index 1e28788f03b3..7c5aa5c27278 100644 --- a/src/modules/events/temperature_calibration/common.h +++ b/src/modules/events/temperature_calibration/common.h @@ -54,7 +54,8 @@ class TemperatureCalibrationBase { public: TemperatureCalibrationBase(float min_temperature_rise, float min_start_temperature, float max_start_temperature) - : _min_temperature_rise(min_temperature_rise), _min_start_temperature(min_start_temperature), _max_start_temperature(max_start_temperature) {} + : _min_temperature_rise(min_temperature_rise), _min_start_temperature(min_start_temperature), + _max_start_temperature(max_start_temperature) {} virtual ~TemperatureCalibrationBase() {} @@ -126,11 +127,14 @@ class TemperatureCalibrationCommon : public TemperatureCalibrationBase for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; 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; } @@ -159,12 +163,14 @@ class TemperatureCalibrationCommon : public TemperatureCalibrationBase polyfitter < PolyfitOrder + 1 > P[Dim]; 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 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) + float ref_temp = + 0.f; // calibration reference temperature, nominally in the middle of the calibration temperature range (deg C) }; PerSensorData _data[SENSOR_COUNT_MAX]; diff --git a/src/modules/events/temperature_calibration/gyro.cpp b/src/modules/events/temperature_calibration/gyro.cpp index aac8bbed5d81..2564794de351 100644 --- a/src/modules/events/temperature_calibration/gyro.cpp +++ b/src/modules/events/temperature_calibration/gyro.cpp @@ -44,7 +44,8 @@ #include "gyro.h" #include -TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, float max_start_temperature, int gyro_subs[], int num_gyros) +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) { for (int i = 0; i < num_gyros; ++i) { @@ -108,6 +109,7 @@ int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int // 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 @@ -115,6 +117,7 @@ int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise; return 1; } + } else { return 1; } diff --git a/src/modules/events/temperature_calibration/gyro.h b/src/modules/events/temperature_calibration/gyro.h index 1289461be727..702849e89054 100644 --- a/src/modules/events/temperature_calibration/gyro.h +++ b/src/modules/events/temperature_calibration/gyro.h @@ -39,7 +39,8 @@ class TemperatureCalibrationGyro : public TemperatureCalibrationCommon<3, 3> { public: - TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, float max_start_temperature, int gyro_subs[], int num_gyros); + TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, float max_start_temperature, + int gyro_subs[], int num_gyros); virtual ~TemperatureCalibrationGyro() {} /** diff --git a/src/modules/events/temperature_calibration/task.cpp b/src/modules/events/temperature_calibration/task.cpp index 9299f942990b..9129593b8b44 100644 --- a/src/modules/events/temperature_calibration/task.cpp +++ b/src/modules/events/temperature_calibration/task.cpp @@ -161,7 +161,8 @@ void TemperatureCalibration::task_main() } if (_gyro) { - calibrators[num_calibrators] = new TemperatureCalibrationGyro(min_temp_rise, min_start_temp, max_start_temp, gyro_sub, num_gyro); + calibrators[num_calibrators] = new TemperatureCalibrationGyro(min_temp_rise, min_start_temp, max_start_temp, gyro_sub, + num_gyro); if (calibrators[num_calibrators]) { ++num_calibrators; @@ -184,6 +185,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. @@ -213,15 +215,18 @@ void TemperatureCalibration::task_main() for (int i = 0; i < num_calibrators; ++i) { ret = calibrators[i]->update(); + 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;