AP_landing: port the rest of plane/landing.cpp

libraries/AP_Landing/AP_Landing.cpp

@@ -19,6 +19,7 @@
 
 #include "AP_Landing.h"
 #include <GCS_MAVLink/GCS.h>
+#include <AP_HAL/AP_HAL.h>
 
 // table of user settable parameters
 const AP_Param::GroupInfo AP_Landing::var_info[] = {
@@ -110,7 +111,8 @@ bool AP_Landing::jump_to_landing_sequence(void)
   itself as that leads to discontinuities close to the landing point,
   which can lead to erratic pitch control
  */
-Location AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc)
+
+void AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location &current_loc, int32_t &target_altitude_offset_cm)
 {
     float total_distance = get_distance(prev_WP_loc, next_WP_loc);
 
@@ -184,7 +186,17 @@ Location AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, cons
     location_update(loc, land_bearing_cd*0.01f, land_projection);
     loc.alt -= slope * land_projection * 100;
 
-    return loc;
+    // 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_proportion = location_path_proportion(current_loc, prev_WP_loc, loc);
+
+    // now setup the glide slope for landing
+    set_target_altitude_proportion_fn(loc, 1.0f - land_proportion);
+
+    // stay within the range of the start and end locations in altitude
+    constrain_target_altitude_location_fn(loc, prev_WP_loc);
 }
 
 void AP_Landing::check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state)
@@ -215,3 +227,154 @@ void AP_Landing::check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder
         }
     }
 }
