From 5175f212254ef6f8e4f59295cdd8f5c04aa711eb Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 11 Mar 2015 10:16:59 +1100
Subject: [PATCH] Plane: removed 1D accel calibration

---
 ArduPlane/GCS_Mavlink.pde | 21 ++++++++++++---------
 ArduPlane/setup.pde       |  9 ---------
 ArduPlane/system.pde      | 10 +++-------
 3 files changed, 15 insertions(+), 25 deletions(-)

diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 001e120d1e..978bcd239b 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -1038,23 +1038,25 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             break;
 
         case MAV_CMD_PREFLIGHT_CALIBRATION:
-            if (packet.param3 == 1) {
-                in_calibration = true;
+            in_calibration = true;
+            if (packet.param1 == 1) {
+                ins.init_gyro();
+                if (ins.gyro_calibrated_ok_all()) {
+                    ahrs.reset_gyro_drift();
+                    result = MAV_RESULT_ACCEPTED;
+                } else {
+                    result = MAV_RESULT_FAILED;
+                }
+            } else if (packet.param3 == 1) {
                 init_barometer();
                 if (airspeed.enabled()) {
                     zero_airspeed(false);
                 }
-                in_calibration = false;
-                result = MAV_RESULT_ACCEPTED;
-            } else if (packet.param1 == 1 ||
-                       packet.param2 == 1) {
-                startup_INS_ground(true);
                 result = MAV_RESULT_ACCEPTED;
             } else if (packet.param4 == 1) {
                 trim_radio();
                 result = MAV_RESULT_ACCEPTED;
-            } 
-            else if (packet.param5 == 1) {
+            } else if (packet.param5 == 1) {
                 float trim_roll, trim_pitch;
                 AP_InertialSensor_UserInteract_MAVLink interact(this);
                 if (g.skip_gyro_cal) {
@@ -1073,6 +1075,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
             else {
                     send_text_P(SEVERITY_LOW, PSTR("Unsupported preflight calibration"));
             }
+            in_calibration = false;
             break;
 
         case MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde
index 341f1e136e..82f2508bd8 100644
--- a/ArduPlane/setup.pde
+++ b/ArduPlane/setup.pde
@@ -6,7 +6,6 @@
 static int8_t   setup_radio                             (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_show                              (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_factory                   (uint8_t argc, const Menu::arg *argv);
-static int8_t   setup_level                             (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_accel_scale                       (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_set                               (uint8_t argc, const Menu::arg *argv);
 static int8_t   setup_erase                             (uint8_t argc, const Menu::arg *argv);
@@ -19,7 +18,6 @@ static const struct Menu::command setup_menu_commands[] PROGMEM = {
     // =======          ===============
     {"reset",                       setup_factory},
     {"radio",                       setup_radio},
-    {"level",                       setup_level},
     {"accel",                       setup_accel_scale},
     {"compass",                     setup_compass},
     {"show",                        setup_show},
@@ -265,13 +263,6 @@ setup_erase(uint8_t argc, const Menu::arg *argv)
     return 0;
 }
 
-static int8_t
-setup_level(uint8_t argc, const Menu::arg *argv)
-{
-    startup_INS_ground(true);
-    return 0;
-}
-
 /*
   handle full accelerometer calibration via user dialog
  */
diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde
index 6c948e56e9..5417a13947 100644
--- a/ArduPlane/system.pde
+++ b/ArduPlane/system.pde
@@ -257,7 +257,7 @@ static void startup_ground(void)
     //INS ground start
     //------------------------
     //
-    startup_INS_ground(false);
+    startup_INS_ground();
 
     // read the radio to set trims
     // ---------------------------
@@ -498,7 +498,7 @@ static void check_short_failsafe()
 }
 
 
-static void startup_INS_ground(bool do_accel_init)
+static void startup_INS_ground(void)
 {
 #if HIL_MODE != HIL_MODE_DISABLED
     while (barometer.get_last_update() == 0) {
@@ -514,7 +514,7 @@ static void startup_INS_ground(bool do_accel_init)
 #endif
 
     AP_InertialSensor::Start_style style;
-    if (g.skip_gyro_cal && !do_accel_init) {
+    if (g.skip_gyro_cal) {
         style = AP_InertialSensor::WARM_START;
         arming.set_skip_gyro_cal(true);
     } else {
@@ -532,10 +532,6 @@ static void startup_INS_ground(bool do_accel_init)
     ahrs.set_wind_estimation(true);
 
     ins.init(style, ins_sample_rate);
-    if (do_accel_init) {
-        ins.init_accel();
-        ahrs.set_trim(Vector3f(0, 0, 0));
-    }
     ahrs.reset();
 
     // read Baro pressure at ground