fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

temperature_calibration: reduce code duplication by adding a TemperatureCalibrationCommon class

Browse Source
This commit is contained in:
Beat Küng 2017-02-03 11:16:36 +01:00 committed by Lorenz Meier
parent 38b4984c36
commit 6583f73cfa
7 changed files with 88 additions and 174 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/events/temperature_calibration/accel.cpp
View File

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

31
src/modules/events/temperature_calibration/accel.h
View File

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

33
src/modules/events/temperature_calibration/baro.cpp
View File

@ -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],

31
src/modules/events/temperature_calibration/baro.h
View File

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

73
src/modules/events/temperature_calibration/common.h
View File

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

34
src/modules/events/temperature_calibration/gyro.cpp
View File

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

31
src/modules/events/temperature_calibration/gyro.h
View File

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