mirror of https://github.com/ArduPilot/ardupilot
AP_TempCalibration: temperature calibration library
this adds opportunistic temperature calibration. This is initially only for barometers, and has been only tested with the ICM-20789, but can be extended to other sensors and other barometers
This commit is contained in:
Andrew Tridgell
parent
57a3bc1397
commit
177284bd6b
|
|
@ -0,0 +1,243 @@
|
|||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
temperature calibration library
|
||||
*/
|
||||
|
||||
#include "AP_TempCalibration.h"
|
||||
#include <stdio.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#define TCAL_DEBUG 0
|
||||
|
||||
#if TCAL_DEBUG
|
||||
# define debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
|
||||
#else
|
||||
# define debug(fmt, args ...)
|
||||
#endif
|
||||
|
||||
// table of user settable and learned parameters
|
||||
const AP_Param::GroupInfo AP_TempCalibration::var_info[] = {
|
||||
|
||||
// @Param: ENABLED
|
||||
// @DisplayName: Temperature calibration enable
|
||||
// @Description: Enable temperature calibration. Set to 0 to disable. Set to 1 to use learned values. Set to 2 to learn new values and use the values
|
||||
// @Values: 0:Disabled,1:Enabled,2:EnableAndLearn
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO_FLAGS("_ENABLED", 1, AP_TempCalibration, enabled, TC_DISABLED, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
// @Param: TEMP_MIN
|
||||
// @DisplayName: Min learned temperature
|
||||
// @Description: Minimum learned temperature. This is automatically set by the learning process
|
||||
// @Units: degrees C
|
||||
// @ReadOnly: True
|
||||
// @Volatile: True
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("_TEMP_MIN", 2, AP_TempCalibration, temp_min, 0),
|
||||
|
||||
// @Param: TEMP_MIN
|
||||
// @DisplayName: Min learned temperature
|
||||
// @Description: Minimum learned temperature. This is automatically set by the learning process
|
||||
// @Units: degrees C
|
||||
// @ReadOnly: True
|
||||
// @Volatile: True
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("_TEMP_MIN", 3, AP_TempCalibration, temp_min, 0),
|
||||
|
||||
// @Param: TEMP_MAX
|
||||
// @DisplayName: Max learned temperature
|
||||
// @Description: Maximum learned temperature. This is automatically set by the learning process
|
||||
// @Units: degrees C
|
||||
// @ReadOnly: True
|
||||
// @Volatile: True
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("_TEMP_MAX", 4, AP_TempCalibration, temp_max, 0),
|
||||
|
||||
// @Param: BARO_EXP
|
||||
// @DisplayName: Barometer exponent
|
||||
// @Description: Learned exponent for barometer temperature correction
|
||||
// @ReadOnly: True
|
||||
// @Volatile: True
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("_BARO_EXP", 5, AP_TempCalibration, baro_exponent, 0),
|
||||
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
AP_TempCalibration::AP_TempCalibration(AP_Baro &_baro, AP_InertialSensor &_ins) :
|
||||
baro(_baro)
|
||||
,ins(_ins)
|
||||
{
|
||||
}
|
||||
|
||||
/*
|
||||
calculate the correction given an exponent and a temperature
|
||||
|
||||
This one parameter correction is deliberately chosen to be very
|
||||
robust for extrapolation. It fits the characteristics of the
|
||||
ICM-20789 barometer nicely.
|
||||
*/
|
||||
float AP_TempCalibration::calculate_correction(float temp, float exponent) const
|
||||
{
|
||||
return powf(MAX(temp - Tzero, 0), exponent);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
setup for learning
|
||||
*/
|
||||
void AP_TempCalibration::setup_learning(void)
|
||||
{
|
||||
learn_temp_start = baro.get_temperature();
|
||||
learn_temp_step = 0.25;
|
||||
learn_count = 200;
|
||||
learn_i = 0;
|
||||
if (learn_values != nullptr) {
|
||||
delete [] learn_values;
|
||||
}
|
||||
learn_values = new float[learn_count];
|
||||
if (learn_values == nullptr) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
calculate the sum of squares range of pressure values we get with
|
||||
the current data. This is the function we try to minimise in the
|
||||
calibration
|
||||
*/
|
||||
float AP_TempCalibration::calculate_p_range(float baro_factor) const
|
||||
{
|
||||
float sum = 0;
|
||||
float P0 = learn_values[0] + calculate_correction(learn_temp_start, baro_factor);
|
||||
for (uint16_t i=0; i<learn_i; i++) {
|
||||
if (is_zero(learn_values[i])) {
|
||||
// gap in the data
|
||||
continue;
|
||||
}
|
||||
float temp = learn_temp_start + learn_temp_step*i;
|
||||
float correction = calculate_correction(temp, baro_factor);
|
||||
float P = learn_values[i] + correction;
|
||||
sum += sq(P - P0);
|
||||
}
|
||||
return sum / learn_i;
|
||||
}
|
||||
|
||||
/*
|
||||
calculate a calibration value
|
||||
|
||||
This fits a simple single value power function to the baro data to
|
||||
find the calibration exponent.
|
||||
*/
|
||||
void AP_TempCalibration::calculate_calibration(void)
|
||||
{
|
||||
float current_err = calculate_p_range(baro_exponent);
|
||||
float test_exponent = baro_exponent + learn_delta;
|
||||
float test_err = calculate_p_range(test_exponent);
|
||||
if (test_err >= current_err) {
|
||||
test_exponent = baro_exponent - learn_delta;
|
||||
test_err = calculate_p_range(test_exponent);
|
||||
}
|
||||
if (test_exponent <= exp_limit_max &&
|
||||
test_exponent >= exp_limit_min &&
|
||||
test_err < current_err) {
|
||||
// move to new value
|
||||
debug("CAL: %.2f\n", test_exponent);
|
||||
baro_exponent.set_and_save_ifchanged(test_exponent);
|
||||
temp_min.set_and_save_ifchanged(learn_temp_start);
|
||||
temp_max.set_and_save_ifchanged(learn_temp_start + learn_i*learn_temp_step);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
update calibration learning
|
||||
*/
|
||||
void AP_TempCalibration::learn_calibration(void)
|
||||
{
|
||||
// just for first baro now
|
||||
if (!baro.healthy(0) ||
|
||||
hal.util->get_soft_armed() ||
|
||||
baro.get_temperature(0) < Tzero) {
|
||||
return;
|
||||
}
|
||||
|
||||
// if we have any movement then we reset learning
|
||||
if (learn_values == nullptr ||
|
||||
!ins.is_still()) {
|
||||
debug("learn reset\n");
|
||||
setup_learning();
|
||||
if (learn_values == nullptr) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
float temp = baro.get_temperature(0);
|
||||
float P = baro.get_pressure(0);
|
||||
uint16_t idx = (temp - learn_temp_start) / learn_temp_step;
|
||||
if (idx >= learn_count) {
|
||||
// could change learn_temp_step here
|
||||
return;
|
||||
}
|
||||
if (is_zero(learn_values[idx])) {
|
||||
learn_values[idx] = P;
|
||||
debug("learning %u %.2f at %.2f\n", idx, learn_values[idx], temp);
|
||||
} else {
|
||||
// filter in new value
|
||||
learn_values[idx] = 0.9 * learn_values[idx] + 0.1 * P;
|
||||
}
|
||||
learn_i = MAX(learn_i, idx);
|
||||
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (now - last_learn_ms > 100 &&
|
||||
idx*learn_temp_step > min_learn_temp_range &&
|
||||
temp - learn_temp_start > temp_max - temp_min) {
|
||||
last_learn_ms = now;
|
||||
// run estimation and update parameters
|
||||
calculate_calibration();
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
apply learned calibration for current temperature
|
||||
*/
|
||||
void AP_TempCalibration::apply_calibration(void)
|
||||
{
|
||||
// just for first baro now
|
||||
if (!baro.healthy(0)) {
|
||||
return;
|
||||
}
|
||||
float temp = baro.get_temperature(0);
|
||||
float correction = calculate_correction(temp, baro_exponent);
|
||||
baro.set_pressure_correction(0, correction);
|
||||
}
|
||||
|
||||
/*
|
||||
called at 10Hz from the main thread. This is called both when armed
|
||||
and disarmed. It only does learning while disarmed, but needs to
|
||||
supply the corrections to the sensor libraries at all times
|
||||
*/
|
||||
void AP_TempCalibration::update(void)
|
||||
{
|
||||
switch (enabled.get()) {
|
||||
case TC_DISABLED:
|
||||
break;
|
||||
case TC_ENABLE_LEARN:
|
||||
learn_calibration();
|
||||
// fall through
|
||||
case TC_ENABLE_USE:
|
||||
apply_calibration();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,79 @@
|
|||
#pragma once
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
temperature calibration library. This monitors temperature changes
|
||||
and opportunistically calibrates sensors when the vehicle is still
|
||||
*/
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <AP_Baro/AP_Baro.h>
|
||||
#include <AP_InertialSensor/AP_InertialSensor.h>
|
||||
|
||||
class AP_TempCalibration
|
||||
{
|
||||
public:
|
||||
// constructor
|
||||
AP_TempCalibration(AP_Baro &baro, AP_InertialSensor &ins);
|
||||
|
||||
// settable parameters
|
||||
static const struct AP_Param::GroupInfo var_info[];
|
||||
|
||||
void update(void);
|
||||
|
||||
enum {
|
||||
TC_DISABLED = 0,
|
||||
TC_ENABLE_USE = 1,
|
||||
TC_ENABLE_LEARN = 2,
|
||||
};
|
||||
|
||||
private:
|
||||
AP_Baro &baro;
|
||||
AP_InertialSensor &ins;
|
||||
|
||||
AP_Int8 enabled;
|
||||
AP_Int8 temp_min;
|
||||
AP_Int8 temp_max;
|
||||
AP_Float baro_exponent;
|
||||
|
||||
Vector3f last_accels;
|
||||
|
||||
float learn_temp_start;
|
||||
float learn_temp_step;
|
||||
uint16_t learn_count;
|
||||
uint16_t learn_i;
|
||||
float *learn_values;
|
||||
uint32_t last_learn_ms;
|
||||
|
||||
// temperature at which baro correction starts
|
||||
const float Tzero = 25;
|
||||
|
||||
const float exp_limit_max = 2;
|
||||
const float exp_limit_min = 0;
|
||||
float learn_delta = 0.01;
|
||||
|
||||
// require observation of at least 5 degrees of temp range to
|
||||
// start learning
|
||||
const float min_learn_temp_range = 7;
|
||||
|
||||
void setup_learning(void);
|
||||
void learn_calibration(void);
|
||||
void apply_calibration(void);
|
||||
void calculate_calibration();
|
||||
float calculate_correction(float temp, float exponent) const;
|
||||
float calculate_p_range(float baro_factor) const;
|
||||
|
||||
};
|
||||
Loading…
Reference in New Issue