Plane: Move landing stages inside AP_Landing and refactor

ArduPlane/ArduPlane.cpp

@@ -865,47 +865,17 @@ void Plane::set_flight_stage(AP_Vehicle::FixedWing::FlightStage fs)
         return;
     }
 
-    switch (fs) {
+    if (fs == AP_Vehicle::FixedWing::FLIGHT_LAND) {
-    case AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH:
-        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude());
         landing.in_progress = true;
-#if GEOFENCE_ENABLED == ENABLED 
-        if (g.fence_autoenable == 1) {
-            if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
-                gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
     } else {
-                gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
-            }
-        } else if (g.fence_autoenable == 2) {
-            if (! geofence_set_floor_enabled(false)) {
-                gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
-            } else {
-                gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
-            }
-        }
-#endif
-        break;
-
-    case AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND:
-        gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm", auto_state.takeoff_altitude_rel_cm/100);
         landing.in_progress = false;
-        break;
+        if (fs == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) {
-
+            gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Landing aborted, climbing to %dm",
-    case AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE:
+                              auto_state.takeoff_altitude_rel_cm/100);
-    case AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL:
+        }
-        landing.in_progress = true;
-        break;
-
-    case AP_Vehicle::FixedWing::FLIGHT_NORMAL:
-    case AP_Vehicle::FixedWing::FLIGHT_VTOL:
-    case AP_Vehicle::FixedWing::FLIGHT_TAKEOFF:
-        landing.in_progress = false;
-        break;
     }
-    
 
     flight_stage = fs;
-    landing.stage = fs;
 
     if (should_log(MASK_LOG_MODE)) {
         Log_Write_Status();
@@ -975,10 +945,11 @@ void Plane::update_flight_stage(void)
                 } else if (landing.get_abort_throttle_enable() && channel_throttle->get_control_in() >= 90) {
                     plane.gcs_send_text(MAV_SEVERITY_INFO,"Landing aborted via throttle");
                     set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND);
-                } else if (landing.is_complete()) {
+                } else {
-                    set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND_FINAL);
+                    set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND);
-                } else if (landing.pre_flare == true) {
+                }
-                    set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_LAND_PREFLARE);
+/*
+   // FIXME: lift this into AP_Landing_Slope
                 } else if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND_APPROACH) {
                     bool heading_lined_up = abs(nav_controller->bearing_error_cd()) < 1000 && !nav_controller->data_is_stale();
                     const bool on_flight_line = fabsf(nav_controller->crosstrack_error()) < 5.0f && !nav_controller->data_is_stale();
@@ -991,7 +962,7 @@ void Plane::update_flight_stage(void)
                     } else {
                         set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
                     }
-                }
+                }*/
             } else if (quadplane.in_assisted_flight()) {
                 set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_VTOL);
             } else {
@@ -1010,6 +981,10 @@ void Plane::update_flight_stage(void)
         set_flight_stage(AP_Vehicle::FixedWing::FLIGHT_NORMAL);
     }
 
+    if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
+        landing.reset();
+    }
+
     // tell AHRS the airspeed to true airspeed ratio
     airspeed.set_EAS2TAS(barometer.get_EAS2TAS());
 }

ArduPlane/commands_logic.cpp

@@ -401,6 +401,24 @@ void Plane::do_land(const AP_Mission::Mission_Command& cmd)
 
     // zero rangefinder state, start to accumulate good samples now
     memset(&rangefinder_state, 0, sizeof(rangefinder_state));
+
+    landing.do_land(cmd, relative_altitude());
+
+#if GEOFENCE_ENABLED == ENABLED 
+    if (g.fence_autoenable == 1) {
+        if (! geofence_set_enabled(false, AUTO_TOGGLED)) {
+            gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence failed (autodisable)");
+        } else {
+            gcs_send_text(MAV_SEVERITY_NOTICE, "Fence disabled (autodisable)");
+        }
+    } else if (g.fence_autoenable == 2) {
+        if (! geofence_set_floor_enabled(false)) {
+            gcs_send_text(MAV_SEVERITY_NOTICE, "Disable fence floor failed (autodisable)");
+        } else {
+            gcs_send_text(MAV_SEVERITY_NOTICE, "Fence floor disabled (auto disable)");
+        }
+    }
+#endif
 }
 
 void Plane::loiter_set_direction_wp(const AP_Mission::Mission_Command& cmd)

