Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: added notify events for temperature calibration

Browse Source
This commit is contained in:
Andrew Tridgell 2021-01-11 07:09:44 +11:00 committed by Peter Barker
parent cc3579ea5d
commit 3fb720354d
3 changed files with 27 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1945,6 +1945,19 @@ bool AP_InertialSensor::calibrating() const
return _calibrating_accel || _calibrating_gyro || (_acal && _acal->active());
}
/// calibrating - returns true if a temperature calibration is running
bool AP_InertialSensor::temperature_cal_running() const
{
#if HAL_INS_TEMPERATURE_CAL_ENABLE
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
if (tcal[i].enable == TCal::Enable::LearnCalibration) {
return true;
}
}
#endif
return false;
}
// initialise and register accel calibrator
// called during the startup of accel cal
void AP_InertialSensor::acal_init()

6
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -107,6 +107,9 @@ public:
/// calibrating - returns true if the gyros or accels are currently being calibrated
bool calibrating() const;
/// calibrating - returns true if a temperature calibration is running
bool temperature_cal_running() const;
/// Perform cold-start initialisation for just the gyros.
///
/// @note This should not be called unless ::init has previously
@ -730,8 +733,9 @@ public:
Vector3f accel_start;
};
private:
AP_Enum<Enable> enable;
private:
AP_Float temp_min;
AP_Float temp_max;
AP_Vector3f accel_coeff[3];

9
libraries/AP_InertialSensor/AP_InertialSensor_tempcal.cpp
View File

@ -328,9 +328,11 @@ void AP_InertialSensor::TCal::update_accel_learning(const Vector3f &accel, float
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: started calibration t=%.1fC tmax=%.1fC",
instance(),
temperature, temp_max.get());
AP_Notify::events.initiated_temp_cal = 1;
}
}
if (learn != nullptr) {
AP_Notify::flags.temp_cal_running = true;
learn->add_sample(accel, temperature, learn->state[0]);
}
}
@ -359,6 +361,7 @@ void AP_InertialSensor::TCal::Learn::finish_calibration(float temperature)
float p[4];
if (!state[0].pfit[i].get_polynomial(p)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: failed accel fit axis %u", instance(), i);
AP_Notify::events.temp_cal_failed = 1;
tcal.enable.set_and_save(int8_t(TCal::Enable::Disabled));
return;
}
@ -374,6 +377,7 @@ void AP_InertialSensor::TCal::Learn::finish_calibration(float temperature)
float p[4];
if (!state[1].pfit[i].get_polynomial(p)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: failed gyro fit axis %u", tcal.instance(), i);
AP_Notify::events.temp_cal_failed = 1;
tcal.enable.set_and_save(int8_t(TCal::Enable::Disabled));
return;
}
@ -389,6 +393,11 @@ void AP_InertialSensor::TCal::Learn::finish_calibration(float temperature)
instance(),
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