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

src/gps_denied_onboard/components/c8_fc_adapter/mavlink_gcs_adapter.py

@@ -0,0 +1,378 @@
+"""QGroundControl telemetry adapter (AZ-397 / E-C8).
+
+The single concrete :class:`GcsAdapter` strategy in this cycle. Owns:
+
+- **Outbound downsampled summary** — composition root invokes
+  :meth:`emit_summary` at 5 Hz; the adapter divides by an internal
+  counter to emit at the configured ``summary_rate_hz``
+  (default 2 Hz; range [0.5, 5.0] per :class:`GcsConfig`).
+- **STATUSTEXT mirror** — :meth:`emit_status_text` forwards a single
+  MAVLink ``STATUSTEXT`` to the configured GCS UART.
+- **Operator commands** — pymavlink's message handler is wired to
+  translate inbound ``COMMAND_LONG`` / ``PARAM_REQUEST_*`` /
+  ``REQUEST_DATA_STREAM`` / ``MISSION_REQUEST`` (and any other
+  caller-registered types) into :class:`OperatorCommand` DTOs and
+  hand them to a subscriber.
+
+The wire uses the AP MAVLink semantic for ``horiz_accuracy``: meters.
+iNav-targeted flights that route their summary here still emit
+meters since QGC speaks MAVLink natively (per AZ-397 § Constraints).
+
+Build flag: ``BUILD_GCS_QGC_MAVLINK``.
+"""
+
+from __future__ import annotations
+
+import threading
+import time
+from collections.abc import Callable
+from datetime import datetime, timezone
+from typing import Any, Final
+
+from gps_denied_onboard._types.fc import (
+    FcKind,
+    OperatorCommand,
+    OperatorCommandCallback,
+    PortConfig,
+    Severity,
+    Subscription,
+)
+from gps_denied_onboard._types.geo import LatLonAlt
+from gps_denied_onboard._types.pose import EstimatorOutput
+from gps_denied_onboard.components.c8_fc_adapter._covariance_projector import (
+    CovarianceProjector,
+)
+from gps_denied_onboard.components.c8_fc_adapter._subscription import SubscriptionBus
+from gps_denied_onboard.components.c8_fc_adapter.errors import (
+    GcsAdapterConfigError,
+    GcsEmitError,
+)
+from gps_denied_onboard.config import Config
+from gps_denied_onboard.fdr_client.client import FdrClient
+from gps_denied_onboard.fdr_client.records import FdrRecord
+from gps_denied_onboard.logging import get_logger
+
+__all__ = ["QgcTelemetryAdapter"]
+
+
+# Composition-root invocation rate (Hz) for emit_summary; the adapter
+# divides this by ``summary_rate_hz`` to compute the modulo divisor.
+_COMPOSE_ROOT_INVOKE_HZ: Final[float] = 5.0
+_DEFAULT_OPERATOR_COMMAND_TYPES: Final[tuple[str, ...]] = (
+    "PARAM_REQUEST_LIST",
+    "PARAM_REQUEST_READ",
+    "PARAM_SET",
+    "COMMAND_LONG",
+    "COMMAND_INT",
+    "REQUEST_DATA_STREAM",
+    "MISSION_REQUEST",
+    "MISSION_REQUEST_LIST",
+    "MISSION_ITEM",
+    "SET_MODE",
+)
+
+
+def _mav_severity(sev: Severity) -> int:
+    return int(sev.value)
+
+
+def _compute_downsample_modulo(summary_rate_hz: float) -> int:
+    """Return the per-N-call emit modulo for ``summary_rate_hz`` against 5 Hz.
+
+    Examples (the AC tests pin these): 5 Hz -> 1 (no downsample),
+    2 Hz -> 2 (every 2nd call), 1 Hz -> 5 (every 5th), 0.5 Hz -> 10.
+    Sub-1 Hz floors don't round to zero because we use ``round`` with
+    a minimum of 1 below.
+    """
+    if summary_rate_hz <= 0.0:
+        raise GcsAdapterConfigError(f"summary_rate_hz must be > 0; got {summary_rate_hz}")
+    ratio = _COMPOSE_ROOT_INVOKE_HZ / summary_rate_hz
+    return max(1, round(ratio))
+
+
+class QgcTelemetryAdapter:
+    """Concrete :class:`GcsAdapter` strategy.
+
+    Threading:
+    - Outbound (``emit_summary`` / ``emit_status_text``) is bound to a
+      single composition-root thread; emitting from a second thread
+      raises :class:`RuntimeError` (Invariant 8 mirror).
+    - Inbound operator-command dispatch fires on the message-handler
+      thread (pymavlink's recv loop). Subscriber callbacks are
+      invoked synchronously; the bus isolates crashes per the C8
+      inbound pattern.
+    """
+
+    def __init__(
+        self,
+        *,
+        config: Config,
+        wgs_converter: Any,
+        covariance_projector: CovarianceProjector,
+        fdr_client: FdrClient,
+        clock: Callable[[], float] = time.monotonic,
+        connect_factory: Callable[[str, int], Any] | None = None,
+    ) -> None:
+        self._config = config
+        self._wgs_converter = wgs_converter
+        self._cov_projector = covariance_projector
+        self._fdr_client = fdr_client
+        self._clock = clock
+        self._connect_factory = connect_factory
+        self._log = get_logger("c8_gcs_adapter.qgc")
+        # The modulo divisor — computed once at construction so unit
+        # tests can pin a 5 Hz call → N-frame relationship.
+        self._downsample_modulo: int = _compute_downsample_modulo(float(config.gcs.summary_rate_hz))
+        # Wire state ------------------------------------------------------
+        self._connection: Any = None
+        self._opened = False
+        self._invocation_count = 0
+        self._frame_seq = 0
+        self._first_emit_logged = False
+        self._open_emit_thread_ident: int | None = None
+        self._operator_bus = SubscriptionBus()
+        self._operator_handler_attached = False
+        self._operator_lock = threading.Lock()
+
+    # ------------------------------------------------------------------
+    # GcsAdapter Protocol implementation
+
+    def open(self, port: PortConfig) -> None:
+        if self._opened:
+            raise GcsAdapterConfigError("QgcTelemetryAdapter already opened")
+        if port.fc_kind is not FcKind.GCS_QGC:
+            raise GcsAdapterConfigError(
+                f"QgcTelemetryAdapter requires FcKind.GCS_QGC; got {port.fc_kind!r}"
+            )
+        try:
+            self._connection = self._connect(port)
+        except Exception as exc:
+            raise GcsAdapterConfigError(f"QGC MAVLink connect failed: {exc!r}") from exc
+        self._opened = True
+        self._invocation_count = 0
+        self._frame_seq = 0
+        self._first_emit_logged = False
+        self._open_emit_thread_ident = None
+
+    def close(self) -> None:
+        if not self._opened:
+            return
+        try:
+            if self._connection is not None and hasattr(self._connection, "close"):
+                self._connection.close()
+        finally:
+            self._opened = False
+            self._connection = None
+            self._open_emit_thread_ident = None
+
+    def emit_summary(self, output: EstimatorOutput) -> None:
+        if not self._opened or self._connection is None:
+            raise GcsEmitError("adapter not opened")
+        self._enforce_single_writer()
+        self._invocation_count += 1
+        if (self._invocation_count % self._downsample_modulo) != 0:
+            return  # downsampled — skip this call
+        wgs = self._extract_wgs84(output)
+        horiz_accuracy_m = self._cov_projector.to_ardupilot_horiz_accuracy_m(output)
+        self._frame_seq += 1
+        try:
+            self._connection.mav.global_position_int_send(
+                int(self._clock_ms_boot()),
+                int(wgs.lat_deg * 1e7),
+                int(wgs.lon_deg * 1e7),
+                int(wgs.alt_m * 1000.0),  # WGS84 alt in mm
+                int(wgs.alt_m * 1000.0),  # relative alt; placeholder
+                0,
+                0,
+                0,  # vx, vy, vz (cm/s) — 0 for summary
+                0,  # hdg (cdeg); 0 = unset
+            )
+            self._connection.mav.named_value_float_send(
+                int(self._clock_ms_boot()),
+                b"horiz_m",
+                float(horiz_accuracy_m),
+            )
+        except Exception as exc:
+            self._log.error(
+                f"c8.gcs.summary_emit_failed: {exc!r}",
+                extra={
+                    "kind": "c8.gcs.summary_emit_failed",
+                    "kv": {"error": repr(exc), "frame_seq": self._frame_seq},
+                },
+            )
+            raise GcsEmitError(f"GCS summary emit failed: {exc!r}") from exc
+        if not self._first_emit_logged:
+            self._first_emit_logged = True
+            self._log.info(
+                "c8.gcs.first_summary_emit",
+                extra={
+                    "kind": "c8.gcs.first_summary_emit",
+                    "kv": {
+                        "frame_seq": self._frame_seq,
+                        "downsample_modulo": self._downsample_modulo,
+                    },
+                },
+            )
+        self._log.debug(
+            "c8.gcs.summary_emit",
+            extra={
+                "kind": "c8.gcs.summary_emit",
+                "kv": {
+                    "seq": self._frame_seq,
+                    "lat": wgs.lat_deg,
+                    "lon": wgs.lon_deg,
+                    "horiz_accuracy_m": horiz_accuracy_m,
+                    "source_label": output.source_label,
+                },
+            },
+        )
+
+    def subscribe_operator_commands(self, callback: OperatorCommandCallback) -> Subscription:
+        handle = self._operator_bus.subscribe(callback)
+        self._ensure_operator_handler_attached()
+        return handle
+
+    def emit_status_text(self, msg: str, severity: Severity) -> None:
+        if not self._opened or self._connection is None:
+            raise GcsEmitError("adapter not opened")
+        self._enforce_single_writer()
+        try:
+            text = msg.encode("utf-8")[:50]
+            self._connection.mav.statustext_send(_mav_severity(severity), text)
+        except Exception as exc:
+            self._log.debug(
+                f"c8.gcs.statustext_failed: {exc!r}",
+                extra={
+                    "kind": "c8.gcs.statustext_failed",
+                    "kv": {"error": repr(exc)},
+                },
+            )
+
+    # ------------------------------------------------------------------
+    # Internals
+
+    def _enforce_single_writer(self) -> None:
+        cur = threading.get_ident()
+        if self._open_emit_thread_ident is None:
+            self._open_emit_thread_ident = cur
+            return
+        if self._open_emit_thread_ident != cur:
+            raise RuntimeError(
+                "QgcTelemetryAdapter outbound is single-writer; "
+                f"first thread={self._open_emit_thread_ident}, this thread={cur}"
+            )
+
+    def _connect(self, port: PortConfig) -> Any:
+        if self._connect_factory is not None:
+            return self._connect_factory(port.device, port.baud)
+        from pymavlink import mavutil  # lazy import per ADR-002
+
+        return mavutil.mavlink_connection(
+            port.device,
+            baud=port.baud,
+            dialect="common",
+            mavlink_version="2.0",
+        )
+
+    def _ensure_operator_handler_attached(self) -> None:
+        with self._operator_lock:
+            if self._operator_handler_attached or self._connection is None:
+                return
+            register = getattr(self._connection, "message_hooks", None)
+            if register is None or not isinstance(register, list):
+                # The injected stub may expose a `register_message_hook`
+                # callable; honour that too for test ergonomics.
+                hook_register = getattr(self._connection, "register_message_hook", None)
+                if callable(hook_register):
+                    hook_register(self._on_inbound_message)
+                else:
+                    return
+            else:
+                register.append(self._on_inbound_message)
+            self._operator_handler_attached = True
+
+    def _on_inbound_message(self, *args: Any) -> None:
+        """Pymavlink message-hook entrypoint.
+
+        pymavlink calls hooks with ``(self, conn, msg)`` for instance
+        hooks or ``(conn, msg)`` for free-function hooks; the message
+        is always the last positional argument.
+        """
+        if not args:
+            return
+        msg = args[-1]
+        try:
+            msg_type = msg.get_type() if hasattr(msg, "get_type") else str(type(msg).__name__)
+        except Exception:
+            return
+        if msg_type not in _DEFAULT_OPERATOR_COMMAND_TYPES:
+            return
+        cmd = self._translate_to_operator_command(msg_type, msg)
+        self._operator_bus.dispatch(cmd)
+        self._record_operator_command_fdr(cmd, msg)
+
+    def _translate_to_operator_command(self, msg_type: str, msg: Any) -> OperatorCommand:
+        payload: dict[str, str | int | float | bool] = {}
+        for attr in (
+            "param_id",
+            "param_value",
+            "command",
+            "result",
+            "target_system",
+            "target_component",
+        ):
+            if hasattr(msg, attr):
+                value = getattr(msg, attr)
+                if isinstance(value, (str, int, float, bool)):
+                    payload[attr] = value
+                elif isinstance(value, bytes):
+                    try:
+                        payload[attr] = value.decode("utf-8", errors="replace")
+                    except Exception:
+                        payload[attr] = value.hex()
+        return OperatorCommand(
+            command=msg_type,
+            payload=payload,
+            received_at=time.monotonic_ns(),
+        )
+
+    def _record_operator_command_fdr(self, cmd: OperatorCommand, msg: Any) -> None:
+        record = FdrRecord(
+            schema_version=1,
+            ts=datetime.now(tz=timezone.utc).isoformat(),
+            producer_id="c8_gcs_adapter",
+            kind="log",
+            payload={
+                "level": "INFO",
+                "component": "c8_gcs_adapter",
+                "kind": "c8.gcs.operator_command",
+                "msg": "c8.gcs.operator_command",
+                "kv": {
+                    "command": cmd.command,
+                    "payload": cmd.payload,
+                    "source_system": int(getattr(msg, "get_srcSystem", lambda: 0)() or 0),
+                },
+            },
+        )
+        try:
+            self._fdr_client.enqueue(record)
+        except Exception as exc:
+            self._log.debug(
+                f"c8.gcs.operator_command_fdr_enqueue_failed: {exc!r}",
+                extra={
+                    "kind": "c8.gcs.operator_command_fdr_enqueue_failed",
+                    "kv": {"error": repr(exc)},
+                },
+            )
+
+    def _extract_wgs84(self, output: EstimatorOutput) -> LatLonAlt:
+        wgs = output.extras.get("wgs84") if output.extras else None
+        if not isinstance(wgs, LatLonAlt):
+            raise GcsEmitError(
+                "EstimatorOutput.extras['wgs84'] missing or not a LatLonAlt; "
+                "composition root must inject the ENU->WGS84 enricher"
+            )
+        return wgs
+
+    def _clock_ms_boot(self) -> int:
+        return int(self._clock() * 1_000)

