Proposal: MSP2_INAV_TELEMETRY — subscription-based push telemetry over MSP

[b14ckyy]

Problem

MSP has no telemetry mode. A ground station that wants live data has to poll every message at the rate it needs, which means:

• permanent two-way traffic and a request scheduler on the GCS side,
• the FC answers at most one command per port per TASK_SERIAL run — measured at ~48 replies/s under HITL while a 115200 link idles at ~20 % load,
• every reply carries its full legacy payload, whether the GCS needs those fields or not.

MAVLink's stream subscription is the right direction but coarse: feeding a GCS takes 15–20 predefined message subscriptions with fixed content.

Proposal

One new MSPv2 command, MSP2_INAV_TELEMETRY (proposed ID 0x2110, reserving 0x2110–0x211F), that lets the GCS compose its own stream:

• data is grouped into blocks (status, attitude, sensors, nav, GCS, thermal, ESC, text — see the reply below for the full catalog),
• the GCS subscribes each block with a rate and a field bitmask, in one round trip per block,
• the FC then pushes the frames autonomously as long as keepalives arrive.

MSP stays request→response for everything else. The push uses the mechanism MSP DisplayPort and the debug log already rely on (mspSerialPushPort), so no new serial plumbing.

Measured against a real GCS polling set with strictly the same fields, the subscription stream needs 35 % fewer bytes, 98 % less uplink and 67 % fewer frames.

Wire format

Request (4 bytes)

Offset	Bits	Meaning
0	7–4	block ID (0–14; 15 = clear + capability)
0	3–0	rate index (0 = one-shot, 1–12 = table below, 13–15 reserved → error)
1–2	15–0	field mask, bit N = field N of the block (u16 LE)
3	7–0	reserved, 0 (mirrored in every push; earmarked for a multi-client ID later)

The payload length selects the request type:

Payload	Meaning
0 bytes	keepalive → answered with an empty ack
4 bytes, block 15	clear all subscriptions on this port + capability response
4 bytes, block 0–14	subscribe / reconfigure / one-shot for that block
other	error

Rate index	1	2	3	4	5	6	7	8	9	10	11	12
Hz	0.25	0.5	1	2	3	4	5	6	7	8	9	10

Per block one active subscription per port; a new request for the same block replaces it. Mask 0 with rate ≠ 0 unsubscribes the block. One-shots (rate 0) are independent of subscriptions and answer once with their own mask. 10 Hz per block is the ceiling by design.

Push / response frame

Every data frame — one-shot reply and periodic push alike — is an MSP2_INAV_TELEMETRY frame:

Offset	Meaning
0	block ID (7–4) and rate index (3–0; 0 for one-shot replies)
1	reserved byte mirrored from the request
2…	the requested fields, ascending bit order, fixed sizes per block
last	rolling counter (u8, shared across all pushes of a port; auto-broadcasts only)

The mask is not echoed; the GCS parses with the mask it requested, and the frame always contains exactly the requested fields:

• a requested field the UAV cannot provide (build option or sensor missing → capability bit 0) is sent zero-filled, never omitted;
• a mask bit beyond the fields the block defines in this firmware → error (an undefined bit has no defined size, so no parseable frame could be built).

Acks and session

• Subscribe / reconfigure is acknowledged with a verbatim echo of the 4 control bytes before streaming with the new config starts. Serial ordering makes reconfigures unambiguous: frames before the echo are old-config, everything after is new-config.
• Block 15 clears every subscription on the port and answers with the capability payload: one u16 field-availability mask per block, ascending block order (build options and sensor presence). A session start is: clear → learn capabilities → subscribe.
• Any received MSP2_INAV_TELEMETRY frame resets the port's keepalive timer. GCS contract: keepalive at least every 2 s; the FC pauses pushes after 2.5 s without one and resumes all subscriptions unchanged on the next keepalive (survives link drops without re-setup). Config is erased only by block 15 or reboot; it is RAM-only, never persisted.
• No protocol version field: evolution is append-only (new fields at the tail of a block, new blocks), like the OSD character map. An older GCS never requests bits it doesn't know; the version hangs off the INAV version.

