Skip to content

AP_Mission: rejoin DO_RETURN_PATH_START at the closest point on the track - #35060

Open
lthall wants to merge 4 commits into
ArduPilot:masterfrom
lthall:20261108_DO_RETURN_PATH_START
Open

lthall wants to merge 4 commits into
ArduPilot:masterfrom
lthall:20261108_DO_RETURN_PATH_START

Conversation

@lthall

@lthall lthall commented Nov 8, 2026 •

Copy link
Copy Markdown
Contributor

Summary

Auto RTL with DO_RETURN_PATH_START now flies to the closest point on the return path and follows the leg from there, instead of flying straight to the waypoint at the end of the closest leg.

Classification & Testing (check all that apply and add your own)

  • Checked by a human programmer
  • Non-functional change
  • No-binary change
  • Infrastructure change (e.g. unit tests, helper scripts)
  • Automated test(s) verify changes (e.g. unit test, autotest)
  • Tested manually, description below (e.g. SITL)
  • Tested on hardware
  • Logs attached
  • Logs available on request

The new Copter autotest AutoRTLReturnPathTrack starts 200 m to the side of a return path leg and checks the cross track error while the vehicle flies the leg. On master it fails with 145 m of error and with this PR it passes with 0.0 m. Its second stage puts a DO_SET_ROI_LOCATION on the return path and goes from 343 m of error to 0.2 m with the second commit. It also covers a repeated waypoint, a leg start moved during the rejoin, leaving AUTO during the rejoin, a mission without the leg end uploaded during the rejoin both in and out of AUTO, a leg ending in a landing, and a return path whose only located command is a DO_LAND_START. AutoRTL and Plane's MAV_CMD_DO_RETURN_PATH_START also pass.

Description

The rejoin search already found the closest point on each return path leg, but it only kept the index of the waypoint at the end of that leg. The vehicle then flew a straight line from wherever it was to that waypoint, often well off the planned track that DO_RETURN_PATH_START exists to keep it on. The closest point is now flown to as a virtual waypoint, using the same approach as the resume rewind's _resume_cmd. The mission then carries on from the leg's start index, so the rest of the leg runs as normal, including its DO commands, which master skipped. The rejoin index is chosen exactly as before; the new conditions only decide whether the part way point can also be used. Legs that end in a spline, arc or landing, have an altitude of 0 at either end, cross a DO_JUMP, or change altitude frame keep the old behaviour. If the mission is changed during the rejoin, or the vehicle leaves AUTO, it continues to the end of the leg instead, and if the leg end has been removed the mission restarts when AUTO is next entered, as on master. The GCS shows the leg's start waypoint as current until the rejoin point is reached.

The second commit stops located commands that are never flown to, such as DO_SET_ROI_LOCATION, from being treated as corners of the track. If the return path has no located nav commands, it still joins at the closest of those commands, as master does. This also matches the wiki, which already says the vehicle rejoins at the closest point.

@github-actions

github-actions Bot commented Nov 8, 2026 •

Copy link
Copy Markdown

Global Size Summary (Total Flash delta in bytes)

Built from 35a02bc rebased onto 89d9af5

Board Periph tracker blimp bootloader copter heli iofw plane rover sub
DrotekP3Pro _ _ _ * _ _ _ _ _ _
Durandal _ +112 +120 _ +616 +628 _ +644 +144 +140
Hitec-Airspeed * _ _ _ _ _ _ _ _ _
KakuteF7 _ +108 +120 _ +592 +592 _ +752 +156 +140
MambaH743v4 _ +124 +152 _ +868 +856 _ +828 +204 +160
MatekF405-CAN _ _ _ * _ _ _ _ _ _
MatekF405 _ +108 +120 _ +592 +592 _ +752 +156 +140
MatekH743-bdshot _ +108 +120 _ +616 +632 _ +768 +140 +144
Pixhawk1-1M _ +108 +116 _ +600 +588 _ +604 +152 +236
disco _ +240 +468 _ +1730 +1762 _ +1610 +1108 +1190
f103-GPS * _ _ _ _ _ _ _ _ _

@Hwurzburg Hwurzburg added the WikiNeeded needs wiki update label Nov 8, 2026
Comment thread libraries/AP_Mission/AP_Mission.cpp Outdated
return false;
}

} else if (set_current_cmd(landing_start_index)) {

@rmackay9 rmackay9 Nov 9, 2026 •

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this is a slightly odd restructuring. I think it makes more sense to either:

  1. move the GCS_SEND_TEXT, "_flags.in_return_path = true" and "return true" lines to where they were (e.g. a couple of lines below the "resume")

OR

  1. change the line above to look like what's below so that it's an early return
} else if (!set_current_cmd(landing_start_index)) {
    return false;
}

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks, done with your option 2. Both paths now return early on failure and share the restart if the mission had stopped.

@rmackay9

rmackay9 commented Nov 9, 2026

Copy link
Copy Markdown
Contributor

The behaviour looks quite good to me. Below are some screenshots of the new behaviour in SITL

Here's the mission which has an outgoing path (on the right) and a return path (on the left). The DO_RETURN_PATH_START command command 5 right at the top.
image

When the vehicle is switched to AutoRTL between waypoints 2 and 3 it cuts across to the return path and then proceeds home (along the path).

image

One small issue was with MAVProxy's display of the DO_RETURN_PATH_START command. It apparently tried to draw a line from 0,0,0 to WP6 (the waypoint after the DO_RETURN_PATH_START command)

@AP-Review

AP-Review commented Nov 9, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-11-09)

Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting.
Verdict: REQUEST CHANGES

Reviewed at head c7a5debf5b.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/35060/1.html#pr35060

The closest-point rejoin works in the PR's own tests, but on a leg into a landing the interpolated rejoin altitude can take a Copter down to near ground level well before the landing point, so this needs a change before merge.

Blocking

  1. AP_Mission.cpp:2925 The rejoin altitude is interpolated from the stored altitudes, but Copter flies a NAV_LAND at the current altitude and treats a waypoint alt of 0 as "hold current altitude". On a leg from a 50 m waypoint to either of these, AUTO_RTL descended to about 7 m more than 20 m short of the leg end in SITL. The merge base held 50 m. Skip the part-way rejoin, or keep the leg start's altitude, when the leg end is a landing command or either end has alt 0.

Should fix

  • AP_Mission.cpp:159 The rejoin waypoint survives mission changes. After a new upload (truncate()) or a replace_cmd while out of AUTO, resume() replaces the re-read stored command with the old _rejoin_cmd, and the vehicle flies to a point that is no longer on the mission. Clear rejoining_return_path when the mission changes, or check that the stored command still matches the leg start.
  • AP_Mission.cpp:2688 An edit during the rejoin makes restart_current_nav_cmd() call set_current_cmd() on the leg start index. That drops the rejoin and sends the vehicle back up the path to the leg start, 200 m further from home in SITL. Restart _rejoin_cmd while rejoining, or recompute the rejoin.
  • AP_Mission.cpp:2932 prev_index is only updated for a leg of non-zero length. With two coincident nav points, the rejoin keeps the first one's index, so after the rejoin point the vehicle flies back to the duplicate. Also set prev_index = temp_cmd.index when mission_vector is zero.

@lthall

lthall commented Nov 14, 2026

Copy link
Copy Markdown
Contributor Author

Blocking, rejoin altitude:

Fixed. Legs ending in a landing, or with alt 0 at either end, now rejoin at the end waypoint as before.

Rejoin survives mission changes:

Fixed. resume() only uses the rejoin point while the stored leg start still matches the one it was calculated from.

Edit during the rejoin:

Fixed. restart_current_nav_cmd() restarts the rejoin point while the leg start is unchanged.

Coincident nav points:

Fixed. prev_index is now updated for zero-length legs too.

@lthall

lthall commented Nov 14, 2026 •

Copy link
Copy Markdown
Contributor Author

Randy's Tests: Thanks for testing. The line from 0,0,0 to WP6 is a pymavlink display bug, not something this PR changes: its mission drawing starts a line at DO_RETURN_PATH_START even when the command has no location.

@lthall
lthall force-pushed the 20261108_DO_RETURN_PATH_START branch from c7a5deb to cfd324c Compare November 14, 2026 06:42
@AP-Review

AP-Review commented Nov 14, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-11-14)

Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting.
Verdict: COMMENT

Reviewed at head cfd324c87b.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/35060/2.html#pr35060

The closest-point rejoin is close to ready, with two mission-edit cases left to fix. Since the last review at c7a5deb, the landing/alt-0 altitude blocker is fixed (SITL now holds 50 m). A new upload no longer resumes the old rejoin point, a leg-end edit no longer sends the vehicle back to the leg start, and coincident waypoints no longer cause a backtrack.

Should fix

  • AP_Mission.cpp:775 Edit during the rejoin (partly fixed, still open for the leg start): if the leg start itself is edited, restart falls back to set_current_cmd() on the leg start index. The vehicle turns back up the return path to it (799 N in SITL; the merge base held 584 N). resume() at AP_Mission.cpp:166 does the same after an edit out of AUTO. Continue toward the leg end, or recompute the rejoin, when the leg start has changed.
  • AP_Mission.cpp:161 Stale rejoin point (partly fixed, still open for the leg end): resume() and restart_current_nav_cmd() only compare the leg start. After an edit to only the leg end, the old rejoin point is still flown even when it is off the new leg. Copter's change detector does trigger the restart for this edit. Also check the leg end, or recompute the rejoin.
Minor (1)
  • arducopter.py:21901 None of the four fixes (landing/alt-0 leg end, leg-start check on resume/restart, zero-length legs) has a regression test. AutoRTLReturnPathTrack only flies 50 m waypoint legs with no edit during the rejoin.

Not fully reviewed: CI for this head was still queued; Plane and QuadPlane were not flown; the leg-end-only edit and out-of-AUTO edits were checked in harnesses only.

@lthall
lthall force-pushed the 20261108_DO_RETURN_PATH_START branch from cfd324c to 56cb662 Compare November 15, 2026 23:29
@lthall

lthall commented Nov 15, 2026

Copy link
Copy Markdown
Contributor Author

Addressed the second review. If the mission changes after the rejoin point is calculated, the vehicle now drops the rejoin and continues to the leg end, as master does. This covers edits to either end of the leg, both during the rejoin and while out of AUTO. AutoRTLReturnPathTrack now also tests a duplicate waypoint, a leg start edited during the rejoin, and a leg ending in a landing.

@AP-Review

AP-Review commented Nov 16, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-11-16)

Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting.
Verdict: COMMENT

Reviewed at head 56cb662b22.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/35060/3.html#pr35060

The closest-point rejoin looks ready apart from one narrow edge case and a missing test, neither blocking. Since the last review at cfd324c, both mission-edit cases are fixed: an edit to either end of the leg, in or out of AUTO, now continues to the leg end, and this was VERIFIED in SITL.

Minor (2)
  • AP_Mission.cpp:166 resume() ignores a failed set_current_cmd(_rejoin_leg_end_index). If a shorter mission that ends at the leg start is uploaded out of AUTO during a rejoin, nothing is started and Copter hovers in AUTO (the merge base restarts the new mission). Fall back to set_current_cmd(_nav_cmd.index) or start() when it fails.
  • arducopter.py:21991 Regression test (still open, narrowed): no subtest leaves AUTO during a rejoin, edits the leg and re-enters AUTO, so the resume() change-time check is untested.

Not fully reviewed: QuadPlane was not flown, an in-AUTO edit that deletes the leg end was not flown, and one CI build job was still running.

@lthall
lthall force-pushed the 20261108_DO_RETURN_PATH_START branch from 56cb662 to d0991fe Compare November 16, 2026 13:13
@lthall

lthall commented Nov 16, 2026

Copy link
Copy Markdown
Contributor Author

AI-Review:
Addressed the third review. resume() now restarts the mission when the leg end no longer exists, as master does. AutoRTLReturnPathTrack now leaves AUTO during a rejoin, uploads a mission without the leg end and checks the new mission restarts, which also covers the resume() change time check. It fails without the fix.

@AP-Review

AP-Review commented Nov 16, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-11-16)

Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting.
Verdict: COMMENT

Reviewed at head d0991fe38e.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/35060/4.html#pr35060

The closest-point rejoin is nearly ready, with one narrow, non-blocking mission-edit case left. Since the last review at 56cb662, both minor items are fixed: resume() now restarts the mission when the leg end is gone, and AutoRTLReturnPathTrack now covers that case (VERIFIED in SITL and by mutation).

Should fix

  • AP_Mission.cpp:781 The same leg-end removal while still in AUTO: restart_current_nav_cmd() calls set_current_cmd() on the deleted leg end. That clears the return path flags before it fails, so Copter drops from AUTO_RTL to AUTO and turns back up the path to the leg start (800 N in SITL; master kept flying towards home). Check that the leg end exists first and handle it as resume() does.

Not fully reviewed: Plane and QuadPlane were not flown at this head, and CI was still running.

}
// get_next_cmd returns the target of a DO_JUMP rather than the command at scan_start
if (temp_cmd.index != scan_start) {
jumped_since_prev = true;

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this can also happen if there is a do command.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

get_next_cmd returns a DO command at its own index, so it doesn't set this. Only a DO_JUMP does, including one whose repeats are used up, and that just keeps the old rejoin at the leg end. I've reworded the comment to say that.

Comment thread libraries/AP_Mission/AP_Mission.cpp Outdated
Comment on lines +2963 to +2968
if (leg_proportion < 1 && straight_leg && !jumped_since_prev &&
!is_landing_type_cmd(temp_cmd.id) &&
!prev_loc.alt_is_zero() && !temp_cmd.content.location.alt_is_zero() &&
(temp_cmd.content.location.get_alt_frame() == frame) &&
prev_loc.get_alt_cm(frame, start_alt_cm) &&
temp_cmd.content.location.get_alt_cm(frame, end_alt_cm)) {

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This adds lots of conditions that mean we will no longer re-join where previously we would. I think the existing conditions should remain for setting the rejoin index. Then the new conditions need to only apply to the new rejoin location. Making sure to clear it each time so you can't end up with a rejoin location that does not match the index.

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The rejoin index is still set before these conditions, using the same closest-point distance as before, and rejoin_leg_start_index is cleared on every new closest leg, so the location can't mismatch the index. To make that clearer I've moved the conditions into calc_rejoin_loc(), which only decides whether the part way point can be used.

You were right that the second commit could fail where master succeeded, though. A return path whose only located commands were never flown to, such as a DO_LAND_START with a location, found no track. It now falls back to the closest of those commands as master did, and there's a subtest for it.

@lthall
lthall force-pushed the 20261108_DO_RETURN_PATH_START branch from d0991fe to 35a02bc Compare November 17, 2026 03:00
@lthall

lthall commented Nov 17, 2026

Copy link
Copy Markdown
Contributor Author

AI-Review:
Addressed the fourth review. restart_current_nav_cmd() now checks the leg end still exists before calling set_current_cmd(), so the return path flags are kept and Copter stays in AUTO_RTL as on master. AutoRTLReturnPathTrack now removes the leg end in AUTO during a rejoin and checks the mode stays AUTO_RTL without flying back up the path. It fails without the fix.

@AP-Review

Copy link
Copy Markdown

Automated review note — AI-generated (Claude+Codex), validated against the live diff. Please sanity-check before acting.
Verdict: COMMENT

Reviewed at head 35a02bcf96.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/35060/5.html#pr35060

The closest-point rejoin is close to ready, with two narrow, non-blocking cases left. Since the last review at d0991fe, the in-AUTO leg-end deletion is fixed: Copter now stays in AUTO_RTL, and AutoRTLReturnPathTrack covers it (VERIFIED in SITL and by mutation).

Should fix

  • AP_Mission.cpp:2919 A return path leg ending at a waypoint with zero lat/lng (flown as the current position) is still interpolated toward geographic 0,0. In SITL the copter flew 243 m north, past the leg start, before holding. With the guard it held where it was. Add end.content.location.lat == 0 && end.content.location.lng == 0 to the conditions that skip the part-way point.
Minor (1)
  • AP_Mission.cpp:2754 While rejoining, _nav_cmd carries the leg start index, and update() saves it for watchdog recovery. After a watchdog during a rejoin, Plane resumes at the leg start (WP 5 in SITL; master resumes the leg end, WP 6). Save _rejoin_leg_end_index while rejoining_return_path is set, as stop() does.

Not fully reviewed: QuadPlane was not flown at this head, and one CI build job was still running.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Projects

None yet

Development

Successfully merging this pull request may close these issues.

6 participants