src/gps_denied_onboard/components/c8_fc_adapter/pymavlink_ardupilot_adapter.py

@@ -54,6 +54,7 @@ from gps_denied_onboard.components.c8_fc_adapter.errors import (
     FcEmitError,
     FcOpenError,
     SigningHandshakeError,
+    SourceSetSwitchError,
 )
 from gps_denied_onboard.config import Config
 from gps_denied_onboard.fdr_client.client import FdrClient
@@ -67,6 +68,10 @@ _GPS_FIX_TYPE_3D: Final[int] = 3
 _NAMED_VALUE_FLOAT_NAME: Final[str] = "src_lbl"
 _SIGNING_KEY_LEN: Final[int] = 32
 _BUILD_DEV_STATIC_KEY_ENV: Final[str] = "BUILD_DEV_STATIC_KEY"
+# ArduPilot-specific command (dialect: ardupilotmega).
+_MAV_CMD_SET_EKF_SOURCE_SET: Final[int] = 42007
+_MAV_RESULT_ACCEPTED: Final[int] = 0
+_SWITCH_RATE_LIMIT_S: Final[float] = 1.0
 
 
 # Maps Severity enum to MAVLink statustext severity numeric value.
@@ -110,6 +115,9 @@ class PymavlinkArdupilotAdapter:
         self._first_emit_logged = False
         self._open_emit_thread_ident: int | None = None
         self._signing_failure_logged_at_count = 0
