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 3efee9d

Download firmware for PR #11717

243 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.