Waypoint Navigation in ArduRover (Boats)

Theory of operation — how a list of waypoints becomes throttle and steering commands

← Back to Autopilot

Conceptual overview of waypoint navigation as implemented in ArduRover, framed for students who already understand the stabilization layer. Detailed analysis of guidance and navigation is outside the scope of this introductory class; this page gives a sufficient mental model and points outward for those who want depth.

Why we need this page

The stabilization layer documented in the Stabilization Layer reference knows nothing about waypoints. Each loop chases one setpoint — a speed (m/s) for the throttle channel, a yaw rate (rad/s) for the steering channel — and that is all.

To execute a waypoint mission, something must turn a list of (lat, lon) targets into a continuous sequence of “right now, the boat should be moving at 1.4 m/s and turning at 0.3 rad/s.” That something is the guidance and navigation (G&N) stack. This page explains what it does, framed as a feedforward + PID cascade at the layer above stabilization.

A waypoint mission, in one paragraph

A mission is an ordered list of (lat, lon, alt) targets uploaded to the autopilot. In AUTO mode the autopilot walks the list, treating each entry as the active target until the boat arrives, then advancing to the next. For a USV the altitude is ignored; only the (lat, lon) pair matters.

The familiar pattern: feedforward + PID, recursively

Each stabilization channel has the same internal structure:

setpoint ──► (+)───► PID ───► (+)───► actuator command ──► plant ──► measurement
              ▲              ▲                              │
              │              │                              │
              │              └──── FF (= setpoint × K_dc⁻¹) │
              │                                             │
              └─────────────────── measurement ◄────────────┘

The feedforward branch makes an open-loop guess of the actuator command that should produce the desired output if the plant matched its nominal model. The PID branch closes the loop and corrects whatever the FF guess got wrong.

ArduRover’s G&N stack applies the same pattern one level up. Its feedforward is a kinematically feasible reference trajectory (computed in advance from the waypoints). Its PID closes the loop on position error. The output of that outer loop becomes the setpoint for the inner loop.

waypoints ──► trajectory gen ──► (pos_ref, vel_ref) ──► (+)──► PID ──► speed_setpoint    ┐
   (open loop, kinematic)                ▲                       └──► yaw_rate_setpoint──┘──►  STABILIZATION
                                         │                                                    (inner loop)
                                  pos_measured ◄── EKF ◄── GPS + IMU

Two layers of “FF + PID,” nested. The outer FF is the velocity reference from the S-curve generator. The outer PID is AC_PosControl. The inner FF + PID is the stabilization layer.

The cascade in three layers

Layer	What it does	Inputs	Outputs
Trajectory generation (S-curve, in AC_WPNav)	Plans a smooth path between waypoints that respects vehicle acceleration / jerk / speed limits. Open-loop — no feedback.	Waypoint list; WP_SPEED, WP_RADIUS, ATC_ACCEL_MAX, ATC_DECEL_MAX, ATC_TURN_MAX_G, jerk limits	Time series of (target position, target velocity, target acceleration) along the path
Position control (AC_PosControl)	Closes the outer loop on position error. Same PID + FF structure as the stabilization layer, just on a slower, geometric quantity.	Target pos/vel from trajectory gen; measured position from EKF	Speed setpoint (throttle channel) and yaw-rate setpoint (steering channel)
Stabilization	Closes the inner loop on speed and yaw-rate errors. Two independent PID + FF loops.	Setpoints from AC_PosControl; measured speed (GPS_Spd/EKF), measured yaw rate (gyro/EKF)	Throttle PWM (RCOU_C3), rudder PWM (RCOU_C1)

The handoff between layers 2 and 3 happens at the same channels logged in the stabilization labs:

• Position controller’s speed output → THR_DesSpeed → speed PID input
• Position controller’s yaw-rate output → STER_DesTurnRate → yaw-rate PID input

In a Lab-2-style step test the pilot generated commands on those channels directly. In a waypoint mission the same channels carry a more complex, time-varying reference — generated by AC_PosControl chasing the S-curve.

What happens each control tick

Within an AUTO mission, on each iteration of the loop:

