mirror of
https://github.com/azaion/gps-denied-onboard.git
synced 2026-06-22 06:01:12 +00:00
[AZ-396] [AZ-397] Batch 11: C8 source-set switch + QGC telemetry adapter
AZ-396: PymavlinkArdupilotAdapter.request_source_set_switch body sends MAV_CMD_SET_EKF_SOURCE_SET, awaits COMMAND_ACK with timeout, enforces Invariant 11 idempotence (1s rate-limit + skip-after-success). Adds runtime_root.SpoofRecoverySink to bridge C5 spoof-promotion-recovered signal to the C8 outbound thread via a bounded dispatch queue. FcConfig gains spoof_recovery_source_set + source_set_switch_timeout_ms. AZ-397: QgcTelemetryAdapter implements GcsAdapter strategy: MAVLink 2.0 to QGC, emit_summary downsamples 5Hz to configurable summary_rate_hz [0.5, 5.0] via integer modulo, emit_status_text mirrors to GCS link, subscribe_operator_commands translates COMMAND_LONG / PARAM_REQUEST_* / REQUEST_DATA_STREAM / MISSION_* / SET_MODE into OperatorCommand DTOs and audits each receipt to FDR. FcKind.GCS_QGC added for PortConfig. Tests: 25 new (12 AZ-396 + 13 AZ-397); full suite 501 passing, 2 skipped. Contracts unchanged (additive FcConfig fields, range relaxation on GcsConfig.summary_rate_hz, additive FcKind enum value). Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
@@ -0,0 +1,97 @@
|
||||
# C8 D-C8-2 source-set switch (AP only) — MAV_CMD_SET_EKF_SOURCE_SET
|
||||
|
||||
**Task**: AZ-396_c8_source_set_switch
|
||||
**Name**: C8 AP D-C8-2 source-set switch — `MAV_CMD_SET_EKF_SOURCE_SET` + spoof-recovery wiring (AC-NEW-2)
|
||||
**Description**: Implement `PymavlinkArdupilotAdapter.request_source_set_switch()` per D-C8-2 (AP F7): send `MAV_CMD_SET_EKF_SOURCE_SET` with the configured source-set ID (from `config.fc.spoof_recovery_source_set`, default 1); wait for the FC `COMMAND_ACK` with timeout = `config.fc.source_set_switch_timeout_ms` (default 1500 ms); raise `SourceSetSwitchError` on timeout or non-success ACK. Idempotence per Invariant 11: re-entry within 1 s is no-op'd; re-entry after a successful switch logs INFO + STATUSTEXT but does not re-issue the command. Wired to the C5 spoof-recovery gate (AZ-385 spoof-promotion gate) via the runtime root: when C5 promotes a previously-spoofed FC GPS source per AC-NEW-2, the runtime root invokes `request_source_set_switch()` on the C8 AP adapter; assertion that this completes within 3 s end-to-end is C8-IT-07 (deferred to E-BBT). On `SourceSetSwitchError`, the system continues to emit `GPS_INPUT` and the operator can manually switch via RC aux per D-C8-2-FALLBACK; ERROR log + STATUSTEXT to GCS + FDR record `kind="c8.ap.source_set_switch_failed"`. Replaces the `NotImplementedError` placeholder in AZ-? (AP outbound task).
|
||||
**Complexity**: 3 points
|
||||
**Dependencies**: AZ-393 (`PymavlinkArdupilotAdapter` skeleton); AZ-390 (`SourceSetSwitchError`); AZ-385 (C5 spoof-promotion gate — runtime-root wiring sink), AZ-273 (FDR), AZ-272, AZ-263, AZ-269, AZ-266
|
||||
**Component**: c8_fc_adapter (epic AZ-261 / E-C8)
|
||||
**Tracker**: AZ-396
|
||||
**Epic**: AZ-261 (E-C8)
|
||||
|
||||
### Document Dependencies
|
||||
|
||||
- `_docs/02_document/contracts/c8_fc_adapter/fc_adapter_protocol.md` — Invariant 11.
|
||||
- `_docs/02_document/components/10_c8_fc_adapter/description.md` — § 5 error handling (D-C8-2-FALLBACK), § 9 logging.
|
||||
- `_docs/02_document/architecture.md` — D-C8-2, ADR-008 (IT-3 gate); AC-NEW-2 (3 s spoofing-promotion latency).
|
||||
|
||||
## Problem
|
||||
|
||||
Without this task, AC-NEW-2 (spoofing-promotion latency < 3 s p95 via source-set switch) is not deliverable: there's no executable surface to switch the FC's EKF source set, and the runtime root has no sink to fire when C5's spoof gate promotes. Without the switch, the FC may continue to use a source set the spoof gate has just discredited.
|
||||
|
||||
## Outcome
|
||||
|
||||
- Body of `PymavlinkArdupilotAdapter.request_source_set_switch()`:
|
||||
- Send `MAV_CMD_SET_EKF_SOURCE_SET` via pymavlink `mav.command_long_send(...)`.
|
||||
- Wait for `COMMAND_ACK` with the configured timeout.
|
||||
- On success: INFO log `kind="c8.ap.source_set_switch_executed"` + FDR record + STATUSTEXT (severity=INFO).
|
||||
- On non-success ACK: `SourceSetSwitchError` raised + ERROR log + FDR + STATUSTEXT (severity=ERROR).
|
||||
- On timeout: `SourceSetSwitchError("ACK timeout after Xms")` + ERROR log + FDR + STATUSTEXT.
|
||||
- Idempotence (Invariant 11): re-entry within 1 s no-op'd; re-entry after successful switch logs INFO + STATUSTEXT but does NOT re-issue.
|
||||
- Runtime-root extension: subscribe to C5's `spoof_promotion_recovered` signal (kind from AZ-385); on receipt, invoke `request_source_set_switch()` on the C8 AP adapter from the C8 outbound thread.
|
||||
- Internal state: `_last_switch_attempt_ns`, `_last_switch_succeeded`.
|
||||
- Unit tests: ACK success path, non-success ACK raises, timeout raises, idempotence within 1 s, idempotence after success, runtime-root signal triggers switch, ERROR log + FDR record on failure.
|
||||
|
||||
## Scope
|
||||
|
||||
### Included
|
||||
- `request_source_set_switch()` body.
|
||||
- Runtime-root wiring to C5 spoof-promotion gate (AZ-385).
|
||||
- Idempotence per Invariant 11.
|
||||
- Error path + STATUSTEXT.
|
||||
- Unit tests.
|
||||
|
||||
### Excluded
|
||||
- iNav source-set switch — not applicable (`SourceSetSwitchNotSupportedError` already in iNav adapter).
|
||||
- C8-IT-07 end-to-end SITL test — deferred to E-BBT.
|
||||
- Manual RC aux switch path (D-C8-2-FALLBACK) — operator-side; out of scope for the companion.
|
||||
|
||||
## Acceptance Criteria
|
||||
|
||||
**AC-1: ACK success path** — emit `MAV_CMD_SET_EKF_SOURCE_SET`; SITL ACKs with `MAV_RESULT_ACCEPTED`; assert INFO log + FDR `kind="c8.ap.source_set_switch_executed"` + STATUSTEXT (severity=INFO).
|
||||
|
||||
**AC-2: Non-success ACK raises** — SITL ACKs with `MAV_RESULT_FAILED`; assert `SourceSetSwitchError` raised + ERROR log + FDR `kind="c8.ap.source_set_switch_failed"` + STATUSTEXT (severity=ERROR).
|
||||
|
||||
**AC-3: ACK timeout raises** — SITL never ACKs; after 1500 ms (default) → `SourceSetSwitchError("ACK timeout after 1500ms")` + ERROR log.
|
||||
|
||||
**AC-4: Configurable timeout** — `config.fc.source_set_switch_timeout_ms = 500` → timeout AC fires at 500 ms.
|
||||
|
||||
**AC-5: Idempotence within 1 s** — call `request_source_set_switch()` twice within 1 s; second call no-op'd; only ONE `MAV_CMD_SET_EKF_SOURCE_SET` on the wire; INFO log on the suppressed call: `kind="c8.ap.source_set_switch_rate_limited"`.
|
||||
|
||||
**AC-6: Idempotence after successful switch** — after a success, call again; second call logs INFO + STATUSTEXT but does NOT re-emit the command.
|
||||
|
||||
**AC-7: Runtime-root spoof-recovery signal triggers switch** — runtime root subscribes to C5's spoof-promotion-recovered signal (AZ-385); inject the signal; assert `request_source_set_switch()` was invoked on the C8 AP adapter.
|
||||
|
||||
**AC-8: source_set_id from config** — `MAV_CMD_SET_EKF_SOURCE_SET.param1` = `config.fc.spoof_recovery_source_set` (default 1).
|
||||
|
||||
**AC-9: Replaces AP outbound `NotImplementedError`** — verify the AZ-393 AP outbound task's `NotImplementedError` placeholder is removed; the method body is now executable.
|
||||
|
||||
**AC-10: STATUSTEXT severity matrix** — success → INFO (6); failure → ERROR (3); rate-limited → INFO (6).
|
||||
|
||||
## Non-Functional Requirements
|
||||
|
||||
- End-to-end (signal → switch → ACK) latency p95 ≤ 2 s (within the 3 s AC-NEW-2 budget).
|
||||
- `request_source_set_switch()` p95 ≤ 1.5 s (dominated by ACK round-trip).
|
||||
|
||||
## Constraints
|
||||
|
||||
- pymavlink bundled unmodified per D-C8-3.
|
||||
- AP-only — `Msp2InavAdapter.request_source_set_switch` already raises `SourceSetSwitchNotSupportedError`.
|
||||
- The runtime-root wiring MUST happen on the C8 outbound thread (single-writer invariant).
|
||||
- D-C8-2-FALLBACK (manual RC aux) is operator-side; this task does NOT implement it.
|
||||
|
||||
## Risks & Mitigation
|
||||
|
||||
- **R-D-C8-2 (firmware-supported but not deployed-precedent)** — *Mitigation*: ADR-008 makes IT-3 (ArduPilot SITL validation) the lock gate; this task's surface is exercised in IT-3 (deferred to E-BBT).
|
||||
- **Risk: ACK timeout under high MAVLink traffic** — *Mitigation*: AC-3 + AC-4 cover; 1.5 s default is generous; operator workflow has manual RC aux fallback.
|
||||
- **Risk: idempotence rate-limit hides operator-initiated re-issues** — *Mitigation*: log every suppressed call at INFO; STATUSTEXT mirror so the operator sees suppression on GCS.
|
||||
|
||||
## Runtime Completeness
|
||||
|
||||
- **Named capability**: AP D-C8-2 source-set switch + spoof-recovery wiring.
|
||||
- **Production code**: real pymavlink `command_long_send` + ACK loop; real runtime-root signal subscription; real idempotence.
|
||||
- **Unacceptable substitutes**: skipping the ACK wait (defeats AC-1/AC-2); always returning success without sending (defeats AC-1).
|
||||
|
||||
## Contract
|
||||
|
||||
Implements `_docs/02_document/contracts/c8_fc_adapter/fc_adapter_protocol.md` — `request_source_set_switch` for AP; Invariant 11. Delivers AC-NEW-2 wire surface (end-to-end test deferred to C8-IT-07 / E-BBT).
|
||||
@@ -0,0 +1,98 @@
|
||||
# C8 GcsAdapter — QgcTelemetryAdapter (1–2 Hz downsampled summary + operator commands)
|
||||
|
||||
**Task**: AZ-397_c8_qgc_telemetry_adapter
|
||||
**Name**: C8 `QgcTelemetryAdapter` — downsampled 1–2 Hz summary out + operator command in (AC-6.1, AC-6.2)
|
||||
**Description**: Implement `QgcTelemetryAdapter` (the single concrete `GcsAdapter` strategy in this cycle): open a MAVLink 2.0 channel to QGroundControl on the configured GCS UART; `emit_summary(EstimatorOutput)` is invoked at 5 Hz by the runtime root and is INTERNALLY downsampled to 1–2 Hz (configurable via `config.gcs.summary_rate_hz`, default 2 Hz; downsampling is rate-based — every Nth call). Each downsampled call emits one MAVLink summary frame containing lat/lon/alt + horiz_accuracy + source_label (full WGS84 from the injected `WgsConverter`; horiz_accuracy from the injected `CovarianceProjector.to_ardupilot_horiz_accuracy_m` since the QGC link uses the AP semantic). Body of `subscribe_operator_commands(callback)`: register a pymavlink message-handler that forwards standard MAVLink command messages (PARAM_REQUEST_LIST, PARAM_REQUEST_READ, COMMAND_LONG, REQUEST_DATA_STREAM, MISSION_REQUEST, etc.) to the registered callback as `OperatorCommand` DTOs. Body of `emit_status_text` mirrors STATUSTEXTs from C5 + C8 outbound to the GCS link (this is the canonical operator-facing surface). Body of `open(...)` opens the MAVLink connection; `close()` closes it. Single-writer thread for outbound (Invariant 8 mirror).
|
||||
**Complexity**: 3 points
|
||||
**Dependencies**: AZ-390 (`GcsAdapter` Protocol + `OperatorCommand` DTO), AZ-392 (`CovarianceProjector` helper), AZ-279 (`WgsConverter`), AZ-273 (FDR), AZ-263, AZ-269, AZ-266
|
||||
**Component**: c8_fc_adapter (epic AZ-261 / E-C8)
|
||||
**Tracker**: AZ-397
|
||||
**Epic**: AZ-261 (E-C8)
|
||||
|
||||
### Document Dependencies
|
||||
|
||||
- `_docs/02_document/contracts/c8_fc_adapter/fc_adapter_protocol.md` — `GcsAdapter` Protocol; Invariant 12.
|
||||
- `_docs/02_document/components/10_c8_fc_adapter/description.md` — § 1 GCS feed at 1–2 Hz; § 3 External API (QGC MAVLink 2.0).
|
||||
- `_docs/02_document/architecture.md` — § 5 External Integrations (GCS link).
|
||||
|
||||
## Problem
|
||||
|
||||
Without this task, the operator has no GCS observability — no downsampled position summary on QGC, no STATUSTEXT mirror, no operator command path. AC-6.1 (1–2 Hz GCS stream), AC-6.2 (operator commands accepted), and AC-6.3 (WGS84 output) are unmet at the GCS surface.
|
||||
|
||||
## Outcome
|
||||
|
||||
- `src/gps_denied_onboard/components/c8_fc_adapter/mavlink_gcs_adapter.py` — `QgcTelemetryAdapter` class implementing `GcsAdapter`.
|
||||
- Constructor: `__init__(self, config, wgs_converter, covariance_projector, fdr_client, clock)`.
|
||||
- Body of `emit_summary` — downsampling counter + emit MAVLink summary every Nth call.
|
||||
- Body of `emit_status_text` — pymavlink statustext_send.
|
||||
- Body of `subscribe_operator_commands` — pymavlink message-handler registration; translate raw MAVLink commands to `OperatorCommand` DTOs.
|
||||
- Body of `open` / `close`.
|
||||
- INFO log on first emit_summary: `kind="c8.gcs.first_summary_emit"`.
|
||||
- DEBUG log per emit: `kind="c8.gcs.summary_emit"` with `{seq, lat, lon, horiz_accuracy_m, source_label}`.
|
||||
- Operator command receipt log: `kind="c8.gcs.operator_command"` (audit trail to FDR per § 9 — STATUSTEXT broadcasts ALWAYS mirrored to FDR).
|
||||
|
||||
## Scope
|
||||
|
||||
### Included
|
||||
- `QgcTelemetryAdapter` class implementing `GcsAdapter`.
|
||||
- Downsampling counter (5 Hz → 1–2 Hz).
|
||||
- Summary frame encoding (MAVLink 2.0).
|
||||
- STATUSTEXT mirror.
|
||||
- Operator command subscription + DTO translation.
|
||||
- `open` / `close` body.
|
||||
- Unit tests: 5 → 2 Hz downsample (10 input → 4 output; assert mod-N), command subscription invokes callback, STATUSTEXT mirror on the wire, configurable rate (5 Hz / 1 Hz / 2 Hz / 5 Hz validated; out-of-range → `GcsAdapterConfigError`).
|
||||
|
||||
### Excluded
|
||||
- FC adapters — owned by AP / iNav / inbound / signing / source-set tasks.
|
||||
- C8-IT-04 / IT-05 / IT-06 end-to-end QGC SITL tests — deferred to E-BBT.
|
||||
- Workflow logic on the operator side (E-C12) — out of scope per epic.
|
||||
|
||||
## Acceptance Criteria
|
||||
|
||||
**AC-1: 5 → 2 Hz downsampling** — drive 100 `emit_summary` calls; assert exactly 40 MAVLink summary frames on the wire (every 2.5th call rounded — implementation choice between mod-2 = 50 and mod-3 = 33; the implementation MUST document its choice; AC asserts the chosen rate).
|
||||
|
||||
**AC-2: Configurable rate** — `config.gcs.summary_rate_hz = 1` → 100 calls produce ~20 frames (every 5th); `summary_rate_hz = 2` → ~40 (every 2.5th); `summary_rate_hz = 5` → 100 (no downsample). Out-of-range (e.g., 10 Hz) → `GcsAdapterConfigError` at config load.
|
||||
|
||||
**AC-3: Summary frame fields** — capture a wire summary; decode via pymavlink; assert lat/lon/alt match `WgsConverter` × 1e7 (MAVLink convention); horiz_accuracy matches `CovarianceProjector.to_ardupilot_horiz_accuracy_m`; source_label encoded per the documented mapping.
|
||||
|
||||
**AC-4: STATUSTEXT mirror** — call `emit_status_text("hello", Severity.INFO)`; assert one MAVLink STATUSTEXT on the wire with text="hello" + severity=6.
|
||||
|
||||
**AC-5: Operator command subscription invokes callback** — register a callback; inject a PARAM_REQUEST_LIST from a fake QGC; assert callback invoked with an `OperatorCommand` DTO.
|
||||
|
||||
**AC-6: Operator command FDR audit trail** — every received operator command emits FDR `kind="c8.gcs.operator_command"` with the command type + source MAVLink id.
|
||||
|
||||
**AC-7: Single-writer thread for outbound** — second-thread `emit_summary` raises `RuntimeError`.
|
||||
|
||||
**AC-8: First emit logged once** — `kind="c8.gcs.first_summary_emit"` INFO log fires exactly once per `open(...)` lifetime.
|
||||
|
||||
**AC-9: AC-6.3 WGS84 round-trip** — emit a summary derived from a known local-tangent-plane position; round-trip via `WgsConverter` + decode wire frame; assert ≤ 1 cm position residual (defensive — backstops the helper that C4 + C8 both rely on; full IT lives in C8-IT-06).
|
||||
|
||||
**AC-10: GcsAdapterConfigError on bad config** — `summary_rate_hz` outside [0.5, 5.0] → `GcsAdapterConfigError` at config load.
|
||||
|
||||
## Non-Functional Requirements
|
||||
|
||||
- `emit_summary` p95 ≤ 5 ms (parallel to FC outbound).
|
||||
- Operator-command callback dispatch p95 ≤ 1 ms (non-blocking enqueue).
|
||||
|
||||
## Constraints
|
||||
|
||||
- pymavlink bundled unmodified per D-C8-3.
|
||||
- Single-writer thread for outbound enforced.
|
||||
- Downsampling is rate-based (every Nth call), NOT selection-based (Invariant 12).
|
||||
- The QGC link uses the AP MAVLink semantic for horiz_accuracy (m) — iNav-targeted flights still emit a meters-based summary on this link, since QGC speaks MAVLink natively.
|
||||
|
||||
## Risks & Mitigation
|
||||
|
||||
- **Risk: pymavlink message-handler thread re-entrance bugs** — *Mitigation*: callback dispatch is explicitly non-blocking (enqueue + drain pattern); tests cover concurrent inject + drain.
|
||||
- **Risk: downsampling rate-config drift between code and operator UI** — *Mitigation*: `summary_rate_hz` is the single source of truth; documented in config schema + Public API.
|
||||
- **Risk: STATUSTEXT flooding under cascading errors** — *Mitigation*: STATUSTEXT itself is rate-limited at the source (C5 / C8 outbound emit at most once per transition); GCS adapter just forwards.
|
||||
|
||||
## Runtime Completeness
|
||||
|
||||
- **Named capability**: GCS downsampled telemetry + operator command subscription.
|
||||
- **Production code**: real pymavlink encode/decode; real downsampling counter; real callback dispatch.
|
||||
- **Unacceptable substitutes**: a "fake QGC" that only logs without writing to the wire (defeats AC-3/AC-4 wire-level fidelity).
|
||||
|
||||
## Contract
|
||||
|
||||
Implements `_docs/02_document/contracts/c8_fc_adapter/fc_adapter_protocol.md` — `GcsAdapter` Protocol; Invariant 12.
|
||||
Reference in New Issue
Block a user