diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index 729d2e9ddf..fbee3da2de 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -808,12 +808,20 @@ static void update_current_flight_mode(void)
 				break;
 
 			case FLY_BY_WIRE_B:
-				// fake Navigation output using sticks
+				// Substitute stick inputs for Navigation control output
 				// We use g.pitch_limit_min because its magnitude is
 				// normally greater than g.pitch_limit_max
 				nav_roll = g.channel_roll.norm_input() * g.roll_limit;
 				altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
 
+				if ((current_loc.alt>=home.alt+g.FBWB_min_altitude) || (g.FBWB_min_altitude == -1)) {
+	 				altitude_error = g.channel_pitch.norm_input() * g.pitch_limit_min;
+				} else {
+					if (g.channel_pitch.norm_input()<0) 
+						altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) + g.channel_pitch.norm_input() * g.pitch_limit_min ; 
+					else altitude_error =( (home.alt + g.FBWB_min_altitude) - current_loc.alt) ;                                    
+				}
+ 
 				if (g.airspeed_enabled == true)
 									{
 					airspeed_fbwB = ((int)(g.flybywire_airspeed_max -
diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index a4c402ead6..e831686f48 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -83,6 +83,7 @@ public:
         //
         k_param_flybywire_airspeed_min = 120,
         k_param_flybywire_airspeed_max,
+        k_param_FBWB_min_altitude,  // -1=disabled, minimum value for altitude in cm (default 30m)
 
         //
         // 130: Sensor parameters
@@ -262,6 +263,7 @@ public:
     //
     AP_Int8     flybywire_airspeed_min;
     AP_Int8     flybywire_airspeed_max;
+    AP_Int16    FBWB_min_altitude;
 
     // Throttle
     //
@@ -411,6 +413,7 @@ public:
         airspeed_cruise         (AIRSPEED_CRUISE_CM,        k_param_airspeed_cruise,        PSTR("TRIM_ARSPD_CM")),
         pitch_trim              (0,                         k_param_pitch_trim,             PSTR("TRIM_PITCH_CD")),
         RTL_altitude            (ALT_HOLD_HOME_CM,          k_param_RTL_altitude,           PSTR("ALT_HOLD_RTL")),
+        FBWB_min_altitude       (ALT_HOLD_FBW_CM,           k_param_FBWB_min_altitude,      PSTR("ALT_HOLD_FBWCM")),
         ground_temperature      (0,                         k_param_ground_temperature,     PSTR("GND_TEMP")),
         ground_pressure         (0,                         k_param_ground_pressure,        PSTR("GND_ABS_PRESS")),
         compass_enabled			(MAGNETOMETER,				k_param_compass_enabled,		PSTR("MAG_ENABLE")),
diff --git a/ArduPlane/config.h b/ArduPlane/config.h
index be0e3729f9..43964df9c6 100644
--- a/ArduPlane/config.h
+++ b/ArduPlane/config.h
@@ -366,6 +366,14 @@
 #ifndef AIRSPEED_FBW_MAX
 # define AIRSPEED_FBW_MAX		22
 #endif
+
+#ifndef ALT_HOLD_FBW
+# define ALT_HOLD_FBW 0
+#endif
+#define ALT_HOLD_FBW_CM ALT_HOLD_FBW*100
+
+
+
 /*  The following parmaeters have no corresponding control implementation
 #ifndef THROTTLE_ALT_P
 # define THROTTLE_ALT_P         0.32