From 03800ec5f4a4566b4be6f15bc2b17bc6ec796652 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 60056b6fc..2cede18da 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 f508e6c38..5519f0cd2 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 4b0bda9ae..3619095c0 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 0dc50c54d..72584598f 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();
 
-- 
GitLab