+        # D-C8-2 source-set switch state (AZ-396).
+        self._last_switch_attempt_ns: int = 0
+        self._last_switch_succeeded: bool = False
         # Inbound bus + decoder (lazily constructed inside ``open``).
         self._bus = SubscriptionBus()
         self._inbound: PymavlinkInboundDecoder | None = None
@@ -291,7 +299,85 @@ class PymavlinkArdupilotAdapter:
         self._send_statustext_internal(msg, severity)
 
     def request_source_set_switch(self) -> None:
-        raise NotImplementedError("Owned by source-set task; install AZ-396 to enable")
+        """D-C8-2 source-set switch (AZ-396 / AC-NEW-2).
+
+        Sends ``MAV_CMD_SET_EKF_SOURCE_SET`` (ardupilotmega cmd 42007)
+        with ``param1 = config.fc.spoof_recovery_source_set`` and waits
+        up to ``config.fc.source_set_switch_timeout_ms`` for the FC's
+        ``COMMAND_ACK``. Idempotence per Invariant 11: a second call
+        within ``_SWITCH_RATE_LIMIT_S`` of the previous attempt is
+        no-op'd; a call after a successful switch logs INFO + STATUSTEXT
+        but does NOT re-issue.
+        """
+        if not self._opened or self._connection is None:
+            raise FcEmitError("adapter not opened")
+        self._enforce_single_writer()
+        now_ns = time.monotonic_ns()
+        if self._last_switch_attempt_ns:
+            elapsed_s = (now_ns - self._last_switch_attempt_ns) / 1_000_000_000
+            if elapsed_s < _SWITCH_RATE_LIMIT_S:
+                self._log.info(
+                    "c8.ap.source_set_switch_rate_limited",
+                    extra={
+                        "kind": "c8.ap.source_set_switch_rate_limited",
+                        "kv": {"elapsed_s": round(elapsed_s, 3)},
+                    },
+                )
+                return
+            if self._last_switch_succeeded:
+                self._log.info(
+                    "c8.ap.source_set_switch_already_active",
+                    extra={"kind": "c8.ap.source_set_switch_already_active", "kv": {}},
+                )
+                self._send_statustext_internal("src-set already active", Severity.INFO)
+                return
+        self._last_switch_attempt_ns = now_ns
+        source_set = int(self._config.fc.spoof_recovery_source_set)
+        timeout_ms = int(self._config.fc.source_set_switch_timeout_ms)
+        try:
+            self._connection.mav.command_long_send(
+                getattr(self._connection, "target_system", 1),
+                getattr(self._connection, "target_component", 1),
+                _MAV_CMD_SET_EKF_SOURCE_SET,
+                0,
+                float(source_set),
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+                0.0,
+            )
+        except Exception as exc:
+            self._handle_source_set_switch_failure(
+                reason=f"command_long_send failed: {exc!r}", source_set=source_set
+            )
+            raise SourceSetSwitchError(f"MAV_CMD_SET_EKF_SOURCE_SET send failed: {exc!r}") from exc
+        ack = self._wait_for_command_ack(_MAV_CMD_SET_EKF_SOURCE_SET, timeout_ms=timeout_ms)
+        if ack is None:
+            self._handle_source_set_switch_failure(
+                reason=f"ACK timeout after {timeout_ms}ms", source_set=source_set
+            )
+            raise SourceSetSwitchError(f"ACK timeout after {timeout_ms}ms")
+        result = int(getattr(ack, "result", -1))
+        if result != _MAV_RESULT_ACCEPTED:
+            self._handle_source_set_switch_failure(
+                reason=f"ACK result={result}", source_set=source_set
+            )
+            raise SourceSetSwitchError(f"MAV_CMD_SET_EKF_SOURCE_SET rejected: result={result}")
+        self._last_switch_succeeded = True
+        self._log.info(
+            "c8.ap.source_set_switch_executed",
+            extra={
+                "kind": "c8.ap.source_set_switch_executed",
+                "kv": {"source_set": source_set},
+            },
+        )
+        self._fdr_signing_event(
+            kind="c8.ap.source_set_switch_executed",
+            kv={"source_set": source_set, "flight_id": self._flight_id},
+        )
+        self._send_statustext_internal(f"src-set switched to {source_set}", Severity.INFO)
 
     def current_flight_state(self) -> FlightStateSignal:
         if self._inbound is None:
