AP_Landing: shadow copy of plane flight_stage into landing

.. and describe stages without using specific stage name

ArduPlane/ArduPlane.cpp

@@ -905,6 +905,7 @@ void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs)
     
 
     flight_stage = fs;
+    landing.stage = fs;
 
     if (should_log(MASK_LOG_MODE)) {
         Log_Write_Status();

libraries/AP_Landing/AP_Landing.cpp

@@ -150,23 +150,66 @@ bool AP_Landing::verify_land(const AP_Vehicle::FixedWing::FlightStage flight_sta
         const int32_t auto_state_takeoff_altitude_rel_cm, const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms, const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range, bool &throttle_suppressed)
 {
     switch (type) {
-    default:
     case TYPE_STANDARD_GLIDE_SLOPE:
         return type_slope_verify_land(flight_stage,prev_WP_loc, next_WP_loc, current_loc,
                 auto_state_takeoff_altitude_rel_cm, height,sink_rate, wp_proportion, last_flying_ms, is_armed, is_flying, rangefinder_state_in_range, throttle_suppressed);
+    default:
+        // returning TRUE while executing verify_land() will increment the
+        // mission index which in many cases will trigger an RTL for end-of-mission
+        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Landing configuration error, invalid LAND_TYPE");
+        return true;
     }
 }
 
 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, const float wp_distance, int32_t &target_altitude_offset_cm)
 {
     switch (type) {
-    default:
     case TYPE_STANDARD_GLIDE_SLOPE:
         type_slope_adjust_landing_slope_for_rangefinder_bump(rangefinder_state, prev_WP_loc, next_WP_loc, current_loc, wp_distance, target_altitude_offset_cm);
         break;
+
+    default:
+        break;
+    }
+}
+
+// return true while the aircraft should be in a flaring state
+bool AP_Landing::is_flaring(void) const
+{
+    switch (type) {
+    case TYPE_STANDARD_GLIDE_SLOPE:
+        return type_slope_is_flaring();
+
+    default:
+        return false;
+    }
+}
+
+// return true while the aircraft is performing a landing approach
+bool AP_Landing::is_on_approach(void) const
+{
+    switch (type) {
+    case TYPE_STANDARD_GLIDE_SLOPE:
+        return type_slope_is_on_approach();
+
+    default:
+        return false;
     }
 }
 
+// return true when at the last stages of a land when an impact with the ground is expected soon
+bool AP_Landing::is_expecting_impact(void) const
+{
+    switch (type) {
+    case TYPE_STANDARD_GLIDE_SLOPE:
+        return type_slope_is_expecting_impact();
+
+    default:
+        return false;
+    }
+}
+
+
 /*
   a special glide slope calculation for the landing approach
 
@@ -179,10 +222,12 @@ void AP_Landing::adjust_landing_slope_for_rangefinder_bump(AP_Vehicle::FixedWing
 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)
 {
     switch (type) {
-    default:
     case TYPE_STANDARD_GLIDE_SLOPE:
         type_slope_setup_landing_glide_slope(prev_WP_loc, next_WP_loc, current_loc, target_altitude_offset_cm);
         break;
+
+    default:
+        break;
     }
 }
 
@@ -332,9 +377,11 @@ int32_t AP_Landing::get_target_airspeed_cm(const AP_Vehicle::FixedWing::FlightSt
 bool AP_Landing::request_go_around(void)
 {
     switch (type) {
-    default:
     case TYPE_STANDARD_GLIDE_SLOPE:
         return type_slope_request_go_around();
+
+    default:
+        return false;
     }
 }
 

libraries/AP_Landing/AP_Landing.h

@@ -58,6 +58,9 @@ public:
         AP_Param::setup_object_defaults(this, var_info);
     }
 
+
+
+    // NOTE: make sure to update is_type_valid()
     enum Landing_Type {
         TYPE_STANDARD_GLIDE_SLOPE = 0,
 //      TODO: TYPE_DEEPSTALL,
@@ -71,6 +74,8 @@ public:
     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);
     bool request_go_around(void);
+    bool is_flaring(void) const;
+    bool is_on_approach(void) const;
 
 
     // helper functions
@@ -92,9 +97,10 @@ public:
     int8_t get_throttle_slewrate(void) const { return throttle_slewrate; }
     bool is_commanded_go_around(void) const { return commanded_go_around; }
     bool is_complete(void) const { return complete; }
-    void set_initial_slope() { initial_slope = slope; }
-
+    void set_initial_slope(void) { initial_slope = slope; }
+    bool is_expecting_impact(void) const;
 
+    AP_Vehicle::FixedWing::FlightStage stage;
 
     // Flag to indicate if we have landed.
     // Set land_complete if we are within 2 seconds distance or within 3 meters altitude of touchdown
@@ -163,5 +169,9 @@ private:
     void type_slope_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 type_slope_check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state);
     bool type_slope_request_go_around(void);
+    bool type_slope_is_flaring(void) const;
+    bool type_slope_is_on_approach(void) const;
+    bool type_slope_is_expecting_impact(void) const;
+
 
 };

libraries/AP_Landing/AP_Landing_Slope.cpp

@@ -37,10 +37,10 @@ bool AP_Landing::type_slope_verify_land(const AP_Vehicle::FixedWing::FlightStage
     // the altitude has been reached, restart the landing sequence
     if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
 
-        throttle_suppressed = false;
-        complete = false;
-        pre_flare = false;
-        nav_controller->update_heading_hold(get_bearing_cd(prev_WP_loc, next_WP_loc));
+    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) {
@@ -53,7 +53,7 @@ bool AP_Landing::type_slope_verify_land(const AP_Vehicle::FixedWing::FlightStage
         }
         // make sure to return false so it leaves the mission index alone
         return false;
-    }
+}
 
 
     /* Set land_complete (which starts the flare) under 3 conditions:
@@ -298,4 +298,20 @@ void AP_Landing::type_slope_setup_landing_glide_slope(const Location &prev_WP_lo
     constrain_target_altitude_location_fn(loc, prev_WP_loc);
 }
 
+bool AP_Landing::type_slope_is_flaring(void) const
+{
+    return (stage == AP_Vehicle::FixedWing::FlightStage::FLIGHT_LAND_FINAL);
+}
+
 
+bool AP_Landing::type_slope_is_on_approach(void) const
+{
+    return (stage == AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH ||
+            stage == AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE);
+}
+
+bool AP_Landing::type_slope_is_expecting_impact(void) const
+{
+    return (stage == AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE ||
+            stage == AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL);
+}