From 24622030b47cfd5bdef134a7cd9c58933b375206 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 27 Aug 2014 14:33:57 +1000
Subject: [PATCH] Plane: improved landing glide slope

we project a point 500m past the landing point to prevent
discontinuites close to the landing point
---
 ArduPlane/altitude.pde | 16 ++--------------
 ArduPlane/landing.pde  | 41 ++++++++++++++++++++++++++++++++++++++---
 2 files changed, 40 insertions(+), 17 deletions(-)

diff --git a/ArduPlane/altitude.pde b/ArduPlane/altitude.pde
index e8e3220097..e125486d0c 100644
--- a/ArduPlane/altitude.pde
+++ b/ArduPlane/altitude.pde
@@ -33,20 +33,7 @@ static void adjust_altitude_target()
         // rate, and ignores the target altitude
         set_target_altitude_location(next_WP_loc);
     } else if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
-        // in land approach use a linear glide slope to the flare point
-        Location loc = next_WP_loc;
-        float flare_distance = gps.ground_speed() * g.land_flare_sec;
-        loc.alt += g.land_flare_alt*100;
-        if (flare_distance >= wp_distance || 
-            flare_distance >= wp_totalDistance || 
-            location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
-            set_target_altitude_location(loc);
-        } else {
-            set_target_altitude_proportion(loc, 
-                                           (wp_distance-flare_distance) / (wp_totalDistance-flare_distance));
-        }
-        // stay within the range of the start and end locations in altitude
-        constrain_target_altitude_location(next_WP_loc, prev_WP_loc);
+        setup_landing_glide_slope();
     } else if (nav_controller->reached_loiter_target() || (wp_distance <= 30) || (wp_totalDistance<=30)) {
         // once we reach a loiter target then lock to the final
         // altitude target
@@ -330,6 +317,7 @@ static void reset_offset_altitude(void)
     target_altitude.offset_cm = 0;
 }
 
+
 /*
   reset the altitude offset used for glide slopes, based on difference
   between altitude at a destination and current altitude. If
diff --git a/ArduPlane/landing.pde b/ArduPlane/landing.pde
index ac540067a9..675acc1c3e 100644
--- a/ArduPlane/landing.pde
+++ b/ArduPlane/landing.pde
@@ -37,12 +37,13 @@ static bool verify_land()
        1) we are within LAND_FLARE_ALT meters of the landing altitude
        2) we are within LAND_FLARE_SEC of the landing point vertically
           by the calculated sink rate
-       3) we have gone past the landing point (to prevent us keeping
-          throttle on after landing if we've had positive baro drift)
+       3) we have gone past the landing point and don't have
+          rangefinder data (to prevent us keeping throttle on 
+          after landing if we've had positive baro drift)
     */
     if (height <= g.land_flare_alt ||
         height <= -auto_state.land_sink_rate * g.land_flare_sec ||
-        location_passed_point(current_loc, prev_WP_loc, next_WP_loc)) {
+        (!rangefinder_state.in_range && location_passed_point(current_loc, prev_WP_loc, next_WP_loc))) {
 
         if (!auto_state.land_complete) {
             gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"), 
@@ -80,3 +81,37 @@ static bool verify_land()
     return false;
 }
 
+
+/*
+  a special glide slope calculation for the landing approach
+
+  During the land approach use a linear glide slope to a point
+  projected through the landing point. We don't use the landing point
+  itself as that leads to discontinuities close to the landing point,
+  which can lead to erratic pitch control
+ */
+static void setup_landing_glide_slope(void)
+{
+        Location loc = next_WP_loc;
+
+        // project a poiunt 500 meters past the landing point, passing
+        // through the landing point
+        const float land_projection = 500;        
+        int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
+        float land_slope = ((next_WP_loc.alt - prev_WP_loc.alt)*0.01f) / (float)wp_totalDistance;
+        location_update(loc, land_bearing_cd*0.01f, land_projection);
+        loc.alt += land_slope * land_projection * 100;
+
+        // setup the offset_cm for set_target_altitude_proportion()
+        target_altitude.offset_cm = loc.alt - prev_WP_loc.alt;
+
+        // calculate the proportion we are to the target
+        float land_distance = get_distance(current_loc, loc);
+        float land_total_distance = get_distance(prev_WP_loc, loc);
+
+        // now setup the glide slope for landing
+        set_target_altitude_proportion(loc, land_distance / land_total_distance);
+
+        // stay within the range of the start and end locations in altitude
+        constrain_target_altitude_location(loc, prev_WP_loc);
+}