@@ -450,6 +536,57 @@ class PymavlinkArdupilotAdapter:
             },
         )
 
+    def _wait_for_command_ack(self, command_id: int, *, timeout_ms: int) -> Any | None:
+        """Wait up to ``timeout_ms`` for a `COMMAND_ACK` for ``command_id``.
+
+        Returns the ACK message on match, or ``None`` on timeout. Other
+        COMMAND_ACK messages (for unrelated commands) are ignored.
+        """
+        deadline = self._clock() + (timeout_ms / 1000.0)
+        while True:
+            remaining = deadline - self._clock()
+            if remaining <= 0:
+                return None
+            try:
+                msg = self._connection.recv_match(
+                    type="COMMAND_ACK", blocking=True, timeout=remaining
+                )
+            except Exception as exc:
+                self._log.debug(
+                    f"c8.ap.recv_match_failed: {exc!r}",
+                    extra={
+                        "kind": "c8.ap.recv_match_failed",
+                        "kv": {"error": repr(exc)},
+                    },
+                )
+                return None
+            if msg is None:
+                return None
+            if int(getattr(msg, "command", -1)) == command_id:
+                return msg
+
+    def _handle_source_set_switch_failure(self, *, reason: str, source_set: int) -> None:
+        self._last_switch_succeeded = False
+        self._log.error(
+            f"c8.ap.source_set_switch_failed: {reason}",
+            extra={
+                "kind": "c8.ap.source_set_switch_failed",
+                "kv": {"reason": reason, "source_set": source_set},
+            },
+        )
+        self._fdr_signing_event(
+            kind="c8.ap.source_set_switch_failed",
+            kv={
+                "reason": reason,
+                "source_set": source_set,
+                "flight_id": self._flight_id,
+            },
+        )
+        try:
+            self._send_statustext_internal(f"src-set switch failed: {reason}", Severity.ERROR)
+        except Exception:
+            pass
+
     def _extract_wgs84(self, output: EstimatorOutput) -> LatLonAlt:
         """Pull the WGS84 fix the composition root pre-attached.