Support target yaw on NAV_WAYPOINT or LOITER to hold position until heading is reached

[Mcluky]

Feature Request

Is your feature request related to a problem? Please describe.
Currently, there is no reliable way to make a vehicle complete a yaw rotation at a waypoint before proceeding to the next navigation command. This is a common requirement for inspection and survey missions where the camera must face a specific direction before capturing an image.

CONDITION_YAW only gates DO commands - it does not prevent NAV command advancement. The NAV and DO/Condition tracks run completely independently in APMission::update(). This means the vehicle starts flying to the next waypoint immediately, regardless of whether the yaw rotation has completed.

Real-world example:

A building inspection mission requires the vehicle to:
Fly to a waypoint
Rotate to face the building facade
Take a photo once the yaw rotation is complete
Fly to the next inspection point

The intuitive mission plan would be:

NAV_WAYPOINT A
CONDITION_YAW 180°, 30°/s, CW
DO_DIGICAM_CONTROL <- take photo after yaw is complete
NAV_WAYPOINT B

Expected behavior: the vehicle stays at WP A, rotates to 180°, takes the photo once the rotation is complete, then proceeds to WP B.

Actual behavior: the vehicle immediately starts flying toward WP B as soon as WP A is reached. CONDITIONYAW only delays the DODIGICAM_CONTROL command - it does not hold the vehicle in place. The photo is taken mid-flight after the yaw finishes, potentially far from WP A and while the vehicle is moving.

The only workaround today is a hardcoded NAV_DELAY:

NAV_WAYPOINT A
CONDITION_YAW 180°, 30°/s, CW
NAV_DELAY 7 sec <- guess: 180° / 30°/s = 6s + margin
DODIGICAMCONTROL
NAV_DELAY 1 sec
NAV_WAYPOINT B

This is unreliable because:
The required delay depends on the vehicle's heading at arrival, which varies based on approach direction from the previous waypoint. A full 180° turn at 30°/s takes 6s, but the vehicle might arrive already close to the target heading.
If the delay is too short, the photo is taken before yaw completes. If too long, the vehicle wastes time hovering unnecessarily.
For missions with many inspection points at different headings, every waypoint needs a manually calculated NAV_DELAY, making the mission fragile and hard to maintain.
Unlike CONDITION_DELAY (which has a NAV equivalent in NAV_DELAY), there is no NAV equivalent for "wait until yaw reaches target heading."

Describe the solution you'd like

Support a yaw parameter in NAV_WAYPOINT or LOITER commands.

NAV_WAYPOINT and the LOITER commands officially support the yaw angle, just Ardupilot doesn't support it yet.

NAV_WAYPOINT A, yaw=180°

The waypoint would only verify as complete once BOTH the position acceptance radius is met AND the vehicle has reached the target yaw heading.

Describe alternatives you've considered
Allow CONDITION_YAW to gate NAV advancement.

Add a parameter to CONDITION_YAW (using one of the unused/reserved fields) that indicates "also hold position until yaw is complete." The NAV track would check if a blocking condition is active before advancing. Backward-compatible since existing missions wouldn't set the flag.

Platform

• All
• AntennaTracker
• Copter
• Plane
• Rover
• Submarine

Additional context

[ja51d]

Took a look at this one. the param4-on-NAV_WAYPOINT route runs straight into the mission storage limit - on copter the waypoint item is already full (p1 holds the hold-time delay, the rest of the 15 bytes is lat/lng/alt), so there's nowhere to put the yaw without expanding the mission structure. there's even a note about it in AP_Mission.cpp:

the 15 byte limit means we can't fit both delay and radius in the cmd structure. When we expand the mission structure we can do this properly

so doing yaw-on-NAV_WAYPOINT "properly" is really tied to that bigger storage-format change.

the CONDITION_YAW alternative you mentioned looks a lot more tractable short-term - it's got spare param slots, so a flag like "also hold position until the yaw target is reached" could gate the NAV track from advancing. backwards compatible since existing missions just don't set it. the nav and condition tracks run independently today, so it'd mean the nav verify checks for an active blocking-yaw condition before it advances.

before i go build anything - would you be open to that approach? don't want to sink time into the storage-expansion version if there's a preferred direction.

[rmackay9]

Hi @Mcluky, @ja51d,

Thanks for the report. I think instead of modifying the mission commands, it would be better to modify the Copter's auto mode so that it does what @Mcluky is asking for perhaps based on a new bit in AUTO_OPTIONS.

CONDITION_YAW is a "do" command which are treated mostly independently of navigation commands. Breaking that assumption will be very difficult I think.

[Mcluky]

I agree with @rmackay9 that modifying the CONDITION_YAW is probably not ideal now that I think about it because it would deviate from the official mavlink spec... I haven't thought about the AUTO_OPTIONS bit but I agree that to me it seems like it would more belong to that place. The only thing I wonder though is what the functionality of that bit would change. Like would that bit only affect the CONDITION_YAW command or all "DO" commands? AFAIK most do commands are also on a fire "fire and forget" basis am I correct? So awaiting something like a DO_DIGI_CAM_CONTROL command wouldn't be possible right?

Personally for me the ideal scenario would be if I can set the a AUTO_OPTIONS flag that all NAV and DO commands happen sequentially... But I assume that would be pretty complicated / a lot of effort right?

For a simpler solution I think it might make sense to just allow the CONDITION_YAW to be treated as a NAV command -> that we wait with the next nav and do command until the condition is met.

