From f97cfd10658f66f4d897cbebdc1e1d7f4bd3b63f Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 19 Nov 2019 19:12:21 +0900
Subject: [PATCH] AP_Compass: fix calibrator update when step one fails

---
 libraries/AP_Compass/CompassCalibrator.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Compass/CompassCalibrator.cpp b/libraries/AP_Compass/CompassCalibrator.cpp
index 977ab0b6a2..4745494ed2 100644
--- a/libraries/AP_Compass/CompassCalibrator.cpp
+++ b/libraries/AP_Compass/CompassCalibrator.cpp
@@ -203,8 +203,9 @@ void CompassCalibrator::update(bool &failure)
             if (is_equal(_fitness, _initial_fitness) || isnan(_fitness)) {  // if true, means that fitness is diverging instead of converging
                 set_status(COMPASS_CAL_FAILED);
                 failure = true;
+            } else {
+                set_status(COMPASS_CAL_RUNNING_STEP_TWO);
             }
-            set_status(COMPASS_CAL_RUNNING_STEP_TWO);
         } else {
             if (_fit_step == 0) {
                 calc_initial_offset();