Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-27 18:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: added online learning of temp cal

Browse Source 
use INS_TCAL1_ENABLE=2 to start learning
This commit is contained in:
Andrew Tridgell 2021-01-09 16:23:18 +11:00 committed by Peter Barker
parent 64d9f43d94
commit fc0f8b990a
3 changed files with 165 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -47,6 +47,7 @@
#include <Filter/LowPassFilter.h>
#include <Filter/NotchFilter.h>
#include <Filter/HarmonicNotchFilter.h>
#include <AP_Math/polyfit.h>
class AP_InertialSensor_Backend;
class AuxiliaryBus;
@ -690,18 +691,59 @@ public:
void correct_gyro(float temperature, float cal_temp, Vector3f &accel) const;
void sitl_apply_accel(float temperature, Vector3f &accel) const;
void sitl_apply_gyro(float temperature, Vector3f &accel) const;
void update_accel_learning(const Vector3f &accel);
void update_gyro_learning(const Vector3f &accel);
enum class Enable : uint8_t {
Disabled = 0,
Enabled = 1,
LearnCalibration = 2,
};
// add samples for learning
void update_accel_learning(const Vector3f &gyro, float temperature);
void update_gyro_learning(const Vector3f &accel, float temperature);
// class for online learning of calibration
class Learn {
public:
Learn(TCal &_tcal, float _start_temp) :
start_temp(_start_temp),
tcal(_tcal) {};
void add_sample(const Vector3f &sample, float temperature, PolyFit<4> pfit[3]);
void finish_calibration(float temperature);
const float start_temp;
PolyFit<4> polyfit_gyro[3];
PolyFit<4> polyfit_accel[3];
TCal &tcal;
uint8_t instance(void) const {
return tcal.instance();
}
Vector3f accel_start;
};
private:
AP_Int8 enable;
AP_Enum<Enable> enable;
AP_Float temp_min;
AP_Float temp_max;
AP_Vector3f accel_coeff[3];
AP_Vector3f gyro_coeff[3];
Vector3f accel_tref;
Vector3f gyro_tref;
Learn *learn;
void correct_sensor(float temperature, float cal_temp, const AP_Vector3f coeff[3], Vector3f &v) const;
Vector3f polynomial_eval(float temperature, const AP_Vector3f coeff[3]) const;
// get instance number
uint8_t instance(void) const;
};
// instance number for logging
uint8_t tcal_instance(const TCal &tc) const {
return &tc - &tcal[0];
}
private:
TCal tcal[INS_MAX_INSTANCES];