What are your takes @rmackay9 @ja51d on this? :)

[ja51d]

yeah @rmackay9 the AUTO_OPTIONS bit feels like the right home for this, and it keeps CONDITION_YAW as-is so we don't break the do/nav independence.

to your questions @Mcluky:

the way I'd scope it: the bit wouldn't really "wait on a DO command" at all. it'd hook the nav waypoint's own completion check, not the DO track. right now verify_nav_wp finishes the wp once we've reached the destination + the loiter timer is up. with the bit set we just add one more condition - don't call the wp done until the yaw target is reached. there's already a auto_yaw.reached_fixed_yaw_target() we can lean on. so:

• it doesn't touch CONDITION_YAW processing, that stays fire-and-forget
• it just makes the wp hold position until the commanded heading is hit, which is exactly the behaviour you asked for in the title

on the "await any DO command" idea (DO_DIGICAM_CONTROL etc) - you're right that's not really doable, most DO commands have no notion of "done" to wait on. yaw is the lucky exception because that reached-target check already exists. and making all NAV+DO strictly sequential would mean teaching every DO command what "complete" means, which is a much bigger architectural job and cuts against the intentional independence randy mentioned.

so the scoped version: new AUTO_OPTIONS bit -> verify_nav_wp (and the loiter verifies) gate completion on yaw-reached when it's set. backwards compatible since the bit defaults off. happy to put up a draft PR along those lines if you're both good with it.

[Mcluky]

@ja51d thank you for outlining this! :) I already assumed that my sequential "DO" command idea was a bit utopic haha :D.

I think your implementation idea sounds sound to me. For me having the option to "await" the rotation would already be a game changer! One last thing I'm not quite sure if I get is how I would use this. Since you mentioned the issue with

the 15 byte limit means we can't fit both delay and radius in the cmd structure

-> Does the approach you would like to do allow to populate the NAV_WAYPOINT param-4 for yaw even with the storage limitation? Or would it take the yaw value for NAV_WAYPOINT from a previous CONDITION_YAW command so that we don't run into the storage limitation? Sorry if this might be a stupid question but this is my first time working with ardupilot and I haven't had the chance yet to go so in depth with it!

To me though either way sounds very good and I would love to see this <3! You'd make me so happy to see a draft PR for this :D

[ja51d]

@Mcluky no stupid questions at all and good instinct, it's the second one. we never touch NAV_WAYPOINT's param4, so the 15-byte storage limit never comes into play.

the yaw comes from a normal CONDITION_YAW. so a mission would look like:

NAV_WAYPOINT   (your point)
CONDITION_YAW  (heading you want)

CONDITION_YAW sets the vehicle's yaw target while it's flying that leg, and with the AUTO_OPTIONS bit on, the waypoint just won't report "done" until that heading is reached — so it parks there, finishes rotating, then carries on to the next command. nothing new in the mission format; you're only adding the existing CONDITION_YAW and flipping the option bit.

i'll put a draft PR up along these lines so you can see it in action.

[ja51d]

draft PR up: #33309

small change - new AUTO_OPTIONS bit that gates the waypoint (and loiter-time) completion on the CONDITION_YAW target being reached. no mission format changes, CONDITION_YAW stays as-is. still need to do a proper SITL mission run to confirm the hold-and-rotate behaviour, but wanted to get the approach in front of you both first. @rmackay9 @Mcluky lmk if this is the shape you had in mind.

[ja51d]

@Mcluky since this scratches your itch, want to take a piece of it? The most useful bit left is the actual SITL test — build copter SITL, set AUTO_OPTIONS bit 3, run a little mission like:

NAV_WAYPOINT   (a point)
CONDITION_YAW  (say 90 deg)
NAV_WAYPOINT   (next point)

and confirm it parks at the first wp, rotates to 90, then carries on. a short clip or a .bin log showing the hold-and-rotate would be perfect, and if you're up for it the proper autotest in Tools/autotest/arducopter.py would be even better — happy to point you at an existing yaw test to copy. That's still super helpful. lmk and i can walk you through the SITL setup. If you can do this we'll co-author it :)

[Mcluky]

Awesome! Thank you so much! I'm happy to have a look at it tomorrow and test it out :)!

[rmackay9]

Maybe @Mcluky could confirm if the issue is only when there is a delay at the waypoint or does it happen on "fast" waypoints as well?

I've commented in the PR but I don't think a condition-yaw should be required and instead the vehicle should change its heading only on waypoints that have a delay. So as soon as the delay timer starts, the vehicle should point towards the next waypoint. If waypoint doesn't have a delay then the vehicle's yaw follows the direction of the path and I don't think that will be easy to change.

[rmackay9]

Hi @Mcluky, by the way, instead of using NAV_DELAY the waypoint command's first field allows a delay to be set. It doesn't solve the problem you've mentioned of course, it's just slightly easier I think than adding an extra command

[Mcluky]

Hi @rmackay9 can you specify what you mean with "fast" waypoints? The rotation doesn't fully finish either way if the hold time is set to 0s or 1s if you mean that?

I've also added a comment on the PR, for my usecase point in direction of the next waypoint is not what I need, but rather point at any place that I define.

I'm aware that I could use the hold time in the NAV_WAYPOINT command as well, but yeah as you already figured this doesn't allow me to wait until a rotation is finished :/