ArduCopter: add MAV_CMD_DO_ORBIT support in Guided mode

[researcher3]

Summary

Implements MAV_CMD_DO_ORBIT (command 34) as an immediate command executable while the vehicle is in Guided mode. The vehicle flies to the edge of the specified circle and then orbits the given GPS coordinate. Addresses #15098.

Testing (more checks increases chance of being merged)

• Checked by a human programmer
• Tested in SITL
• Tested on hardware
• Logs attached
• Logs available on request
• Autotest included

Description

Changes:

• Added SubMode::Orbit to ModeGuided
• Added circle_start(), orbit_run() and orbit_apply_yaw_behaviour() to ModeGuided
• Added handle_MAV_CMD_DO_ORBIT() to GCS_MAVLink_Copter
• Added MAV_CMD_DO_ORBIT to command_long_stores_location() in GCS_Common
• Added AC_COPTER_MODEGUIDED_ORBIT_ENABLED flash guard (tied to MODE_GUIDED_ENABLED), registered in build_options.py and extract_features.py
• Added autotest DO_ORBIT to arducopter.py

Behaviour:

• Positive radius = clockwise, negative = counter-clockwise (MAVLink spec)
• If vehicle is more than 3m from orbit edge, it flies to the edge first
• param2 sets tangential speed in m/s, converted to angular rate internally
• param3 sets yaw behaviour (all 5 ORBIT_YAW_BEHAVIOUR values supported: face center, hold initial heading, uncontrolled, tangent to circle, RC controlled, unchanged)
• param4 sets number of turns (0 = forever, NaN = preserve existing turn count)
• After completing the requested turns, vehicle holds position in place
• Yaw behaviour is consistent during approach, orbiting, and after orbit completion

Known limitations / Future work

• ORBIT_EXECUTION_STATUS not yet sent — requires MAVLink submodule update, will be addressed in a follow-up PR
• Not yet tested on hardware.

DO_ORBIT_SITL.bin.log

[peterbarker]

Nice!

Can you outline why you chose to add to guided mode rather than forcing the vehicle into circle mode, please? I'm not saying what you did was wrong.

[peterbarker]

I think we also need to support param4 - otherwise we're dangerously not compliant with the spec.

It may also be that @lthall would like us to use the arc-waypoint infrastructure for this. I suggest waiting for Leonard to chime in before making adjustments to the guided mode implementation here

[rmackay9]

@researcher3, @peterbarker,

Re using the new Arc method, it's a reasonable idea but it's too much scope creep. If we were going to move to use Arcs we would do that in Circle mode or Auto mode first I suspect. So let's stick with the direction this PR is taking

[lthall]

@researcher3, @peterbarker,

Re using the new Arc method, it's a reasonable idea but it's too much scope creep. If we were going to move to use Arcs we would do that in Circle mode or Auto mode first I suspect. So let's stick with the direction this PR is taking

Yes I tend to agree. When I come along later I can work through it all.

[researcher3]

Nice!

Can you outline why you chose to add to guided mode rather than forcing the vehicle into circle mode, please? I'm not saying what you did was wrong.

Thanks for the question.
My main reason for implementing this in Guided mode was that MAV_CMD_DO_ORBIT is defined in MAVLink as a command rather than a flight mode. Guided mode is already the entry point in ArduPilot for externally driven MAVLink commands and trajectory control, so it seemed like the most natural place to handle it.

I did consider switching to Circle mode. However, Circle mode relies on persistent parameters such as CIRCLE_RADIUS and CIRCLE_RATE, while MAV_CMD_DO_ORBIT provides its own parameters (radius, velocity, center point, etc.). Implementing it via Circle mode would likely require temporarily overriding parameters or adapting that mode’s behaviour, which felt less clean.

Keeping it in Guided mode also avoids a forced mode transition and allows the vehicle to remain under companion/GCS command control.