FC side

• Per-port subscription state next to mspPort_t: { u16 mask; u8 rate; timeUs_t lastPush; } × 15 blocks plus keepalive timestamp and counter ≈ 120 B per port. Multi-port is intended; the real cost gets measured first.
• Emission from the serial task via mspSerialPushPort(..., MSP_V2_NATIVE).
• Regular MSP request/response always has priority; telemetry yields behind it in the TX buffer.
• Never queue backlog: a tick that cannot go out (TX buffer full, task late, guard) is skipped and the next tick sends fresh data. Telemetry wants freshness, not completeness.
• Aggregate guard per port, currently 50 msg/s; a byte budget per second (CLI setting, matched to the link) may be the better metric given block sizes from 22 to 103 B.
• Feature-gated (USE_MSP_TELEMETRY), F722 included until the flash limit forces the question.

Open points

1. Command ID: reserve 0x2110–0x211F?
2. Flow control on a saturated OTA link. The FC cannot see it: it only sees its 256 B UART TX ring, and mspSerialSendFrame already drops a push that does not fit — which only helps when the UART baud is the bottleneck. A radio module that buffers and drops internally is invisible (no UART hardware flow control in INAV). The GCS is the only party that can detect loss (rolling counter gaps, subscribed vs. received rate) and it already has the lever (reconfigure to a lower rate/mask). Options if FC-side adaptation is wanted, all append-compatible: (a) a 1-byte keepalive payload = frames received since the last keepalive, so the FC can compute a delivery ratio and throttle, with the mirrored rate nibble redefined as the effective rate; (b) a "skipped pushes" counter in the keepalive ack; (c) the byte-budget setting. My preference for V1: GCS-side control + (b) + (c).
3. Block content — see the reply below.

Timing: no code until 10.0 is locked; implementation is planned for the post-10.0 window. Feedback on the mechanism and on the catalog is what I am after here.

[b14ckyy]

Block catalog

Ground rules

• Units: cm, cm/s, decidegrees, 0.01 V, 0.01 A, 1e-7° lat/lon, whole °C. No inherited MSP quirks: signedness by physics (altitudes signed, current signed for regen/charge, voltage unsigned).
• Attitude signs = INAV convention: pitch positive = nose down, roll and yaw positive = right — the sign of the stick that produces it.
• Every field has a fixed size; values only ever consumed together share one bit (lat+lon, dBm+SNR, airspeed+source).
• Two invalidity mechanisms, never mixed: capability-0-but-requested → zero-filled; momentarily invalid → per-type sentinel (table at the end). Capability bits reflect build options and sensor presence (e.g. raw airspeed needs a physical pitot, ESC bits follow the telemetry motor count, current follows isAmperageConfigured()).
• Frame = 2 B header + fields + 1 B counter; MSPv2 adds 9 B on the wire.

Block	Fields max	On the wire (all fields)
0 Status	38 B	50 B
1 Attitude	23 B	35 B
2 Sensors (raw)	26 B	38 B
3 Nav	47 B	59 B
4 GCS	42 B	54 B
5 Thermal	10 B	22 B
6 ESC	91 B	103 B
7 Text	29 B	41 B

Every realistic subscription is a small fraction of this; the per-port guard caps the worst case anyway.

Block 0 — Status

