aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: start auto-learning at boot

Browse Source 
This makes it easier to setup the params. The ENABLE=2 value only
takes effect at boot
This commit is contained in:
Andrew Tridgell 2021-01-16 12:23:17 +11:00 committed by Peter Barker
parent e8ab8ed29e
commit 8caea96ab1
4 changed files with 29 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -862,6 +862,15 @@ AP_InertialSensor::init(uint16_t loop_rate)
_gyro_harmonic_notch_filter[i].init(_gyro_raw_sample_rates[i], _calculated_harmonic_notch_freq_hz[0],
_harmonic_notch_filter.bandwidth_hz(), _harmonic_notch_filter.attenuation_dB());
}
#if HAL_INS_TEMPERATURE_CAL_ENABLE
/*
see if user has setup for on-boot enable of temperature learning
*/
if (temperature_cal_running()) {
tcal_learning = true;
}
#endif
}
bool AP_InertialSensor::_add_backend(AP_InertialSensor_Backend *backend)
@ -1566,6 +1575,14 @@ void AP_InertialSensor::update(void)
_last_update_usec = AP_HAL::micros();
_have_sample = false;
#if HAL_INS_TEMPERATURE_CAL_ENABLE
if (tcal_learning && !temperature_cal_running()) {
AP_Notify::flags.temp_cal_running = false;
AP_Notify::events.temp_cal_saved = 1;
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL finished all IMUs");
}
#endif
}
/*

4
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -738,8 +738,8 @@ public:
AP_Enum<Enable> enable;
private:
AP_Float temp_min;
AP_Float temp_max;
AP_Float temp_min;
AP_Vector3f accel_coeff[3];
AP_Vector3f gyro_coeff[3];
Vector3f accel_tref;
@ -763,6 +763,8 @@ private:
// temperature that last calibration was run at
AP_Float caltemp_accel[INS_MAX_INSTANCES];
AP_Float caltemp_gyro[INS_MAX_INSTANCES];
bool tcal_learning;
int8_t tcal_old_brd_target;
#endif
};

8
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -96,7 +96,9 @@ void AP_InertialSensor_Backend::_rotate_and_correct_accel(uint8_t instance, Vect
accel.rotate(_imu._accel_orientation[instance]);
#if HAL_INS_TEMPERATURE_CAL_ENABLE
_imu.tcal[instance].update_accel_learning(accel, _imu.get_temperature(instance));
if (_imu.tcal_learning) {
_imu.tcal[instance].update_accel_learning(accel, _imu.get_temperature(instance));
}
#endif
if (!_imu._calibrating_accel && (_imu._acal == nullptr || !_imu._acal->active())) {
@ -131,7 +133,9 @@ void AP_InertialSensor_Backend::_rotate_and_correct_gyro(uint8_t instance, Vecto
gyro.rotate(_imu._gyro_orientation[instance]);
#if HAL_INS_TEMPERATURE_CAL_ENABLE
_imu.tcal[instance].update_gyro_learning(gyro, _imu.get_temperature(instance));
if (_imu.tcal_learning) {
_imu.tcal[instance].update_gyro_learning(gyro, _imu.get_temperature(instance));
}
#endif
if (!_imu._calibrating_gyro) {

10
libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp
View File

@ -26,7 +26,7 @@
// 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
#define TEMP_RANGE_MIN 10
extern const AP_HAL::HAL& hal;
@ -35,9 +35,10 @@ 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
// @Description: Enable the use of temperature calibration parameters for this IMU. For automatic learning set to 2 and also set the INS_TCALn_TMAX to the target temperature, then reboot
// @Values: 0:Disabled,1:Enabled,2:LearnCalibration
// @User: Advanced
// @RebootRequired: True
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_InertialSensor::TCal, enable, float(Enable::Disabled), AP_PARAM_FLAG_ENABLE),
// @Param: TMIN
@ -413,11 +414,6 @@ void AP_InertialSensor::TCal::Learn::finish_calibration(float temperature)
instance()+1,
tcal.temp_min.get(), tcal.temp_max.get());
tcal.enable.set_and_save(int8_t(TCal::Enable::Enabled));
if (!AP::ins().temperature_cal_running()) {
AP_Notify::flags.temp_cal_running = false;
AP_Notify::events.temp_cal_saved = 1;
}
}
uint8_t AP_InertialSensor::TCal::instance(void) const