Skip to content

Copter, AP_Mission: fly arcs only from a defined start and fix lookahead after a jump to a DO command - #34713

Open
lthall wants to merge 2 commits into
ArduPilot:masterfrom
lthall:20261011_Arc_Not_Streight_Fix
Open

lthall wants to merge 2 commits into
ArduPilot:masterfrom
lthall:20261011_Arc_Not_Streight_Fix

Conversation

@lthall

@lthall lthall commented Oct 11, 2026 •

Copy link
Copy Markdown
Contributor

Summary

Fixes ARC_WAYPOINTs being flown as straight lines when the previous leg did not preload them.

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

WPArcFlownAsArc fails without the Copter commit (8 of its 10 arcs are flown straight) and passes with it. WPArcs, WPArcs2, JumpNextNavCmd, SplineLastWaypoint and SplineNavDelay still pass.

Description

An arc waypoint is now always flown as an arc, the same way a spline waypoint is always flown as a spline. The firmware no longer overrides the waypoint type the user commanded. Previously an arc only curved when the previous leg preloaded it. As a result, arcs after a takeoff, a hold, a delay, a loiter or LOITER_TURNS, or set current, were flown straight to their end.

do_nav_wp() now passes the arc angle to the waypoint controller. The leg starts from the vehicle's current location in the same way as a normal waypoint: from the previous destination if it was reached, otherwise from the stopping point. That covers arcs reached by a DO_JUMP, mission start, resume or set current. The test also checks that an arc reached by a DO_JUMP still curves.

@github-actions

github-actions Bot commented Oct 11, 2026 •

Copy link
Copy Markdown

Global Size Summary (Total Flash delta in bytes)

Built from 198f6d2 rebased onto 2a54162

Board Periph tracker blimp bootloader copter heli iofw plane rover sub
DrotekP3Pro _ _ _ * _ _ _ _ _ _
Durandal _ +224 +228 _ +452 +448 _ +232 +228 +228
Hitec-Airspeed * _ _ _ _ _ _ _ _ _
KakuteF7 _ +92 +224 _ +440 +440 _ +228 +224 +92
MambaH743v4 _ +192 +212 _ +560 +568 _ +232 +212 +192
MatekF405-CAN _ _ _ * _ _ _ _ _ _
MatekF405 _ +92 +224 _ +440 +440 _ +228 +224 +92
MatekH743-bdshot _ +224 +228 _ +452 +456 _ +232 +228 +228
Pixhawk1-1M _ +92 +224 _ +452 +444 _ +228 +224 +92
disco _ +368 +638 _ +860 +864 _ +548 +630 +376
f103-GPS * _ _ _ _ _ _ _ _ _

@lthall
lthall force-pushed the 20261011_Arc_Not_Streight_Fix branch from 52365b5 to a8186c0 Compare October 11, 2026 08:57
@lthall lthall added the WIP label Oct 11, 2026
@andyp1per andyp1per mentioned this pull request Oct 24, 2026
3 of 4 tasks
@lthall lthall removed the WIP label Nov 8, 2026
@lthall
lthall force-pushed the 20261011_Arc_Not_Streight_Fix branch from 05ccfe7 to b36a4f8 Compare November 8, 2026 10:31
@lthall
lthall requested a balanced review from Copilot November 8, 2026 10:31

Copilot AI left a comment

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.

Warning

Copilot couldn't run its full agentic review because it didn't start before the timeout. Make sure your repository has a runner available, or add a copilot-code-review.yml file specifying one with the runs-on attribute. See the docs for more details.

Copilot review overview

1 open issue
What changed in this PR

Fixes Copter mission behavior so ARC_WAYPOINT legs are correctly flown as arcs when not preloaded, and mission lookahead behaves correctly after DO_JUMP/DO_JUMP_TAG when the jump target is a DO command.

Changes:

  • Update mission lookahead to continue scanning from the returned command index (DO_JUMP-safe), and add deterministic loop bounds.
  • Track the last completed “arc-start-capable” command in Copter Auto mode and use it to decide whether an arc can start curved vs. straight.
  • Add two Copter autotests covering arc start behavior and DO_JUMP-to-DO-command lookahead.
