From 78af6d61de456b138b183f9c82c0da50690823c3 Mon Sep 17 00:00:00 2001 From: Jonathan Challinger Date: Wed, 2 Sep 2015 11:20:15 -0700 Subject: [PATCH] AP_Compass: only play compass cal cancel tone if a cal was running --- libraries/AP_Compass/AP_Compass_Calibration.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Compass/AP_Compass_Calibration.cpp b/libraries/AP_Compass/AP_Compass_Calibration.cpp index 26cff1eee6..c103a48233 100644 --- a/libraries/AP_Compass/AP_Compass_Calibration.cpp +++ b/libraries/AP_Compass/AP_Compass_Calibration.cpp @@ -94,9 +94,12 @@ Compass::start_calibration_all(bool retry, bool autosave, float delay, bool auto void Compass::cancel_calibration(uint8_t i) { - _calibrator[i].clear(); - AP_Notify::events.compass_cal_canceled = 1; AP_Notify::events.initiated_compass_cal = 0; + + if (_calibrator[i].running() || _calibrator[i].get_status() == COMPASS_CAL_WAITING_TO_START) { + AP_Notify::events.compass_cal_canceled = 1; + } + _calibrator[i].clear(); } void