Plane: migrate two helper functions to AP_Landing

- move functions restart_landing_sequence() and jump_to_landing_sequence() to AP_Landing
- NOTE: jump_to function can not set mode, so it is now done externally in vehicle

ArduPlane/ArduPlane.cpp

@@ -802,7 +802,10 @@ void Plane::update_navigation()
             reached_loiter_target() && 
             labs(altitude_error_cm) < 1000) {
             // we've reached the RTL point, see if we have a landing sequence
-            jump_to_landing_sequence();
+            if (landing.jump_to_landing_sequence()) {
+                // switch from RTL -> AUTO
+                set_mode(AUTO, MODE_REASON_UNKNOWN);
+            }
 
             // prevent running the expensive jump_to_landing_sequence
             // on every loop
@@ -811,7 +814,10 @@ void Plane::update_navigation()
         else if (g.rtl_autoland == 2 &&
             !landing.checked_for_autoland) {
             // Go directly to the landing sequence
-            jump_to_landing_sequence();
+            if (landing.jump_to_landing_sequence()) {
+                // switch from RTL -> AUTO
+                set_mode(AUTO, MODE_REASON_UNKNOWN);
+            }
 
             // prevent running the expensive jump_to_landing_sequence
             // on every loop

ArduPlane/GCS_Mavlink.cpp

@@ -1335,7 +1335,8 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg)
             result = MAV_RESULT_FAILED;
             
             // attempt to switch to next DO_LAND_START command in the mission
-            if (plane.jump_to_landing_sequence()) {
+            if (plane.landing.jump_to_landing_sequence()) {
+                plane.set_mode(AUTO, MODE_REASON_UNKNOWN);
                 result = MAV_RESULT_ACCEPTED;
             } 
             break;

ArduPlane/Plane.h

@@ -918,7 +918,6 @@ private:
     void disarm_if_autoland_complete();
     void setup_landing_glide_slope(void);
     void adjust_landing_slope_for_rangefinder_bump(void);
-    bool jump_to_landing_sequence(void);
     float tecs_hgt_afe(void);
     void set_nav_controller(void);
     void loiter_angle_reset(void);
@@ -1084,7 +1083,6 @@ private:
     bool verify_command_callback(const AP_Mission::Mission_Command& cmd);
     void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode);
     void run_cli(AP_HAL::UARTDriver *port);
-    bool restart_landing_sequence();
     void log_init();
     void init_capabilities(void);
     void dataflash_periodic(void);

ArduPlane/landing.cpp

@@ -27,7 +27,7 @@ bool Plane::verify_land()
         if (adjusted_relative_altitude_cm() > auto_state.takeoff_altitude_rel_cm) {
             next_WP_loc = current_loc;
             mission.stop();
-            bool success = restart_landing_sequence();
+            bool success = landing.restart_landing_sequence();
             mission.resume();
             if (!success) {
                 // on a restart failure lets RTL or else the plane may fly away with nowhere to go!
@@ -298,85 +298,7 @@ void Plane::setup_landing_glide_slope(void)
         constrain_target_altitude_location(loc, prev_WP_loc);
 }
 
-/*
-     Restart a landing by first checking for a DO_LAND_START and
-     jump there. Otherwise decrement waypoint so we would re-start
-     from the top with same glide slope. Return true if successful.
- */
-bool Plane::restart_landing_sequence()
-{
-    if (mission.get_current_nav_cmd().id != MAV_CMD_NAV_LAND) {
-        return false;
-    }
-
-    uint16_t do_land_start_index = mission.get_landing_sequence_start();
-    uint16_t prev_cmd_with_wp_index = mission.get_prev_nav_cmd_with_wp_index();
-    bool success = false;
-    uint16_t current_index = mission.get_current_nav_index();
-    AP_Mission::Mission_Command cmd;
-
-    if (mission.read_cmd_from_storage(current_index+1,cmd) &&
-            cmd.id == MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT &&
-            (cmd.p1 == 0 || cmd.p1 == 1) &&
-            mission.set_current_cmd(current_index+1))
-    {
-        // if the next immediate command is MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT to climb, do it
-        gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing sequence. Climbing to %dm", cmd.content.location.alt/100);
-        success =  true;
-    }
-    else if (do_land_start_index != 0 &&
-            mission.set_current_cmd(do_land_start_index))
-    {
-        // look for a DO_LAND_START and use that index
-        gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing via DO_LAND_START: %d",do_land_start_index);
-        success =  true;
-    }
-    else if (prev_cmd_with_wp_index != AP_MISSION_CMD_INDEX_NONE &&
-               mission.set_current_cmd(prev_cmd_with_wp_index))
-    {
-        // if a suitable navigation waypoint was just executed, one that contains lat/lng/alt, then
-        // repeat that cmd to restart the landing from the top of approach to repeat intended glide slope
-        gcs_send_text_fmt(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index);
-        success =  true;
-    } else {
-        gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Unable to restart landing sequence");
-        success =  false;
-    }
 
-    if (success) {
-        // exit landing stages if we're no longer executing NAV_LAND
-        update_flight_stage();
-    }
-    return success;
-}
-
-/* 
-   find the nearest landing sequence starting point (DO_LAND_START) and
-   switch to that mission item.  Returns false if no DO_LAND_START
-   available.
- */
-bool Plane::jump_to_landing_sequence(void) 
-{
-    uint16_t land_idx = mission.get_landing_sequence_start();
-    if (land_idx != 0) {
-        if (mission.set_current_cmd(land_idx)) {
-
-            // in case we're in RTL
-            set_mode(AUTO, MODE_REASON_UNKNOWN);
-
-            //if the mission has ended it has to be restarted
-            if (mission.state() == AP_Mission::MISSION_STOPPED) {
-                mission.resume();
-            }
-
-            gcs_send_text(MAV_SEVERITY_INFO, "Landing sequence start");
-            return true;
-        }            
-    }
-
-    gcs_send_text(MAV_SEVERITY_WARNING, "Unable to start landing sequence");
-    return false;
-}
 
 /*
   the height above field elevation that we pass to TECS

ArduPlane/system.cpp

@@ -545,7 +545,10 @@ void Plane::exit_mode(enum FlightMode mode)
 
             if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND)
             {
-                restart_landing_sequence();
+                landing.restart_landing_sequence();
+
+                // exit landing stages if we're no longer executing NAV_LAND
+                update_flight_stage();
             }
         }
         auto_state.started_flying_in_auto_ms = 0;

libraries/AP_Landing/AP_Landing.cpp

@@ -18,6 +18,7 @@
  */
 
 #include "AP_Landing.h"
+#include <GCS_MAVLink/GCS.h>
 
 // table of user settable parameters
 const AP_Param::GroupInfo AP_Landing::var_info[] = {
@@ -26,3 +27,78 @@ const AP_Param::GroupInfo AP_Landing::var_info[] = {
 };
 
 
+
+/*
+     Restart a landing by first checking for a DO_LAND_START and
+     jump there. Otherwise decrement waypoint so we would re-start
+     from the top with same glide slope. Return true if successful.
+ */
+bool AP_Landing::restart_landing_sequence()
+{
+    if (mission.get_current_nav_cmd().id != MAV_CMD_NAV_LAND) {
+        return false;
+    }
+
+    uint16_t do_land_start_index = mission.get_landing_sequence_start();
+    uint16_t prev_cmd_with_wp_index = mission.get_prev_nav_cmd_with_wp_index();
+    bool success = false;
+    uint16_t current_index = mission.get_current_nav_index();
+    AP_Mission::Mission_Command cmd;
+
+    if (mission.read_cmd_from_storage(current_index+1,cmd) &&
+            cmd.id == MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT &&
+            (cmd.p1 == 0 || cmd.p1 == 1) &&
+            mission.set_current_cmd(current_index+1))
+    {
+        // if the next immediate command is MAV_CMD_NAV_CONTINUE_AND_CHANGE_ALT to climb, do it
+        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_NOTICE, "Restarted landing sequence. Climbing to %dm", cmd.content.location.alt/100);
+        success =  true;
+    }
+    else if (do_land_start_index != 0 &&
+            mission.set_current_cmd(do_land_start_index))
+    {
+        // look for a DO_LAND_START and use that index
+        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_NOTICE, "Restarted landing via DO_LAND_START: %d",do_land_start_index);
+        success =  true;
+    }
+    else if (prev_cmd_with_wp_index != AP_MISSION_CMD_INDEX_NONE &&
+               mission.set_current_cmd(prev_cmd_with_wp_index))
+    {
+        // if a suitable navigation waypoint was just executed, one that contains lat/lng/alt, then
+        // repeat that cmd to restart the landing from the top of approach to repeat intended glide slope
+        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_NOTICE, "Restarted landing sequence at waypoint %d", prev_cmd_with_wp_index);
+        success =  true;
+    } else {
+        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "Unable to restart landing sequence");
+        success =  false;
+    }
+
+    return success;
+}
+
+
+/*
+   find the nearest landing sequence starting point (DO_LAND_START) and
+   switch to that mission item.  Returns false if no DO_LAND_START
+   available.
+ */
+bool AP_Landing::jump_to_landing_sequence(void)
+{
+    uint16_t land_idx = mission.get_landing_sequence_start();
+    if (land_idx != 0) {
+        if (mission.set_current_cmd(land_idx)) {
+
+            //if the mission has ended it has to be restarted
+            if (mission.state() == AP_Mission::MISSION_STOPPED) {
+                mission.resume();
+            }
+
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "Landing sequence start");
+            return true;
+        }
+    }
+
+    GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "Unable to start landing sequence");
+    return false;
+}
+

libraries/AP_Landing/AP_Landing.h

@@ -30,6 +30,9 @@ public:
         AP_Param::setup_object_defaults(this, var_info);
     }
 
+    bool restart_landing_sequence();
+    bool jump_to_landing_sequence(void);
+
     static const struct AP_Param::GroupInfo var_info[];
 
     // Flag to indicate if we have landed.