diff --git a/src/modules/events/temperature_calibration/accel.cpp b/src/modules/events/temperature_calibration/accel.cpp index ee6213e5a9..c148aa1a18 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) - : TemperatureCalibrationCommon(min_temperature_rise) +TemperatureCalibrationAccel::TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature) + : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature) { //init subscriptions diff --git a/src/modules/events/temperature_calibration/accel.h b/src/modules/events/temperature_calibration/accel.h index fd98383cd5..ac97c96d55 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); + TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature); virtual ~TemperatureCalibrationAccel(); /** diff --git a/src/modules/events/temperature_calibration/baro.cpp b/src/modules/events/temperature_calibration/baro.cpp index 9bc58f869c..6df349d891 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) - : TemperatureCalibrationCommon(min_temperature_rise) +TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature) + : TemperatureCalibrationCommon(min_temperature_rise, min_start_temperature) { //init subscriptions diff --git a/src/modules/events/temperature_calibration/baro.h b/src/modules/events/temperature_calibration/baro.h index 7a2ed559a7..dc522b604d 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); + TemperatureCalibrationBaro(float min_temperature_rise, float min_start_temperature); virtual ~TemperatureCalibrationBaro(); /** diff --git a/src/modules/events/temperature_calibration/common.h b/src/modules/events/temperature_calibration/common.h index bf704a70a3..8855f8e4d0 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) - : _min_temperature_rise(min_temperature_rise) {} + TemperatureCalibrationBase(float min_temperature_rise, float min_start_temperature) + : _min_temperature_rise(min_temperature_rise), _min_start_temperature(min_start_temperature) {} virtual ~TemperatureCalibrationBase() {} @@ -85,6 +85,7 @@ protected: inline int set_parameter(const char *format_str, unsigned index, const void *value); float _min_temperature_rise; ///< minimum difference in temperature before the process finishes + float _min_start_temperature; ///< minimum temperature before the process starts }; @@ -110,8 +111,8 @@ template class TemperatureCalibrationCommon : public TemperatureCalibrationBase { public: - TemperatureCalibrationCommon(float min_temperature_rise) - : TemperatureCalibrationBase(min_temperature_rise) {} + TemperatureCalibrationCommon(float min_temperature_rise, float min_start_temperature) + : TemperatureCalibrationBase(min_temperature_rise, min_start_temperature) {} virtual ~TemperatureCalibrationCommon() {} diff --git a/src/modules/events/temperature_calibration/gyro.cpp b/src/modules/events/temperature_calibration/gyro.cpp index a55a970cdd..04a167d58d 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, int gyro_subs[], int num_gyros) - : TemperatureCalibrationCommon(min_temperature_rise) +TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_rise, float min_start_temperature, int gyro_subs[], int num_gyros) + : TemperatureCalibrationCommon(min_temperature_rise, min_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 ba8b6fe331..db7f32957c 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, int gyro_subs[], int num_gyros); + TemperatureCalibrationGyro(float min_temperature_rise, float min_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 c1eb0e069b..172ee17e5e 100644 --- a/src/modules/events/temperature_calibration/task.cpp +++ b/src/modules/events/temperature_calibration/task.cpp @@ -127,13 +127,16 @@ void TemperatureCalibration::task_main() param_get(param_find("SYS_CAL_TDEL"), &min_temp_rise); PX4_INFO("Waiting for %i degrees difference in sensor temperature", min_temp_rise); + int32_t min_start_temp = 5; + param_get(param_find("SYS_CAL_TMIN"), &min_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); + calibrators[num_calibrators] = new TemperatureCalibrationAccel(min_temp_rise,min_start_temp); if (calibrators[num_calibrators]) { ++num_calibrators; @@ -144,7 +147,7 @@ void TemperatureCalibration::task_main() } if (_baro) { - calibrators[num_calibrators] = new TemperatureCalibrationBaro(min_temp_rise); + calibrators[num_calibrators] = new TemperatureCalibrationBaro(min_temp_rise, min_start_temp); if (calibrators[num_calibrators]) { ++num_calibrators; @@ -155,7 +158,7 @@ void TemperatureCalibration::task_main() } if (_gyro) { - calibrators[num_calibrators] = new TemperatureCalibrationGyro(min_temp_rise, gyro_sub, num_gyro); + calibrators[num_calibrators] = new TemperatureCalibrationGyro(min_temp_rise, min_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 cbd83fc665..34cfb78a84 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -221,3 +221,13 @@ PARAM_DEFINE_INT32(SYS_CAL_BARO, 0); * @group System */ PARAM_DEFINE_INT32(SYS_CAL_DELT, 24); + +/** + * Minimum starting temperature for thermal calibration + * + * Temperature calibration for each sensor will ignore data if the temperature is lower than the value set by SYS_CAL_TMIN. + * + * @unit deg C + * @group System + */ +PARAM_DEFINE_INT32(SYS_CAL_TMIN, 5);