File Description
libraries/​AP_Mission/​AP_Mission.cpp Makes get_next_nav_cmd() advance from the returned cmd’s index to handle DO_JUMP targets correctly and prevents DO-only infinite lookahead loops.
ArduCopter/​mode_auto.cpp Adds arc start tracking and uses it to decide whether ARC_WAYPOINT should apply curvature when not preloaded.
ArduCopter/​mode.h Updates wp_start() signature and adds arc-start helper declarations/state.
Tools/​autotest/​arducopter.py Adds regression tests for arc start waypoint behavior and DO_JUMP-to-DO-command lookahead correctness.

Review effort: Lite

Comment thread ArduCopter/mode_auto.cpp Resolved
@lthall
lthall force-pushed the 20261011_Arc_Not_Streight_Fix branch from b36a4f8 to 198f6d2 Compare November 8, 2026 11:25
@lthall
lthall requested a balanced review from Copilot November 8, 2026 11:26
@lthall lthall changed the title Copter, AP_Mission: fix arcs flown straight and lookahead after a jump to a DO command Copter, AP_Mission: fly arcs only from a defined start and fix lookahead after a jump to a DO command Nov 8, 2026
@AP-Review

AP-Review commented Nov 8, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-11-08)

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

Reviewed at head b36a4f803d.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/34713/1.html#pr34713

Reviewed at b36a4f8. The PR has since moved to 198f6d2, which this review does not cover. The arc and jump-lookahead fixes test well in SITL, but the new LOITER_TO_ALT origin has an altitude-frame bug.

Blocking

  1. ArduCopter/mode_auto.cpp:485: get_pos_desired_NED_m() leaves out the surface-tracking terrain offset, but wp_and_spline_init_m() clears the terrain and uses this vector as an EKF-origin origin. After LOITER_TO_ALT (or PAYLOAD_PLACE) over ground that is not level with the EKF origin, the next leg's target altitude jumps by the terrain offset. Add pos_control->get_pos_terrain_D_m() to the D component, or take only NE from the desired position, and add a nonzero-terrain test.

Notes

  • ArduCopter/mode_auto.cpp:1699: a preloaded arc reached by DO_JUMP from another nav command is now flown straight (on master it was curved). This is intended and tested, but worth agreeing on at the dev call and mentioning in the release notes.
  • ArduCopter/mode_auto.cpp:1726: after LOITER_TURNS, the arc starts from the orbit exit point rather than the row's centre, so the flown chord can differ from the one drawn from the mission rows.

Copilot's index-0 concern is not a bug, since row 0 is home and is never flown.

Not fully reviewed: the new head 198f6d2, the flight response to the altitude step, other-vehicle callers of get_next_nav_cmd, and the CI failures.

Head moved during review: observed 198f6d212fc37e3213b4cdbf10e49f1a812a7ac8. This review covers only b36a4f803df149549bd694a1700f0968d4402a77; followup eligible.

Copilot AI left a comment

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.

Warning

Copilot couldn't run its full agentic review because it didn't start before the timeout. Make sure your repository has a runner available, or add a copilot-code-review.yml file specifying one with the runs-on attribute. See the docs for more details.

Copilot review overview

4 open issues
1 resolved since last review

Review effort: Lite

