From f048aafb76bd1baa0412dcaf7526949205888648 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <magicrub@gmail.com>
Date: Fri, 13 May 2016 23:03:22 -0700
Subject: [PATCH] Plane: store auto land slope

- also changed order of landing slope calc but is functionally the same
---
 ArduPlane/Plane.h            |  3 +++
 ArduPlane/commands_logic.cpp |  2 ++
 ArduPlane/landing.cpp        | 25 +++++++++++++++----------
 3 files changed, 20 insertions(+), 10 deletions(-)

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index b9745e3f90..77f0f537e1 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -507,6 +507,9 @@ private:
         // time stamp of when we start flying while in auto mode in milliseconds
         uint32_t started_flying_in_auto_ms;
 
+        // calculated approach slope during auto-landing: ((prev_WP_loc.alt - next_WP_loc.alt)*0.01f - aparm.land_flare_sec * sink_rate) / get_distance(prev_WP_loc, next_WP_loc)
+        float land_slope;
+
         // barometric altitude at start of takeoff
         float baro_takeoff_alt;
 
diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp
index 401b741d10..42b60078e9 100644
--- a/ArduPlane/commands_logic.cpp
+++ b/ArduPlane/commands_logic.cpp
@@ -393,6 +393,8 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
         auto_state.takeoff_pitch_cd = 1000;
     }
 
+    auto_state.land_slope = 0;
+
 #if RANGEFINDER_ENABLED == ENABLED
     // zero rangefinder state, start to accumulate good samples now
     memset(&rangefinder_state, 0, sizeof(rangefinder_state));
diff --git a/ArduPlane/landing.cpp b/ArduPlane/landing.cpp
index b2dd5ecdfc..57b28e90b8 100644
--- a/ArduPlane/landing.cpp
+++ b/ArduPlane/landing.cpp
@@ -196,12 +196,6 @@ void Plane::adjust_landing_slope_for_rangefinder_bump(void)
  */
 void Plane::setup_landing_glide_slope(void)
 {
-        Location loc = next_WP_loc;
-
-        // project a point 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 total_distance = get_distance(prev_WP_loc, next_WP_loc);
 
         // If someone mistakenly puts all 0's in their LAND command then total_distance
@@ -239,6 +233,14 @@ void Plane::setup_landing_glide_slope(void)
             aim_height = g.land_flare_alt*2;
         }
 
+        // calculate slope to landing point
+        bool is_first_calc = is_zero(auto_state.land_slope);
+        auto_state.land_slope = (sink_height - aim_height) / total_distance;
+        if (is_first_calc) {
+            gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing glide slope %.1f degrees", (double)degrees(atanf(auto_state.land_slope)));
+        }
+
+
         // time before landing that we will flare
         float flare_time = aim_height / SpdHgt_Controller->get_land_sinkrate();
 
@@ -251,17 +253,20 @@ void Plane::setup_landing_glide_slope(void)
             flare_distance = total_distance/2;
         }
 
+        // project a point 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);
+
         // now calculate our aim point, which is before the landing
         // point and above it
+        Location loc = next_WP_loc;
         location_update(loc, land_bearing_cd*0.01f, -flare_distance);
         loc.alt += aim_height*100;
 
-        // calculate slope to landing point
-        float land_slope = (sink_height - aim_height) / total_distance;
-
         // calculate point along that slope 500m ahead
         location_update(loc, land_bearing_cd*0.01f, land_projection);
-        loc.alt -= land_slope * land_projection * 100;
+        loc.alt -= auto_state.land_slope * land_projection * 100;
 
         // setup the offset_cm for set_target_altitude_proportion()
         target_altitude.offset_cm = loc.alt - prev_WP_loc.alt;