mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 22:41:13 +00:00
[AZ-391] C8 inbound: MAVLink + MSP2 decoders + rings + bus + warm-start
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>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,101 @@
|
||||
# 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 → `FcTelemetryFrame` translation).
|
||||
- `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` — `Subscription` handle + multi-subscriber fan-out.
|
||||
- Body of `subscribe_telemetry` on both `PymavlinkArdupilotAdapter` (AZ-393 produces the class shell — this task fills the inbound body) and `Msp2InavAdapter` (AZ-394 — same) — registers the callback against the multi-subscriber bus.
|
||||
- Body of `current_flight_state` on both adapters — returns the latest `FlightStateSignal` from 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 + `Subscription` handle.
|
||||
- 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.
|
||||
- `GcsAdapter` inbound (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 `FcTelemetryFrame`s 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.
|
||||
Reference in New Issue
Block a user