From 9c811671f157d50f4bf7f8faa2a6999ded00c0df Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 20 Nov 2012 18:26:51 +1100
Subject: [PATCH] INS: make calibrate_accel() return a bool

this will allow APM to auto set MANUAL_LEVEL to 1
---
 .../AP_InertialSensor/AP_InertialSensor.cpp   | 33 +++++++++----------
 .../AP_InertialSensor/AP_InertialSensor.h     |  2 +-
 2 files changed, 17 insertions(+), 18 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 0d962bff82..c08e4e8dd9 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -232,7 +232,7 @@ AP_InertialSensor::_init_accel(void (*delay_cb)(unsigned long t), void (*flash_l
 }
 
 // perform accelerometer calibration including providing user instructions and feedback
-void AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on), void (*send_msg)(const prog_char_t *, ...))
+bool AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on), void (*send_msg)(const prog_char_t *, ...))
 {
     Vector3f samples[6];
     Vector3f new_offsets;
@@ -274,9 +274,9 @@ void AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void
                 break;
         }
         if (send_msg == NULL) {
-            Serial.printf_P(PSTR("USER: Place APM %S and press any key.\n"), msg);
-        }else{
             send_msg(PSTR("USER: Place APM %S and press any key.\n"), msg);
+        }else{
+            Serial.printf_P(PSTR("USER: Place APM %S and press any key.\n"), msg);
         }
 
         // wait for user input
@@ -305,29 +305,28 @@ void AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void
 
     // run the calibration routine
     if( _calibrate_accel(samples, new_offsets, new_scaling) ) {
-
         if (send_msg == NULL) {
-            Serial.printf_P(PSTR("Calibration successful\n"));
-        }else{
             send_msg(PSTR("Calibration successful\n"));
+        }else{
+            Serial.printf_P(PSTR("Calibration successful\n"));
         }
 
         // set and save calibration
         _accel_offset.set(new_offsets);
         _accel_scale.set(new_scaling);
         _save_parameters();
-    }else{
-        if (send_msg == NULL) {
-            Serial.printf_P(PSTR("Calibration failed\n"));
-        }else{
-            send_msg(PSTR("Calibration failed\n"));
-        }
-
-        // restore original scaling and offsets
-        _accel_offset.set(orig_offset);
-        _accel_scale.set(orig_scale);
+        return true;
     }
-    delay_cb(100);
+
+    if (send_msg == NULL) {
+        send_msg(PSTR("Calibration failed\n"));
+    } else {
+        Serial.printf_P(PSTR("Calibration failed\n"));
+    }
+    // restore original scaling and offsets
+    _accel_offset.set(orig_offset);
+    _accel_scale.set(orig_scale);
+    return false;
 }
 
 // _calibrate_model - perform low level accel calibration
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 6113bffdc5..0a11616b9b 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -50,7 +50,7 @@ public:
     virtual void        init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on));
 
     // perform accelerometer calibration including providing user instructions and feedback
-    virtual void        calibrate_accel(void (*delay_cb)(unsigned long t),
+    virtual bool        calibrate_accel(void (*delay_cb)(unsigned long t),
                                         void (*flash_leds_cb)(bool on) = NULL,
                                         void (*send_msg)(const prog_char_t *, ...) = NULL);