From b0e7990c9038703d307fa82cc21c82e057f2c21a Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 9 Mar 2015 13:21:43 +0900
Subject: [PATCH] INS: set gyro_cal_ok only after completing calibration

---
 libraries/AP_InertialSensor/AP_InertialSensor.cpp | 8 ++------
 1 file changed, 2 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index b92f98be31..5c0d9823e4 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -716,7 +716,6 @@ AP_InertialSensor::_init_gyro()
         best_diff[k] = 0;
         last_average[k].zero();
         converged[k] = false;
-        _gyro_cal_ok[k] = true; // default calibration ok flag to true
     }
 
     for(int8_t c = 0; c < 5; c++) {
@@ -794,11 +793,6 @@ AP_InertialSensor::_init_gyro()
     // stop flashing leds
     AP_Notify::flags.initialising = false;
 
-    if (num_converged == num_gyros) {
-        // all OK
-        return;
-    }
-
     // we've kept the user waiting long enough - use the best pair we
     // found so far
     hal.console->println();
@@ -809,6 +803,8 @@ AP_InertialSensor::_init_gyro()
             _gyro_offset[k] = best_avg[k];
             // flag calibration as failed for this gyro
             _gyro_cal_ok[k] = false;
+        } else {
+            _gyro_cal_ok[k] = true;
         }
     }
 }