MSP-over-MAVLink tunnel

[xznhj8129]

⚠ Part 7/7 of the mavlink_multiport2 split — depends on #11717.
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:

• `4a9ff41b1` Add MSP-over-MAVLink tunnel
• `942c32cb6` Add MAVLink user documentation and regenerate Settings.md
• `d972b07c1` Add MSP-over-MAVLink tunnel smoke test

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

Part 7/7 of the mavlink_multiport2 stack — final part; the tree now carries the complete feature set.

MSP-over-MAVLink tunnel

MSP transport over the MAVLink Tunnel service (private payload type 0x8001, MAVLink 2 only), so the INAV Configurator can talk MSP over an existing MAVLink telemetry link — typically a radio that exposes no separate MSP transport. The companion configurator PR adds a "MAVLink Tunnel" option under Wireless mode with SYSID selection for multi-vehicle networks.

• Reuses the existing MSP parser/encoder through three narrow msp_serial seams (byte parser, encode-to-buffer, command processing) — no duplicate MSP framing logic.
• Replies are fragmented across TUNNEL messages and returned on the ingress port only; target_component = 0 is accepted for local delivery but never fanned out to other ports.
• MSP framing symmetry is preserved end-to-end: MSPv1 requests get MSPv1 replies, MSPv2 gets MSPv2.
• MSP_REBOOT post-processing is supported over the tunnel; serial passthrough and ESC 4-way passthrough are rejected before dispatch.
• Malformed payload lengths are dropped without poisoning parser state; stale partial frames reset after a timeout; a new sender displaces a stale partial transaction.
• Reply/frame buffers are file-scope, not telemetry-task stack (avoids stack overflow on FLASHFS targets).
• Not a general-purpose serial tunnel: CLI mode is not carried over this path.

Documentation

This PR also lands the complete user-facing docs/Mavlink.md (multiport/routing, datastream groups and CLI settings, mLRS integration, supported messages/commands, mode mappings, mission behavior and MSP parity gaps, tunnel, high-latency mode) and regenerates docs/Settings.md for the per-port mavlink_port{1-4}_* settings.

Testing

• Full mavlink_unittest suite: 86/86 passing (tunnel cases: malformed-length rejection, pre-dispatch passthrough rejection, reboot ingress-port handling, stale-frame reset, multi-message reply fragmentation).
• Full SITL build, warnings-as-errors, clean.
• Previously validated live end-to-end on the development branch: Configurator connect over tunnel, EEPROM write, reboot, reconnect after reboot.
• Includes the live tunnel smoke-test harness (src/test/mavlink/tunnel/) covering that same sequence against a running SITL/FC.
• Configurator branch tested, Tunnel works over ELRS-Mavlink and mLRS

Companion PRs: inav-configurator:mavlink_multiport2 (tunnel UI + wrapping), mspapi2:mavlink_multiport2.

