temperature_calibration: reduce code duplication by adding a TemperatureCalibrationCommon class

src/modules/events/temperature_calibration/accel.cpp

@@ -45,7 +45,7 @@
 #include <mathlib/mathlib.h>
 
 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++) {

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];
 };

src/modules/events/temperature_calibration/baro.cpp

@@ -45,7 +45,7 @@
 #include <mathlib/mathlib.h>
 
 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],

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];
 };

src/modules/events/temperature_calibration/common.h

@@ -41,11 +41,14 @@
 #endif
 
 #include <px4_log.h>
+#include <mathlib/mathlib.h>
+
+#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 <int Dim, int PolyfitOrder>
+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];
+};

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++) {

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;
 };