Bit	Field	Size	Encoding	Source
0	Core flags	u16	bits 0–1 arm state (0 not ready, 1 ready, 2 armed); 2 failsafe active; 3 auto-throttle; 4 GCS command valid; 5 sensors calibrating; 6 landing detected; 7 RX link lost; 8–12 last disarm reason (disarmReason_e); 13–15 reserved	armingFlags, failsafeIsActive(), navigationIsControllingThrottle(), isGCSValid(), areSensorsCalibrating(), STATE(LANDING_DETECTED), rxIsReceivingSignal(), getDisarmReason()
1	Flight mode	char[4]	exactly the OSD FLYMODE text ("ANGL", "CRUZ", "!FS!", …) via a shared helper	OSD OSD_FLYMODE logic
2	Active box modes	16 B	activeModes bitmask verbatim (same bytes as MSP2_INAV_STATUS), zero-padded to 128 bits; GCS maps it with MSP_BOXIDS once at connect	packBoxModeFlags()
3	Sensor health	u16	2 bits per sensor × 8 (gyro, acc, mag, baro, GPS, rangefinder, pitot, opflow) = hardwareSensorStatus_e	MSP_SENSOR_STATUS order
4	Arming disable flags	u32	armingFlags verbatim	MSP2_INAV_STATUS
5	Uptime	u32	s	MSP2_INAV_MISC2
6	Flight time	u32	s	getFlightTime()
7	Active profiles	u8	bits 0–1 control, 2–3 battery, 4–5 mixer	getConfig*Profile()
8	CPU load	u8	%	averageSystemLoadPercent

Block 1 — Attitude (the real-time block)

Bit	Field	Size	Encoding	Source
0	Roll	i16	decideg	attitude.values.roll
1	Pitch	i16	decideg, nose-down positive	attitude.values.pitch
2	Yaw	u16	decideg	attitude.values.yaw
3	G-forces X/Y/Z	3×i8	0.1 g, ±12.7 g	acc.accADCf[]
4	Fused altitude	i32	cm, relative to the estimator origin	getEstimatedActualPosition(Z)
5	Vertical speed	i16	cm/s, up positive	getEstimatedActualVelocity(Z)
6	AGL	u32	bit 31 source (1 rangefinder, 0 terrain data), bit 30 reserved, bits 0–29 signed cm; rangefinder in range has priority	surface estimate / terrain data
7	Throttle	u8	%, scaled (mixer throttle between idle and max, includes auto-throttle)	getThrottlePercent(true)
8	Airspeed + source	u16 + u8	cm/s, best available; source 0 none, 1 pitot, 2 GPS-virtual	getAirspeedEstimate()

Block 2 — Sensors (raw)

Bit	Field	Size	Encoding	Source
0	Raw baro altitude	i32	cm	baroGetLatestAltitude()
1	Raw rangefinder	u16	cm	rangefinderGetLatestAltitude()
2	Battery voltage	u16	0.01 V, pack	getBatteryVoltage()
3	Current	i16	0.01 A, signed	getAmperage()
4	mAh drawn	u32	mAh	getMAhDrawn()
5	Battery state	u16	bits 0–1 state, 2–5 cells, 6–12 percent, 13 was full when plugged in, 14–15 reserved	getBatteryState(), getBatteryCellCount(), calculateBatteryPercentage()
6	Raw airspeed	u16	cm/s, physical pitot only	pitot.airSpeed
7	RSSI	u8	%	getRSSI()
8	RSSI dBm + SNR	i8 + i8	dBm, dB, signed raw	rxLinkStatistics
9	LQ	u8	%	rxLinkStatistics.uplinkLQ
10	Optical flow surface speed X/Y	2×i16	cm/s	position estimator

Block 3 — Nav

Bit	Field	Size	Encoding	Source
0	Position	2×i32	1e-7° lat, lon	gpsSol.llh
1	Raw GPS altitude	i32	cm	gpsSol.llh.alt
2	Fix type + sats	u8 + u8	0 none, 1 2D, 2 3D, 3 estimated (dead reckoning); sats raw	gpsSol, STATE(GPS_ESTIMATED_FIX)
3	Groundspeed	u16	cm/s	gpsSol.groundSpeed
4	COG	u16	decideg	gpsSol.groundCourse
5	HDOP	u16	0.01	gpsSol.hdop
6	Nav status (legacy)	4×u8	mode, state, WP action, error	NAV_Status
7	Active WP + count	u16 + u16		NAV_Status, getWaypointCount()
8	Home distance + direction	u16 + u16	m, deg (to home)	GPS_distanceToHome, GPS_directionToHome
9	Nav FSM state	u8	NAV_PERSISTENT_ID_* (full resolution: autoland stages, trackback, launch, cruise sub-states …)	navFSM[].persistentId
10	Geozone action state	u8	geozoneActionState_e (needs a getter)	navigation_geozone
11	Guidance heading + target altitude	u16 + i32	decideg, cm	navDesiredHeading, posControl.desiredState.pos.z
12	Wind	u16 + u16 + u8	cm/s, deg, valid	MSP2_INAV_WIND
13	Trip distance	u16	100 m steps	getTotalTravelDistance()

