From 5f26a36060e9eaf1593c7ae38c0162402029e329 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 9 Mar 2015 15:37:11 +0900
Subject: [PATCH] INS: protect against two calibrations running at the same
 time

---
 .../AP_InertialSensor/AP_InertialSensor.cpp   | 22 ++++++++++++++-----
 1 file changed, 16 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 7f884ada3d..1a6780eccf 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -610,10 +610,10 @@ AP_InertialSensor::_init_accel()
     memset(max_offset, 0, sizeof(max_offset));
     memset(total_change, 0, sizeof(total_change));
 
-    // cold start
-    hal.scheduler->delay(100);
-
-    hal.console->print_P(PSTR("Init Accel"));
+    // exit immediately if calibration is already in progress
+    if (_calibrating) {
+        return;
+    }
 
     // record we are calibrating
     _calibrating = true;
@@ -621,6 +621,11 @@ AP_InertialSensor::_init_accel()
     // flash leds to tell user to keep the IMU still
     AP_Notify::flags.initialising = true;
 
+    // cold start
+    hal.scheduler->delay(100);
+
+    hal.console->print_P(PSTR("Init Accel"));
+
     // clear accelerometer offsets and scaling
     for (uint8_t k=0; k<num_accels; k++) {
         _accel_offset[k] = Vector3f(0,0,0);
@@ -711,8 +716,10 @@ AP_InertialSensor::_init_gyro()
     float best_diff[INS_MAX_INSTANCES];
     bool converged[INS_MAX_INSTANCES];
 
-    // cold start
-    hal.console->print_P(PSTR("Init Gyro"));
+    // exit immediately if calibration is already in progress
+    if (_calibrating) {
+        return;
+    }
 
     // record we are calibrating
     _calibrating = true;
@@ -720,6 +727,9 @@ AP_InertialSensor::_init_gyro()
     // flash leds to tell user to keep the IMU still
     AP_Notify::flags.initialising = true;
 
+    // cold start
+    hal.console->print_P(PSTR("Init Gyro"));
+
     // remove existing gyro offsets
     for (uint8_t k=0; k<num_gyros; k++) {
         _gyro_offset[k] = Vector3f(0,0,0);