ArduPlane/navigation.cpp

@@ -95,7 +95,7 @@ void Plane::calc_airspeed_errors()
 
     } else if (control_mode == AUTO && landing.in_progress) {
         // Landing airspeed target
-        target_airspeed_cm = landing.get_target_airspeed_cm(flight_stage);
+        target_airspeed_cm = landing.get_target_airspeed_cm();
 
     } else {
         // Normal airspeed target

libraries/AP_Landing/AP_Landing.cpp

@@ -141,6 +141,16 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = {
     AP_GROUPEND
 };
 
+void AP_Landing::do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude) {
+    switch (type) {
+    case TYPE_STANDARD_GLIDE_SLOPE:
+        type_slope_do_land(cmd, relative_altitude);
+        break;
+    default:
+        // a incorrect type is handled in the verify_land
+        break;
+    }
+}
 
 /*
   update navigation for landing. Called when on landing approach or

libraries/AP_Landing/AP_Landing.h

@@ -68,6 +68,7 @@ public:
 //      TODO: TYPE_HELICAL,
     };
 
+    void do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);
     bool verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
             const int32_t auto_state_takeoff_altitude_rel_cm, bool &throttle_suppressed);
     bool verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
@@ -157,10 +158,13 @@ private:
     AP_Int8 type;
 
     // Land Type STANDARD GLIDE SLOPE
-    enum slope_stage {SLOPE_APPROACH,
+    enum slope_stage {
+        SLOPE_APPROACH,
         SLOPE_PREFLARE,
-                       SLOPE_FINAL};
+        SLOPE_FINAL
+    };
     slope_stage type_slope_stage;
+    void type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude);
     void type_slope_verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed);
     bool type_slope_verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
             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);

libraries/AP_Landing/AP_Landing_Slope.cpp

@@ -21,6 +21,12 @@
 #include <GCS_MAVLink/GCS.h>
 #include <AP_HAL/AP_HAL.h>
 
+void AP_Landing::type_slope_do_land(const AP_Mission::Mission_Command& cmd, const float relative_altitude)
+{
+        type_slope_stage = SLOPE_APPROACH;
+        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Landing approach start at %.1fm", (double)relative_altitude);
+}
+
 void AP_Landing::type_slope_verify_abort_landing(const Location &prev_WP_loc, Location &next_WP_loc, bool &throttle_suppressed)
 {
     // when aborting a landing, mimic the verify_takeoff with steering hold. Once
@@ -38,6 +44,7 @@ void AP_Landing::type_slope_verify_abort_landing(const Location &prev_WP_loc, Lo
 bool AP_Landing::type_slope_verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location &current_loc,
         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)
 {
+        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, "Stage: %d Complete: %d", type_slope_stage, complete);
     // 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
@@ -325,18 +332,20 @@ int32_t AP_Landing::type_slope_get_target_airspeed_cm(void) {
 
 bool AP_Landing::type_slope_is_flaring(void) const
 {
-    return type_slope_stage == SLOPE_FINAL;
+    return in_progress && type_slope_stage == SLOPE_FINAL;
 }
 
 
 bool AP_Landing::type_slope_is_on_approach(void) const
 {
-    return (type_slope_stage == SLOPE_APPROACH ||
+    return in_progress &&
+           (type_slope_stage == SLOPE_APPROACH ||
             type_slope_stage == SLOPE_PREFLARE);
 }
 
 bool AP_Landing::type_slope_is_expecting_impact(void) const
 {
-    return (type_slope_stage == SLOPE_PREFLARE ||
+    return in_progress &&
+            (type_slope_stage == SLOPE_PREFLARE ||
             type_slope_stage == SLOPE_FINAL);
 }