Plane: added GLIDE_SLOPE_MIN parameter

this is used to prevent slow glide slopes being used for small
altitude changes in missions. This allows more accurate tracking of
altitude with terrain changes

ArduPlane/ArduPlane.pde

@@ -1392,10 +1392,14 @@ static void update_navigation()
     }
 }
 
-static void update_flight_stage(AP_SpdHgtControl::FlightStage fs) {
+/*
+  set the flight stage
+ */
+static void set_flight_stage(AP_SpdHgtControl::FlightStage fs) 
+{
     //if just now entering land flight stage
     if (fs == AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
-            flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
+        flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
 
 #if GEOFENCE_ENABLED == ENABLED 
         if (g.fence_autoenable == 1) {
@@ -1406,7 +1410,6 @@ static void update_flight_stage(AP_SpdHgtControl::FlightStage fs) {
             }
         }
 #endif
-
     }
     
     flight_stage = fs;
@@ -1421,21 +1424,29 @@ static void update_alt()
 
     geofence_check(true);
 
+    update_flight_stage();
+}
+
+/*
+  recalculate the flight_stage
+ */
+static void update_flight_stage(void)
+{
     // Update the speed & height controller states
     if (auto_throttle_mode && !throttle_suppressed) {        
         if (control_mode==AUTO) {
             if (auto_state.takeoff_complete == false) {
-                update_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF);
+                set_flight_stage(AP_SpdHgtControl::FLIGHT_TAKEOFF);
             } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND && 
                        auto_state.land_complete == true) {
-                update_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL);
+                set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_FINAL);
             } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
-                update_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH); 
+                set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_APPROACH); 
             } else {
-                update_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
+                set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
             }
         } else {
-            update_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
+            set_flight_stage(AP_SpdHgtControl::FLIGHT_NORMAL);
         }
 
         SpdHgt_Controller->update_pitch_throttle(relative_target_altitude_cm(),

ArduPlane/Parameters.h

@@ -119,6 +119,7 @@ public:
         k_param_terrain,
         k_param_terrain_follow,
         k_param_stab_pitch_down_cd,
+        k_param_glide_slope_threshold,
 
         // 100: Arming parameters
         k_param_arming = 100,
@@ -452,6 +453,7 @@ public:
 #if AP_TERRAIN_AVAILABLE
     AP_Int8 terrain_follow;
 #endif
+    AP_Int16 glide_slope_threshold;
 
     // RC channels
     RC_Channel rc_1;

ArduPlane/Parameters.pde

@@ -113,6 +113,15 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Advanced
     GSCALAR(stab_pitch_down_cd, "STAB_PITCH_DN_CD",   200),
 
+    // @Param: GLIDE_SLOPE_MIN
+    // @DisplayName: Glide slope threshold
+    // @Description: This controls the minimum altitude change for a waypoint before a glide slope will be used instead of an immediate altitude change. The default value is 15 meters, which helps to smooth out waypoint missions where small altitude changes happen near waypoints. If you don't want glide slopes to be used in missions then you can set this to zero, which will disable glide slope calculations. Otherwise you can set it to a minimum number of meters of altitude error to the destination waypoint before a glide slope will be used to change altitude.
+    // @Range: 0 1000
+    // @Increment: 1
+    // @Units: meters
+    // @User: Advanced
+    GSCALAR(glide_slope_threshold, "GLIDE_SLOPE_MIN", 15),
+
     // @Param: STICK_MIXING
     // @DisplayName: Stick Mixing
     // @Description: When enabled, this adds user stick input to the control surfaces in auto modes, allowing the user to have some degree of flight control without changing modes.  There are two types of stick mixing available. If you set STICK_MIXING to 1 then it will use "fly by wire" mixing, which controls the roll and pitch in the same way that the FBWA mode does. This is the safest option if you usually fly ArduPlane in FBWA or FBWB mode. If you set STICK_MIXING to 2 then it will enable direct mixing mode, which is what the STABILIZE mode uses. That will allow for much more extreme maneuvers while in AUTO mode.

ArduPlane/altitude.pde

@@ -325,13 +325,17 @@ static void set_offset_altitude_location(const Location &loc)
     }
 #endif
 
-    // if we are within 15 meters of the target altitude then reset
-    // the offset to not use a glide slope. This allows for more
-    // accurate flight of missions where the aircraft may lose or gain
-    // a bit of altitude near waypoint turn points due to local
-    // terrain changes
-    if (labs(target_altitude.offset_cm) < 1500) {
-        target_altitude.offset_cm = 0;
+    if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
+        flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
+        // if we are within GLIDE_SLOPE_MIN meters of the target altitude
+        // then reset the offset to not use a glide slope. This allows for
+        // more accurate flight of missions where the aircraft may lose or
+        // gain a bit of altitude near waypoint turn points due to local
+        // terrain changes
+        if (g.glide_slope_threshold <= 0 ||
+            labs(target_altitude.offset_cm)*0.01f < g.glide_slope_threshold) {
+            target_altitude.offset_cm = 0;
+        }
     }
 }
 

ArduPlane/commands.pde

@@ -79,6 +79,7 @@ static void set_guided_WP(void)
     // -----------------------------------------------
     set_target_altitude_current();
 
+    update_flight_stage();
     setup_glide_slope();
     setup_turn_angle();
 

ArduPlane/commands_logic.pde

@@ -261,6 +261,7 @@ static void do_RTL(void)
         loiter.direction = 1;
     }
 
+    update_flight_stage();
     setup_glide_slope();
     setup_turn_angle();
 
@@ -643,6 +644,7 @@ static void exit_mission_callback()
             rally.calc_best_rally_or_home_location(current_loc, get_RTL_altitude());
         auto_rtl_command.id = MAV_CMD_NAV_LOITER_UNLIM;
         setup_terrain_target_alt(auto_rtl_command.content.location);
+        update_flight_stage();
         setup_glide_slope();
         setup_turn_angle();
         start_command(auto_rtl_command);