From 7db7d7db777758f8c09b4898739b9df56d898186 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 28 Mar 2013 10:27:25 +1100
Subject: [PATCH] Plane: change FBWB altitude control algorithm

this makes FBWB much less sensitive to airframe tuning. When the
elevator stick first goes neutral it locks in the current altitude as
the target altitude. When the elevator stick is off neutral, it moves
the target altitude in proportion to the elevator, at a rate goverened
by the new FBWB_CLIMB_RATE parameter

This prevents the aircraft from slowly drifting in altitude in FBWB,
and gives a more intuitive control mechanism for altitude.

Thanks to Chris Miser from Falcon UAV for help in designing this
change
---
 ArduPlane/ArduPlane.pde  | 27 +++++++++++++++++++--------
 ArduPlane/Parameters.h   |  2 ++
 ArduPlane/Parameters.pde | 13 +++++++++++++
 ArduPlane/system.pde     |  3 +++
 4 files changed, 37 insertions(+), 8 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index 871ba9a04d..27f4435884 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -1160,7 +1160,8 @@ static void update_current_flight_mode(void)
             break;
         }
 
-        case FLY_BY_WIRE_B:
+        case FLY_BY_WIRE_B: {
+            static float last_elevator_input;
             // Substitute stick inputs for Navigation control output
             // We use g.pitch_limit_min because its magnitude is
             // normally greater than g.pitch_limit_max
@@ -1175,16 +1176,26 @@ static void update_current_flight_mode(void)
             if (g.flybywire_elev_reverse) {
                 elevator_input = -elevator_input;
             }
-            if ((adjusted_altitude_cm() >= home.alt+g.FBWB_min_altitude_cm) || (g.FBWB_min_altitude_cm == 0)) {
-                altitude_error_cm = elevator_input * g.pitch_limit_min_cd;
-            } else {
-                altitude_error_cm = (home.alt + g.FBWB_min_altitude_cm) - adjusted_altitude_cm();
-                if (elevator_input < 0) {
-                    altitude_error_cm += elevator_input * g.pitch_limit_min_cd;
-                }
+
+            target_altitude_cm += g.flybywire_climb_rate * elevator_input * delta_ms_fast_loop * 0.1f;
+
+            if (elevator_input == 0.0f && last_elevator_input != 0.0f) {
+                // the user has just released the elevator, lock in
+                // the current altitude
+                target_altitude_cm = current_loc.alt;
             }
+
+            // check for FBWB altitude limit
+            if (g.FBWB_min_altitude_cm != 0 && target_altitude_cm < home.alt + g.FBWB_min_altitude_cm) {
+                target_altitude_cm = home.alt + g.FBWB_min_altitude_cm;
+            }
+            altitude_error_cm = target_altitude_cm - adjusted_altitude_cm();
+
+            last_elevator_input = elevator_input;
+
             calc_throttle();
             calc_nav_pitch();
+        }
             break;
 
         case STABILIZE:
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index 39d0f127f0..ca2dfc6002 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -99,6 +99,7 @@ public:
         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,
+        k_param_flybywire_climb_rate,
 
         //
         // 130: Sensor parameters
@@ -285,6 +286,7 @@ public:
     AP_Int16 flybywire_airspeed_min;
     AP_Int16 flybywire_airspeed_max;
     AP_Int8 flybywire_elev_reverse;
+    AP_Int8 flybywire_climb_rate;
 
     // Throttle
     //
diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde
index cfaa43f327..0e59381534 100644
--- a/ArduPlane/Parameters.pde
+++ b/ArduPlane/Parameters.pde
@@ -286,6 +286,14 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(flybywire_elev_reverse, "FBWB_ELEV_REV",  0),
 
+    // @Param: FBWB_CLIMB_RATE
+    // @DisplayName: Fly By Wire B alttitude change rate
+    // @Description: This sets the rate in m/s at which FBWB will change its target altitude for full elevator deflection. Note that the actual climb rate of the aircraft can be lower than this, depending on your airspeed and throttle control settings. If you have this parameter set to the default value of 2.0, then holding the elevator at maximum deflection for 10 seconds would change the target altitude by 20 meters.
+    // @Range: 1-10
+	// @Increment: 0.1
+    // @User: Standard
+    GSCALAR(flybywire_climb_rate, "FBWB_CLIMB_RATE",  2.0f),
+
     // @Param: THR_MIN
     // @DisplayName: Minimum Throttle
     // @Description: The minimum throttle setting to which the autopilot will apply.
@@ -549,6 +557,11 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: User
     GSCALAR(RTL_altitude_cm,        "ALT_HOLD_RTL",   ALT_HOLD_HOME_CM),
 
+    // @Param: ALT_HOLD_FBWCM
+    // @DisplayName: Minimum altitude for FBWB mode
+    // @Description: This is the minimum altitude in centimeters that FBWB will allow. If you attempt to descend below this altitude then the plane will level off. A value of zero means no limit.
+    // @Units: centimeters
+    // @User: User
     GSCALAR(FBWB_min_altitude_cm,   "ALT_HOLD_FBWCM", ALT_HOLD_FBW_CM),
 
     // @Param: MAG_ENABLE
diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde
index 7d96e51594..ed3cbcd255 100644
--- a/ArduPlane/system.pde
+++ b/ArduPlane/system.pde
@@ -340,7 +340,10 @@ static void set_mode(enum FlightMode mode)
     case STABILIZE:
     case TRAINING:
     case FLY_BY_WIRE_A:
+        break;
+
     case FLY_BY_WIRE_B:
+        target_altitude_cm = current_loc.alt;
         break;
 
     case CIRCLE: