AP_InertialSensor: start auto-learning at boot
This makes it easier to setup the params. The ENABLE=2 value only takes effect at boot
This commit is contained in:
parent
e8ab8ed29e
commit
8caea96ab1
@ -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
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -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
|
||||
};
|
||||
|
||||
|
@ -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) {
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user