|
|
|
@ -115,7 +115,7 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg, int16_t m
@@ -115,7 +115,7 @@ float Airspeed_Calibration::update(float airspeed, const Vector3f &vg, int16_t m
|
|
|
|
|
void AP_Airspeed::update_calibration(uint8_t i, const Vector3f &vground, int16_t max_airspeed_allowed_during_cal) |
|
|
|
|
{ |
|
|
|
|
#if AP_AIRSPEED_AUTOCAL_ENABLE |
|
|
|
|
if (!param[i].autocal) { |
|
|
|
|
if (!param[i].autocal && !calibration_enabled) { |
|
|
|
|
// auto-calibration not enabled
|
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -146,6 +146,7 @@ void AP_Airspeed::update_calibration(uint8_t i, const Vector3f &vground, int16_t
@@ -146,6 +146,7 @@ void AP_Airspeed::update_calibration(uint8_t i, const Vector3f &vground, int16_t
|
|
|
|
|
param[i].ratio.save(); |
|
|
|
|
state[i].last_saved_ratio = param[i].ratio; |
|
|
|
|
state[i].counter = 0; |
|
|
|
|
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Airspeed %u ratio reset: %f", i , static_cast<double> (param[i].ratio)); |
|
|
|
|
} |
|
|
|
|
} else { |
|
|
|
|
state[i].counter++; |
|
|
|
|