+
+bool AP_Landing::verify_land(AP_SpdHgtControl::FlightStage flight_stage, const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
+        int32_t auto_state_takeoff_altitude_rel_cm, float height, float sink_rate, float wp_proportion, uint32_t last_flying_ms, bool is_armed, bool is_flying, bool rangefinder_state_in_range, bool &throttle_suppressed)
+{
+    // we don't 'verify' landing in the sense that it never completes,
+    // so we don't verify command completion. Instead we use this to
+    // adjust final landing parameters
+
+    // when aborting a landing, mimic the verify_takeoff with steering hold. Once
+    // the altitude has been reached, restart the landing sequence
+    if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT) {
+
+        throttle_suppressed = false;
+
+
+        complete = false;
+        pre_flare = false;
+        nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));
+
+        // see if we have reached abort altitude
+        if (adjusted_relative_altitude_cm_fn() > auto_state_takeoff_altitude_rel_cm) {
+            next_WP_loc = current_loc;
+            mission.stop();
+            if (restart_landing_sequence()) {
+                mission.resume();
+            }
+            // else we're in AUTO with a stopped mission and handle_auto_mode() will set RTL
+        }
+        // make sure to return false so it leaves the mission index alone
+        return false;
+    }
+
+
+    /* 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 vertically
+          by the calculated sink rate (if LAND_FLARE_SEC != 0)
+       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)
+    */
+
+    // flare check:
+    // 1) below flare alt/sec requires approach stage check because if sec/alt are set too
+    //    large, and we're on a hard turn to line up for approach, we'll prematurely flare by
+    //    skipping approach phase and the extreme roll limits will make it hard to line up with runway
+    // 2) passed land point and don't have an accurate AGL
+    // 3) probably crashed (ensures motor gets turned off)
+
+    bool on_approach_stage = (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH ||
+                              flight_stage == AP_SpdHgtControl::FLIGHT_LAND_PREFLARE);
+    bool below_flare_alt = (height <= aparm.land_flare_alt);
+    bool below_flare_sec = (aparm.land_flare_sec > 0 && height <= sink_rate * aparm.land_flare_sec);
+    bool probably_crashed = (aparm.crash_detection_enable && fabsf(sink_rate) < 0.2f && !is_flying);
+
+    if ((on_approach_stage && below_flare_alt) ||
+        (on_approach_stage && below_flare_sec && (wp_proportion > 0.5)) ||
+        (!rangefinder_state_in_range && wp_proportion >= 1) ||
+        probably_crashed) {
+
+        if (!complete) {
+            post_stats = true;
+            if (is_flying && (AP_HAL::millis()-last_flying_ms) > 3000) {
+                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Flare crash detected: speed=%.1f", (double)ahrs.get_gps().ground_speed());
+            } else {
+                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Flare %.1fm sink=%.2f speed=%.1f dist=%.1f",
+                                  (double)height, (double)sink_rate,
+                                  (double)ahrs.get_gps().ground_speed(),
+                                  (double)get_distance(current_loc, next_WP_loc));
+            }
+            complete = true;
+            update_flight_stage_fn();
+        }
+
+
+        if (ahrs.get_gps().ground_speed() < 3) {
+            // reload any airspeed or groundspeed parameters that may have
+            // been set for landing. We don't do this till ground
+            // speed drops below 3.0 m/s as otherwise we will change
+            // target speeds too early.
+            aparm.airspeed_cruise_cm.load();
+            aparm.min_gndspeed_cm.load();
+            aparm.throttle_cruise.load();
+        }
+    } else if (!complete && !pre_flare && aparm.land_pre_flare_airspeed > 0) {
+        bool reached_pre_flare_alt = aparm.land_pre_flare_alt > 0 && (height <= aparm.land_pre_flare_alt);
+        bool reached_pre_flare_sec = aparm.land_pre_flare_sec > 0 && (height <= sink_rate * aparm.land_pre_flare_sec);
+        if (reached_pre_flare_alt || reached_pre_flare_sec) {
+            pre_flare = true;
+            update_flight_stage_fn();
+        }
+    }
+
+    /*
+      when landing we keep the L1 navigation waypoint 200m ahead. This
+      prevents sudden turns if we overshoot the landing point
+     */
+    struct Location land_WP_loc = next_WP_loc;
+    int32_t land_bearing_cd = get_bearing_cd(prev_WP_loc, next_WP_loc);
+    location_update(land_WP_loc,
+                    land_bearing_cd*0.01f,
+                    get_distance(prev_WP_loc, current_loc) + 200);
+    nav_controller->update_waypoint(prev_WP_loc, land_WP_loc);
+
+    // once landed and stationary, post some statistics
+    // this is done before disarm_if_autoland_complete() so that it happens on the next loop after the disarm
+    if (post_stats && !is_armed) {
+        post_stats = false;
+        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Distance from LAND point=%.2fm", (double)get_distance(current_loc, next_WP_loc));
+    }
+
+    // check if we should auto-disarm after a confirmed landing
+    disarm_if_autoland_complete_fn();
+
+    /*
+      we return false as a landing mission item never completes
+
+      we stay on this waypoint unless the GCS commands us to change
+      mission item, reset the mission, command a go-around or finish
+      a land_abort procedure.
+     */
+    return false;
+}
+
+void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc, float wp_distance, int32_t &target_altitude_offset_cm)
+{
+    // check the rangefinder correction for a large change. When found, recalculate the glide slope. This is done by
+    // determining the slope from your current location to the land point then following that back up to the approach
+    // altitude and moving the prev_wp to that location. From there
+    float correction_delta = fabsf(rangefinder_state.last_stable_correction) - fabsf(rangefinder_state.correction);
+
+    if (aparm.land_slope_recalc_shallow_threshold <= 0 ||
+            fabsf(correction_delta) < aparm.land_slope_recalc_shallow_threshold) {
+        return;
+    }
+
+    rangefinder_state.last_stable_correction = rangefinder_state.correction;
+
+    float corrected_alt_m = (adjusted_altitude_cm_fn() - next_WP_loc.alt)*0.01f - rangefinder_state.correction;
+    float total_distance_m = get_distance(prev_WP_loc, next_WP_loc);
+    float top_of_glide_slope_alt_m = total_distance_m * corrected_alt_m / wp_distance;
+    prev_WP_loc.alt = top_of_glide_slope_alt_m*100 + next_WP_loc.alt;
+
+    // re-calculate auto_state.land_slope with updated prev_WP_loc
+    setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm);
+
+    check_if_need_to_abort(rangefinder_state);
+}
+
+
+

libraries/AP_Landing/AP_Landing.h

@@ -20,6 +20,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_SpdHgtControl/AP_SpdHgtControl.h>
 #include <AP_Vehicle/AP_Vehicle.h>
+#include <AP_Navigation/AP_Navigation.h>
 
 /// @class  AP_Landing
 /// @brief  Class managing ArduPlane landing methods
