Copter: support DO_LAND_REJOIN mavlink command and try rejoin first i…

…n AUTO_RTL mode switch
ArduPilot · Mar 3, 2024 · 83836ea · 83836ea
1 parent 334ffd5
commit 83836ea
Show file tree

Hide file tree

Showing 5 changed files with 97 additions and 28 deletions.
diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp
@@ -841,6 +841,12 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_int_packet(const mavlink_command_i
         return MAV_RESULT_ACCEPTED;
 
 #if MODE_AUTO_ENABLED == ENABLED
+    case MAV_CMD_DO_LAND_REJOIN:
+        if (copter.mode_auto.rejoin_landing_sequence_auto_RTL(ModeReason::GCS_COMMAND)) {
+            return MAV_RESULT_ACCEPTED;
+        }
+        return MAV_RESULT_FAILED;
+
     case MAV_CMD_DO_LAND_START:
         if (copter.mode_auto.jump_to_landing_sequence_auto_RTL(ModeReason::GCS_COMMAND)) {
             return MAV_RESULT_ACCEPTED;

diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp
@@ -276,7 +276,8 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
 #if MODE_AUTO_ENABLED == ENABLED
     if (mode == Mode::Number::AUTO_RTL) {
         // Special case for AUTO RTL, not a true mode, just AUTO in disguise
-        return mode_auto.jump_to_landing_sequence_auto_RTL(reason);
+        // Attempt to rejoin, fallback to do-land-start
+        return mode_auto.rejoin_or_jump_to_landing_sequence_auto_RTL(reason);
     }
 #endif
 
@@ -1057,3 +1058,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 };
+}
diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h
@@ -185,6 +185,9 @@ class Mode {
     virtual bool allows_weathervaning() const { return false; }
 #endif
 
+    // Return stopping point as a location with above origin alt frame
+    Location get_stopping_point() const;
+
 protected:
 
     // helper functions
@@ -544,6 +547,12 @@ class ModeAuto : public Mode {
     // Go straight to landing sequence via DO_LAND_START, if succeeds pretend to be Auto RTL mode
     bool jump_to_landing_sequence_auto_RTL(ModeReason reason);
 
+    // Rejoin mission after DO_LAND_REJOIN waypoint, if succeeds pretend to be Auto RTL mode
+    bool rejoin_landing_sequence_auto_RTL(ModeReason reason);
+
+    // Try rejoin else do land start
+    bool rejoin_or_jump_to_landing_sequence_auto_RTL(ModeReason reason);
+
     // lua accessors for nav script time support
     bool nav_script_time(uint16_t &id, uint8_t &cmd, float &arg1, float &arg2, int16_t &arg3, int16_t &arg4);
     void nav_script_time_done(uint16_t id);
@@ -580,6 +589,9 @@ class ModeAuto : public Mode {
         AllowWeatherVaning                 = (1 << 7U),
     };
 
+    // Enter auto rtl pseudo mode
+    bool enter_auto_rtl(ModeReason reason);
+
     bool start_command(const AP_Mission::Mission_Command& cmd);
     bool verify_command(const AP_Mission::Mission_Command& cmd);
     void exit_mission();

diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
@@ -219,35 +219,79 @@ bool ModeAuto::jump_to_landing_sequence_auto_RTL(ModeReason reason)
 {
 
     // Use stopping point as location for start of auto RTL
-    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);
-    Location stopping_point { stopping_point_NEU.tofloat(), Location::AltFrame::ABOVE_ORIGIN };
-
-    if (mission.jump_to_landing_sequence(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)) {
-            auto_RTL = true;
+    Location stopping_point = get_stopping_point();
+    if (!mission.jump_to_landing_sequence(stopping_point)) {
+        LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
+        // make sad noise
+        if (copter.ap.initialised) {
+            AP_Notify::events.user_mode_change_failed = 1;
+        }
+        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No landing sequence found");
+        return false;
+    }
+
+    return enter_auto_rtl(reason);
+}
+
+// Rejoin mission after DO_LAND_REJOIN waypoint, if succeeds pretend to be Auto RTL mode
+bool ModeAuto::rejoin_landing_sequence_auto_RTL(ModeReason reason)
+{
+    // Use stopping point as location for start of auto RTL
+    Location stopping_point = get_stopping_point();
+    if (!mission.jump_to_closest_mission_leg(stopping_point)) {
+        LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
+        // make sad noise
+        if (copter.ap.initialised) {
+            AP_Notify::events.user_mode_change_failed = 1;
+        }
+        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No rejoin found");
+        return false;
+    }
+
+    return enter_auto_rtl(reason);
+}
+
+// Try rejoin else do land start
+bool ModeAuto::rejoin_or_jump_to_landing_sequence_auto_RTL(ModeReason reason)
+{
+    // Use stopping point as location for start of auto RTL
+    Location stopping_point = get_stopping_point();
+    if (!mission.jump_to_closest_mission_leg(stopping_point) && !mission.jump_to_landing_sequence(stopping_point)) {
+        LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
+        // make sad noise
+        if (copter.ap.initialised) {
+            AP_Notify::events.user_mode_change_failed = 1;
+        }
+        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No rejoin or landing sequence found");
+        return false;
+    }
+
+    return enter_auto_rtl(reason);
+}
+
+// Enter auto rtl pseudo mode
+bool ModeAuto::enter_auto_rtl(ModeReason reason) 
+{
+    mission.set_force_resume(true);
+
+    // if not already in auto switch to auto
+    if ((copter.flightmode == this) || set_mode(Mode::Number::AUTO, reason)) {
+        auto_RTL = true;
 #if HAL_LOGGING_ENABLED
-            // log entry into AUTO RTL
-            copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), reason);
+        // log entry into AUTO RTL
+        copter.logger.Write_Mode((uint8_t)copter.flightmode->mode_number(), reason);
 #endif
 
-            // make happy noise
-            if (copter.ap.initialised) {
-                AP_Notify::events.user_mode_change = 1;
-            }
-            return true;
+        // make happy noise
+        if (copter.ap.initialised) {
+            AP_Notify::events.user_mode_change = 1;
         }
-        // mode change failed, revert force resume flag
-        mission.set_force_resume(false);
-
-        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed");
-    } else {
-        gcs().send_text(MAV_SEVERITY_WARNING, "Mode change to AUTO RTL failed: No landing sequence found");
+        return true;
     }
 
+    // mode change failed, revert force resume flag
+    mission.set_force_resume(false);
+
     LOGGER_WRITE_ERROR(LogErrorSubsystem::FLIGHT_MODE, LogErrorCode(Number::AUTO_RTL));
     // make sad noise
     if (copter.ap.initialised) {

diff --git a/ArduCopter/mode_rtl.cpp b/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