MAVLink and MSP command-control parity

[xznhj8129]

⚠ Part 6/7 of the mavlink_multiport2 split — depends on #11716.
GitHub can't base an upstream PR on a fork branch, so this targets maintenance-10.x and its diff also contains the earlier parts of the stack. Review only this part's own commits:

• `57cb25ba5` Add MAVLink and MSP command-control parity

This part in isolation: xznhj8129/inav@mav/05-mission...mav/06-command-guided
Merge order: parts 1 → 7 in sequence.

Part 6/7 of the mavlink_multiport2 stack.

Command-control parity between MAVLink, MSP, and the Programming Framework, so GCS and companion software can arm, navigate, and land through standard commands.

Arming

• MAV_CMD_COMPONENT_ARM_DISARM and MSP2_INAV_ARM_DISARM (0x2227) go through the normal INAV arming/disarming path and ACK success only when the requested state is actually reached.

RTH

• MAV_CMD_NAV_RETURN_TO_LAUNCH, the ArduPilot-style MAV_CMD_DO_SET_MODE RTL request, MSP2_INAV_ACTIVATE_RTH (0x2225), and Programming Framework operation 61 all enter the normal INAV RTH mode path via activateRTHMode(), which adds a temporary BOXNAVRTH source to the RC mode selector. This deliberately does not use the failsafe/geozone forced-RTH latch — live testing showed activateForcedRTH() is not equivalent to switching into RTH mode. The temporary source clears on a pilot RC flight-mode change or disarm, so a command-triggered RTH cannot survive into a later arming attempt.

Pause / position hold

