Adds the C8 inbound producer side: - TelemetryRing[T]: bounded drop-oldest ring; first-overflow INFO log + monotonic dropped_count. - SubscriptionBus + SubscriptionHandle: synchronous fan-out, lock- released-before-callback to avoid deadlock; subscriber crash caught + DEBUG-logged so one bad subscriber cannot kill the decode loop. - PymavlinkInboundDecoder: pymavlink-based AP decoder for RAW_IMU, SCALED_IMU2, ATTITUDE, GPS_RAW_INT, GPS2_RAW, HEARTBEAT, STATUSTEXT. Out-of-order drop (Invariant 7) per-kind WARN. STATUSTEXT spoofing sentinel promotes subsequent GPS to GpsStatus.SPOOFED within 5 s. AC-5.1 warm-start hint cached on first 3D+ fix; embedded into every FlightStateSignal. - Msp2InavInboundDecoder: YAMSPy-based iNav polling decoder for IMU / attitude / GPS / flight-state. signed=False always (RESTRICT-COMM-2); GpsStatus.SPOOFED is unreachable on iNav. Adds yamspy>=0.3.3 + pyserial>=3.5 to pyproject.toml. Tests: 443 pass / 2 skip / 0 fail (+33 in batch 9). Contract: no drift on fc_adapter_protocol.md v1.0.0; this batch implements the inbound producer side without changing signatures. Co-authored-by: Cursor <cursoragent@cursor.com>
7.6 KiB
C8 Inbound subscription — MAVLink + MSP2 telemetry decoders
Task: AZ-391_c8_inbound_subscription
Name: C8 inbound subscription path — IMU/attitude/GPS-health/MAV_STATE producer
Description: Implement the inbound telemetry decode path for both PymavlinkArdupilotAdapter and Msp2InavAdapter. Decode AP wire frames (RAW_IMU/SCALED_IMU2, ATTITUDE, GPS_RAW_INT/GPS2_RAW, HEARTBEAT, MAV_STATE from HEARTBEAT.system_status, STATUSTEXT) via pymavlink. Decode iNav wire frames (MSP2_INAV_ANALOG, attitude+IMU stream) via YAMSPy. Both paths produce a unified FcTelemetryFrame stream + maintain bounded telemetry rings (drop-oldest on overflow per § 7) for ImuWindow (AZ-276 helper consumer side), AttitudeWindow, GpsHealth, FlightStateSignal. The decode thread is independent from the outbound emit thread (per Invariant 8). subscribe_telemetry(callback) returns a Subscription handle; multiple subscribers fan out from a single decode loop. Out-of-order timestamp drop + WARN log per Invariant 7. AC-5.1 warm-start: at first GPS_RAW_INT with valid fix, populate FlightStateSignal.last_valid_gps_hint_wgs84 + last_valid_gps_age_ms for the C5 warm-start consumer. AC-5.1 surface deadline ≤ 1 s after C8 ready.
Complexity: 5 points
Dependencies: AZ-390 (Protocol + DTOs + composition), AZ-263, AZ-269, AZ-266, AZ-272 (FDR), AZ-273 (FdrClient), AZ-276 (ImuPreintegrator consumer side — this task feeds raw IMU samples)
Component: c8_fc_adapter (epic AZ-261 / E-C8)
Tracker: AZ-391
Epic: AZ-261 (E-C8)
Document Dependencies
_docs/02_document/contracts/c8_fc_adapter/fc_adapter_protocol.md— Invariants 1, 7, 8._docs/02_document/components/10_c8_fc_adapter/description.md— § 1 inbound, § 4 caching strategy, § 7 race conditions._docs/02_document/architecture.md— § 5 External Integrations (per-message rate/auth/failure-mode table).
Problem
Without this task, C8 has no inbound — C1 (VIO) gets no FC IMU prior, C5 (StateEstimator) gets no FC IMU window or GpsHealth or warm-start hint, and the per-FC ports never decode the wire stream. The Protocol from AZ-390 has subscribe_telemetry declared but no body.
Outcome
src/gps_denied_onboard/components/c8_fc_adapter/_inbound_mavlink.py— AP inbound decoder (pymavlink-based loop,MAVLink_message_handler, frame →FcTelemetryFrametranslation).src/gps_denied_onboard/components/c8_fc_adapter/_inbound_msp2.py— iNav inbound decoder (YAMSPy + INAV-Toolkit; periodic poll loop since MSP2 is request-response).src/gps_denied_onboard/components/c8_fc_adapter/_telemetry_rings.py— bounded ring buffers per kind; drop-oldest semantics; thread-safe deque-based.src/gps_denied_onboard/components/c8_fc_adapter/_subscription.py—Subscriptionhandle + multi-subscriber fan-out.- Body of
subscribe_telemetryon bothPymavlinkArdupilotAdapter(AZ-393 produces the class shell — this task fills the inbound body) andMsp2InavAdapter(AZ-394 — same) — registers the callback against the multi-subscriber bus. - Body of
current_flight_stateon both adapters — returns the latestFlightStateSignalfrom the cached ring. - WARN log on out-of-order frame:
kind="c8.inbound.out_of_order_frame_dropped"with{kind, prev_ns, this_ns}. - DEBUG log on every decode error:
kind="c8.inbound.decode_error".
Scope
Included
- AP MAVLink 2.0 inbound decoder (5 message types).
- iNav MSP2 inbound decoder (poll loop + decode).
- Bounded telemetry rings + drop-oldest.
- Multi-subscriber fan-out +
Subscriptionhandle. - AC-5.1 warm-start hint surfacing.
- Out-of-order drop + log per Invariant 7.
- Unit tests: AP frame decode, iNav frame decode, ring overflow drops oldest, multi-subscriber fan-out, out-of-order drop logged, warm-start hint surfaces within 1 s of first GPS_RAW_INT.
Excluded
- Outbound encoding paths — owned by AP / iNav outbound tasks.
- Signing handshake — owned by signing task.
CovarianceProjector— owned by projector task.GcsAdapterinbound (operator commands) — owned by GCS task.- C8-IT/PT/ST tests — deferred to E-BBT.
Acceptance Criteria
AC-1: AP RAW_IMU decode — pymavlink RAW_IMU frame → FcTelemetryFrame(kind=IMU_SAMPLE, payload=ImuSample(...)) with timestamp + 6-axis values; AC fails if received_at not set to monotonic_ns() at decode boundary.
AC-2: AP ATTITUDE decode — ATTITUDE frame → FcTelemetryFrame(kind=ATTITUDE, payload=AttitudeSample(...)) with roll/pitch/yaw.
AC-3: AP GPS_RAW_INT → GpsHealth — GPS_RAW_INT.fix_type mapped to GpsStatus per the documented table (NO_FIX/DEGRADED/STABLE); STABLE_NON_SPOOFED requires the GPS_RAW_INT.signed_flag (or equivalent) to be set; SPOOFED requires the FC's spoofing-detection telemetry (not always present — degraded to STABLE if absent).
AC-4: AP HEARTBEAT → FlightState — HEARTBEAT.system_status mapped to FlightState per the table.
AC-5: iNav MSP2 decode — MSP2_INAV_ANALOG + attitude/IMU poll responses → matching FcTelemetryFrames with the SAME unified DTO shape as AP. iNav has no spoofing-detection — GpsStatus.SPOOFED is unreachable for iNav.
AC-6: Bounded ring drop-oldest — push 1000 frames into a 100-capacity ring; assert oldest 900 dropped; ring contains the latest 100; INFO log emitted at first overflow with kind="c8.inbound.ring_overflow".
AC-7: Multi-subscriber fan-out — register 3 subscribers; emit one frame; assert all 3 callbacks invoked; cancel one subscription; emit another frame; assert remaining 2 invoked.
AC-8: AC-5.1 warm-start hint within 1 s — current_flight_state() returns FlightStateSignal with last_valid_gps_hint_wgs84 != None within 1 s of the first GPS_RAW_INT decode.
AC-9: Out-of-order drop + WARN — inject a frame with received_at < previous frame of same kind; assert frame dropped + WARN log emitted.
AC-10: Decode-error isolation — corrupt frame → DEBUG log + frame dropped; subsequent valid frames still processed (the decoder MUST NOT crash on a single malformed frame).
Non-Functional Requirements
- Inbound IMU callback p95 ≤ 1 ms (C8-PT-01 budget).
- AP decode loop: 200 Hz IMU sustained without dropping > 1% of frames.
- iNav poll loop: 100 Hz attitude+IMU sustained.
Constraints
- Single decode thread per adapter; thread-safe ring access.
- pymavlink is bundled unmodified per D-C8-3.
- YAMSPy + INAV-Toolkit at the project's pinned versions.
- The decode thread MUST NOT block on subscriber callbacks longer than 100 µs; slow subscribers must use a non-blocking enqueue + drain on their own thread.
Risks & Mitigation
- Risk: pymavlink message-handler performance under 200 Hz IMU — Mitigation: profile early; if marginal, offload decode to a small C extension. Project's pin of pymavlink is known to handle this rate.
- Risk: iNav poll-rate drift on slow UART — Mitigation: configurable poll period; degrade gracefully (lower rate, log WARN once per minute).
- Risk: Out-of-order frames silently mask bugs (R05-style) — Mitigation: AC-9 mandates WARN on every drop; aggregate counter + INFO log every 60 s.
Runtime Completeness
- Named capability: C8 inbound telemetry decode + multi-subscriber fan-out.
- Production code: real pymavlink decoder, real YAMSPy decoder, real ring buffers, real subscription handles.
- Allowed external stubs: SITL fakes for tests; no production stubs.
- Unacceptable substitutes: a periodic-fake-IMU generator in production.
Contract
Implements _docs/02_document/contracts/c8_fc_adapter/fc_adapter_protocol.md — subscribe_telemetry, current_flight_state, Invariants 1, 7, 8.