Another difference I noticed is that MAV_CMD_DO_ORBIT explicitly defines the orbit center as part of the command (lat/lon/alt), while Circle mode implicitly uses the vehicle's position at the time the mode is entered as the center of the circle.
Because of that difference in behaviour, mapping MAV_CMD_DO_ORBIT directly onto Circle mode would likely require additional logic to reposition the vehicle or modify how Circle mode determines its center.
Implementing the orbit behaviour in Guided mode made it easier to directly follow the MAVLink command semantics.

[researcher3]

param4

Implemented - param4 now sets the number of turns (0 = orbit forever).

[peterbarker]

The concept behind the guards is to not use up flash space on boards which can't spare it.

For reference, this new feature takes up 1168 bytes of Pixhawk1-1M's 1976 bytes remaining!

[researcher3]

Implemented param3 (yaw behaviour) fully, covering all five ORBIT_YAW_BEHAVIOUR values from the MAVLink spec (face center, hold initial heading, uncontrolled, tangent to circle, RC controlled, unchanged). Additionally fixed two yaw-related bugs discovered during manual SITL testing: (1) after orbit completion the vehicle now correctly maintains the requested yaw behaviour instead of always snapping to face center, and (2) during the approach to the orbit edge the vehicle now looks in the direction of travel rather than holding its previous heading. Also fixed a separate bug where the vehicle would fly back toward its start position and descend after completing the requested number of turns — replaced pos_control_start() with hold_position() which correctly stops the vehicle in place.

[researcher3]

Found and fixed the root cause of the 2x speed bug. orbit_run() was calling update_ms() twice per loop — once before the turn-count check (without parameters) and once after (with 0.0f). Since each update_ms() call advances _angle_rad and ramps _angular_vel_rads, two calls per loop resulted in exactly double the requested speed, which matches the observed behaviour (1.0 m/s requested → 2.0 m/s measured, consistently across all speeds). Fixed by removing the redundant first call.

[researcher3]

Tested on hardware (Pixhawk 4 with custom firmware build). MAV_CMD_DO_ORBIT returns MAV_RESULT_ACCEPTED. Flight test with full drone pending.

[peterbarker]

Make sure the code builds without AC_COPTER_MODEGUIDED_ORBIT_ENABLED

[peterbarker]

@researcher3 we've merged ORBIT_STATUS into our XML now (submodule reference not updated to point to a commit referencing it yet at point of me writing this, however)

What's your intention with this PR?

[researcher3]

@researcher3 we've merged ORBIT_STATUS into our XML now (submodule reference not updated to point to a commit referencing it yet at point of me writing this, however)

What's your intention with this PR?

Thank you for merging ORBIT_EXECUTION_STATUS into the XML!
For this PR: the implementation is complete and ready for review/merge as-is. ORBIT_EXECUTION_STATUS sending is intentionally left for the follow-up draft PR, which I will update and mark ready for review once the mavlink submodule is updated to include the new message definition and once this PR is merged.
Is there anything else needed on this PR before it can be merged?

[peterbarker]

Thank you for merging ORBIT_EXECUTION_STATUS into the XML! For this PR: the implementation is complete and ready for review/merge as-is.

Yes, I agree this could be merged once the minor requests I've made have been addressed (need Randy's approval for merge, however)

ORBIT_EXECUTION_STATUS sending is intentionally left for the follow-up draft PR, which I will update and mark ready for review once the mavlink submodule is updated to include the new message definition and once this PR is merged.

The submodule reference now points to a commit which includes ORBIT_EXECUTION_STATUS.

I was unable to find a PR which adds support for that message. I'm guessing it's pretty straight forward, so if you can present some patches on top of this one in a PR then it should be straight-forward to review.

[researcher3]

Thank you for merging ORBIT_EXECUTION_STATUS into the XML! For this PR: the implementation is complete and ready for review/merge as-is.

Yes, I agree this could be merged once the minor requests I've made have been addressed (need Randy's approval for merge, however)

ORBIT_EXECUTION_STATUS sending is intentionally left for the follow-up draft PR, which I will update and mark ready for review once the mavlink submodule is updated to include the new message definition and once this PR is merged.

