AP_Misison: take location in landing sequence functions, copter passes stopping point.

ArduCopter/mode.cpp

@@ -1062,3 +1062,12 @@ uint16_t Mode::get_pilot_speed_dn()
 {
     return copter.get_pilot_speed_dn();
 }
+
+// Return stopping point as a location with above origin alt frame
+Location Mode::get_stopping_point() const
+{
+    Vector3p stopping_point_NEU;
+    copter.pos_control->get_stopping_point_xy_cm(stopping_point_NEU.xy());
+    copter.pos_control->get_stopping_point_z_cm(stopping_point_NEU.z);
+    return Location { stopping_point_NEU.tofloat(), Location::AltFrame::ABOVE_ORIGIN };
+}

ArduCopter/mode.h

@@ -197,6 +197,9 @@ class Mode {
     void zero_throttle_and_hold_attitude();
     void make_safe_ground_handling(bool force_throttle_unlimited = false);
 
+    // Return stopping point as a location with above origin alt frame
+    Location get_stopping_point() const;
+
     // functions to control normal landing.  pause_descent is true if vehicle should not descend
     void land_run_horizontal_control();
     void land_run_vertical_control(bool pause_descent = false);

ArduCopter/mode_auto.cpp

@@ -216,7 +216,7 @@ bool ModeAuto::allows_weathervaning() const
 // Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
 bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason)
 {
-    if (mission.jump_to_landing_sequence()) {
+    if (mission.jump_to_landing_sequence(get_stopping_point())) {
         mission.set_force_resume(true);
         // if not already in auto switch to auto
         if ((copter.flightmode == &copter.mode_auto) || set_mode(Mode::Number::AUTO, reason)) {

ArduCopter/mode_rtl.cpp

@@ -387,10 +387,7 @@ void ModeRTL::land_run(bool disarm_on_land)
 void ModeRTL::build_path()
 {
     // origin point is our stopping point
-    Vector3p stopping_point;
-    pos_control->get_stopping_point_xy_cm(stopping_point.xy());
-    pos_control->get_stopping_point_z_cm(stopping_point.z);
-    rtl_path.origin_point = Location(stopping_point.tofloat(), Location::AltFrame::ABOVE_ORIGIN);
+    rtl_path.origin_point = get_stopping_point();
     rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME);
 
     // compute return target

ArduPlane/AP_Arming.cpp

@@ -437,7 +437,7 @@ bool AP_Arming_Plane::mission_checks(bool report)
 {
     // base checks
     bool ret = AP_Arming::mission_checks(report);
-    if (plane.mission.get_landing_sequence_start() > 0 && plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) {
+    if (plane.mission.contains_item(MAV_CMD_DO_LAND_START) && plane.g.rtl_autoland == RtlAutoland::RTL_DISABLE) {
         ret = false;
         check_failed(ARMING_CHECK_MISSION, report, "DO_LAND_START set and RTL_AUTOLAND disabled");
     }

ArduPlane/ArduPlane.cpp

@@ -701,7 +701,7 @@ bool Plane::trigger_land_abort(const float climb_to_alt_m)
         plane.is_land_command(mission_id);
     if (is_in_landing) {
         // fly a user planned abort pattern if available
-        if (plane.mission.jump_to_abort_landing_sequence()) {
+        if (plane.have_position && plane.mission.jump_to_abort_landing_sequence(plane.current_loc)) {
             return true;
         }
 

ArduPlane/GCS_Mavlink.cpp

@@ -1065,7 +1065,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in
 
     case MAV_CMD_DO_LAND_START:
         // attempt to switch to next DO_LAND_START command in the mission
-        if (plane.mission.jump_to_landing_sequence()) {
+        if (plane.have_position && plane.mission.jump_to_landing_sequence(plane.current_loc)) {
             plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND);
             return MAV_RESULT_ACCEPTED;
         }

ArduPlane/events.cpp

@@ -269,14 +269,14 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
                 already_landing = true;
             }
 #endif
-            if (!already_landing) {
+            if (!already_landing && plane.have_position) {
                 // never stop a landing if we were already committed
-                if (plane.mission.is_best_land_sequence()) {
+                if (plane.mission.is_best_land_sequence(plane.current_loc)) {
                     // continue mission as it will reach a landing in less distance
                     plane.mission.set_in_landing_sequence_flag(true);
                     break;
                 }
-                if (plane.mission.jump_to_landing_sequence()) {
+                if (plane.mission.jump_to_landing_sequence(plane.current_loc)) {
                     plane.set_mode(mode_auto, ModeReason::BATTERY_FAILSAFE);
                     break;
                 }
@@ -293,7 +293,7 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
 #endif
             if (!already_landing) {
                 // never stop a landing if we were already committed
-                if (g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START && plane.mission.is_best_land_sequence()) {
+                if ((g.rtl_autoland == RtlAutoland::RTL_IMMEDIATE_DO_LAND_START) && plane.have_position && plane.mission.is_best_land_sequence(plane.current_loc)) {
                     // continue mission as it will reach a landing in less distance
                     plane.mission.set_in_landing_sequence_flag(true);
                     break;

ArduPlane/mode_rtl.cpp

@@ -102,7 +102,7 @@ void ModeRTL::navigate()
             labs(plane.altitude_error_cm) < 1000))
             {
                 // we've reached the RTL point, see if we have a landing sequence
-                if (plane.mission.jump_to_landing_sequence()) {
+                if (plane.have_position && plane.mission.jump_to_landing_sequence(plane.current_loc)) {
                     // switch from RTL -> AUTO
                     plane.mission.set_force_resume(true);
                     if (plane.set_mode(plane.mode_auto, ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND)) {

libraries/AP_Landing/AP_Landing.cpp

@@ -474,7 +474,11 @@ bool AP_Landing::restart_landing_sequence()
         return false;
     }
 
-    uint16_t do_land_start_index = mission.get_landing_sequence_start();
+    uint16_t do_land_start_index = 0;
+    Location loc;
+    if (ahrs.get_location(loc)) {
+        do_land_start_index = mission.get_landing_sequence_start(loc);
+    }
     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();

libraries/AP_Mission/AP_Mission.cpp

@@ -2317,14 +2317,8 @@ void AP_Mission::check_eeprom_version()
 // find the nearest landing sequence starting point (DO_LAND_START) and
 // return its index.  Returns 0 if no appropriate DO_LAND_START point can
 // be found.
-uint16_t AP_Mission::get_landing_sequence_start()
+uint16_t AP_Mission::get_landing_sequence_start(const Location &current_loc)
 {
-    Location current_loc;
-
-    if (!AP::ahrs().get_location(current_loc)) {
-        return 0;
-    }
-
     const Location::AltFrame current_loc_alt_frame = current_loc.get_alt_frame();
     uint16_t landing_start_index = 0;
     float min_distance = -1;
@@ -2369,9 +2363,9 @@ uint16_t AP_Mission::get_landing_sequence_start()
    switch to that mission item.  Returns false if no DO_LAND_START
    available.
  */
-bool AP_Mission::jump_to_landing_sequence(void)
+bool AP_Mission::jump_to_landing_sequence(const Location &current_loc)
 {
-    uint16_t land_idx = get_landing_sequence_start();
+    uint16_t land_idx = get_landing_sequence_start(current_loc);
     if (land_idx != 0 && set_current_cmd(land_idx)) {
 
         //if the mission has ended it has to be restarted
@@ -2389,29 +2383,25 @@ bool AP_Mission::jump_to_landing_sequence(void)
 }
 
 // jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abort
-bool AP_Mission::jump_to_abort_landing_sequence(void)
+bool AP_Mission::jump_to_abort_landing_sequence(const Location &current_loc)
 {
-    Location current_loc;
-
     uint16_t abort_index = 0;
-    if (AP::ahrs().get_location(current_loc)) {
-        float min_distance = FLT_MAX;
+    float min_distance = FLT_MAX;
 
-        const auto count = num_commands();
-        for (uint16_t i = 1; i < count; i++) {
-            if (get_command_id(i) != uint16_t(MAV_CMD_DO_GO_AROUND)) {
-                continue;
-            }
-            Mission_Command tmp;
-            if (!read_cmd_from_storage(i, tmp)) {
-                continue;
-            }
-            if (tmp.id == MAV_CMD_DO_GO_AROUND) {
-                float tmp_distance = tmp.content.location.get_distance(current_loc);
-                if (tmp_distance < min_distance) {
-                    min_distance = tmp_distance;
-                    abort_index = i;
-                }
+    const auto count = num_commands();
+    for (uint16_t i = 1; i < count; i++) {
+        if (get_command_id(i) != uint16_t(MAV_CMD_DO_GO_AROUND)) {
+            continue;
+        }
+        Mission_Command tmp;
+        if (!read_cmd_from_storage(i, tmp)) {
+            continue;
+        }
+        if (tmp.id == MAV_CMD_DO_GO_AROUND) {
+            float tmp_distance = tmp.content.location.get_distance(current_loc);
+            if (tmp_distance < min_distance) {
+                min_distance = tmp_distance;
+                abort_index = i;
             }
         }
     }
@@ -2434,7 +2424,7 @@ bool AP_Mission::jump_to_abort_landing_sequence(void)
 }
 
 // check which is the shortest route to landing an RTL via a DO_LAND_START or continuing on the current mission plan
-bool AP_Mission::is_best_land_sequence(void)
+bool AP_Mission::is_best_land_sequence(const Location &current_loc)
 {
     // check if there is even a running mission to interupt
     if (_flags.state != MISSION_RUNNING) {
@@ -2457,19 +2447,12 @@ bool AP_Mission::is_best_land_sequence(void)
 
     // go through the mission for the nearest DO_LAND_START first as this is the most probable route
     // to a landing with the minimum number of WP.
-    uint16_t do_land_start_index = get_landing_sequence_start();
+    uint16_t do_land_start_index = get_landing_sequence_start(current_loc);
     if (do_land_start_index == 0) {
         // then no DO_LAND_START commands are in mission and normal failsafe behaviour should be maintained
         return false;
     }
 
-    // get our current location
-    Location current_loc;
-    if (!AP::ahrs().get_location(current_loc)) {
-        // we don't know where we are!!
-        return false;
-    }
-
     // get distance to landing if travelled to nearest DO_LAND_START via RTL
     float dist_via_do_land;
     if (!distance_to_landing(do_land_start_index, dist_via_do_land, current_loc)) {
@@ -2927,6 +2910,30 @@ void AP_Mission::format_conversion(uint8_t tag_byte, const Mission_Command &cmd,
 #endif
 }
 
+// Helpers to fill in location for scripting
+#if AP_SCRIPTING_ENABLED
+bool AP_Mission::jump_to_landing_sequence(void)
+{
+    Location loc;
+    if (AP::ahrs().get_location(loc)) {
+        return jump_to_landing_sequence(loc);
+    }
+    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start landing sequence");
+    return false;
+}
+
+bool AP_Mission::jump_to_abort_landing_sequence(void)
+{
+    Location loc;
+    if (AP::ahrs().get_location(loc)) {
+        return jump_to_abort_landing_sequence(loc);
+    }
+    GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Unable to start find a landing abort sequence");
+    return false;
+}
+#endif // AP_SCRIPTING_ENABLED
+
+
 // singleton instance
 AP_Mission *AP_Mission::_singleton;
 

libraries/AP_Mission/AP_Mission.h

@@ -665,18 +665,24 @@ class AP_Mission
     // find the nearest landing sequence starting point (DO_LAND_START) and
     // return its index.  Returns 0 if no appropriate DO_LAND_START point can
     // be found.
-    uint16_t get_landing_sequence_start();
+    uint16_t get_landing_sequence_start(const Location &current_loc);
 
     // 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 jump_to_landing_sequence(void);
+    bool jump_to_landing_sequence(const Location &current_loc);
 
     // jumps the mission to the closest landing abort that is planned, returns false if unable to find a valid abort
+    bool jump_to_abort_landing_sequence(const Location &current_loc);
+
+    // Scripting helpers for the above functions to fill in the location
+#if AP_SCRIPTING_ENABLED
+    bool jump_to_landing_sequence(void);
     bool jump_to_abort_landing_sequence(void);
+#endif
 
     // check which is the shortest route to landing an RTL via a DO_LAND_START or continuing on the current mission plan
-    bool is_best_land_sequence(void);
+    bool is_best_land_sequence(const Location &current_loc);
 
     // set in_landing_sequence flag
     void set_in_landing_sequence_flag(bool flag)