8
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -95,6 +95,10 @@ void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vect
// rotate for sensor orientation
accel.rotate(_imu._accel_orientation[instance]);
#if HAL_INS_TEMPERATURE_CAL_ENABLE
_imu.tcal[instance].update_accel_learning(accel, _imu.get_temperature(instance));
#endif
if (!_imu._calibrating_accel && (_imu._acal == nullptr || !_imu._acal->active())) {
#if HAL_INS_TEMPERATURE_CAL_ENABLE
@ -125,6 +129,10 @@ void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vecto
{
// rotate for sensor orientation
gyro.rotate(_imu._gyro_orientation[instance]);
#if HAL_INS_TEMPERATURE_CAL_ENABLE
_imu.tcal[instance].update_gyro_learning(gyro, _imu.get_temperature(instance));
#endif
if (!_imu._calibrating_gyro) {

120
libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp
View File

@ -20,15 +20,24 @@
#if HAL_INS_TEMPERATURE_CAL_ENABLE
#include <GCS_MAVLink/GCS.h>
// this scale factor ensures params are easy to work with in GUI parameter editors
#define SCALE_FACTOR 1.0e6
#define INV_SCALE_FACTOR 1.0e-6
#define TEMP_RANGE_MIN 15
extern const AP_HAL::HAL& hal;
// temperature calibration parameters, per IMU
const AP_Param::GroupInfo AP_InertialSensor::TCal::var_info[] = {
// @Param: ENABLE
// @DisplayName: Enable temperature calibration
// @Description: Enable the use of temperature calibration parameters for this IMU
// @Values: 0:Disabled,1:Enabled
// @Values: 0:Disabled,1:Enabled,2:LearnCalibration
// @User: Advanced
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_InertialSensor::TCal, enable, 0, AP_PARAM_FLAG_ENABLE),
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_InertialSensor::TCal, enable, float(Enable::Disabled), AP_PARAM_FLAG_ENABLE),
// @Param: TMIN
// @DisplayName: Temperature calibration min
@ -179,9 +188,7 @@ Vector3f AP_InertialSensor::TCal::polynomial_eval(float tdiff, const AP_Vector3f
{
// evaluate order 3 polynomial
const Vector3f *c = (Vector3f *)&coeff[0];
// this scale factor ensures params are easy to work with in GUI parameter editors
const float scale_factor = 1.0e-6;
return (c[0] + (c[1] + c[2]*tdiff)*tdiff)*tdiff*scale_factor;
return (c[0] + (c[1] + c[2]*tdiff)*tdiff)*tdiff*INV_SCALE_FACTOR;
}
/*
@ -189,7 +196,7 @@ Vector3f AP_InertialSensor::TCal::polynomial_eval(float tdiff, const AP_Vector3f
*/
void AP_InertialSensor::TCal::correct_sensor(float temperature, float cal_temp, const AP_Vector3f coeff[3], Vector3f &v) const
{
if (!enable) {
if (enable != Enable::Enabled) {
return;
}
temperature = constrain_float(temperature, temp_min, temp_max);
@ -235,4 +242,105 @@ void AP_InertialSensor::TCal::sitl_apply_gyro(float temperature, Vector3f &gyro)
gyro -= v;
}
/*
update polyfit with new sample
*/
void AP_InertialSensor::TCal::Learn::add_sample(const Vector3f &sample, float temperature, PolyFit<4> pfit[3])
{
const float tmid = 0.5 * (tcal.temp_max + start_temp);
const float tdiff = temperature - tmid;
pfit[0].update(tdiff, sample.x);
pfit[1].update(tdiff, sample.y);
pfit[2].update(tdiff, sample.z);
if (temperature >= tcal.temp_max &&
temperature - start_temp >= TEMP_RANGE_MIN) {
finish_calibration(temperature);
}
}
/*
update accel temperature compensation learning
*/
void AP_InertialSensor::TCal::update_accel_learning(const Vector3f &accel, float temperature)
{
if (enable != Enable::LearnCalibration) {
return;
}
if (learn == nullptr && hal.scheduler->is_system_initialized()) {
learn = new Learn(*this, temperature);
if (learn) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: started calibration t=%.1fC tmax=%.1fC",
instance(),
temperature, temp_max.get());
}
}
if (learn != nullptr) {
if (learn->accel_start.is_zero()) {
learn->accel_start = accel;
}
learn->add_sample(accel - learn->accel_start, temperature, learn->polyfit_accel);
}
}
/*
update gyro temperature compensation learning
*/
void AP_InertialSensor::TCal::update_gyro_learning(const Vector3f &gyro, float temperature)
{
if (enable != Enable::LearnCalibration) {
return;
}
if (learn != nullptr) {
learn->add_sample(gyro, temperature, learn->polyfit_gyro);
}
}
/*
finish and save calibration
*/
void AP_InertialSensor::TCal::Learn::finish_calibration(float temperature)
{
Vector3f coefficients[3];
for (uint8_t i=0; i<3; i++) {
float p[4];
if (!polyfit_gyro[i].get_polynomial(p)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: failed gyro fit axis %u", tcal.instance(), i);
tcal.enable.set_and_save(int8_t(TCal::Enable::Disabled));
return;
}
for (uint8_t k=0; k<3; k++) {
coefficients[k][i] = p[2-k] * SCALE_FACTOR;
}
}
for (uint8_t k=0; k<3; k++) {
tcal.gyro_coeff[k].set_and_save(coefficients[k]);
}
for (uint8_t i=0; i<3; i++) {
float p[4];
if (!polyfit_accel[i].get_polynomial(p)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: failed accel fit axis %u", instance(), i);
tcal.enable.set_and_save(int8_t(TCal::Enable::Disabled));
return;
}
for (uint8_t k=0; k<3; k++) {
coefficients[k][i] = p[2-k] * SCALE_FACTOR;
}
}
for (uint8_t k=0; k<3; k++) {
tcal.accel_coeff[k].set_and_save(coefficients[k]);
}
tcal.temp_min.set_and_save(start_temp);
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: completed calibration tmin=%.1f tmax=%.1f",
instance(),
tcal.temp_min.get(), tcal.temp_max.get());
tcal.enable.set_and_save(int8_t(TCal::Enable::Enabled));
}
uint8_t AP_InertialSensor::TCal::instance(void) const
{
return AP::ins().tcal_instance(*this);
}
#endif // HAL_INS_TEMPERATURE_CAL_ENABLE