diff --git a/src/modules/events/temperature_calibration/accel.cpp b/src/modules/events/temperature_calibration/accel.cpp index 7b86a558d3..601c10aea1 100644 --- a/src/modules/events/temperature_calibration/accel.cpp +++ b/src/modules/events/temperature_calibration/accel.cpp @@ -43,6 +43,7 @@ #include "accel.h" #include #include +#include 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; } diff --git a/src/modules/events/temperature_calibration/baro.cpp b/src/modules/events/temperature_calibration/baro.cpp index 0ae7f0ff02..1ab97399fe 100644 --- a/src/modules/events/temperature_calibration/baro.cpp +++ b/src/modules/events/temperature_calibration/baro.cpp @@ -43,6 +43,7 @@ #include "baro.h" #include #include +#include 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; } diff --git a/src/modules/events/temperature_calibration/common.h b/src/modules/events/temperature_calibration/common.h index 40441b6c8a..1e28788f03 100644 --- a/src/modules/events/temperature_calibration/common.h +++ b/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]; diff --git a/src/modules/events/temperature_calibration/gyro.cpp b/src/modules/events/temperature_calibration/gyro.cpp index 17033bd7b4..aac8bbed5d 100644 --- a/src/modules/events/temperature_calibration/gyro.cpp +++ b/src/modules/events/temperature_calibration/gyro.cpp @@ -42,6 +42,7 @@ #include #include #include "gyro.h" +#include 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; } diff --git a/src/modules/events/temperature_calibration/task.cpp b/src/modules/events/temperature_calibration/task.cpp index 1be1885500..9299f94299 100644 --- a/src/modules/events/temperature_calibration/task.cpp +++ b/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]; }