From 4c4c38f69a6e395b5aa21f40c87399913db185ca Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 29 Mar 2012 12:39:28 +1100
Subject: [PATCH] APM: Added MANUAL_LEVEL option

when MANUAL_LEVEL is set to 1, we don't do accelerometer levelling at
startup, and instead used the values saved in the EEPROM. This makes
it easier to do levelling on the bench, or once for a series of
flights for the day
---
 ArduPlane/GCS_Mavlink.pde |  4 ++--
 ArduPlane/Parameters.h    |  2 ++
 ArduPlane/Parameters.pde  |  1 +
 ArduPlane/system.pde      | 11 ++++++++---
 4 files changed, 13 insertions(+), 5 deletions(-)

diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 60056b6fcc..2cede18da0 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -1083,7 +1083,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 if (packet.param1 == 1 ||
                     packet.param2 == 1 ||
                     packet.param3 == 1) {
-                    startup_IMU_ground();
+                    startup_IMU_ground(true);
                 }
                 if (packet.param4 == 1) {
                     trim_radio();
@@ -1188,7 +1188,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
                 case MAV_ACTION_CALIBRATE_ACC:
                 case MAV_ACTION_CALIBRATE_PRESSURE:
                 case MAV_ACTION_REBOOT:  // this is a rough interpretation
-                    startup_IMU_ground();
+                    startup_IMU_ground(true);
                     result=1;
                     break;
 
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index f508e6c38d..5519f0cd22 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -49,6 +49,7 @@ public:
         k_param_num_resets,
         k_param_log_last_filenumber,		// *** Deprecated - remove with next eeprom number change
         k_param_reset_switch_chan,
+        k_param_manual_level,
 
 
 		// 110: Telemetry control
@@ -300,6 +301,7 @@ public:
     AP_Int16    log_bitmask;
     AP_Int16	log_last_filenumber;		// *** Deprecated - remove with next eeprom number change
     AP_Int8		reset_switch_chan;
+    AP_Int8		manual_level;
     AP_Int16    airspeed_cruise;
     AP_Int16    min_gndspeed;
     AP_Int16    pitch_trim;
diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde
index 4b0bda9ae0..3619095c05 100644
--- a/ArduPlane/Parameters.pde
+++ b/ArduPlane/Parameters.pde
@@ -23,6 +23,7 @@ static const AP_Param::Info var_info[] PROGMEM = {
 	GSCALAR(kff_rudder_mix,         "KFF_RDDRMIX"),
 	GSCALAR(kff_pitch_to_throttle,  "KFF_PTCH2THR"),
 	GSCALAR(kff_throttle_to_pitch,  "KFF_THR2PTCH"),
+	GSCALAR(manual_level,           "MANUAL_LEVEL"),
 
 	GSCALAR(crosstrack_gain,        "XTRK_GAIN_SC"),
 	GSCALAR(crosstrack_entry_angle, "XTRK_ANGLE_CD"),
diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde
index 0dc50c54d9..72584598fa 100644
--- a/ArduPlane/system.pde
+++ b/ArduPlane/system.pde
@@ -318,7 +318,7 @@ static void startup_ground(void)
 	//IMU ground start
 	//------------------------
     //
-	startup_IMU_ground();
+	startup_IMU_ground(false);
 
 	// read the radio to set trims
 	// ---------------------------
@@ -441,7 +441,7 @@ static void check_short_failsafe()
 }
 
 
-static void startup_IMU_ground(void)
+static void startup_IMU_ground(bool force_accel_level)
 {
 #if HIL_MODE != HIL_MODE_ATTITUDE
     gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC..."));
@@ -454,7 +454,12 @@ static void startup_IMU_ground(void)
 	mavlink_delay(1000);
 
 	imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
-	imu.init_accel(mavlink_delay, flash_leds);
+    if (force_accel_level || g.manual_level == 0) {
+        // when MANUAL_LEVEL is set to 1 we don't do accelerometer
+        // levelling on each boot, and instead rely on the user to do
+        // it once via the ground station
+        imu.init_accel(mavlink_delay, flash_leds);
+    }
 	ahrs.set_centripetal(1);
     ahrs.reset();