Skip to content

Rover: add support for MAV_CMD_NAV_ARC_WAYPOINT - #35099

Open
robot-to-society wants to merge 10 commits into
ArduPilot:masterfrom
robot-to-society:rover-arc-waypoint
Open

robot-to-society wants to merge 10 commits into
ArduPilot:masterfrom
robot-to-society:rover-arc-waypoint

Conversation

@robot-to-society

Copy link
Copy Markdown
Contributor

Summary

Adds Rover support for MAV_CMD_NAV_ARC_WAYPOINT (ID 36), which Copter
already supports: in Auto mode the vehicle follows a circular arc with the
commanded signed angle (p1, degrees) to the item's location instead of a
straight line. Useful for defined-radius turns, e.g. headland turns in
mowing.

Fixes #32295

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

Description

  • The arc angle is threaded from ModeAuto through
    AR_WPNav::set_desired_location() (new defaulted arc_rad /
    next_arc_rad parameters, mirroring AC_WPNav) into SCurve, which
    already supports arc segments. Other callers (Guided, RTL, SmartRTL)
    are unchanged. For this command p1 is the angle, not a loiter time,
    so the waypoint loiter is disabled.
  • Crosstrack error is measured relative to the arc, as requested in
    Rover: add Arc waypoint support #32295. Pivot turns and the next-waypoint corner angle use the arc's
    entry/exit tangents.
  • Arc speed is capped by the lateral acceleration limit (ATC_TURN_MAX_G),
    passed to SCurve as accel_c. No new parameters. SCurve reads
    accel_c only for arc segments, so straight-line behaviour is
    unchanged.
  • Pre-arm check (ground steering only): arming is refused if a mission
    arc's radius is smaller than TURN_RADIUS. Skid steering is exempt;
    the check is in the mission category and can be skipped with
    ARMING_SKIPCHK.
  • The arc centre/radius are computed in AR_WPNav (~10 lines, the same
    formulas as SCurve::calculate_track()) because the crosstrack and
    pivot logic need the geometry of the current and next legs, which
    SCurve does not expose.

Limitations

  • Obstacle avoidance does not know about arcs yet (phase 2, as discussed
    in Rover: add Arc waypoint support #32295): if OA intervenes, the rest of the leg goes straight.
  • Resuming mid-arc re-plans the arc from the stopping point with the
    full angle, same as Copter.
  • Angles outside −360°..+360° do whatever SCurve does (400° = a full
    circle plus 40°; an exact multiple of 360° = a straight line).
  • Sailboats: an arc that requires sailing upwind is not tracked (the
    tack logic steers by the bearing to the destination); the mission
    still completes. Other arcs track normally.
  • Auto missions only; there is no MAVLink message to command an arc in
    Guided.

Testing

SITL, re-run after rebasing onto current master. All PASSED:

  • New autotests: test.Rover.WPArcs (±90°/180°/270° arcs tracked within
    0.5 m of the theoretical circle, delay entries, chained arcs,
    crosstrack sign in both directions, speed cap, reverse arc, pivot
    entry), WPArcsMission (resume, DO_JUMP into an arc), WPArcsPreArm
    (tight arc refused; exemptions work), WPArcsSkid.
  • Mutation check: with the accel_c commit reverted, WPArcs and
    WPArcsSkid fail (4.23 / 3.54 m/s, over the cap); with it they pass
    (3.19 / 2.81 m/s).
  • Regressions: test.Rover.DriveMission, test.Rover.SkidSteer.
  • The same arc mission was also flown on the motorboat, motorboat-skid,
    sailboat and sailboat-motor SITL models: all completed, no failsafes
    (sailboat 90° arc within 0.6 m; see Limitations for the upwind case).
  • Manual SITL testing, including arc angles beyond ±360°. SITL and
    CubeOrange builds clean; check_branch_conventions.py passed.

AI assistance. This contribution was developed with AI assistance.
I have reviewed the changes and take full responsibility for them; the
commits carry Co-Authored-By trailers accordingly.

@github-actions

github-actions Bot commented Nov 10, 2026 •

Copy link
Copy Markdown

Global Size Summary (Total Flash delta in bytes)

Built from ef9687b rebased onto 89d9af5

Board Periph tracker blimp bootloader copter heli iofw plane rover sub
DrotekP3Pro _ _ _ * _ _ _ _ _ _
Durandal _ * * _ * * _ * +3160 *
Hitec-Airspeed * _ _ _ _ _ _ _ _ _
KakuteF7 _ * * _ * * _ * +3180 *
MambaH743v4 _ * * _ * * _ * +3628 *
MatekF405-CAN _ _ _ * _ _ _ _ _ _
MatekF405 _ * * _ * * _ * +3180 *
MatekH743-bdshot _ * * _ * * _ * +3164 *
Pixhawk1-1M _ * * _ * * _ * +3160 *
disco _ * * _ * * _ * +4264 *
f103-GPS * _ _ _ _ _ _ _ _ _

@AP-Review

AP-Review commented Nov 11, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-11-11)

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