The submodule reference now points to a commit which includes ORBIT_EXECUTION_STATUS.

I was unable to find a PR which adds support for that message. I'm guessing it's pretty straight forward, so if you can present some patches on top of this one in a PR then it should be straight-forward to review.

The follow-up PR with ORBIT_EXECUTION_STATUS sending support is already
prepared at researcher3#1 — currently
draft pending merge of this PR. I will rebase it onto this branch, remove
the manual XML addition from the mavlink submodule (since the submodule
now includes the message natively), and mark it ready for review once
this PR is merged.

[peterbarker]

LGTM

[peterbarker]

Board                    AP_Periph  antennatracker  blimp  bootloader  copter  heli  iofirmware  plane  rover  sub
CubeOrange-periph-heavy  0                                 *                                                   
Durandal                            8               0      *           1296    1312              0      8      0
Hitec-Airspeed           *                                 *                                                   
KakuteH7-bdshot                     0               0      *           1312    1304              8      8      0
MatekF405                           *               *      *           *       *                 *      *      *
Pixhawk1-1M-bdshot                  *               *                  *       *                 *      *      *
SITL_x86_64_linux_gnu               -104            -112               4304    4304              -4200  -104   -104
YJUAV_A6SE                          8               8      *           1320    1320              0      0      0
f103-QiotekPeriph        *                                 *                                                   
f303-MatekGPS            *                                 *                                                   
f303-Universal           *                                 *                                                   
iomcu                                                                                *                         
mindpx-v2                           0               8      *           1312    1320              0      0      8
revo-mini                           *               *      *           *       *                 *      *      *
speedybeef4                         *               *      *           *       *                 *      *      *

[researcher3]

Wiki PR submitted: ArduPilot/ardupilot_wiki#7628

[Ryanf55]

Neat PR - I'm happy to see something like this going in.

For our use case, we want to do orbits with precise radius. Being 3 meters off from the orbit location can lead to a crash (because our orbit radius is ~5m typically), and may also will result in the poor sensor data from our payload because the distance we want to use it around the target is not what is actually flown.

Why do we not try to follow the exact path?
Does circle mode say if I'm within 3m radius, close enough?
If you don't have a strong reason, I'd much prefer to have the vehicle target the exact radius rather than have a hard-coded deadband. The mavlink spec does not say anything about a deadband.

[peterbarker]

Neat PR - I'm happy to see something like this going in.

For our use case, we want to do orbits with precise radius. Being 3 meters off from the orbit location can lead to a crash (because our orbit radius is ~5m typically), and may also will result in the poor sensor data from our payload because the distance we want to use it around the target is not what is actually flown.

Why do we not try to follow the exact path? Does circle mode say if I'm within 3m radius, close enough? If you don't have a strong reason, I'd much prefer to have the vehicle target the exact radius rather than have a hard-coded deadband. The mavlink spec does not say anything about a deadband.

The 3m thing is all about how you get to the edge, not the radius at which you circle....

Are you suggesting that for your purposes 3m is insane and it should always fly to the circle edge in a straight line, not circle out onto the edge?

[peterbarker]

Needs a rebase

[researcher3]

I reduced the approach threshold from 3m to 0.1m — the vehicle now flies to within 0.1m of the exact orbit edge before beginning the orbit. This ensures more precise orbit positioning for small-radius use cases with RTK-equipped vehicles, while still avoiding unnecessary micro-corrections at floating-point scale.
If 0.1 is too small we could raise it. I chose 3m based on our use cases. We could even make it proportional, like:
dist_to_edge_m > MAX(0.5f, radius_m * 0.1f)
That's not implemented yet. I'll leave it up to you guys to discuss that.

[researcher3]

As a first-time contributor I'm not fully familiar with the merge process — is there anything still needed from my side to move this forward, or is it purely waiting for the next Dev Call?
Also with regard to my pull request #7628 (add MAV_CMD_DO_ORBIT documentation)

[rmackay9]

Hi @researcher3, we just need a dev to review it.. probably me mostly..