From 6583f73cfa1aabb409ebc127a14d785dffd30dcb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Fri, 3 Feb 2017 11:16:36 +0100 Subject: [PATCH] temperature_calibration: reduce code duplication by adding a TemperatureCalibrationCommon class --- .../events/temperature_calibration/accel.cpp | 29 +------- .../events/temperature_calibration/accel.h | 31 +------- .../events/temperature_calibration/baro.cpp | 33 +-------- .../events/temperature_calibration/baro.h | 31 +------- .../events/temperature_calibration/common.h | 73 ++++++++++++++++++- .../events/temperature_calibration/gyro.cpp | 34 ++------- .../events/temperature_calibration/gyro.h | 31 +------- 7 files changed, 88 insertions(+), 174 deletions(-) diff --git a/src/modules/events/temperature_calibration/accel.cpp b/src/modules/events/temperature_calibration/accel.cpp index e2f231370d..e054c2bb4e 100644 --- a/src/modules/events/temperature_calibration/accel.cpp +++ b/src/modules/events/temperature_calibration/accel.cpp @@ -45,7 +45,7 @@ #include TemperatureCalibrationAccel::TemperatureCalibrationAccel(float min_temperature_rise) - : TemperatureCalibrationBase(min_temperature_rise) + : TemperatureCalibrationCommon(min_temperature_rise) { //init subscriptions @@ -145,33 +145,6 @@ int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int return 1; } -int TemperatureCalibrationAccel::update() -{ - - 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]); - } - - if (num_not_complete > 0) { - // calculate progress - float min_diff = _min_temperature_rise; - - for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) { - float cur_diff = _data[uorb_index].high_temp - _data[uorb_index].low_temp; - - if (cur_diff < min_diff) { - min_diff = cur_diff; - } - } - - return math::min(100, (int)(min_diff / _min_temperature_rise * 100.f)); - } - - return 110; -} - int TemperatureCalibrationAccel::finish() { for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) { diff --git a/src/modules/events/temperature_calibration/accel.h b/src/modules/events/temperature_calibration/accel.h index 02e20ba934..fd98383cd5 100644 --- a/src/modules/events/temperature_calibration/accel.h +++ b/src/modules/events/temperature_calibration/accel.h @@ -36,17 +36,12 @@ #include "common.h" #include "polyfit.hpp" -class TemperatureCalibrationAccel : public TemperatureCalibrationBase +class TemperatureCalibrationAccel : public TemperatureCalibrationCommon<3, 3> { public: TemperatureCalibrationAccel(float min_temperature_rise); virtual ~TemperatureCalibrationAccel(); - /** - * @see TemperatureCalibrationBase::update() - */ - int update(); - /** * @see TemperatureCalibrationBase::finish() */ @@ -59,29 +54,7 @@ public: private: - struct PerSensorData { - float sensor_sample_filt[4]; - polyfitter<4> P[3]; - 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; - }; - - PerSensorData _data[SENSOR_COUNT_MAX]; - - /** - * update a single sensor instance - * @return 0 when done, 1 not finished yet - */ - inline int update_sensor_instance(PerSensorData &data, int sensor_sub); + virtual inline int update_sensor_instance(PerSensorData &data, int sensor_sub); inline int finish_sensor_instance(PerSensorData &data, int sensor_index); - - int _num_sensor_instances; - int _sensor_subs[SENSOR_COUNT_MAX]; }; diff --git a/src/modules/events/temperature_calibration/baro.cpp b/src/modules/events/temperature_calibration/baro.cpp index 170cb40065..9bc58f869c 100644 --- a/src/modules/events/temperature_calibration/baro.cpp +++ b/src/modules/events/temperature_calibration/baro.cpp @@ -45,7 +45,7 @@ #include TemperatureCalibrationBaro::TemperatureCalibrationBaro(float min_temperature_rise) - : TemperatureCalibrationBase(min_temperature_rise) + : TemperatureCalibrationCommon(min_temperature_rise) { //init subscriptions @@ -124,38 +124,11 @@ int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int //update linear fit matrices data.sensor_sample_filt[1] -= data.ref_temp; - data.P.update((double)data.sensor_sample_filt[1], (double)data.sensor_sample_filt[0]); + data.P[0].update((double)data.sensor_sample_filt[1], (double)data.sensor_sample_filt[0]); return 1; } -int TemperatureCalibrationBaro::update() -{ - - 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]); - } - - if (num_not_complete > 0) { - // calculate progress - float min_diff = _min_temperature_rise; - - for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) { - float cur_diff = _data[uorb_index].high_temp - _data[uorb_index].low_temp; - - if (cur_diff < min_diff) { - min_diff = cur_diff; - } - } - - return math::min(100, (int)(min_diff / _min_temperature_rise * 100.f)); - } - - return 110; -} - int TemperatureCalibrationBaro::finish() { for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) { @@ -179,7 +152,7 @@ int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int } double res[POLYFIT_ORDER + 1] = {}; - data.P.fit(res); + data.P[0].fit(res); res[POLYFIT_ORDER] = 0.0; // normalise the correction to be zero at the reference temperature by setting the X^0 coefficient to zero PX4_INFO("Result baro %u %.20f %.20f %.20f %.20f %.20f %.20f", sensor_index, (double)res[0], diff --git a/src/modules/events/temperature_calibration/baro.h b/src/modules/events/temperature_calibration/baro.h index 48a03154ec..7a2ed559a7 100644 --- a/src/modules/events/temperature_calibration/baro.h +++ b/src/modules/events/temperature_calibration/baro.h @@ -39,17 +39,12 @@ #define POLYFIT_ORDER 5 -class TemperatureCalibrationBaro : public TemperatureCalibrationBase +class TemperatureCalibrationBaro : public TemperatureCalibrationCommon<1, POLYFIT_ORDER> { public: TemperatureCalibrationBaro(float min_temperature_rise); virtual ~TemperatureCalibrationBaro(); - /** - * @see TemperatureCalibrationBase::update() - */ - int update(); - /** * @see TemperatureCalibrationBase::finish() */ @@ -62,29 +57,7 @@ public: private: - struct PerSensorData { - float sensor_sample_filt[2]; - polyfitter < POLYFIT_ORDER + 1 > P; - 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; - }; - - PerSensorData _data[SENSOR_COUNT_MAX]; - - /** - * update a single sensor instance - * @return 0 when done, 1 not finished yet - */ - inline int update_sensor_instance(PerSensorData &data, int sensor_sub); + virtual int update_sensor_instance(PerSensorData &data, int sensor_sub); inline int finish_sensor_instance(PerSensorData &data, int sensor_index); - - int _num_sensor_instances; - int _sensor_subs[SENSOR_COUNT_MAX]; }; diff --git a/src/modules/events/temperature_calibration/common.h b/src/modules/events/temperature_calibration/common.h index 59672c1a2e..bf704a70a3 100644 --- a/src/modules/events/temperature_calibration/common.h +++ b/src/modules/events/temperature_calibration/common.h @@ -41,11 +41,14 @@ #endif #include +#include + +#include "polyfit.hpp" #define SENSOR_COUNT_MAX 3 /** - * Base class for temperature calibration types (for all different sensor types) + * Base class for temperature calibration types with abstract methods (for all different sensor types) */ class TemperatureCalibrationBase { @@ -99,3 +102,71 @@ int TemperatureCalibrationBase::set_parameter(const char *format_str, unsigned i return result; } +/** + ** class TemperatureCalibrationCommon + * Common base class for all sensor types, contains shared code & data. + */ +template +class TemperatureCalibrationCommon : public TemperatureCalibrationBase +{ +public: + TemperatureCalibrationCommon(float min_temperature_rise) + : TemperatureCalibrationBase(min_temperature_rise) {} + + virtual ~TemperatureCalibrationCommon() {} + + /** + * @see TemperatureCalibrationBase::update() + */ + int update() + { + 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]); + } + + if (num_not_complete > 0) { + // calculate progress + float min_diff = _min_temperature_rise; + + for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) { + float cur_diff = _data[uorb_index].high_temp - _data[uorb_index].low_temp; + + if (cur_diff < min_diff) { + min_diff = cur_diff; + } + } + + return math::min(100, (int)(min_diff / _min_temperature_rise * 100.f)); + } + + return 110; + } + +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; + }; + + PerSensorData _data[SENSOR_COUNT_MAX]; + + /** + * update a single sensor instance + * @return 0 when done, 1 not finished yet + */ + virtual int update_sensor_instance(PerSensorData &data, int sensor_sub) = 0; + + int _num_sensor_instances; + int _sensor_subs[SENSOR_COUNT_MAX]; +}; diff --git a/src/modules/events/temperature_calibration/gyro.cpp b/src/modules/events/temperature_calibration/gyro.cpp index f2dfb44327..6f6f7b1057 100644 --- a/src/modules/events/temperature_calibration/gyro.cpp +++ b/src/modules/events/temperature_calibration/gyro.cpp @@ -44,8 +44,13 @@ #include "gyro.h" TemperatureCalibrationGyro::TemperatureCalibrationGyro(float min_temperature_rise, int gyro_subs[], int num_gyros) - : TemperatureCalibrationBase(min_temperature_rise), _num_sensor_instances(num_gyros), _sensor_subs(gyro_subs) + : TemperatureCalibrationCommon(min_temperature_rise) { + for (int i = 0; i < num_gyros; ++i) { + _sensor_subs[i] = gyro_subs[i]; + } + + _num_sensor_instances = num_gyros; } @@ -127,33 +132,6 @@ int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int return 1; } -int TemperatureCalibrationGyro::update() -{ - - 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]); - } - - if (num_not_complete > 0) { - // calculate progress - float min_diff = _min_temperature_rise; - - for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) { - float cur_diff = _data[uorb_index].high_temp - _data[uorb_index].low_temp; - - if (cur_diff < min_diff) { - min_diff = cur_diff; - } - } - - return math::min(100, (int)(min_diff / _min_temperature_rise * 100.f)); - } - - return 110; -} - int TemperatureCalibrationGyro::finish() { for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) { diff --git a/src/modules/events/temperature_calibration/gyro.h b/src/modules/events/temperature_calibration/gyro.h index 1ac16b7d46..ba8b6fe331 100644 --- a/src/modules/events/temperature_calibration/gyro.h +++ b/src/modules/events/temperature_calibration/gyro.h @@ -36,17 +36,12 @@ #include "common.h" #include "polyfit.hpp" -class TemperatureCalibrationGyro : public TemperatureCalibrationBase +class TemperatureCalibrationGyro : public TemperatureCalibrationCommon<3, 3> { public: TemperatureCalibrationGyro(float min_temperature_rise, int gyro_subs[], int num_gyros); virtual ~TemperatureCalibrationGyro() {} - /** - * @see TemperatureCalibrationBase::update() - */ - int update(); - /** * @see TemperatureCalibrationBase::finish() */ @@ -59,29 +54,7 @@ public: private: - struct PerSensorData { - float sensor_sample_filt[4]; - polyfitter<4> P[3]; - 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; - }; - - PerSensorData _data[SENSOR_COUNT_MAX]; - - /** - * update a single sensor instance - * @return 0 when done, 1 not finished yet - */ - inline int update_sensor_instance(PerSensorData &data, int sensor_sub); + virtual int update_sensor_instance(PerSensorData &data, int sensor_sub); inline int finish_sensor_instance(PerSensorData &data, int sensor_index); - - int _num_sensor_instances; - int *_sensor_subs; };