Reviewed at head 5994bf4c69.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/35099/1.html#pr35099

This adds Rover arc waypoints with good test coverage, but a resumed arc can drive an unplanned tight loop, and the new WPArcsMission test fails in CI.

Blocking

  1. libraries/AR_WPNav/AR_WPNav.cpp:229: when an interrupted arc is resumed (e.g. HOLD then AUTO), it is re-planned from the stopping point with the full commanded angle. In SITL that drove a 189-264 degree loop on a 1-4 m circle, up to ~9 m off the planned arc. This is not the same as Copter, whose do_nav_wp/wp_start passes no arc angle and so flies straight. Either sweep only the remaining angle from the original origin, or fall back to a straight leg.
  2. Tools/autotest/rover.py:547: WPArcsMission fails in this PR's sitltest-rover job ('Arc deviation near end 0.506m exceeds 0.500m'). Local runs measure 0.49 m against the fixed 0.5 m limit, so the limits need real margin, or the deviation needs fixing.

Should fix

  • Rover/AP_Arming_Rover.cpp:273: the arc radius pre-arm check walks mission storage order and ignores DO_JUMP, so it can accept an arc whose real predecessor makes the radius below TURN_RADIUS, and can reject a safe one. Follow execution order (with bounded jump handling) and add a test.
Minor (1)
  • libraries/AR_WPNav/AR_WPNav_OA.cpp:158: after obstacle avoidance clears, the restore at AR_WPNav_OA.cpp:70 drops the arc and the rest of the leg goes straight. This is documented in the PR as a phase-2 limitation and is the safer fallback, so it is just a note.

@Hwurzburg Hwurzburg added the WikiNeeded needs wiki update label Nov 11, 2026
robot-to-society added a commit to robot-to-society/ardupilot_wiki that referenced this pull request Nov 15, 2026
Document the arc waypoint mission command on the shared mission
command page: a new MAV_CMD_NAV_ARC_WAYPOINT section (Copter and
Rover) with the parameter table, Rover notes on the arc radius,
TURN_RADIUS arming check and obstacle avoidance, and entries in the
Copter and Rover supported-command lists. Rover support for this
command is added by ArduPilot/ardupilot#35099.

Co-Authored-By: Muse Spark <noreply@anthropic.com>
robot-to-society added a commit to robot-to-society/ardupilot_wiki that referenced this pull request Nov 15, 2026
Document the arc waypoint mission command on the shared mission
command page: a new MAV_CMD_NAV_ARC_WAYPOINT section (Copter and
Rover) with the parameter table, Rover notes on the arc radius,
TURN_RADIUS arming check and obstacle avoidance, and entries in the
Copter and Rover supported-command lists. Rover support for this
command is added by ArduPilot/ardupilot#35099.

Co-Authored-By: Muse Spark <noreply@anthropic.com>
@AP-Review

AP-Review commented Nov 15, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-11-15)

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

Reviewed at head 1091326f2a.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/35099/2.html#pr35099

Since the last review the pre-arm arc scan follows execution order and an interrupted arc resumes with its remaining angle, but the new scan breaks H7 builds and the PR's own WPArcsMission test still fails. Reviewed at 1091326. The live head 749fdd1 is the same commits rebased onto master, with only a guard rename.

Blocking

  1. Rover/AP_Arming_Rover.cpp:287: the three AP_MISSION_MAX_NUM_DO_JUMP_COMMANDS arrays (also lines 299 and 338) make mission_arc_checks a 1336-byte frame on 100-jump boards. MatekH743 fails with -Werror=frame-larger-than=1300, and CI's rebased Durandal/MambaH743v4/MatekH743-bdshot builds fail too. Move the state off the stack or shrink it.
  2. Tools/autotest/rover.py:971: WPArcsMission still fails (still open), now in 'Arc resumed near its exit does not loop': after the mid-arc WP_SPEED drop to 0.3 the rover leaves the arc in a straight line and never reaches the exit. It fails every time locally. Find and fix the low-speed arc behaviour; do not just relax the test.

Should fix

  • libraries/AR_WPNav/AR_WPNav.cpp:659: resumed-arc loop (still open) when the stop is more than 2 m off the circle, or the arc is over 360 degrees (line 635). The full angle is re-planned, which in SITL gave a 276-degree loop up to 16-22 m off the arc. Fall back to a straight leg instead.
  • Rover/AP_Arming_Rover.cpp:313: the visit budget refuses valid finite lap missions. WP, a wide ARC and DO_JUMP(1,1000) is rejected with 'Mission too complex to check arcs', while -1 and 100 laps arm.
Minor (3)
  • Rover/AP_Arming_Rover.cpp:494: RTL is not treated as moving the vehicle, so an arc after an in-mission RTL is checked from the waypoint before RTL rather than home/rally. Treat its predecessor as unknown.
  • PR description: the Limitations section still says resume re-plans the full angle 'same as Copter'. That is no longer true, and Copter resumes straight.
  • libraries/AR_WPNav/AR_WPNav_OA.cpp:158: OA arc loss (still open). This is the documented phase-2 limitation, and the straight fallback is fine.

Head moved during review: observed 749fdd1b8fdef8262d9b063658ab3b65db298ded. This review covers only 1091326f2a7f612e68c24d04f6a520423ed2d14b; followup eligible.

robot-to-society and others added 9 commits November 16, 2026 01:51
The crosstrack error produced by calc_crosstrack_error() is
positive when the vehicle is on the path's left side, but three
comments said the opposite.  Fix the comments to match the
calculation; no functional change.

Co-Authored-By: Muse Spark <noreply@anthropic.com>
Pass the lateral acceleration limit (derived from
ATC_TURN_MAX_G) to SCurve::calculate_track() as the corner
acceleration, instead of the longitudinal (forward-back)
acceleration limit.

SCurve consumes this limit only for arc segments, as the
centripetal limit that caps the arc speed at
sqrt(lat_accel * radius), so straight legs are unaffected.
Arc segments need it so their speed is governed by the
lateral acceleration the vehicle can sustain rather than by
the longitudinal limit.

Co-Authored-By: Muse Spark <noreply@anthropic.com>
Rover's SCurve trajectory generator already supports arc
segments, but AR_WPNav always passed it an arc angle of zero,
so Rover could not execute MAV_CMD_NAV_ARC_WAYPOINT.

Thread the mission arc angle through set_desired_location()
to both calculate_track() calls, make the crosstrack error
relative to the arc path (as requested on ArduPilot#32295), and use
the arc's entry and exit tangents for pivot-turn decisions.

Co-Authored-By: Muse Spark <noreply@anthropic.com>
Handle the command in start_command() and verify_command()
via the existing waypoint path, passing the item's signed
arc angle (and the next item's, if it is also an arc) down
to AR_WPNav.

For this command p1 holds the arc angle in degrees, not a
loiter time, so the loiter is forced off; Rover reads the
angle at item start, so a preceding WP-delay or NAV_DELAY
item cannot lose it.

Co-Authored-By: Muse Spark <noreply@anthropic.com>
A ground-steering vehicle cannot track an arc whose radius
is smaller than TURN_RADIUS, so fail pre-arm if the stored
mission contains such an arc.  Each arc's radius is computed
from the chord to the previous destination item, found by a
linear scan of the stored mission.  Skid-steering vehicles
are exempt, as TURN_RADIUS does not apply to them.

The check runs under the MISSION arming-check category, so
ARMING_SKIPCHK can disable it like other mission checks, and
the failure message names the offending mission item.

The scan stops at the first DO_JUMP or DO_JUMP_TAG: a jump
changes the execution order, so an arc stored after a jump
cannot be measured reliably from the item stored before it,
and such arcs are not checked.  Following the full execution
order (mirroring AP_Mission's jump handling) was considered
but rejected: it needs far more code and stack than a pre-arm
check can justify on ChibiOS boards.

Co-Authored-By: Muse Spark <noreply@anthropic.com>
WPArcs covers the core geometry: 90 degree CW/CCW arcs, a
180 degree headland turn and a 270 degree arc, each checked
for radial deviation from the theoretical circle and for
the angle actually swept around its centre in the commanded
direction.  It also checks the arc-relative crosstrack
error sign for both turn directions, and the ATC_TURN_MAX_G
speed limit on a tight arc with parameters chosen so that a
longitudinal-acceleration limit would exceed the bound.

WPArcsMission covers arcs after WP-delay and NAV_DELAY
items (including an assertion that the arc item completes
promptly, guarding against the arc angle being misread as
a loiter time), chained and directly consecutive arcs, a
reverse arc (asserting the vehicle really travels
backwards), resume-after-HOLD re-planning, and DO_JUMP both
back into an arc and landing directly on an arc item.

WPArcsPreArm covers the TURN_RADIUS pre-arm check, and
WPArcsSkid covers skid steering: the lateral speed limit,
no pivot on a tangent-aligned entry (including the
transition into the item), and a required pivot onto a
misaligned entry tangent.

Co-Authored-By: Muse Spark <noreply@anthropic.com>
… arc

Add get_origin() and get_remaining_arc_angle_rad(), which returns
the part of an arc from origin to dest that remains to be driven
from the vehicle's stopping point, measured about the arc's centre.

The full angle is returned when the remaining angle cannot be
determined: the arc spans more than a whole turn, its geometry is
invalid, no stopping point is available, or the stopping point is
more than 2 m off the arc's circle (the vehicle is no longer on the
interrupted arc).  Zero is returned when the stopping point is past
the destination along the arc.  A stopping point in the sector of
the circle the arc does not cover is attributed to the closer of
the origin and the destination.

Co-Authored-By: Muse Spark <noreply@anthropic.com>
An arc waypoint command that is started again after an interruption
(e.g. leaving and re-entering AUTO mid-arc) was re-planned with its
full angle from the vehicle's position, which produced a much
smaller circle and a large excursion from the original path.

ModeAuto now remembers an arc execution that has started but not
completed (command index, origin, destination and angle).  When the
same command is started again, AR_WPNav's
get_remaining_arc_angle_rad() supplies the angle remaining from the
stopping point, so the vehicle rejoins the original circle.  A
fresh start (signalled by the helper returning the full angle)
replaces the record.

The record is cleared when the arc completes, when a different
navigation command starts (do and condition commands run alongside
the navigation command and leave it untouched), and on entering
AUTO unless the mission resumes with the arc still current.

Note that explicitly re-selecting the arc (MISSION_SET_CURRENT,
stopped or running, including after a mission reset) is
indistinguishable from a resume and also applies the remaining
angle; it flies the original circle to the same destination.

Co-Authored-By: Muse Spark <noreply@anthropic.com>
WPArcsMission gains subtests for interrupting an arc and resuming
it: mid-arc, near the exit and at the start, with a do command and
with a condition command active at the interruption, after a
MIS_RESTART (which must fly the full angle), and re-selecting a
completed arc (which must start fresh, checked from a point on the
original circle).  Resumed arcs are checked against the original
circle with a limit that accounts for the re-planned circle being
fitted through the stopping point.

Also fix two WPArcsMission checks that ran at the edge of their
limits: the waypoint-delay subtest now approaches along the arc's
entry tangent and uses a single 0.75 m limit, and the NAV_DELAY
subtest measures deviation against the circle planned from the
actual arc start (after a delay the arc is re-planned from the
stopping point) with a 2.0 m limit and a 0.75 m converged limit.
check_arc_deviation gains a converged_limit_m parameter for this.

Co-Authored-By: Muse Spark <noreply@anthropic.com>
robot-to-society added a commit to robot-to-society/ardupilot_wiki that referenced this pull request Nov 15, 2026
Document the arc waypoint mission command on the shared mission
command page: a new MAV_CMD_NAV_ARC_WAYPOINT section (Copter and
Rover) with the parameter table, Rover notes on the arc radius,
TURN_RADIUS arming check and obstacle avoidance, and entries in the
Copter and Rover supported-command lists. Rover support for this
command is added by ArduPilot/ardupilot#35099.

Co-Authored-By: Muse Spark <noreply@anthropic.com>
@AP-Review

AP-Review commented Nov 15, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-11-15)

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

Reviewed at head c203f5fe14.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/35099/3.html#pr35099

Still REQUEST CHANGES at c203f5f: the PR's own WPArcsMission still fails in CI. Since the last review the H7 stack-frame build break and the lap-mission false reject are fixed, now that the pre-arm scan stops at the first DO_JUMP.

Blocking

  1. Tools/autotest/rover.py:982: WPArcsMission (still open) fails CI's sitltest-rover: after WP_SPEED drops to 0.3 early in the arc (line 973) the rover leaves the circle and never reaches the exit. A DO_CHANGE_SPEED 0.3 before an arc triggers the same thing in an ordinary mission. Fix the low-speed arc tracking, not the test.

Should fix

  • libraries/AR_WPNav/AR_WPNav.cpp:659: full-angle resume (still open) when the stop is more than 2 m off the circle or the arc is over 360 degrees (line 635); it re-plans a much smaller loop. Fall back to a straight leg.
Minor (5)
  • Rover/AP_Arming_Rover.cpp:292: RTL predecessor (still open). An arc after an in-mission RTL is measured from the waypoint before the RTL, not home or rally. Stop the scan at RTL as for DO_JUMP.
  • Rover/AP_Arming_Rover.cpp:273: the scan starts at item 0, so synthetic home replaces current_loc as the first arc's origin. That only differs with a locked home. Start at AP_MISSION_FIRST_REAL_COMMAND.
  • PR description (still open): Limitations still says resume uses the full angle 'same as Copter', it doesn't mention that arcs after a DO_JUMP go unchecked, and it lists WPArcsMission as passing.
  • Rover/AP_Arming_Rover.cpp:277: note: arcs stored after a DO_JUMP are not checked. That's a reasonable bound, and simply continuing past the jump would falsely reject missions with forward jumps.
  • libraries/AR_WPNav/AR_WPNav.h:46: note: OA arc loss (still open), the documented phase-2 limitation. The OA restore picks up this arc_rad = 0 default.

The three other sitltest-rover failures (Offboard, CameraMission, MAVProxy_SetModeUsingSwitch) look like connection errors and were not attributed to this PR.

Re-timing the current S-Curve track after a speed limit reduction
leaves the target far behind the vehicle on an arc leg, and the
vehicle cuts across the inside of the arc trying to reach it.
Re-plan the leg from the vehicle's current location with the angle
that remains instead, falling back to re-timing when the remaining
angle cannot be determined or too little of the arc is left.

get_remaining_arc_angle_rad() is refactored to share its
calculation with the new re-plan path.

Co-Authored-By: Muse Spark <noreply@anthropic.com>
@AP-Review

Copy link
Copy Markdown

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

Reviewed at head ef9687b542.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/35099/4.html#pr35099

Still REQUEST CHANGES at ef9687b. The PR's sitltest-rover failures are caused by the new pre-arm scan, and multi-turn arcs still fail on a speed drop. Since c203f5f the WPArcsMission low-speed arc failure is fixed: the new commit re-plans the arc when its speed drops, and WPArcsMission and DO_CHANGE_SPEED-before-arc missions now complete.

Blocking

  1. Rover/AP_Arming_Rover.cpp:296: mission_arc_checks() seeds prev_loc from rover.current_loc (line 271), which is still (0,0,0) after boot. sanitize() then panics SITL ('Should not be called on invalid location'), and that is why MAVProxy_SetModeUsingSwitch, CameraMission, GripperMission and Offboard fail sitltest-rover. Return true while !prev_loc.initialised().
  2. libraries/AR_WPNav/AR_WPNav.cpp:652: the speed-drop re-plan gives up on arcs over 360 degrees and falls back to re-timing (line 890). A 540 degree arc with WP_SPEED dropped early leaves the circle by about 60 m and never completes. Count whole turns, or stop and re-plan instead of re-timing.

Should fix

  • libraries/AR_WPNav/AR_WPNav.cpp:639: full-angle resume (still open).
  • libraries/AR_WPNav/AR_WPNav.cpp:764: replan_arc_this_leg() can run after the next leg has started blending: the v²/2a guard is shorter than the jerk-limited decel window. The new arc then carries the next leg's progress and finishes early, off the exit. Skip the re-plan when _scurve_next_leg.get_time_elapsed() is non-zero.
Minor (6)
  • libraries/AR_WPNav/AR_WPNav.cpp:893: a preloaded next arc keeps its old lateral-acceleration speed cap when ATC_TURN_MAX_G is reduced, because set_speed_max() uses the cached accel_c_max.
  • Rover/AP_Arming_Rover.cpp:292: RTL predecessor (still open).
  • Rover/AP_Arming_Rover.cpp:273: item-0 start (still open). This also applies with an unlocked home that moved less than 0.5 m, not only a locked home.
  • PR description (still open): Limitations still says resume uses the full angle, and the body doesn't describe the speed-drop re-plan or its limits over 360 degrees and more than 2 m off the circle.
  • Rover/AP_Arming_Rover.cpp:277: note: arcs after a DO_JUMP are not checked, which is a reasonable bound.
  • libraries/AR_WPNav/AR_WPNav.h:46: note: OA arc loss, the documented phase-2 limitation.

The previous review's note that the other sitltest-rover failures were unrelated connection errors was wrong: they are blocker 1.

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.

Rover: add Arc waypoint support

4 participants