@@ -27,19 +28,54 @@ class AP_Landing
 {
 public:
 
+    FUNCTOR_TYPEDEF(set_target_altitude_proportion_fn_t, void, const Location&, float);
+    set_target_altitude_proportion_fn_t set_target_altitude_proportion_fn;
+
+    FUNCTOR_TYPEDEF(constrain_target_altitude_location_fn_t, void, const Location&, const Location&);
+    constrain_target_altitude_location_fn_t constrain_target_altitude_location_fn;
+
+    FUNCTOR_TYPEDEF(adjusted_altitude_cm_fn_t, int32_t);
+    adjusted_altitude_cm_fn_t adjusted_altitude_cm_fn;
+
+    FUNCTOR_TYPEDEF(adjusted_relative_altitude_cm_fn_t, int32_t);
+    adjusted_relative_altitude_cm_fn_t adjusted_relative_altitude_cm_fn;
+
+    FUNCTOR_TYPEDEF(disarm_if_autoland_complete_fn_t, void);
+    disarm_if_autoland_complete_fn_t disarm_if_autoland_complete_fn;
+
+    FUNCTOR_TYPEDEF(update_flight_stage_fn_t, void);
+    update_flight_stage_fn_t update_flight_stage_fn;
+
     // constructor
-    AP_Landing(AP_Mission &_mission, AP_AHRS &_ahrs, AP_SpdHgtControl *_SpdHgt_Controller, const AP_Vehicle::FixedWing &_aparm) :
+    AP_Landing(AP_Mission &_mission, AP_AHRS &_ahrs, AP_SpdHgtControl *_SpdHgt_Controller, AP_Navigation *_nav_controller, AP_Vehicle::FixedWing &_aparm,
-            mission(_mission),
+            set_target_altitude_proportion_fn_t _set_target_altitude_proportion_fn,
-            ahrs(_ahrs),
+            constrain_target_altitude_location_fn_t _constrain_target_altitude_location_fn,
-            SpdHgt_Controller(_SpdHgt_Controller),
+            adjusted_altitude_cm_fn_t _adjusted_altitude_cm_fn,
-            aparm(_aparm) {
+            adjusted_relative_altitude_cm_fn_t _adjusted_relative_altitude_cm_fn,
+            disarm_if_autoland_complete_fn_t _disarm_if_autoland_complete_fn,
+            update_flight_stage_fn_t _update_flight_stage_fn):
+            mission(_mission)
+            ,ahrs(_ahrs)
+            ,SpdHgt_Controller(_SpdHgt_Controller)
+            ,nav_controller(_nav_controller)
+            ,aparm(_aparm)
+            ,set_target_altitude_proportion_fn(_set_target_altitude_proportion_fn)
+            ,constrain_target_altitude_location_fn(_constrain_target_altitude_location_fn)
+            ,adjusted_altitude_cm_fn(_adjusted_altitude_cm_fn)
+            ,adjusted_relative_altitude_cm_fn(_adjusted_relative_altitude_cm_fn)
+            ,disarm_if_autoland_complete_fn(_disarm_if_autoland_complete_fn)
+            ,update_flight_stage_fn(_update_flight_stage_fn) {
         AP_Param::setup_object_defaults(this, var_info);
     }
 
     bool restart_landing_sequence();
     bool jump_to_landing_sequence(void);
+    bool verify_land(AP_SpdHgtControl::FlightStage flight_stage, const Location &prev_WP_loc, Location &next_WP_loc,const  Location &current_loc,
+            int32_t auto_state_takeoff_altitude_rel_cm, float height, float sink_rate, float wp_proportion, uint32_t last_flying_ms, bool is_armed, bool is_flying, bool rangefinder_state_in_range, bool &throttle_suppressed);
 
-    Location setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc);
+    void adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state, Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc, float wp_distance, int32_t &target_altitude_offset_cm);
+
+    void setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc, const Location &current_loc, int32_t &target_altitude_offset_cm);
     void check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state);
 
     static const struct AP_Param::GroupInfo var_info[];
@@ -80,7 +116,7 @@ private:
     AP_Mission &mission;
     AP_AHRS &ahrs;
     AP_SpdHgtControl *SpdHgt_Controller;
+    AP_Navigation *nav_controller;
 
-    const AP_Vehicle::FixedWing &aparm;
+    AP_Vehicle::FixedWing &aparm;
-
 };