Plane: improved landing approach and flare

flare if we are within the specified time of landing either vertically
or horizontally

ArduPlane/ArduPlane.pde

@@ -1170,16 +1170,16 @@ static void handle_auto_mode(void)
 
     case MAV_CMD_NAV_LAND:
         calc_nav_roll();
+        calc_nav_pitch();
         
         if (auto_state.land_complete) {
             // during final approach constrain roll to the range
             // allowed for level flight
             nav_roll_cd = constrain_int32(nav_roll_cd, -g.level_roll_limit*100UL, g.level_roll_limit*100UL);
             
-            // hold pitch constant in final approach
-            nav_pitch_cd = g.land_pitch_cd;
+            // hold pitch above the specified land pitch in final approach
+            nav_pitch_cd = constrain_int32(nav_pitch_cd, g.land_pitch_cd, nav_pitch_cd);
         } else {
-            calc_nav_pitch();
             if (!airspeed.use()) {
                 // when not under airspeed control, don't allow
                 // down pitch in landing

ArduPlane/altitude.pde

@@ -28,7 +28,24 @@ static void adjust_altitude_target()
         control_mode == CRUISE) {
         return;
     }
-    if (nav_controller->reached_loiter_target() || (wp_distance <= 30) || (wp_totalDistance<=30)) {
+    if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) {
+        // in land final TECS uses TECS_LAND_SINK as a target sink
+        // 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) {
+            set_target_altitude_location(loc);
+        } else {
+            set_target_altitude_proportion(next_WP_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);
+    } 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
         set_target_altitude_location(next_WP_loc);
@@ -398,6 +415,28 @@ static int32_t adjusted_altitude_cm(void)
     return current_loc.alt - (g.alt_offset*100);
 }
 
+/*
+  return the height in meters above the next_WP_loc altitude
+ */
+static float height_above_target(void)
+{
+    float target_alt = next_WP_loc.alt*0.01;
+    if (!next_WP_loc.flags.relative_alt) {
+        target_alt -= ahrs.get_home().alt*0.01;
+    }
+
+#if AP_TERRAIN_AVAILABLE
+    // also record the terrain altitude if possible
+    float terrain_altitude;
+    if (next_WP_loc.flags.terrain_alt && 
+        terrain.height_above_terrain(terrain_altitude, true)) {
+        return terrain_altitude - target_alt;
+    }
+#endif
+
+    return adjusted_altitude_cm()*0.01f - target_alt;
+}
+
 /*
   work out target altitude adjustment from terrain lookahead
  */

ArduPlane/commands_logic.pde

@@ -378,11 +378,32 @@ static bool verify_land()
     // so we don't verify command completion. Instead we use this to
     // adjust final landing parameters
 
-    // Set land_complete if we are within 2 seconds distance or within
-    // 3 meters altitude of the landing point
-    if ((wp_distance <= (g.land_flare_sec * gps.ground_speed()))
-        || (adjusted_altitude_cm() <= next_WP_loc.alt + g.land_flare_alt*100)) {
+    float height = height_above_target();
 
+    // calculate the sink rate. 
+    float sink_rate, ground_speed;
+    Vector3f vel;
+    if (ahrs.get_velocity_NED(vel)) {
+        sink_rate = vel.z;
+        ground_speed = pythagorous2(vel.x, vel.y);
+    } else {
+        sink_rate = gps.velocity().z;
+        ground_speed = gps.ground_speed();
+    }
+    
+    /* Set land_complete (which starts the flare) under 3 conditions:
+       1) we are within LAND_FLARE_ALT meters of the landing altitude
+       2) we are within LAND_FLARE_SEC of the landing point
+          horizontally
+       3) we are within LAND_FLARE_SEC of the landing point vertically
+    */
+    if (wp_distance <= g.land_flare_sec * ground_speed || 
+        height <= g.land_flare_alt ||
+        height <= -sink_rate * g.land_flare_sec) {
+
+        if (!auto_state.land_complete) {
+            gcs_send_text_fmt(PSTR("Flare %.1fm sink=%.2f speed=%.1f"), height, sink_rate, ground_speed);
+        }
         auto_state.land_complete = true;
 
         if (gps.ground_speed() < 3) {