AP_Landing: port more from plane

ArduPlane/ArduPlane.cpp

@@ -972,7 +972,7 @@ void Plane::update_flight_stage(void)
             } else if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
 
                 if ((g.land_abort_throttle_enable && channel_throttle->get_control_in() >= 90) ||
-                        auto_state.commanded_go_around ||
+                        landing.commanded_go_around ||
                         flight_stage == AP_SpdHgtControl::FLIGHT_LAND_ABORT){
                     // abort mode is sticky, it must complete while executing NAV_LAND
                     set_flight_stage(AP_SpdHgtControl::FLIGHT_LAND_ABORT);

ArduPlane/GCS_Mavlink.cpp

@@ -1360,7 +1360,7 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
                 if (!is_zero(packet.param1)) {
                     plane.auto_state.takeoff_altitude_rel_cm = packet.param1 * 100;
                 }
-                plane.auto_state.commanded_go_around = true;
+                plane.landing.commanded_go_around = true;
                
                 result = MAV_RESULT_ACCEPTED;
                 plane.gcs_send_text(MAV_SEVERITY_INFO,"Go around command accepted");

ArduPlane/Plane.h

@@ -472,9 +472,6 @@ private:
         // in FBWA taildragger takeoff mode
         bool fbwa_tdrag_takeoff_mode:1;
 
-        // denotes if a go-around has been commanded for landing
-        bool commanded_go_around:1;
-
         // Altitude threshold to complete a takeoff command in autonomous modes.  Centimeters
         // are we in idle mode? used for balloon launch to stop servo
         // movement until altitude is reached

ArduPlane/commands_logic.cpp

@@ -25,7 +25,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         auto_state.takeoff_complete = true;
 
         // if a go around had been commanded, clear it now.
-        auto_state.commanded_go_around = false;
+        landing.commanded_go_around = false;
 
         // start non-idle
         auto_state.idle_mode = false;
@@ -145,7 +145,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
 
     case MAV_CMD_DO_LAND_START:
         //ensure go around hasn't been set
-        auto_state.commanded_go_around = false;
+        landing.commanded_go_around = false;
         break;
 
     case MAV_CMD_DO_FENCE_ENABLE:
@@ -383,7 +383,7 @@ void Plane::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 
 void Plane::do_land(const AP_Mission::Mission_Command& cmd)
 {
-    auto_state.commanded_go_around = false;
+    landing.commanded_go_around = false;
     set_next_WP(cmd.content.location);
 
     // configure abort altitude and pitch

ArduPlane/landing.cpp

@@ -157,31 +157,7 @@ void Plane::adjust_landing_slope_for_rangefinder_bump(void)
     // re-calculate auto_state.land_slope with updated prev_WP_loc
     setup_landing_glide_slope();
 
-    if (rangefinder_state.correction >= 0) { // we're too low or object is below us
+    landing.check_if_need_to_abort(rangefinder_state);
-        // correction positive means we're too low so we should continue on with
-        // the newly computed shallower slope instead of pitching/throttling up
-
-    } else if (aparm.land_slope_recalc_steep_threshold_to_abort > 0) {
-        // correction negative means we're too high and need to point down (and speed up) to re-align
-        // to land on target. A large negative correction means we would have to dive down a lot and will
-        // generating way too much speed that we can not bleed off in time. It is better to remember
-        // the large baro altitude offset and abort the landing to come around again with the correct altitude
-        // offset and "perfect" slope.
-
-        // calculate projected slope with projected alt
-        float new_slope_deg = degrees(atan(landing.slope));
-        float initial_slope_deg = degrees(atan(landing.initial_slope));
-
-        // is projected slope too steep?
-        if (new_slope_deg - initial_slope_deg > aparm.land_slope_recalc_steep_threshold_to_abort) {
-            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)",
-                                             (double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg));
-            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!");
-            landing.alt_offset = rangefinder_state.correction;
-            auto_state.commanded_go_around = 1;
-            aparm.land_slope_recalc_steep_threshold_to_abort = 0; // disable this feature so we only perform it once
-        }
-    }
 }
 
 /*

ArduPlane/system.cpp

@@ -338,7 +338,7 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
     landing.checked_for_autoland = false;
 
     // reset go around command
-    auto_state.commanded_go_around = false;
+    landing.commanded_go_around = false;
 
     // not in pre-flare
     landing.pre_flare = false;

libraries/AP_Landing/AP_Landing.cpp

@@ -187,4 +187,31 @@ Location AP_Landing::setup_landing_glide_slope(const Location &prev_WP_loc, cons
     return loc;
 }
 
+void AP_Landing::check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state)
+{
+    if (rangefinder_state.correction >= 0) { // we're too low or object is below us
+        // correction positive means we're too low so we should continue on with
+        // the newly computed shallower slope instead of pitching/throttling up
 
+    } else if (aparm.land_slope_recalc_steep_threshold_to_abort > 0 && !has_aborted_due_to_slope_recalc) {
+        // correction negative means we're too high and need to point down (and speed up) to re-align
+        // to land on target. A large negative correction means we would have to dive down a lot and will
+        // generating way too much speed that we can not bleed off in time. It is better to remember
+        // the large baro altitude offset and abort the landing to come around again with the correct altitude
+        // offset and "perfect" slope.
+
+        // calculate projected slope with projected alt
+        float new_slope_deg = degrees(atan(slope));
+        float initial_slope_deg = degrees(atan(initial_slope));
+
+        // is projected slope too steep?
+        if (new_slope_deg - initial_slope_deg > aparm.land_slope_recalc_steep_threshold_to_abort) {
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Steep landing slope (%.0fm %.1fdeg)",
+                                             (double)rangefinder_state.correction, (double)(new_slope_deg - initial_slope_deg));
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "aborting landing!");
+            alt_offset = rangefinder_state.correction;
+            commanded_go_around = true;
+            has_aborted_due_to_slope_recalc = true; // only allow this once.
+        }
+    }
+}

libraries/AP_Landing/AP_Landing.h

@@ -19,6 +19,7 @@
 #include <AP_Mission/AP_Mission.h>
 #include <AP_Common/AP_Common.h>
 #include <AP_SpdHgtControl/AP_SpdHgtControl.h>
+#include <AP_Vehicle/AP_Vehicle.h>
 
 /// @class  AP_Landing
 /// @brief  Class managing ArduPlane landing methods
@@ -39,6 +40,7 @@ public:
     bool jump_to_landing_sequence(void);
 
     Location setup_landing_glide_slope(const Location &prev_WP_loc, const Location &next_WP_loc);
+    void check_if_need_to_abort(const AP_Vehicle::FixedWing::Rangefinder_State &rangefinder_state);
 
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -67,8 +69,14 @@ public:
     // have we checked for an auto-land?
     bool checked_for_autoland;
 
+    // denotes if a go-around has been commanded for landing
+    bool commanded_go_around;
+
+
 private:
 
+    bool has_aborted_due_to_slope_recalc;
+
     AP_Mission &mission;
     AP_AHRS &ahrs;
     AP_SpdHgtControl *SpdHgt_Controller;