Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -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()); 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 // initialise and register accel calibrator
// called during the startup of accel cal // called during the startup of accel cal
void AP_InertialSensor::acal_init() 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 /// calibrating - returns true if the gyros or accels are currently being calibrated
bool calibrating() const; 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. /// Perform cold-start initialisation for just the gyros.
/// ///
/// @note This should not be called unless ::init has previously /// @note This should not be called unless ::init has previously
@ -730,8 +733,9 @@ public:
Vector3f accel_start; Vector3f accel_start;
}; };
private:
AP_Enum<Enable> enable; AP_Enum<Enable> enable;
private:
AP_Float temp_min; AP_Float temp_min;
AP_Float temp_max; AP_Float temp_max;
AP_Vector3f accel_coeff[3]; 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", GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: started calibration t=%.1fC tmax=%.1fC",
instance(), instance(),
temperature, temp_max.get()); temperature, temp_max.get());
AP_Notify::events.initiated_temp_cal = 1;
} }
} }
if (learn != nullptr) { if (learn != nullptr) {
AP_Notify::flags.temp_cal_running = true;
learn->add_sample(accel, temperature, learn->state[0]); learn->add_sample(accel, temperature, learn->state[0]);
} }
} }
@ -359,6 +361,7 @@ void AP_InertialSensor::TCal::Learn::finish_calibration(float temperature)
float p[4]; float p[4];
if (!state[0].pfit[i].get_polynomial(p)) { if (!state[0].pfit[i].get_polynomial(p)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: failed accel fit axis %u", instance(), i); 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)); tcal.enable.set_and_save(int8_t(TCal::Enable::Disabled));
return; return;
} }
@ -374,6 +377,7 @@ void AP_InertialSensor::TCal::Learn::finish_calibration(float temperature)
float p[4]; float p[4];
if (!state[1].pfit[i].get_polynomial(p)) { if (!state[1].pfit[i].get_polynomial(p)) {
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "TCAL[%u]: failed gyro fit axis %u", tcal.instance(), i); 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)); tcal.enable.set_and_save(int8_t(TCal::Enable::Disabled));
return; return;
} }
@ -389,6 +393,11 @@ void AP_InertialSensor::TCal::Learn::finish_calibration(float temperature)
instance(), instance(),
tcal.temp_min.get(), tcal.temp_max.get()); tcal.temp_min.get(), tcal.temp_max.get());
tcal.enable.set_and_save(int8_t(TCal::Enable::Enabled)); 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 uint8_t AP_InertialSensor::TCal::instance(void) const