From 5ba171af9aa88fd6d853a4792419579074bc0d69 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 2 May 2013 13:38:28 +1000
Subject: [PATCH] Plane: fixed mavlink levelling

this was broken by a recent commit (after 2.72 was released)
---
 ArduPlane/GCS_Mavlink.pde | 2 +-
 ArduPlane/setup.pde       | 2 +-
 ArduPlane/system.pde      | 8 ++++++--
 3 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 5047fa974a..7968b10515 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -1136,7 +1136,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
         case MAV_CMD_PREFLIGHT_CALIBRATION:
             if (packet.param1 == 1 ||
                 packet.param2 == 1) {
-                startup_INS_ground();
+                startup_INS_ground(true);
             } else if (packet.param3 == 1) {
                 init_barometer();
                 if (airspeed.enabled()) {
diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde
index 58fd921b29..136ecff4e3 100644
--- a/ArduPlane/setup.pde
+++ b/ArduPlane/setup.pde
@@ -376,7 +376,7 @@ setup_erase(uint8_t argc, const Menu::arg *argv)
 static int8_t
 setup_level(uint8_t argc, const Menu::arg *argv)
 {
-    startup_INS_ground();
+    startup_INS_ground(true);
     return 0;
 }
 
diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde
index 2d428bfaaf..a16a71730e 100644
--- a/ArduPlane/system.pde
+++ b/ArduPlane/system.pde
@@ -281,7 +281,7 @@ static void startup_ground(void)
     //INS ground start
     //------------------------
     //
-    startup_INS_ground();
+    startup_INS_ground(false);
 
     // read the radio to set trims
     // ---------------------------
@@ -427,7 +427,7 @@ static void check_short_failsafe()
 }
 
 
-static void startup_INS_ground(void)
+static void startup_INS_ground(bool do_accel_init)
 {
 #if HIL_MODE != HIL_MODE_DISABLED
     while (!barometer.healthy) {
@@ -453,6 +453,10 @@ static void startup_INS_ground(void)
     ins.init(AP_InertialSensor::COLD_START, 
              ins_sample_rate,
              flash_leds);
+    if (do_accel_init) {
+        ins.init_accel(flash_leds);
+        ahrs.set_trim(Vector3f(0, 0, 0));
+    }
     ahrs.reset();
 
     // read Baro pressure at ground