1. Active waypoint. Pick the next item in the mission list. This is the navigation-level setpoint.
2. Geometry. Compute the vector from current (GPS / EKF) position to the active waypoint, yielding a desired bearing and a remaining distance.
3. Cross-track error. Decompose the position error relative to the planned path (a line from the previous waypoint to the active one) into along-track and cross-track components. The cross-track component is what gets pushed against to keep the boat on the line.
4. Desired speed. Capped above by WP_SPEED (the cruise setting); capped from below by what the kinematic limits (ATC_ACCEL_MAX, ATC_DECEL_MAX, ATC_TURN_MAX_G) allow at this point on the path. The boat slows down in tight corners on purpose.
5. Desired yaw rate. Computed so the boat steers toward the bearing while respecting the turning g-cap; includes a proportional response on cross-track error.
6. Setpoint handoff. Desired speed → THR_DesSpeed → speed PID. Desired yaw rate → STER_DesTurnRate → yaw-rate PID.
7. Arrival check. The autopilot decides the active waypoint has been reached (see When is a waypoint “reached”? below) and advances to the next waypoint. If this was the last waypoint, the mission is complete.

When is a waypoint “reached”?

This is the surprising one, and worth getting right because it shapes how the autopilot behaves at speed.

The autopilot considers a waypoint reached when either of the following becomes true:

1. The boat enters the waypoint’s acceptance radius (WP_RADIUS).
2. The boat crosses a “finish line” — a line through the waypoint, perpendicular to the path segment leading to it. Once the boat is on the far side of that line, the waypoint is considered reached even if the boat never came inside WP_RADIUS.

Condition 2 is the one that catches people out. The intuitive mental model is “I set WP_RADIUS = 1 m, so the boat will pass within 1 m of each waypoint.” That is not what the autopilot promises. What it promises is “I will not loop back to retry a waypoint that I have already gone past.” That promise is essential — without it, an overshooting boat would orbit waypoints forever — but it means that at speed, the boat can advance through a mission without ever physically entering WP_RADIUS around any waypoint.

The threshold for “at speed” is geometry-dependent. On a course where consecutive waypoints are 30-50 m apart and the boat is moving above ~3 m/s, overshoots of several meters are routine, and the perpendicular check carries most of the mission advancement work. On a tight course at low speed, the radius check dominates.

Two practical consequences:

• On-water observation can disagree with strict radius scoring. A team that sees their boat cleanly traverse all waypoints (per the autopilot’s MSG log: "Reached waypoint #N" for N = 1 .. 7 in order) may find that a post-processing script which only checks WP_RADIUS entry reports a DNF. The autopilot’s MSG events are the authoritative signal; radius-only scoring undercounts.
• WP_RADIUS is not a “how close did I pass” tolerance. It is one of two arrival criteria, and on a fast course it is often the one that doesn’t fire. To force the boat closer to each waypoint, you usually need to reduce speed (so overshoot drops) rather than just tighten WP_RADIUS.

The behavior is documented (loosely) in the ArduRover → Tuning Navigation page (look for WP_OVERSHOOT and the “finish line” discussion). ArduPilot issue #23457 — “AR_WPNav: Rover 4.3+ does not honor WP_RADIUS” — has the candid version of how the perpendicular check has effectively superseded the radius check since the 4.3 navigation rewrite. For an in-class example of the behavior, see the AY26 Q3 leaderboard, where team3’s Day-1 results were initially misclassified as DNF for exactly this reason.

The S-curve, briefly

ArduPilot’s S-curve is not a Bézier curve. It is a jerk-limited 7-segment kinematic trajectory:

“SCurves calculate paths between waypoints (including the corners) using specified speed, acceleration and jerk limits.” — libraries/AP_Math/SCurve.h

The 7 segments are: jerk-up, constant accel, jerk-down, cruise, jerk-down (decel), constant decel, jerk-up. Corner-blending between consecutive waypoints uses a spline so the path stays continuous in velocity, but the speed profile along that path is the jerk-limited segmented profile, not a Bézier in time.

The point is that the S-curve produces a reference the boat is physically capable of following — it will not ask for an acceleration the actuators cannot deliver, or a turn rate the hull cannot sustain at the requested speed. From the Rover docs:

“S-Curves are used to plan a smooth path that brings the vehicle close to each waypoint while not exceeding speed or acceleration limits. […] The vehicle will cut the corners more at higher speeds and more if the maximum accelerations are reduced, but will slow down in the corners if necessary to pass within WP_RADIUS of the waypoint without exceeding the maximum accelerations.”

This is feedforward in the strictest sense: an open-loop plan generated from the waypoints and the vehicle’s kinematic limits, fed into the loop below as a reference.

Closing the position loop (AC_PosControl)

The position controller is a PID with feedforward, structurally identical to the stabilization channels:

Term	Parameter	Role
FF (velocity)	(from S-curve)	open-loop velocity reference along the planned path
P (position)	PSC_POS_P	proportional gain on position error
P, I, D (velocity)	PSC_VEL_P, PSC_VEL_I, PSC_VEL_D	inner velocity loop inside the position controller

Position error feeds a P controller that produces a desired velocity correction; that gets added to the S-curve’s vel_ref to produce the composite velocity setpoint; the velocity PID then produces a throttle / rudder demand — which, for Rover, is delivered as a speed setpoint to the stabilization layer rather than directly to the motors.

If that structure feels recursive, it is. It is the same stabilization-layer block diagram with one extra outer wrap.

The PID analogy, side by side

Concept	Stabilization layer	Navigation layer
Setpoint	speed (m/s), yaw-rate (rad/s)	active waypoint (lat, lon)
Measurement	GPS_Spd, EKF yaw rate	EKF position
Error	speed − measured speed	cross-track + along-track distance
Proportional gain	ATC_SPEED_P, ATC_STR_RAT_P	implicit, inside AC_PosControl (parameters PSC_*)
Feedforward	ATC_*_FF × setpoint (≈ 1/K_dc)	S-curve velocity reference — kinematically feasible by construction
Output	throttle PWM, rudder PWM	THR_DesSpeed, STER_DesTurnRate (= next layer’s setpoint)

Two takeaways: each layer has the same internal pattern (setpoint → error → P/I/D → output, with an optional feedforward branch); and each layer’s output is the next layer’s setpoint. Waypoint mission execution is the stabilization block diagram nested inside one more wrapper.

Why the stabilization layer still matters

The G&N stack only generates setpoints that are achievable in the ideal. Whether the boat actually achieves them is the inner loop’s job.

If the stabilization PIDs are sluggish, the boat will lag the S-curve and overshoot corners. If they are oscillatory, the trajectory will be tracked with high-frequency jitter on top. If they saturate, the S-curve’s “kinematic feasibility” guarantee is broken — the plant cannot deliver what was planned.

A well-tuned trajectory on top of poorly-tuned PIDs is wasted. The stabilization layer is what enables the outer loops to do their job.

What students should think about (and what to leave alone)

Worth knowing — these dominate mission performance:

• WP_SPEED — cruise speed along the path (m/s).
• WP_RADIUS — how close counts as “arrived”.
• WP_OVERSHOOT — how much lateral overshoot is tolerated when cutting corners.
• ATC_ACCEL_MAX, ATC_DECEL_MAX, ATC_TURN_MAX_G — the kinematic caps; these constrain what the navigation layer is allowed to ask for.
• That cross-track error exists and is what pulls the boat back onto the path when wind / current push it off.

Outside the scope of this course (but visible in the logs):

• S-curve internals (jerk-limited 7-segment trajectory generator math).
• PSC_* position-controller PID gains.
• L1 navigation algorithm derivation, NPFG, spline blending.
• EKF / state estimation.

Further reading

Architecture overview (start here):

• Copter Position Control and Navigation — dev wiki — Rover uses the same AC_WPNav / AC_PosControl / SCurve libraries the wiki describes; despite the “Copter” title it is the right architecture doc.

The S-curve algorithm, from the author:

• Trigonometric S-Curve Navigation — ArduPilot UnConference 2019 (Leonard Hall, ~30 min) — closest thing to a paper. Why trig-shaped jerk profiles, what is wrong with simple trapezoidal accel, how corner-blending works.
• S-Curves by Leonard Hall — shorter companion talk.

Motivation (before / after):

• Leash vs S-Curve navigation — what S-curve replaced (the “leash” method in Copter 4.0) and why the rewrite happened.
• S-Curve in action — flight-test comparison plots.

Rover parameter handbook (what you actually set):

• ArduRover → Navigation — what the autopilot does in AUTO mode; mission semantics.
• ArduRover → Position Control — how the boat is steered toward a target.
• ArduRover → Tuning Navigation — per-parameter handbook for WP_*, PSC_*, the S-curve caps.
• ArduRover → AUTO Mode — mission upload, mode switching, behavior on mission end.

Source-level (when you need the truth):

• AC_WPNav.h and SCurve.h — header comments more candid than the user wiki.
• DeepWiki: Attitude and Position Control — auto-generated walkthrough that cross-references the code.

Origin of the Rover port:

• GitHub Issue #11193 — Rover: improve navigation using S-curves — the discussion that brought S-curves to Rover.

Within this course:

• Stabilization Layer — the inner loop the navigation layer hands off to. The mental model on this page connects directly to that one.
• Lab 3 overview — where the conceptual model gets exercised in a field session.