From fce01464b1441ca788987be323e28c7735439984 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 28 Aug 2012 13:15:04 +1000
Subject: [PATCH] APM: added ALT_CTRL_ALG parameter

this allows you to select different altitude control algorithms. The
current choices are for the default (automatic based on if airspeed is
available), or to force a non-airspeed algorithm

The idea is to make it possible to use airspeed for some things (like
wind speed, speed scaling) but not for alt control
---
 ArduPlane/ArduPlane.pde  |  4 ++--
 ArduPlane/Attitude.pde   | 10 ++++++++--
 ArduPlane/Parameters.h   |  2 ++
 ArduPlane/Parameters.pde |  7 +++++++
 ArduPlane/defines.h      |  7 +++++++
 ArduPlane/radio.pde      |  2 +-
 6 files changed, 27 insertions(+), 5 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index 82c6b8df80..9072199d6d 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -1073,7 +1073,7 @@ static void update_current_flight_mode(void)
                 nav_roll_cd = 0;
             }
 
-            if(airspeed.use()) {
+            if (alt_control_airspeed()) {
                 calc_nav_pitch();
                 if (nav_pitch_cd < takeoff_pitch_cd)
                     nav_pitch_cd = takeoff_pitch_cd;
@@ -1092,7 +1092,7 @@ static void update_current_flight_mode(void)
 
             calc_nav_pitch();
             calc_throttle();
-            if (!airspeed.use() || land_complete) {
+            if (!alt_control_airspeed() || land_complete) {
                 // hold pitch constant in final approach
                 nav_pitch_cd = g.land_pitch_cd;
             }
diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde
index c228f45035..f219691349 100644
--- a/ArduPlane/Attitude.pde
+++ b/ArduPlane/Attitude.pde
@@ -134,7 +134,7 @@ static void crash_checker()
 
 static void calc_throttle()
 {
-    if (!airspeed.use()) {
+    if (!alt_control_airspeed()) {
         int16_t throttle_target = g.throttle_cruise + throttle_nudge;
 
         // TODO: think up an elegant way to bump throttle when
@@ -200,7 +200,7 @@ static void calc_nav_pitch()
 {
     // Calculate the Pitch of the plane
     // --------------------------------
-    if (airspeed.use()) {
+    if (alt_control_airspeed()) {
         nav_pitch_cd = -g.pidNavPitchAirspeed.get_pid(airspeed_error_cm);
     } else {
         nav_pitch_cd = g.pidNavPitchAltitude.get_pid(altitude_error_cm);
@@ -470,3 +470,9 @@ static void demo_servos(byte i) {
         i--;
     }
 }
+
+// return true if we should use airspeed for altitude/throttle control
+static bool alt_control_airspeed(void)
+{
+    return airspeed.use() && g.alt_control_algorithm == ALT_CONTROL_DEFAULT;
+}
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index 073afaa559..c032ea0d1e 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -88,6 +88,7 @@ public:
         k_param_flybywire_airspeed_max,
         k_param_FBWB_min_altitude_cm,  // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm)
         k_param_flybywire_elev_reverse,
+        k_param_alt_control_algorithm,
 
         //
         // 130: Sensor parameters
@@ -238,6 +239,7 @@ public:
     // Estimation
     //
     AP_Float altitude_mix;
+    AP_Int8  alt_control_algorithm;
 
     // Waypoints
     //
diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde
index 516fdd67c2..d333ccbc25 100644
--- a/ArduPlane/Parameters.pde
+++ b/ArduPlane/Parameters.pde
@@ -128,6 +128,13 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Advanced
     GSCALAR(altitude_mix,           "ALT_MIX",        ALTITUDE_MIX),
 
+    // @Param: ALT_CTRL_ALG
+    // @DisplayName: Altitude control algorithm
+    // @Description: This sets what algorithm will be used for altitude control. The default is to select the algorithm based on whether airspeed is enabled. If you set it to 1, then the airspeed based algorithm won't be used for altitude control, but airspeed can be used for other flight control functions
+    // @Values: 0:Default Method,1:non-airspeed
+    // @User: Advanced
+    GSCALAR(alt_control_algorithm, "ALT_CTRL_ALG",    ALT_CONTROL_DEFAULT),
+
     GSCALAR(command_total,          "CMD_TOTAL",      0),
     GSCALAR(command_index,          "CMD_INDEX",      0),
 
diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h
index d914dcdcc2..117c05c465 100644
--- a/ArduPlane/defines.h
+++ b/ArduPlane/defines.h
@@ -255,4 +255,11 @@ enum gcs_severity {
 #define AP_BARO_BMP085   1
 #define AP_BARO_MS5611   2
 
+// altitude control algorithms
+enum {
+    ALT_CONTROL_DEFAULT=0,
+    ALT_CONTROL_NON_AIRSPEED=1
+};
+
+
 #endif // _DEFINES_H
diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde
index df6046bd5e..3010f82a4f 100644
--- a/ArduPlane/radio.pde
+++ b/ArduPlane/radio.pde
@@ -95,7 +95,7 @@ static void read_radio()
     g.channel_throttle.servo_out = g.channel_throttle.control_in;
 
     if (g.channel_throttle.servo_out > 50) {
-        if (airspeed.use()) {
+        if (alt_control_airspeed()) {
             airspeed_nudge_cm = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise_cm) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
         } else {
             throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);