• MAV_CMD_DO_SET_MODE also accepts ArduPilot pause-style modes (PLANE_MODE_LOITER, COPTER_MODE_LOITER, COPTER_MODE_POSHOLD, COPTER_MODE_BRAKE) and enters normal INAV PosHold at the current position/altitude/heading via a temporary BOXNAVPOSHOLD source (this is what QGC's Pause button sends). Other GCS mode changes remain UNSUPPORTED.

Landing

• MAV_CMD_NAV_LAND, MSP2_INAV_ACTIVATE_LANDING (0x2224), and PF operation 62 perform a normal waypoint-style landing at the current position through a transient waypoint — the uploaded mission is not modified, and this is not emergency landing. Mission LAND items keep their supplied coordinates; direct LAND commands ignore coordinate fields.

Home, time, guided

• MAV_CMD_DO_SET_HOME uses the existing waypoint-0 backend (param1 = 1 = current GNSS position; explicit global location otherwise) and retains all native WP#0 safety gates. MSP set-home remains MSP_SET_WP waypoint 0 — no duplicate command added.
• MSP2_INAV_TIMESYNC (0x2228) returns the same monotonic nanosecond boot clock as MAVLink TIMESYNC.
• SET_POSITION_TARGET_GLOBAL_INT / _LOCAL_NED guided handling (waypoint 255 / altitude-only requests, gated on valid GCS navigation).
• MAV_CMD_CONDITION_YAW heading changes while the nav state has yaw control.
• Temporary fixed-wing PosHold loiter-radius override: MAV_CMD_DO_REPOSITION.param3 in meters, or int32 loiterRadius (cm) appended to MSP2_INAV_SET_GLOBAL_TARGET (17-byte payload; 13-byte payload = no change; 0 clears). MSP2_INAV_NAV_TARGET reports the active override. Volatile: cleared on disarm/reboot, applied only while the nav FSM is in PosHold so RTH/mission/autoland loiter keeps configured defaults.
• Explicit MAV_CMD_NAV_TAKEOFF stub returning UNSUPPORTED (foundation for later takeoff work, no behavior introduced).
• OSD flight-mode element shows GCSN while GCS-assisted navigation is actually active.

All float command parameters are validated (finite/range) before integer conversion.

Companion PR: mspapi2:mavlink_multiport2 (synchronized message metadata + arm/RTH/land/timesync helpers).

Testing

• Unit slice: 81/81 passing (mavlink_unittest).
• Full SITL build, warnings-as-errors, clean. MSP dev docs regenerated.
• X-Plane + QGC SITl test successful

[qodo-code-review]

Qodo reviews are paused for this user.

Troubleshooting steps vary by plan Learn more →

On a Teams plan?
Reviews resume once this user has a paid seat and their Git account is linked in Qodo.
Link Git account →

Using GitHub Enterprise Server, GitLab Self-Managed, or Bitbucket Data Center?
These require an Enterprise plan - Contact us
Contact us →

[github-actions]

Test firmware build ready — commit a7a2ba7

Download firmware for PR #11717

244 targets built. Find your board's .hex file by name on that page (e.g. MATEKF405SE.hex). Files are individually downloadable — no GitHub login required.

Development build for testing only. Use Full Chip Erase when flashing.

[sensei-hacker]

While reviewing this part, I noticed rcModeFlightModeMask()'s watched-switch list doesn't include BOXGCSNAV — so toggling that switch while a GCS-initiated RTH/PosHold override is active won't cancel the override, the way other flight-mode switches do.

Digging a bit further: the existing GCS-assisted guided-position commands (SET_POSITION_TARGET_GLOBAL_INT/_LOCAL_NED, WP 254/255) are gated on isGCSValid() / BOXGCSNAV being active, but the new arm/RTH/pause/land commands added in this part (activateRTHMode(), activatePositionHoldMode()) don't check BOXGCSNAV at all.

Is that intentional — i.e. is this feature designed so BOXGCSNAV is expected to always be on while a companion computer has authority, and toggling to any other mode should cancel the override? If so, it might make sense for arm/RTH/land/pause to also require BOXGCSNAV, and to add it to the watched-switch list. Or is there a reason those commands are meant to work independent of BOXGCSNAV? Want to make sure I understand the intended design here before flagging it as a gap.

[sensei-hacker]

A few more things I noticed while going through this part — flagging as questions since I may be missing context on the intended design:

mavlink_guided.c — float validation on SET_POSITION_TARGET_GLOBAL_INT/_LOCAL_NED
Most of the new float-parameter handling in this part (DO_SET_HOME, CONDITION_YAW, the DO_REPOSITION loiter-radius override) validates with isfinite()/range checks before converting to fixed-point, matching the pattern in mavlinkSetAltitudeTargetFromFrame(). In these two guided handlers though, msg.alt/msg.z go straight into a cast/lrintf() without that check first, and the fabsf(msg.x) > 0.01f "was motion requested" guard would pass a NaN through silently (NaN comparisons are always false). Was validation intentionally left out here, or would it make sense to route these through the same isfinite() pattern as the rest of the PR?

rc_modes.c — override slot gets cleared before the new mode is confirmed
rcModeSetActivationOverride() clears any existing override immediately, before activateRTHMode()/activatePositionHoldMode() know whether the FSM actually accepted the transition. If a prior override (e.g. PosHold) is active and a later RTH command is sent but the FSM declines it (e.g. a transient GPS/position-estimate hiccup while selectNavEventFromBoxModeInput() evaluates), the PosHold override is already gone and doesn't get restored on the RTH failure path. Net effect looks like it'd drop to raw RC with no active nav mode. Is there a reason the override is cleared up front rather than swapped only after the new state is confirmed, or is a save/restore-on-failure worth adding?

navigation.c — FW_LANDING_ABORT now goes to IDLE for commanded landings
For a mission-triggered landing abort, the existing behavior falls back to resume-waypoint or RTH. For a command-triggered landing (forcedLandingActivated), this part now routes the abort straight to NAV_STATE_IDLE (no nav assistance) instead. Given landing abort typically happens at low altitude/energy, was IDLE a deliberate choice for this case (e.g. because a companion computer is expected to be actively driving), or should this fall back to RTH/PosHold like the mission case?

navigation.c — NAV_FSM_EVENT_SWITCH_TO_LANDING bypasses the FSM transition table
navProcessFSMEvents() special-cases this one event to jump straight to NAV_STATE_WAYPOINT_RTH_LAND from any current state, with activateForcedLanding() manually replicating the setup a table-driven transition would normally do (resetPositionController(), resetAltitudeController(), etc.). Totally understand why — adding proper edges for every valid origin state is more work — just want to double check this is meant to be a permanent pattern rather than a placeholder, since it's easy for the manual setup to drift from the real initialization path as the FSM evolves.

fc_core.c — MSP/MAVLink disarm reason
Disarm via MSP2_INAV_ARM_DISARM or MAV_CMD_COMPONENT_ARM_DISARM currently logs DISARM_SWITCH, even though no RC switch was involved. Given disarm reason is used for blackbox/incident review, would a dedicated reason code (e.g. DISARM_MSP/DISARM_MAVLINK) be worth adding, or is reusing DISARM_SWITCH fine here?

Minor, non-blocking:

• MAV_CMD_CONDITION_YAW: targetHeadingCd is computed via the absolute-angle formula first, then fully overwritten when param4 != 0 — could branch once on relative/absolute instead, purely a readability thing.
• MAV_CMD_DO_SET_HOME: navCanSetHome() gets checked once in the handler and then again inside setWaypoint(0, ...) — harmless (single-threaded), just redundant.
• rcdevice_unittest.cc widens the test double's armingFlags from uint8_t to uint32_t — looks like a reasonable fix riding along, just flagging so it doesn't read as unrelated scope creep.

None of this is meant as a blocker dump — mostly trying to understand the intended design on a couple of these (especially the override-clearing order and the landing-abort fallback) before suggesting specific changes.

[xznhj8129]

While reviewing this part, I noticed rcModeFlightModeMask()'s watched-switch list doesn't include BOXGCSNAV — so toggling that switch while a GCS-initiated RTH/PosHold override is active won't cancel the override, the way other flight-mode switches do.

Digging a bit further: the existing GCS-assisted guided-position commands (SET_POSITION_TARGET_GLOBAL_INT/_LOCAL_NED, WP 254/255) are gated on isGCSValid() / BOXGCSNAV being active, but the new arm/RTH/pause/land commands added in this part (activateRTHMode(), activatePositionHoldMode()) don't check BOXGCSNAV at all.

Is that intentional — i.e. is this feature designed so BOXGCSNAV is expected to always be on while a companion computer has authority, and toggling to any other mode should cancel the override? If so, it might make sense for arm/RTH/land/pause to also require BOXGCSNAV, and to add it to the watched-switch list. Or is there a reason those commands are meant to work independent of BOXGCSNAV? Want to make sure I understand the intended design here before flagging it as a gap.

That's a good question because it calls into question the whole purpose of BOXGCSNAV as a user-switched mode-but-not-really thing, it's a legacy concept that doesn't really make a whole lot of sense now that there are multiple types of modes and actions that can be triggered by a GCS. This PR doesn't change the existing GCS NAV semantics, but a follow-up is going to look into using the (currently used by Geozone) SEND_TO FSM path into a generic external GOTO primitive shared by MSP and MAVLink, and i think we're going to do away with the GCS NAV switch completely.

[sensei-hacker]

Gentle reminder — outstanding review items

Following up on the review from 2026-08-03 above. Thanks for the reply on the BOXGCSNAV question — makes sense as a "not in scope for this part" given the planned follow-up.

The rest of that review is still open, in particular two items that look safety-relevant and haven't had a response yet:

1. rc_modes.c — override cleared before the new mode is confirmed. rcModeSetActivationOverride() clears any existing override immediately, before activateRTHMode()/activatePositionHoldMode() know whether the FSM actually accepted the transition. If a prior override (e.g. PosHold) is active and a later RTH command is declined by the FSM (e.g. a transient GPS/position-estimate hiccup), the PosHold override is already gone and isn't restored — net effect looks like it drops to raw RC with no active nav mode.
2. mavlink_guided.c — missing isfinite() validation on SET_POSITION_TARGET_GLOBAL_INT/_LOCAL_NED. Most of this PR's new float handling validates with isfinite()/range checks before converting to fixed-point (matching mavlinkSetAltitudeTargetFromFrame()), but msg.alt/msg.z in these two handlers go straight into a cast/lrintf() without that check, and the fabsf(msg.x) > 0.01f guard would let a NaN through silently.

Three more from that same comment, no response needed on these if they're intentional — just flagging so they don't get lost:
3. Commanded landing abort now falls to NAV_STATE_IDLE instead of RTH/PosHold (mission-triggered abort still falls back normally) — deliberate for the command case, or worth aligning?
4. NAV_FSM_EVENT_SWITCH_TO_LANDING bypasses the FSM transition table with manual setup in activateForcedLanding() — fine as a permanent pattern, or a placeholder?
5. MSP/MAVLink-triggered disarm is logged as DISARM_SWITCH even though no RC switch was involved — worth a dedicated reason code for blackbox/incident review?

No pressure on timeline, just want to make sure these didn't slip through given the gap since the last update. Happy to help narrow down a fix for #1/#2 if useful.

[xznhj8129]

Sorry, i had written up a response but apparently i forgot to actually send it... The PR is on the workbench, i'll address and respond tomorrow

[xznhj8129]

Sorry for the delay

A few more things I noticed while going through this part — flagging as questions since I may be missing context on the intended design:

mavlink_guided.c — float validation on SET_POSITION_TARGET_GLOBAL_INT/_LOCAL_NED Most of the new float-parameter handling in this part (DO_SET_HOME, CONDITION_YAW, the DO_REPOSITION loiter-radius override) validates with isfinite()/range checks before converting to fixed-point, matching the pattern in mavlinkSetAltitudeTargetFromFrame(). In these two guided handlers though, msg.alt/msg.z go straight into a cast/lrintf() without that check first, and the fabsf(msg.x) > 0.01f "was motion requested" guard would pass a NaN through silently (NaN comparisons are always false). Was validation intentionally left out here, or would it make sense to route these through the same isfinite() pattern as the rest of the PR?

agreed, that was missed. I’ll add that validation.

rc_modes.c — override slot gets cleared before the new mode is confirmed rcModeSetActivationOverride() clears any existing override immediately, before activateRTHMode()/activatePositionHoldMode() know whether the FSM actually accepted the transition. If a prior override (e.g. PosHold) is active and a later RTH command is sent but the FSM declines it (e.g. a transient GPS/position-estimate hiccup while selectNavEventFromBoxModeInput() evaluates), the PosHold override is already gone and doesn't get restored on the RTH failure path. Net effect looks like it'd drop to raw RC with no active nav mode. Is there a reason the override is cleared up front rather than swapped only after the new state is confirmed, or is a save/restore-on-failure worth adding?

Yes, this should preserve the previous temporary nav override if the requested replacement is rejected by the FSM. I’ll fix this so a failed RTH/PosHold transition can’t silently drop an existing override and fall back to raw RC.

navigation.c — FW_LANDING_ABORT now goes to IDLE for commanded landings For a mission-triggered landing abort, the existing behavior falls back to resume-waypoint or RTH. For a command-triggered landing (forcedLandingActivated), this part now routes the abort straight to NAV_STATE_IDLE (no nav assistance) instead. Given landing abort typically happens at low altitude/energy, was IDLE a deliberate choice for this case (e.g. because a companion computer is expected to be actively driving), or should this fall back to RTH/PosHold like the mission case?

greed this should not fall to IDLE. For a command-triggered fixed-wing landing, abort will fall back to RTH rather than resuming an unrelated mission or leaving navigation entirely.

navigation.c — NAV_FSM_EVENT_SWITCH_TO_LANDING bypasses the FSM transition table navProcessFSMEvents() special-cases this one event to jump straight to NAV_STATE_WAYPOINT_RTH_LAND from any current state, with activateForcedLanding() manually replicating the setup a table-driven transition would normally do (resetPositionController(), resetAltitudeController(), etc.). Totally understand why — adding proper edges for every valid origin state is more work — just want to double check this is meant to be a permanent pattern rather than a placeholder, since it's easy for the manual setup to drift from the real initialization path as the FSM evolves.

this one is intentional. Commanded LAND needs to be able to break into the landing path from arbitrary nav states, including RTH. I had trouble using the table-driven transition because it made it so RTH couldn't be exited/aborted once entered. So the cross-state SWITCH_TO_LANDING handling is intended as a deliberate exception rather than a placeholder.

fc_core.c — MSP/MAVLink disarm reason Disarm via MSP2_INAV_ARM_DISARM or MAV_CMD_COMPONENT_ARM_DISARM currently logs DISARM_SWITCH, even though no RC switch was involved. Given disarm reason is used for blackbox/incident review, would a dedicated reason code (e.g. DISARM_MSP/DISARM_MAVLINK) be worth adding, or is reusing DISARM_SWITCH fine here?

that could be worth adding later, telemetry logging would cover this case in the meanwhile

the other minor ones i'll keep in mind for 10.x improvements