Block 4 — GCS (slow, 0.25–1 Hz)

Bit	Field	Size	Encoding	Source
0	Home position + altitude	2×i32 + i32	1e-7°, cm absolute (GPS datum); zero when not set	GPS_home
1	Nav target position	2×i32	1e-7°; loiter center / guided target / WP carrot; zero when inactive	posControl.desiredState.pos geodetic
2	EPH + EPV	u16 + u16	cm	gpsSol.eph/epv
3	RC link extras	u8 + u8 + u16 + u16	downlink LQ %, RF mode, uplink TX power mW, downlink TX power mW	rxLinkStatistics
4	Remaining time + distance before RTH	u16 + u16	s, m (FC energy/wind model)	calculateRemaining*BeforeRTH()
5	RTC	u32 + u16	unix s + ms	rtcGet()
6	Loiter radius	u16	cm, effective	nav target

Block 5 — Thermal (one bit per source)

Bit	Field	Size	Encoding	Source
0	IMU temperature	i8	°C	getIMUTemperature()
1	Baro temperature	i8	°C	getBaroTemperature()
2–9	External probe 0–7	i8	°C	getSensorTemperature(i)

Block 6 — ESC (one field per ESC data set)

Bit	Field	Size	Encoding	Source
0	Combined	7 B	temp i8 °C (max), RPM u16 (avg), voltage u16 0.01 V (avg), current u16 0.01 A (sum)	escSensorGetData()
1–12	ESC 0–11	7 B each	temp i8, RPM u16, voltage u16, current u16; capability = telemetry motor count	getEscTelemetry(i)

Block 7 — Text

Bit	Field	Size	Encoding	Source
0	System message	u8 + char[28]	hi nibble = active message count, lo nibble = index; ASCII NUL-padded; the FC cycles through the current OSD message set one per push	OSD system messages (shared helper)

Blocks 8–14 free, 15 reserved (clear + capability).

Sentinels (momentarily invalid; the value can never be a real reading)

Field	Sentinel
Raw rangefinder, raw airspeed, EPH/EPV, RTH remaining time/distance, ESC RPM/V/A	0xFFFF
AGL	30-bit value 0x20000000 (most negative), source bit 0
Airspeed	source 0, value 0
RSSI dBm / SNR / LQ	0 / +127 / 0xFF
Guidance heading	0xFFFF
Positions (home, nav target)	all zero; nav FSM state is the gate
GPS-sourced fields	none — fix type is the gate (numSat reads 99 during an estimated fix, do not interpret)
Temperatures	−128
RTC	0 = never set
Loiter radius	0 = not applicable
Text	count 0 = no message

Notes for reviewers

• The OSD FLYMODE string and the OSD system messages need small refactors into shared helpers so OSD, DJI OSD and telemetry can never diverge.
• The geozone action state needs a getter and a wire-frozen enum.
• The RC link extras in block 4 are the first MSP exposure of those rxLinkStatistics members (MSP2_INAV_GET_LINK_STATS sends RSSI/LQ/SNR only).
• The catalog was cross-checked against what MAVLink, CRSF, LTM, GHST, the RC-link telemetry protocols, the OSD and the existing MSP replies deliver; ADS-B, DroneCAN nodes, logic-condition status, RC channels, servo/motor outputs and GPS receiver statistics deliberately stay request/response.