Comment thread ArduCopter/mode.h Outdated
// Delay the next navigation command
uint32_t nav_delay_time_max_ms; // used for delaying the navigation commands (eg land,takeoff etc.)
uint32_t nav_delay_time_start_ms;
bool nav_delay_has_defined_start; // true if the vehicle started the delay at a fixed point it reached in order
Comment thread libraries/AP_Mission/AP_Mission.cpp Outdated
Comment on lines +581 to +582
uint16_t jumps_followed[8];
uint8_t num_jumps_followed = 0;
Comment thread libraries/AP_Mission/AP_Mission.cpp Outdated
Comment on lines +611 to +614
if (num_jumps_followed >= ARRAY_SIZE(jumps_followed)) {
return false;
}
jumps_followed[num_jumps_followed++] = jump_index;
Comment thread libraries/AP_Mission/AP_Mission.cpp Outdated
Comment on lines +2258 to +2268
_nav_cmd = cmd;
_flags.nav_cmd_in_order = in_order;
if (start_command(_nav_cmd)) {
_flags.nav_cmd_loaded = true;
if (_jump_tag.age > 0 && _jump_tag.age < UINT16_MAX) {
// we're tracking a tag so increase it's age on every new NAV item
_jump_tag.age++;
}
} else {
// the vehicle skipped this command, so the next one does not follow on from the previous one
in_order = false;
@AP-Review

AP-Review commented Nov 8, 2026 •

Copy link
Copy Markdown

Deprecated — see below for the updated review.

Previous review (2026-11-08)

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

Reviewed at head 198f6d212f.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/34713/2.html#pr34713

The PR still needs changes: the new jump lookahead can preload the wrong waypoint in some nested DO_JUMP missions.
Since the review at b36a4f8, the LOITER_TO_ALT altitude-frame blocker has been fixed by removing that origin branch, and the arc-after-LOITER_TURNS note no longer applies.

Blocking

  1. libraries/AP_Mission/AP_Mission.cpp:608: when a jump the lookahead has already followed comes round again, the search moves on to jump_index + 1. That ignores the counts of the other jumps followed in between. With 2=DO, 3=DO_JUMP→5 ×1, 4=WP B, 5=DO, 6=DO_JUMP→3 ×1, 7=WP C, the lookahead from 2 returns C, but the mission (and master's lookahead) goes to B. Copter preloads C, and AC_WPNav reuses that preloaded leg when B starts, so the vehicle tracks toward C. Simulate jump run counts locally during the lookahead, or skip the preload when the next nav command can't be proven. Add this case to DoJumpToDoCommand.

Should fix

  • libraries/AP_Mission/AP_Mission.cpp:585: the lookahead reports "no next nav command" for missions that do reach one. The i < _cmd_total bound counts loop re-walks: with three nested DO-only loops, Copter stops at WP_A (0.01 m/s vs 7.1 m/s on master). The 8-entry jumps_followed cap at line 611 gives up at nine distinct jumps, although 15/100 are supported. Plane's loiter-heading and landing-approach checks also see the false result. The fix for the blocker should cover both.

Notes

  • ArduCopter/mode_auto.cpp:1679: jump-to-arc flown straight (still open). This is intended and tested; a racetrack lap that jumps back to its first arc will now fly that leg straight.

Copilot's uninitialised nav_delay_has_defined_start and set-flag-after-start_command comments are not bugs: ModeAuto lives in the zeroed global copter, and the flag is read inside start_command.

@rmackay9

rmackay9 commented Nov 9, 2026

Copy link
Copy Markdown
Contributor

PR #35049 fixes up some related bugs in AP_Mission

@lthall
lthall force-pushed the 20261011_Arc_Not_Streight_Fix branch from 198f6d2 to 3520b80 Compare November 20, 2026 14:51
@lthall

lthall commented Nov 20, 2026

Copy link
Copy Markdown
Contributor Author

Rebased on master and reworked to the agreed rule: if a waypoint is defined as an arc, it is flown as an arc, however the mission reaches it. A jump, mission start, resume, set current, or an arc after LOITER_TURNS now curves from the current location in the same way a normal waypoint starts, instead of being flown straight. The "defined start" logic and the in-order flag in AP_Mission are gone.

The AP_Mission lookahead commit is dropped because master now has "AP_Mission: follow jumps when looking ahead for the next nav command", which fixes the same problem, and JumpNextNavCmd covers it. That leaves one Copter commit, plus WPArcFlownAsArc, which replaces WPArcStartWaypoint and DoJumpToDoCommand.

@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 3520b808d7.
Full report: https://firmware.ardupilot.org/Tools/APReview/DevCallReviews/PRReviews/ardupilot/ardupilot/34713/3.html#pr34713

No blockers remain: the PR is now a small Copter change that flies every arc waypoint as an arc, and its new test passes in SITL and fails on master.
Since the review at 198f6d2, the AP_Mission lookahead commit that held the nested-jump blocker and the loop-bound issue has been dropped in favour of master's f8c66de, and an arc reached by a jump now curves.

Notes

  • PR title: still says "Copter, AP_Mission: fly arcs only from a defined start and fix lookahead after a jump to a DO command", but the PR no longer touches AP_Mission and now always flies an arc as an arc. Something like the Copter commit's subject would fit.
  • ArduCopter/mode_auto.cpp:1600: a resumed, set-current or jumped-to arc uses its full angle over a chord from wherever the vehicle is, so the arc grows with that distance (radius about 5.7 times the chord at 350 degrees). Resumed 200 m short of a 40 m 180-degree arc, SITL flew a 120 m-radius semicircle. This matches the agreed rule; worth confirming at the dev call and documenting for pilots.

Not fully reviewed: CI (still pending), the other arc and jump autotests at this head, terrain-frame arcs and arcs over 180 degrees reached without a preload (code reading only).

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.

4 participants