[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 →

[xznhj8129]

This #11472 PR has been split into a sequence of PR's by inav-claude for easier review and handling, but i'll include a whole laydown of what it entails as final product here.

#	PR	Branch	Contents	Tests
1	#11712	mav/01-sitl-fixes	SITL MAVLink-RX gate, SITL load accounting, X-Plane altitude inputs	—
2	#11713	mav/02-headers	STorM32 dialect regen + generate.sh fix + include switch	—
3	#11714	mav/03-core	modular split, multi-port runtime, routing, half-duplex, mLRS, HIGH_LATENCY2, port benchmark harness	19/19
4	#11715	mav/04-streams-protocol	PING/TIMESYNC, stream/interval control, protocol replies, GPS accuracy, STATUSTEXT behaviors	38/38
5	#11716	mav/05-mission	mission transfer state machine + translation + mission rig + routing harness + SITL sanity harness	58/58
6	#11717	mav/06-command-guided	arm/RTH/LAND/home/mode commands, MSPv2 parity, PF ops, guided, loiter-radius override	81/81
7	#11718	mav/07-tunnel	MSP-over-MAVLink tunnel + smoke test + user docs (docs/Mavlink.md, Settings.md)	86/86 full

Upstream PRs can't base on fork branches, so all 7 target maintenance-10.x; each body declares its own commit range with a fork compare link. As each part merges, the next one's diff auto-cleans.

What this is

A ground-up rework of INAV's MAVLink implementation: from a single-port, single-file telemetry bolt-on into a modular MAVLink subsystem with:

• concurrent multi-port support
• ArduPilot-style routing
• a real mission protocol
• GCS command parity
• protocol services
• native mLRS integration
• high-latency mode
• MSP-over-MAVLink tunneling so the Configurator can ride an existing telemetry radio

Routing design follows ArduPilot's MAVLink_routing.cpp. The mission transfer state machine is adapted from Betaflight's GPLv3 mavlink_mission.c (87d4bd63) and reshaped for the routed multi-port runtime.

Developed on inav:mavlink_multiport2, shipped upstream as a 7-part stacked PR series rebuilt on current maintenance-10.x.

Architecture

The monolithic src/main/telemetry/mavlink.c (~1.5k LOC) became modules under src/main/mavlink/, with telemetry/mavlink.c reduced to a thin telemetry-API adapter:

Module	Owns
mavlink_runtime.c	shared context, active-port switching, RX loop, TX fan-out, half-duplex backoff
mavlink_ports.c	port lifecycle (configure/free), per-port state reset
mavlink_streams.c	stream scheduling, per-message intervals, all periodic senders, STATUSTEXT
mavlink_routing.c	route learning (sysid, compid) → ingress port, forwarding decisions
mavlink_mission.c	mission transfer state machine + MAVLink↔INAV translation
mavlink_modes.c	ArduPlane/ArduCopter custom_mode maps (+ optional Standard Modes)
mavlink_command.c	MAV_CMD_* handling
mavlink_guided.c	SET_POSITION_TARGET / frame helpers / altitude targets
fc/fc_mavlink.c	incoming-message dispatch, mLRS handlers, MSP tunnel
mavlink_internal.h / mavlink_types.h	shared private context / shared types

Headers: built against the checked-in STorM32 dialect (337-message STorM32/ArduPilot/common superset) because it carries OlliW's mLRS-specific messages. The regen moved the merged MAV_CMD enum into storm32/storm32.h; the first include (rx/mavlink.h) points there, and the old local APM mode-enum copies were dropped for the generated PLANE_MODE/COPTER_MODE.

Feature laydown

Multi-port

• Up to 4 concurrent MAVLink ports (MAX_MAVLINK_PORTS) with per-port runtime state.
• Per-port settings: mavlink_port{1-4}_{ext_status,rc_chan,pos,extra1,extra2,extra3}_rate, _min_txbuffer, _radio_type (GENERIC/ELRS/SIK/MLRS), _high_latency.
• Common settings: mavlink_sysid, mavlink_autopilot_type (GENERIC/ARDUPILOT), mavlink_version.
• Port 1 streams fully by default; ports 2–4 boot heartbeat-only until a GCS requests streams.
• Flow control uses RADIO_STATUS.txbuf (or mLRS flow control) against min_txbuffer; without flow-control input the port falls back to blind 20 ms frame pacing.
• Fixed local identity (mavlink_sysid, MAV_COMP_ID_AUTOPILOT1) — no per-port FC compids; attached devices are learned as remote components.
• RX parsing bounded per scheduler invocation; periodic deadlines phased against startup bursts.
• MAVLink moved behind the MCU_FLASH_SIZE > 512 gate.

Routing

• Routes learned from incoming traffic as (sysid, compid) → ingress port.
• Targeted messages forwarded only on known routes; broadcasts fanned to other ports without repack.
• RADIO_STATUS and mLRS flow control stay local to their ingress port.
• Local/system broadcasts execute locally for control-class messages.
• Same-sysid different-compid traffic is a remote component, not dropped as self.
• Caveat: first targeted frame to a never-seen endpoint won't forward until that endpoint has transmitted once.

Streams & protocol services

• Legacy REQUEST_DATA_STREAM group control plus per-message SET_MESSAGE_INTERVAL/GET_MESSAGE_INTERVAL overrides layered on top.
• MAV_CMD_REQUEST_MESSAGE (incl. MESSAGE_INTERVAL query).
• REQUEST_PROTOCOL_VERSION and REQUEST_AUTOPILOT_CAPABILITIES (advertises ArduPilot-compatible 4.7.0 + MAVLink2 / mission / command-int / set-position-target capabilities).
• GET_HOME_POSITION.
• Protocol-correct PING and TIMESYNC (monotonic ns boot clock).
• SYSTEM_TIME with RTC Unix time when valid.

Telemetry detail

• EXTENDED_SYS_STATE landed-state reporting.
• UBlox ellipsoid altitude and speed/heading accuracy retained and emitted in GPS_RAW_INT.
• Changed arming-disable reasons broadcast as STATUSTEXT once a port is active.
• INAV flight-mode transitions broadcast as Notice: INAV: Entering <mode> with explicit GCS NAV enter/exit.
• OSD-derived STATUSTEXT de-spammed per port (changed text immediate; unchanged notice 30 s / warning 10 s / critical 5 s) instead of every EXTRA3 tick.

Missions

• Owned transfer state machine: partner + ingress-port bound, upload retries (1.5 s × 5), download timeout (5 s), one-item retransmission, out-of-sequence recovery, explicit cancel.
• Legacy MISSION_REQUEST answered with MISSION_ITEM_INT.
• Uploads are staged — translate whole mission → temp list → validate → commit — so failures never half-write.
• Uploads accept up to NAV_MAX_WAYPOINTS * 4 + 1 MAVLink items; QGC planned-home item 0 skipped; NaN/unused params tolerated.
• Modifier items (DO_CHANGE_SPEED, CONDITION_DELAY, altitude changes) fold onto the correct waypoint preserving p3 bitfield semantics.
• Persistence-failure restores the previous mission.
• 1 Hz MISSION_CURRENT; MISSION_ITEM_REACHED broadcast from a navigation-side latch.
• Caveat: translation is lossy (leg speed, LAND elevation, RTH land flag, p3 user bits); downloads are best-effort reconstruction, not canonical backups.

Command parity (MAVLink + MSPv2 + Programming Framework)

• Arm/disarm: MAV_CMD_COMPONENT_ARM_DISARM / MSP2_INAV_ARM_DISARM (0x2227) through the normal arming path; ACK success only when the state is reached.
• RTH: NAV_RETURN_TO_LAUNCH, ArduPilot DO_SET_MODE RTL, MSP2_INAV_ACTIVATE_RTH (0x2225), PF op 61 → activateRTHMode() (temporary BOXNAVRTH RC-mode source, not the failsafe/geozone forced-RTH latch); cleared by pilot mode change or disarm.
• Pause: DO_SET_MODE Loiter/PosHold/Brake → normal PosHold at current position via temporary BOXNAVPOSHOLD (what QGC Pause sends).
• Land: NAV_LAND / MSP2_INAV_ACTIVATE_LANDING (0x2224) / PF op 62 → normal waypoint-style landing at current position via transient waypoint; mission untouched; not emergency landing.
• Home: DO_SET_HOME through the native waypoint-0 backend with all its gates.
• Time: MSP2_INAV_TIMESYNC (0x2228) = same ns boot clock as MAVLink TIMESYNC.
• Guided: DO_REPOSITION (waypoint 255), SET_POSITION_TARGET_GLOBAL_INT/_LOCAL_NED (position or altitude-only), DO_CHANGE_ALTITUDE, CONDITION_YAW (the latter two originating from the earlier mav_fix2 work) — all gated on valid GCS navigation and hardened against non-finite/out-of-range params.
• Loiter radius: temporary fixed-wing PosHold override via DO_REPOSITION.param3 (m) or appended int32 cm on MSP2_INAV_SET_GLOBAL_TARGET; reported in MSP2_INAV_NAV_TARGET; volatile, PosHold-only, cleared on disarm/reboot.
• NAV_TAKEOFF: explicit UNSUPPORTED stub.
• OSD shows GCSN while GCS nav is active.

mLRS integration

• With radio_type = MLRS: native MLRS_RADIO_LINK_STATS / _INFORMATION / _FLOW_CONTROL from MAV_COMP_ID_TELEMETRY_RADIO, stored per ingress port.
• Only the MAVLink-serial-RX port updates global rxLinkStatistics; flow control stays port-local.
• RADIO_STATUS remains the GENERIC/ELRS/SIK path.

High latency

• Per-port HIGH_LATENCY2 (setting or MAV_CMD_CONTROL_HIGH_LATENCY), MAVLink2-only.
• Replaces normal streams with one summary every 5 s for satellite/cellular/LoRa-class links.

MSP-over-MAVLink tunnel

• TUNNEL payload type 0x8001, MAVLink2-only.
• MSP parser/encoder reused via three narrow msp_serial seams.
• Replies fragmented on the ingress port only; MSPv1↔v1 / v2↔v2 framing preserved.
• MSP_REBOOT supported with post-process; serial/ESC-4way passthrough rejected pre-dispatch.
• Malformed lengths dropped, stale partials time out; file-scope buffers (no task-stack overflow).
• Not a generic serial tunnel; no CLI.
• Configurator side: "MAVLink Tunnel" under Wireless mode with SYSID selection (0 = first heartbeat), source identity 253/25, tunnel off by default per session, CLI tab hidden; inbound truncated MAVLink2 payload tails are zero-padded back to payload_length before MSP parsing.
• Tested over ELRS-MAVLink and mLRS.

SITL/simulator fixes

• SITL now defines USE_TELEMETRY_MAVLINK/USE_SERIALRX_MAVLINK explicitly (the flash-size gate predates SITL's TARGET_FLASH_SIZE, so serialrx_provider = MAVLINK used to silently reset to NONE).
• SITL load accounting uses busy-time-over-wall-time excluding the host sleep (kills false SYSTEM OVERLOADED arming blocks).
• X-Plane feeds GPS NED velocity from local velocity and fake baro from aircraft elevation (fixes fixed-wing loiter sink).

Design decisions worth remembering

• RTH backend: command RTH ≠ forced RTH. activateForcedRTH() sets the failsafe/geozone latch and is not equivalent to switching modes — proven in live testing. Commands use the temporary RC-mode-source mechanism instead.
• One local identity: transport ≠ component. The FC is always AUTOPILOT1; routing learns everything else. Per-port compid settings were removed.
• Mode reporting: ArduPilot heartbeat custom_mode is the compatibility path; MAVLink Standard Modes is compile-gated (USE_MAVLINK_STANDARD_MODES) and off — advertising GUIDED as a standard mode confused QGC.
• Half-duplex: keyed to serialrx_halfduplex on the shared RX port, not telemetry_halfduplex (using the latter caused a benchmark regression).
• STorM32 dialect: chosen for the mLRS messages; superset of ardupilotmega+common.
• Stack base: rebuilt on current maintenance-10.x, so the feared upstream re-merge is already done, in reviewable pieces.

Verification

• mavlink_unittest: 86/86 at the tip; per-part slices 19/38/58/81 all green.
• Full SITL builds warnings-as-errors clean at every stack point (incl. the headers-only intermediate).
• Live (on the dev branch):
  • routing compliance 11/11
  • mission rig 8/8
  • SITL sanity rig 8/8 checks
  • MSP tunnel end-to-end incl. reboot+reconnect
  • companion mspapi2 command helper tests 4/4
  • X-Plane + QGC flights — missions load and fly, pause/RTH/land behave
• In-tree harnesses: src/test/mavlink/{routing,missions,ports,sanity,tunnel}/.
• Benchmark baselines (short run, 1/2/3/4 ports): MSP failure rates ~2.7% / 4.0% / 0.7% / 1.3%.

Known limits / non-goals

• Not a full ArduPilot/PX4-class MAVLink stack.
• Parameter API is a deliberate stub — configure via Configurator/CLI.
• Mission support is intentionally partial vs INAV's MSP model.
• Ports 2–4 don't stream by default.
• Directed forwarding needs route learning.
• No CLI over the tunnel.
• NAV_TAKEOFF recognized but unsupported.
• Open questions from the original draft still stand:
  • is 4 ports the right cap?
  • final flash/RAM cost across representative targets?
  • should the tunnel payload type be registered formally with MAVLink?

[sensei-hacker]

Went through this part's isolated diff (the MSP-over-MAVLink tunnel) — a couple of things I noticed, flagging as questions since I may be missing context:

msp_serial.c — mspSerialEncodePacket() duplicates mspSerialEncode()'s framing/CRC logic
The new mspSerialEncodePacket() (writes to a caller-supplied buffer instead of streaming to a serial port) looks like a near line-for-line copy of the existing mspSerialEncode() — same header construction and CRC calculation for MSP_V1/MSP_V2_OVER_V1/MSP_V2_NATIVE, just a different sink at the end. That means the wire-framing/CRC logic now exists in two places that'd need to be kept in sync by hand. Given the PR description calls out "no duplicate MSP framing logic" as a design goal, would it make sense to factor the shared header/CRC-building into a helper that both mspSerialEncode() (calls mspSerialSendFrame()) and mspSerialEncodePacket() (does a memcpy into the caller's buffer) build on top of?

mavlink_internal.h — tunnel buffer RAM footprint
tunnelReplyPayloadBuf[MSP_PORT_OUTBUF_SIZE] and tunnelFrameBuf[MAVLINK_TUNNEL_MSP_FRAMEBUF_SIZE] are unconditional fields of mavlinkContext_t. On USE_FLASHFS builds MSP_PORT_OUTBUF_SIZE is 4112 bytes, so together these two add ~8.2KB of static BSS whenever USE_TELEMETRY_MAVLINK is enabled — including on boards that only want ordinary MAVLink telemetry and never use the tunnel feature. Was that traded off deliberately (e.g. simplicity vs. an extra feature define), or would it be worth gating these two buffers behind their own USE_MAVLINK_MSP_TUNNEL-style define so non-tunnel builds don't pay for them? Also wondering if CI's build-size checks were run against the smallest F405+FLASHFS+MAVLink target combination in the tree.

fc_mavlink.c — handleIncoming_TUNNEL() size
This one function handles version/target gating, stale-state timeout, the MSP parse loop, the passthrough pre-dispatch gate, dispatch, post-process gating, and reply transmission — quite a bit in one place for code this safety-relevant (passthrough/reboot gating). Would splitting out the staleness handling and the post-process gating into small named helpers be worth it for readability, or is keeping it together intentional here?

Minor, non-blocking:

• The staleness check uses > against the 1000ms timeout, so a message arriving at exactly the boundary doesn't reset stale state — probably fine, just noting the edge.
• The 3-line tunnel-state reset sequence appears in both resetMAVLinkPortRuntimeState() (mavlink_ports.c) and mavlinkResetTunnelState() (fc_mavlink.c) — could the latter be exposed and reused instead of duplicated?
• If a single TUNNEL message happens to contain more than one complete MSP frame back-to-back, only the first is processed and the rest of that message's payload is discarded (unlike plain-serial, where leftover bytes stay in the RX FIFO for the next tick). Probably fine for the Configurator's request/response pattern — might be worth a short comment noting the difference in semantics.

On the positive side, the passthrough/4-way and reboot gating logic checked out against fc_msp.c (the pre-dispatch rejection genuinely prevents mspSerialPassthroughFn/esc4wayProcess from ever being reached over the tunnel), the file-scope buffers really do avoid the stack-overflow risk the PR description calls out, and CLI mode does look unreachable via this path. Nice test coverage on the safety-relevant cases too (malformed length, stale-frame reset, passthrough rejection).

[sensei-hacker]

One more thought on the tunnelFrameBuf RAM point above — wanted to sketch what I meant, since "eliminate the buffer" is easy to say and harder to picture:

The header (≤16 bytes) and CRC (1-2 bytes) that mspSerialEncodePacket() builds are tiny, and the payload data is already sitting in tunnelReplyPayloadBuf. Right now those three pieces get copied into one contiguous ~4.1KB tunnelFrameBuf just so the fragmentation loop can walk a single buffer in 128-byte chunks. Could that loop instead treat {header, data, crc} as three segments of one virtual stream and pull each 128-byte TUNNEL chunk from whichever segment(s) it falls in — occasionally straddling a boundary — without ever concatenating them?

Something like:

static void mavlinkSendTunnelMspReply(...)
{
    uint8_t hdrBuf[16];
    uint8_t crcBuf[2];
    int hdrLen, crcLen;
    // build header+crc only, no data copy - data stays where it already is
    int dataLen = mspSerialBuildFrameHeader(reply, mspVersion, hdrBuf, &hdrLen, crcBuf, &crcLen);
    const uint8_t *dataBuf = replyPayloadHead;  // already in tunnelReplyPayloadBuf

    const int totalLen = hdrLen + dataLen + crcLen;
    uint8_t chunk[MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN];

    for (int offset = 0; offset < totalLen; offset += sizeof(chunk)) {
        int chunkLen = MIN((int)sizeof(chunk), totalLen - offset);
        int written = 0;
        // each call copies whatever part of [offset, offset+chunkLen) falls
        // within that segment, returns bytes written, 0 if no overlap -
        // a chunk boundary can straddle header/data or data/crc
        written += copyFromSegment(chunk + written, hdrBuf,  hdrLen, offset,                     chunkLen - written);
        written += copyFromSegment(chunk + written, dataBuf, dataLen, offset - hdrLen,            chunkLen - written);
        written += copyFromSegment(chunk + written, crcBuf,  crcLen,  offset - hdrLen - dataLen,  chunkLen - written);

        mavlinkSendTunnelReply(targetSystem, targetComponent, chunk, chunkLen);
    }
}

That would drop tunnelFrameBuf entirely — just a 16-byte header buf, a 2-byte CRC buf, and a 128-byte chunk buf instead of a ~4.1KB frame buf, so roughly 4KB less static RAM for this feature. Does something along these lines seem reasonable, or is there a reason the current single-buffer-then-fragment approach was preferred (simplicity, an edge case I'm not accounting for)?