diff --git a/src/modules/events/temperature_calibration/accel.cpp b/src/modules/events/temperature_calibration/accel.cpp index 0eae3327cd..7b86a558d3 100644 --- a/src/modules/events/temperature_calibration/accel.cpp +++ b/src/modules/events/temperature_calibration/accel.cpp @@ -44,8 +44,8 @@ #include #include -TemperatureCalibrationAccel::TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature) - : TemperatureCalibrationCommon(min_temperature_rise, min_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) { //init subscriptions diff --git a/src/modules/events/temperature_calibration/accel.h b/src/modules/events/temperature_calibration/accel.h index ac97c96d55..2cb1fac348 100644 --- a/src/modules/events/temperature_calibration/accel.h +++ b/src/modules/events/temperature_calibration/accel.h @@ -39,7 +39,7 @@ class TemperatureCalibrationAccel : public TemperatureCalibrationCommon<3, 3> { public: - TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature); + TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature, float max_start_temperature); virtual ~TemperatureCalibrationAccel(); /** diff --git a/src/modules/events/temperature_calibration/baro.cpp b/src/modules/events/temperature_calibration/baro.cpp index 8cf5cc8fe9..0ae7f0ff02 100644 --- a/src/modules/events/temperature_calibration/baro.cpp +++ b/src/modules/events/temperature_calibration/baro.cpp @@ -44,8 +44,8 @@ #include #include -TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature) - : TemperatureCalibrationCommon(min_temperature_rise, min_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) { //init subscriptions diff --git a/src/modules/events/temperature_calibration/baro.h b/src/modules/events/temperature_calibration/baro.h index dc522b604d..2b91f3380a 100644 --- a/src/modules/events/temperature_calibration/baro.h +++ b/src/modules/events/temperature_calibration/baro.h @@ -42,7 +42,7 @@ class TemperatureCalibrationBaro : public TemperatureCalibrationCommon<1, POLYFIT_ORDER> { public: - TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature); + TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature, float max_start_temperature); virtual ~TemperatureCalibrationBaro(); /** diff --git a/src/modules/events/temperature_calibration/common.h b/src/modules/events/temperature_calibration/common.h index 8855f8e4d0..40441b6c8a 100644 --- a/src/modules/events/temperature_calibration/common.h +++ b/src/modules/events/temperature_calibration/common.h @@ -53,8 +53,8 @@ class TemperatureCalibrationBase { public: - TemperatureCalibrationBase(float min_temperature_rise, float min_start_temperature) - : _min_temperature_rise(min_temperature_rise), _min_start_temperature(min_start_temperature) {} + 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) {} virtual ~TemperatureCalibrationBase() {} @@ -86,6 +86,7 @@ protected: float _min_temperature_rise; ///< minimum difference in temperature before the process finishes float _min_start_temperature; ///< minimum temperature before the process starts + float _max_start_temperature; ///< maximum temperature above which the process does not start and an error is declared }; @@ -111,8 +112,8 @@ template class TemperatureCalibrationCommon : public TemperatureCalibrationBase { public: - TemperatureCalibrationCommon(float min_temperature_rise, float min_start_temperature) - : TemperatureCalibrationBase(min_temperature_rise, min_start_temperature) {} + TemperatureCalibrationCommon(float min_temperature_rise, float min_start_temperature, float max_start_temperature) + : TemperatureCalibrationBase(min_temperature_rise, min_start_temperature, max_start_temperature) {} virtual ~TemperatureCalibrationCommon() {} @@ -164,7 +165,7 @@ protected: /** * update a single sensor instance - * @return 0 when done, 1 not finished yet + * @return 0 when done, 1 not finished yet, -1 for an error that requires the test to be repeated */ virtual int update_sensor_instance(PerSensorData &data, int sensor_sub) = 0; diff --git a/src/modules/events/temperature_calibration/gyro.cpp b/src/modules/events/temperature_calibration/gyro.cpp index 437068e424..17033bd7b4 100644 --- a/src/modules/events/temperature_calibration/gyro.cpp +++ b/src/modules/events/temperature_calibration/gyro.cpp @@ -43,8 +43,8 @@ #include #include "gyro.h" -TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, int gyro_subs[], int num_gyros) - : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature) +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) { _sensor_subs[i] = gyro_subs[i]; diff --git a/src/modules/events/temperature_calibration/gyro.h b/src/modules/events/temperature_calibration/gyro.h index db7f32957c..1289461be7 100644 --- a/src/modules/events/temperature_calibration/gyro.h +++ b/src/modules/events/temperature_calibration/gyro.h @@ -39,7 +39,7 @@ class TemperatureCalibrationGyro : public TemperatureCalibrationCommon<3, 3> { public: - TemperatureCalibrationGyro(float min_temperature_rise, float min_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 172ee17e5e..1be1885500 100644 --- a/src/modules/events/temperature_calibration/task.cpp +++ b/src/modules/events/temperature_calibration/task.cpp @@ -130,13 +130,16 @@ void TemperatureCalibration::task_main() int32_t min_start_temp = 5; param_get(param_find("SYS_CAL_TMIN"), &min_start_temp); + int32_t max_start_temp = 10; + param_get(param_find("SYS_CAL_TMAX"), &max_start_temp); + //init calibrators TemperatureCalibrationBase *calibrators[3]; bool error_reported[3] = {}; int num_calibrators = 0; if (_accel) { - calibrators[num_calibrators] = new TemperatureCalibrationAccel(min_temp_rise,min_start_temp); + calibrators[num_calibrators] = new TemperatureCalibrationAccel(min_temp_rise, min_start_temp, max_start_temp); if (calibrators[num_calibrators]) { ++num_calibrators; @@ -147,7 +150,7 @@ void TemperatureCalibration::task_main() } if (_baro) { - calibrators[num_calibrators] = new TemperatureCalibrationBaro(min_temp_rise, min_start_temp); + calibrators[num_calibrators] = new TemperatureCalibrationBaro(min_temp_rise, min_start_temp, max_start_temp); if (calibrators[num_calibrators]) { ++num_calibrators; @@ -158,7 +161,7 @@ void TemperatureCalibration::task_main() } if (_gyro) { - calibrators[num_calibrators] = new TemperatureCalibrationGyro(min_temp_rise, min_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; diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index e33e050550..774e78823b 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -231,3 +231,13 @@ PARAM_DEFINE_INT32(SYS_CAL_TDEL, 24); * @group System */ PARAM_DEFINE_INT32(SYS_CAL_TMIN, 5); + +/** + * Maximum starting temperature for thermal calibration + * + * Temperature calibration will not start if the temperature of any sensor is higher than the value set by SYS_CAL_TMAX. + * + * @unit deg C + * @group System + */ +PARAM_DEFINE_INT32(SYS_CAL_TMAX, 10);