[AZ-393] [AZ-394] [AZ-395] C8 outbound chain + AP MAVLink2 signing

AZ-393 ArduPilot outbound: PymavlinkArdupilotAdapter encodes
EstimatorOutput to MAVLink2 GPS_INPUT via gps_input_send; emits
NAMED_VALUE_FLOAT(name="src_lbl") every frame and STATUSTEXT on
source_label transition (1 Hz per-severity cap). Smoothed-output
guard (Invariant 6), single-writer thread (Invariant 8), SPD
propagation. Shared helper _outbound_provenance.py owns the
canonical source-label-to-float table + transition rate-limiter.

AZ-394 iNav outbound: Msp2InavAdapter encodes EstimatorOutput to
hand-rolled MSP2_SENSOR_GPS (0x1F03, 52-byte LE payload via
_msp2_sensor_gps_encoder.py + YAMSPy send_RAW_msg). Secondary
unsigned MAVLink channel for STATUSTEXT transitions. open()
rejects non-None signing_key (RESTRICT-COMM-2 / Invariant 2);
request_source_set_switch raises SourceSetSwitchNotSupportedError
(Invariant 9 verified: never calls setup_signing on secondary).

AZ-395 AP MAVLink2 signing: ephemeral per-flight 32-byte key
from secrets.token_bytes; pymavlink setup_signing handshake at
open(); in-place bytearray zeroisation on close(); mid-flight
signing-failure detection (ERROR log + WARNING STATUSTEXT + no
raise; threshold configurable). Key never logged / persisted /
serialised (regex-scanned by AC-4/AC-5). BUILD_DEV_STATIC_KEY=ON
enables repeatable static-key dev path; rejected at open() when
the build flag is absent.

Shared: EstimatorOutput.smoothed (default False) added for the
Invariant 6 gate at the C8 boundary; FcConfig extended with
dev_static_signing_key + signing_failure_threshold (additive
defaults; cross-field validation in __post_init__).

Tests: 33 new AC tests (11 + 11 + 11) covering all 30 ACs; full
suite 476 passing / 2 skipped / 0 failing (was 443). Contract
surfaces unchanged at fc_adapter_protocol v1.0.0 and
composition_root v1.2.0.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Oleksandr Bezdieniezhnykh
2026-05-11 04:47:44 +03:00
parent a61d2d3f4b
commit 1e0be08e8a
16 changed files with 2198 additions and 4 deletions
@@ -0,0 +1,156 @@
"""MSP2_SENSOR_GPS wire-format encoder/decoder (AZ-394 / E-C8).
iNav-specific message ``MSP2_SENSOR_GPS = 0x1F03`` — host-to-FC
external GPS injection. YAMSPy's MSP code table is BetaFlight-derived
and does NOT include this iNav-only code, so we encode the payload by
hand and ship it through the transport's ``send_RAW_msg(code, data)``
entry point.
Wire payload (little-endian, packed; iNav source of truth:
``inav/src/main/io/msp.c`` — search ``MSP2_SENSOR_GPS``):
| Offset | Field | Type | Notes |
|--------|-----------------------|--------|------------------------------------|
| 0 | instance | u8 | 0 (single external GPS instance) |
| 1 | gps_week | u16 | 0 (unused for external injection) |
| 3 | ms_tow | u32 | 0 (unused) |
| 7 | fix_type | u8 | 3 (3D fix); contract Invariant 5 |
| 8 | satellites_visible | u8 | cosmetic; we send 10 |
| 9 | h_pos_accuracy_mm | u16 | from CovarianceProjector (clamped) |
| 11 | v_pos_accuracy_mm | u16 | 0 (we don't project vertical) |
| 13 | h_vel_accuracy_mm_s | u16 | 0 |
| 15 | hdop | u16 | 0 |
| 17 | longitude_e7 | i32 | wgs84 lon * 1e7 |
| 21 | latitude_e7 | i32 | wgs84 lat * 1e7 |
| 25 | msl_altitude_cm | i32 | wgs84 alt * 100 |
| 29 | ned_vel_north_cm_s | i32 | 0 |
| 33 | ned_vel_east_cm_s | i32 | 0 |
| 37 | ned_vel_down_cm_s | i32 | 0 |
| 41 | ground_course_cdeg | u16 | 0 |
| 43 | true_yaw_cdeg | u16 | 0 |
| 45 | year | u16 | 0 |
| 47 | month | u8 | 0 |
| 48 | day | u8 | 0 |
| 49 | hour | u8 | 0 |
| 50 | min | u8 | 0 |
| 51 | sec | u8 | 0 |
| 52 | (end) | | total length = 52 bytes |
"""
from __future__ import annotations
import struct
from dataclasses import dataclass
from typing import Final
__all__ = [
"MSP2_SENSOR_GPS_CODE",
"MSP2_SENSOR_GPS_PAYLOAD_LEN",
"Msp2SensorGpsPayload",
"decode_msp2_sensor_gps",
"encode_msp2_sensor_gps",
]
MSP2_SENSOR_GPS_CODE: Final[int] = 0x1F03
MSP2_SENSOR_GPS_PAYLOAD_LEN: Final[int] = 52
# little-endian, no padding; matches inav's __packed__ struct layout.
_PACK_FMT: Final[str] = "<BHIBBHHHHiiiiiiHHHBBBBB"
@dataclass(frozen=True, slots=True)
class Msp2SensorGpsPayload:
"""Decoded MSP2_SENSOR_GPS payload (subset we exercise)."""
instance: int
fix_type: int
satellites_visible: int
h_pos_accuracy_mm: int
longitude_e7: int
latitude_e7: int
msl_altitude_cm: int
def encode_msp2_sensor_gps(
*,
fix_type: int,
satellites_visible: int,
h_pos_accuracy_mm: int,
longitude_e7: int,
latitude_e7: int,
msl_altitude_cm: int,
ned_vel_north_cm_s: int = 0,
ned_vel_east_cm_s: int = 0,
ned_vel_down_cm_s: int = 0,
) -> bytes:
"""Encode an MSP2_SENSOR_GPS payload (52 bytes)."""
return struct.pack(
_PACK_FMT,
0, # instance
0, # gps_week
0, # ms_tow
int(fix_type),
int(satellites_visible),
int(h_pos_accuracy_mm) & 0xFFFF,
0, # v_pos_accuracy_mm
0, # h_vel_accuracy_mm_s
0, # hdop
int(longitude_e7),
int(latitude_e7),
int(msl_altitude_cm),
int(ned_vel_north_cm_s),
int(ned_vel_east_cm_s),
int(ned_vel_down_cm_s),
0, # ground_course_cdeg
0, # true_yaw_cdeg
0, # year
0,
0,
0,
0,
0, # month/day/hour/min/sec
)
def decode_msp2_sensor_gps(buf: bytes) -> Msp2SensorGpsPayload:
"""Decode the subset of MSP2_SENSOR_GPS we exercise."""
if len(buf) != MSP2_SENSOR_GPS_PAYLOAD_LEN:
raise ValueError(
f"MSP2_SENSOR_GPS expects {MSP2_SENSOR_GPS_PAYLOAD_LEN} bytes; got {len(buf)}"
)
unpacked = struct.unpack(_PACK_FMT, buf)
(
instance,
_gps_week,
_ms_tow,
fix_type,
satellites_visible,
h_pos_accuracy_mm,
_v_pos_acc,
_h_vel_acc,
_hdop,
longitude_e7,
latitude_e7,
msl_altitude_cm,
_vn,
_ve,
_vd,
_gc,
_yaw,
_year,
_mo,
_d,
_h,
_mn,
_s,
) = unpacked
return Msp2SensorGpsPayload(
instance=instance,
fix_type=fix_type,
satellites_visible=satellites_visible,
h_pos_accuracy_mm=h_pos_accuracy_mm,
longitude_e7=longitude_e7,
latitude_e7=latitude_e7,
msl_altitude_cm=msl_altitude_cm,
)
@@ -0,0 +1,107 @@
"""Outbound provenance side-channel helpers (AZ-393 / AZ-394 / E-C8).
Two pieces shared by AP and iNav outbound paths:
1. :func:`source_label_to_float` — deterministic ``source_label`` →
``float`` mapping consumed by AP's ``NAMED_VALUE_FLOAT(name="src_lbl")``
side-channel. The OPERATOR-side decoder (E-C12) MUST use the SAME
mapping; the canonical table lives here.
2. :class:`StatusTextTransitionRateLimiter` — emits ``STATUSTEXT(...)``
exactly once per ``source_label`` transition (AC-4 / AZ-393 AC-3 /
AZ-394 AC-3), with a defensive 1-per-second per-severity hard cap
against pathological transition spam (constraint § 4 of AZ-393).
The rate limiter has no I/O; the adapter passes ``send_statustext``
in. This keeps the helper pure for unit testing and matches the
per-adapter wire dispatch responsibility.
"""
from __future__ import annotations
import threading
import time
from collections.abc import Callable
from typing import Final
from gps_denied_onboard._types.fc import Severity
__all__ = [
"SOURCE_LABEL_TO_FLOAT",
"StatusTextTransitionRateLimiter",
"source_label_to_float",
]
# Canonical source-label-to-float mapping (AZ-393 AC-3 / D-C8-7).
# Operator-side decoder in C12 MUST mirror this table.
SOURCE_LABEL_TO_FLOAT: Final[dict[str, float]] = {
"unknown": 0.0,
"visual_propagated": 1.0,
"sat_anchored": 2.0,
"imu_only": 3.0,
"warm_start": 4.0,
"smoothed": 5.0,
"ac52_fallback": 6.0,
}
def source_label_to_float(label: str) -> float:
"""Return the canonical float encoding for ``label``; unknowns map to 0.0."""
return SOURCE_LABEL_TO_FLOAT.get(label, SOURCE_LABEL_TO_FLOAT["unknown"])
class StatusTextTransitionRateLimiter:
"""Emits a STATUSTEXT exactly once per ``source_label`` transition.
Defensive secondary cap: at most one emit per second per severity,
in case a producer flaps source_label every frame at 5 Hz.
Thread-safe; ``note_label_and_maybe_emit`` may be called from any
thread. The send callback is invoked OUTSIDE the lock.
"""
def __init__(
self,
send_statustext: Callable[[str, Severity], None],
*,
min_interval_s: float = 1.0,
clock: Callable[[], float] = time.monotonic,
) -> None:
self._send = send_statustext
self._min_interval_s = min_interval_s
self._clock = clock
self._lock = threading.Lock()
self._last_label: str | None = None
self._last_emit_at_by_sev: dict[Severity, float] = {}
def note_label_and_maybe_emit(
self,
new_label: str,
*,
severity: Severity = Severity.INFO,
) -> bool:
"""Record ``new_label`` and emit a STATUSTEXT iff this is a transition.
Returns ``True`` if a STATUSTEXT was emitted, ``False`` otherwise.
"""
with self._lock:
if self._last_label == new_label:
return False
previous = self._last_label
self._last_label = new_label
# Hard cap: at most one emit per second per severity.
now = self._clock()
last_emit = self._last_emit_at_by_sev.get(severity, float("-inf"))
if (now - last_emit) < self._min_interval_s:
return False
self._last_emit_at_by_sev[severity] = now
msg = f"src={new_label}" if previous is None else f"src {previous}->{new_label}"
# Send OUTSIDE the lock — pymavlink statustext_send may block on UART.
self._send(msg, severity)
return True
@property
def last_label(self) -> str | None:
with self._lock:
return self._last_label
@@ -0,0 +1,283 @@
"""iNav MSP2 + secondary-MAVLink adapter (AZ-394 / E-C8).
Outbound: encodes ``EstimatorOutput`` to MSP2_SENSOR_GPS and writes
through a YAMSPy-style transport (``send_RAW_msg(code, data)``).
Side-channel: STATUSTEXT on the SECONDARY MAVLink telemetry link
(NOT on the primary MSP2 link) — iNav supports MAVLink for telemetry
output but not for primary positioning per RESTRICT-COMM-2. The
secondary link is ALWAYS unsigned (Invariant 9).
Build flag: ``BUILD_FC_INAV``.
"""
from __future__ import annotations
import threading
import time
from collections.abc import Callable
from typing import Any, Final
from gps_denied_onboard._types.emitted import EmittedExternalPosition
from gps_denied_onboard._types.fc import (
FcKind,
FlightState,
FlightStateSignal,
PortConfig,
Severity,
Subscription,
TelemetryCallback,
)
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._msp2_sensor_gps_encoder import (
MSP2_SENSOR_GPS_CODE,
encode_msp2_sensor_gps,
)
from gps_denied_onboard.components.c8_fc_adapter._outbound_provenance import (
StatusTextTransitionRateLimiter,
)
from gps_denied_onboard.components.c8_fc_adapter._subscription import SubscriptionBus
from gps_denied_onboard.components.c8_fc_adapter.errors import (
FcAdapterConfigError,
FcEmitError,
FcOpenError,
SourceSetSwitchNotSupportedError,
)
from gps_denied_onboard.config import Config
from gps_denied_onboard.fdr_client.client import FdrClient
from gps_denied_onboard.logging import get_logger
__all__ = ["Msp2InavAdapter"]
_FIX_TYPE_3D: Final[int] = 3
_INAV_DEFAULT_SATS: Final[int] = 10
def _mav_severity(sev: Severity) -> int:
return int(sev.value)
class Msp2InavAdapter:
"""iNav FcAdapter (MSP2 primary + unsigned MAVLink secondary)."""
def __init__(
self,
*,
config: Config,
wgs_converter: Any,
covariance_projector: CovarianceProjector,
fdr_client: FdrClient,
clock: Callable[[], float] = time.monotonic,
msp_connect_factory: Callable[[str, int], Any] | None = None,
secondary_mavlink_factory: Callable[[], 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._msp_connect_factory = msp_connect_factory
self._secondary_mavlink_factory = secondary_mavlink_factory
self._log = get_logger("c8_fc_adapter.inav_adapter")
# Wire state ------------------------------------------------------
self._msp: Any = None
self._secondary_mav: Any = None
self._opened = False
self._sequence_number = 0
self._first_emit_logged = False
self._open_emit_thread_ident: int | None = None
# Inbound bus stub — AZ-394 doesn't wire the inbound (iNav
# polling decoder lands in AZ-391; the per-adapter inbound
# composition happens in a follow-up batch).
self._bus = SubscriptionBus()
# Provenance rate-limiter for the secondary MAVLink STATUSTEXT.
self._provenance = StatusTextTransitionRateLimiter(
send_statustext=self._send_statustext_secondary,
clock=time.monotonic,
)
# ------------------------------------------------------------------
# FcAdapter Protocol implementation
def open(self, port: PortConfig, signing_key: bytes | None) -> None:
if self._opened:
raise FcOpenError("Msp2InavAdapter already opened")
if port.fc_kind is not FcKind.INAV:
raise FcOpenError(f"Msp2InavAdapter requires FcKind.INAV; got {port.fc_kind!r}")
if signing_key is not None:
# Invariant 2 / RESTRICT-COMM-2 — iNav has no signing.
raise FcAdapterConfigError("iNav does not support MAVLink signing per RESTRICT-COMM-2")
try:
self._msp = self._connect_msp(port)
except Exception as exc:
raise FcOpenError(f"MSP2 connect failed: {exc!r}") from exc
try:
self._secondary_mav = (
self._secondary_mavlink_factory()
if self._secondary_mavlink_factory is not None
else None
)
except Exception as exc:
self._log.warning(
f"c8.inav.secondary_mavlink_open_failed: {exc!r}",
extra={
"kind": "c8.inav.secondary_mavlink_open_failed",
"kv": {"error": repr(exc)},
},
)
self._secondary_mav = None
self._opened = True
def close(self) -> None:
if not self._opened:
return
try:
for conn in (self._msp, self._secondary_mav):
if conn is not None and hasattr(conn, "close"):
try:
conn.close()
except Exception:
pass
finally:
self._opened = False
self._msp = None
self._secondary_mav = None
self._open_emit_thread_ident = None
self._first_emit_logged = False
def subscribe_telemetry(self, callback: TelemetryCallback) -> Subscription:
return self._bus.subscribe(callback)
def emit_external_position(self, output: EstimatorOutput) -> EmittedExternalPosition:
if not self._opened or self._msp is None:
raise FcEmitError("adapter not opened")
self._enforce_single_writer()
if output.smoothed:
self._log_emit_failed("smoothed_rejected", output.frame_id)
raise FcEmitError("smoothed output cannot be emitted to FC (Invariant 6)")
h_pos_accuracy_mm = self._cov_projector.to_inav_h_pos_accuracy_mm(output)
wgs = self._extract_wgs84(output)
emitted_at = time.monotonic_ns()
self._sequence_number = (self._sequence_number + 1) & 0xFF
seq = self._sequence_number
payload = encode_msp2_sensor_gps(
fix_type=_FIX_TYPE_3D,
satellites_visible=_INAV_DEFAULT_SATS,
h_pos_accuracy_mm=h_pos_accuracy_mm,
longitude_e7=int(wgs.lon_deg * 1e7),
latitude_e7=int(wgs.lat_deg * 1e7),
msl_altitude_cm=int(wgs.alt_m * 100.0),
)
try:
self._msp.send_RAW_msg(MSP2_SENSOR_GPS_CODE, payload)
except Exception as exc:
self._log_emit_failed(repr(exc), output.frame_id)
raise FcEmitError(f"iNav MSP2 outbound wire emit failed: {exc!r}") from exc
self._provenance.note_label_and_maybe_emit(output.source_label, severity=Severity.INFO)
if not self._first_emit_logged:
self._first_emit_logged = True
self._log.info(
"c8.inav.first_emit",
extra={
"kind": "c8.inav.first_emit",
"kv": {"frame_id": output.frame_id, "seq": seq},
},
)
self._log.debug(
"c8.inav.emit",
extra={
"kind": "c8.inav.emit",
"kv": {
"frame_seq": seq,
"h_pos_accuracy_mm": h_pos_accuracy_mm,
"source_label": output.source_label,
},
},
)
return EmittedExternalPosition(
fc_kind=FcKind.INAV,
horiz_accuracy_m=h_pos_accuracy_mm / 1000.0,
source_label=output.source_label,
emitted_at=emitted_at,
sequence_number=seq,
)
def emit_status_text(self, msg: str, severity: Severity) -> None:
if not self._opened:
raise FcEmitError("adapter not opened")
self._enforce_single_writer()
self._send_statustext_secondary(msg, severity)
def request_source_set_switch(self) -> None:
raise SourceSetSwitchNotSupportedError("iNav: no MAV_CMD_SET_EKF_SOURCE_SET equivalent")
def current_flight_state(self) -> FlightStateSignal:
# Inbound iNav state ring is not wired into this adapter shell;
# the inbound decoder (AZ-391) exposes its own state_ring that
# the composition root can read directly. Default to INIT here.
return FlightStateSignal(
state=FlightState.INIT,
last_valid_gps_hint_wgs84=None,
last_valid_gps_age_ms=None,
captured_at=time.monotonic_ns(),
)
# ------------------------------------------------------------------
# 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(
"Msp2InavAdapter outbound is single-writer; "
f"first thread={self._open_emit_thread_ident}, this thread={cur}"
)
def _connect_msp(self, port: PortConfig) -> Any:
if self._msp_connect_factory is not None:
return self._msp_connect_factory(port.device, port.baud)
# Lazy import so the test path with an injected factory does NOT
# require yamspy at module-import time.
from yamspy import MSPy
return MSPy(device=port.device, baudrate=port.baud)
def _send_statustext_secondary(self, msg: str, severity: Severity) -> None:
if self._secondary_mav is None:
return
text = msg.encode("utf-8")[:50]
try:
self._secondary_mav.mav.statustext_send(_mav_severity(severity), text)
except Exception as exc:
self._log.debug(
f"c8.inav.secondary_statustext_failed: {exc!r}",
extra={
"kind": "c8.inav.secondary_statustext_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 FcEmitError(
"EstimatorOutput.extras['wgs84'] missing or not a LatLonAlt; "
"composition root must inject the ENU->WGS84 enricher"
)
return wgs
def _log_emit_failed(self, reason: str, frame_id: int) -> None:
self._log.error(
f"c8.inav.emit_failed: {reason}",
extra={
"kind": "c8.inav.emit_failed",
"kv": {"reason": reason, "frame_id": frame_id},
},
)
@@ -0,0 +1,499 @@
"""ArduPilot MAVLink 2.0 adapter (AZ-393 outbound, AZ-395 signing, E-C8).
Composes :class:`PymavlinkInboundDecoder` (AZ-391) for the inbound
producer side and the local outbound encoder for `emit_external_position` +
`emit_status_text`. Implements `FcAdapter` per the contract.
Production wiring (composition-root, AZ-393 / AZ-395 follow-up):
- Constructor receives the WgsConverter (AZ-279), CovarianceProjector
(AZ-392), FdrClient (AZ-273), and a clock callable.
- `open(port, signing_key)` opens the pymavlink connection on the
configured port and, if signing is enabled in config, completes
the per-flight signing handshake (AZ-395).
- A single composition-root thread is bound for outbound emit via
:func:`bind_outbound_emit_thread`; emitting from a second thread
raises :class:`RuntimeError`.
Build flag: ``BUILD_FC_ARDUPILOT_PLANE``.
"""
from __future__ import annotations
import os
import secrets
import threading
import time
from collections.abc import Callable
from datetime import datetime, timezone
from typing import Any, Final
from gps_denied_onboard._types.emitted import EmittedExternalPosition
from gps_denied_onboard._types.fc import (
FcKind,
FlightState,
FlightStateSignal,
PortConfig,
Severity,
Subscription,
TelemetryCallback,
)
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._inbound_mavlink import (
PymavlinkInboundDecoder,
)
from gps_denied_onboard.components.c8_fc_adapter._outbound_provenance import (
StatusTextTransitionRateLimiter,
source_label_to_float,
)
from gps_denied_onboard.components.c8_fc_adapter._subscription import SubscriptionBus
from gps_denied_onboard.components.c8_fc_adapter.errors import (
FcEmitError,
FcOpenError,
SigningHandshakeError,
)
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__ = ["PymavlinkArdupilotAdapter"]
_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"
# Maps Severity enum to MAVLink statustext severity numeric value.
def _mav_severity(sev: Severity) -> int:
return int(sev.value)
class PymavlinkArdupilotAdapter:
"""ArduPilot Plane FcAdapter (MAVLink 2.0).
Threading: outbound (``emit_*``) is bound to a single composition-
root thread; inbound (decode loop) runs on its own thread inside
``PymavlinkInboundDecoder.run_decode_loop`` (AZ-391).
"""
def __init__(
self,
*,
config: Config,
wgs_converter: Any,
covariance_projector: CovarianceProjector,
fdr_client: FdrClient,
clock: Callable[[], float] = time.monotonic,
flight_id: str = "",
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._flight_id = flight_id
self._connect_factory = connect_factory
self._signing_failure_threshold = max(1, int(config.fc.signing_failure_threshold))
self._log = get_logger("c8_fc_adapter.ap_adapter")
# Wire state ------------------------------------------------------
self._connection: Any = None
self._signing_key: bytearray | None = None
self._opened = False
self._sequence_number = 0
self._first_emit_logged = False
self._open_emit_thread_ident: int | None = None
self._signing_failure_logged_at_count = 0
# Inbound bus + decoder (lazily constructed inside ``open``).
self._bus = SubscriptionBus()
self._inbound: PymavlinkInboundDecoder | None = None
self._inbound_thread: threading.Thread | None = None
# Outbound provenance rate limiter.
self._provenance = StatusTextTransitionRateLimiter(
send_statustext=self._send_statustext_internal,
clock=time.monotonic,
)
# ------------------------------------------------------------------
# FcAdapter Protocol implementation
def open(self, port: PortConfig, signing_key: bytes | None) -> None:
if self._opened:
raise FcOpenError("PymavlinkArdupilotAdapter already opened")
if port.fc_kind is not FcKind.ARDUPILOT_PLANE:
raise FcOpenError(
f"PymavlinkArdupilotAdapter requires FcKind.ARDUPILOT_PLANE; got {port.fc_kind!r}"
)
try:
self._connection = self._connect(port)
except Exception as exc:
raise FcOpenError(f"pymavlink connect failed: {exc!r}") from exc
# Per-flight signing key handling (AZ-395). ----------------------
# In production, `signing_key_source = "ephemeral_per_flight"`; we
# generate a fresh 32-byte key. The caller (composition root) is
# also free to pass an explicit signing_key for test paths.
source = self._config.fc.signing_key_source
if source == "none":
pass
elif source == "ephemeral_per_flight":
self._signing_key = self._materialise_signing_key(signing_key)
try:
self._setup_signing(self._signing_key)
except Exception as exc:
self._handle_signing_handshake_failure(exc)
raise SigningHandshakeError(f"AP signing handshake failed: {exc!r}") from exc
self._fdr_signing_event(
kind="c8.ap.signing_key_rotated",
kv={"flight_id": self._flight_id, "key_age_s": 0},
)
elif source == "dev_static":
if os.environ.get(_BUILD_DEV_STATIC_KEY_ENV) != "ON":
raise FcOpenError(
"signing_key_source='dev_static' requires "
f"{_BUILD_DEV_STATIC_KEY_ENV}=ON at build time"
)
self._signing_key = self._materialise_dev_static_key()
try:
self._setup_signing(self._signing_key)
except Exception as exc:
self._handle_signing_handshake_failure(exc)
raise SigningHandshakeError(f"AP signing handshake failed: {exc!r}") from exc
self._log.warning(
"c8.ap.signing_dev_static_key",
extra={"kind": "c8.ap.signing_dev_static_key", "kv": {}},
)
else:
raise FcOpenError(f"unknown signing_key_source={source!r}")
# Inbound decoder + thread.
self._inbound = PymavlinkInboundDecoder(self._connection, self._bus)
thread = threading.Thread(
target=self._inbound.run_decode_loop,
name="c8.ap.inbound",
daemon=True,
)
self._inbound_thread = thread
thread.start()
self._open_emit_thread_ident = None
self._opened = True
def close(self) -> None:
if not self._opened:
return
if self._inbound is not None:
self._inbound.stop()
# Zeroise the signing-key buffer BEFORE we let it be deallocated
# (AZ-395 AC-7 / Invariant 10).
if self._signing_key is not None:
self._zeroise_signing_key(self._signing_key)
self._log.info(
"c8.ap.signing_key_zeroised",
extra={"kind": "c8.ap.signing_key_zeroised", "kv": {}},
)
self._signing_key = None
try:
if self._connection is not None and hasattr(self._connection, "close"):
self._connection.close()
finally:
self._opened = False
self._connection = None
self._inbound = None
self._open_emit_thread_ident = None
self._first_emit_logged = False
def subscribe_telemetry(self, callback: TelemetryCallback) -> Subscription:
return self._bus.subscribe(callback)
def emit_external_position(self, output: EstimatorOutput) -> EmittedExternalPosition:
if not self._opened or self._connection is None:
raise FcEmitError("adapter not opened")
self._enforce_single_writer()
if output.smoothed:
self._log_emit_failed("smoothed_rejected", output.frame_id)
raise FcEmitError("smoothed output cannot be emitted to FC (Invariant 6)")
horiz_accuracy_m = self._cov_projector.to_ardupilot_horiz_accuracy_m(output)
wgs = self._extract_wgs84(output)
emitted_at = time.monotonic_ns()
self._sequence_number += 1
seq = self._sequence_number
try:
self._connection.mav.gps_input_send(
int(self._clock_us()),
0, # gps_id (primary)
0, # ignore_flags
0, # time_week_ms
0, # time_week
_GPS_FIX_TYPE_3D,
int(wgs.lat_deg * 1e7),
int(wgs.lon_deg * 1e7),
float(wgs.alt_m),
0.0, # hdop
0.0, # vdop
0.0, # vn
0.0, # ve
0.0, # vd
0.0, # speed_accuracy
float(horiz_accuracy_m),
0.0, # vert_accuracy
10, # satellites_visible (synthetic; cosmetic for AP EKF)
0, # yaw
)
self._connection.mav.named_value_float_send(
int(self._clock_ms_boot()),
_NAMED_VALUE_FLOAT_NAME.encode("utf-8"),
source_label_to_float(output.source_label),
)
except Exception as exc:
self._log_emit_failed(repr(exc), output.frame_id)
raise FcEmitError(f"AP outbound wire emit failed: {exc!r}") from exc
# AC-NEW-4 / AZ-395 AC-6: poll signing-failure counter post-emit;
# only log on the transition past the configured threshold.
self._poll_signing_failure_counter()
# Source-label transition STATUSTEXT (AC-4).
self._provenance.note_label_and_maybe_emit(output.source_label, severity=Severity.INFO)
if not self._first_emit_logged:
self._first_emit_logged = True
self._log.info(
"c8.ap.first_emit",
extra={
"kind": "c8.ap.first_emit",
"kv": {"frame_id": output.frame_id, "seq": seq},
},
)
self._log.debug(
"c8.ap.emit",
extra={
"kind": "c8.ap.emit",
"kv": {
"frame_seq": seq,
"horiz_accuracy_m": horiz_accuracy_m,
"source_label": output.source_label,
},
},
)
return EmittedExternalPosition(
fc_kind=FcKind.ARDUPILOT_PLANE,
horiz_accuracy_m=horiz_accuracy_m,
source_label=output.source_label,
emitted_at=emitted_at,
sequence_number=seq,
)
def emit_status_text(self, msg: str, severity: Severity) -> None:
if not self._opened or self._connection is None:
raise FcEmitError("adapter not opened")
self._enforce_single_writer()
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")
def current_flight_state(self) -> FlightStateSignal:
if self._inbound is None:
raise FcEmitError("current_flight_state requires open(); no inbound decoder yet")
latest = self._inbound.state_ring.peek_latest()
if latest is None:
return FlightStateSignal(
state=FlightState.INIT,
last_valid_gps_hint_wgs84=None,
last_valid_gps_age_ms=None,
captured_at=time.monotonic_ns(),
)
payload = latest.payload
assert isinstance(payload, FlightStateSignal)
return payload
# ------------------------------------------------------------------
# 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(
"PymavlinkArdupilotAdapter 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)
# Lazy import so the test path with an injected factory does NOT
# require pymavlink at module-import time.
from pymavlink import mavutil
return mavutil.mavlink_connection(
port.device,
baud=port.baud,
dialect="common",
mavlink_version="2.0",
)
def _setup_signing(self, key: bytearray) -> None:
if not hasattr(self._connection, "setup_signing"):
return
# pymavlink's setup_signing accepts bytes; pass the buffer (it
# converts to bytes internally so the buffer here stays the
# authoritative copy we zeroise on close).
self._connection.setup_signing(bytes(key))
def _materialise_signing_key(self, supplied: bytes | None) -> bytearray:
if supplied is None:
return bytearray(secrets.token_bytes(_SIGNING_KEY_LEN))
if len(supplied) != _SIGNING_KEY_LEN:
raise SigningHandshakeError(
f"signing_key must be {_SIGNING_KEY_LEN} bytes; got {len(supplied)}"
)
return bytearray(supplied)
def _materialise_dev_static_key(self) -> bytearray:
"""Hex-decode FcConfig.dev_static_signing_key into a 32-byte buffer."""
hex_str = self._config.fc.dev_static_signing_key.strip()
try:
raw = bytes.fromhex(hex_str)
except ValueError as exc:
raise SigningHandshakeError(
"FcConfig.dev_static_signing_key must be valid hex"
) from exc
if len(raw) != _SIGNING_KEY_LEN:
raise SigningHandshakeError(
f"FcConfig.dev_static_signing_key decodes to {len(raw)} bytes; "
f"expected {_SIGNING_KEY_LEN}"
)
return bytearray(raw)
@staticmethod
def _zeroise_signing_key(buf: bytearray) -> None:
for i in range(len(buf)):
buf[i] = 0
def _handle_signing_handshake_failure(self, exc: BaseException) -> None:
self._log.error(
f"c8.ap.signing_handshake_failed: {exc!r}",
extra={
"kind": "c8.ap.signing_handshake_failed",
"kv": {"error_class": type(exc).__name__},
},
)
self._fdr_signing_event(
kind="c8.ap.signing_handshake_failed",
kv={
"flight_id": self._flight_id,
"error_class": type(exc).__name__,
},
)
def _poll_signing_failure_counter(self) -> None:
if self._signing_key is None:
return
count = getattr(self._connection.mav, "signing", None)
if count is None:
return
failure_count = int(getattr(count, "sig_count", 0) or 0)
if failure_count <= self._signing_failure_logged_at_count:
return
if failure_count < self._signing_failure_threshold:
return
# Transition past the threshold — log + FDR + STATUSTEXT, no raise.
self._signing_failure_logged_at_count = failure_count
self._log.error(
f"c8.ap.signing_failure: count={failure_count}",
extra={
"kind": "c8.ap.signing_failure",
"kv": {"failure_count": failure_count},
},
)
self._fdr_signing_event(
kind="c8.ap.signing_failure",
kv={
"flight_id": self._flight_id,
"failure_count": failure_count,
},
)
try:
self._send_statustext_internal(
f"signing failure count={failure_count}", Severity.WARNING
)
except Exception:
# STATUSTEXT failure on the same UART is non-fatal; the
# ERROR log + FDR already captured the event.
pass
def _send_statustext_internal(self, msg: str, severity: Severity) -> None:
if self._connection is None:
return
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.ap.statustext_failed: {exc!r}",
extra={
"kind": "c8.ap.statustext_failed",
"kv": {"error": repr(exc)},
},
)
def _log_emit_failed(self, reason: str, frame_id: int) -> None:
self._log.error(
f"c8.ap.emit_failed: {reason}",
extra={
"kind": "c8.ap.emit_failed",
"kv": {"reason": reason, "frame_id": frame_id},
},
)
def _extract_wgs84(self, output: EstimatorOutput) -> LatLonAlt:
"""Pull the WGS84 fix the composition root pre-attached.
C5 emits its estimate in the local ENU frame; the composition
root injects a WgsConverter-backed enricher that attaches the
WGS84 conversion to ``output.extras["wgs84"]`` BEFORE handing
the output to C8. If the enricher is missing the wgs84 key,
that is a composition bug — fail loudly rather than guess.
"""
wgs = output.extras.get("wgs84") if output.extras else None
if not isinstance(wgs, LatLonAlt):
raise FcEmitError(
"EstimatorOutput.extras['wgs84'] missing or not a LatLonAlt; "
"composition root must inject the ENU->WGS84 enricher"
)
return wgs
def _clock_us(self) -> int:
return int(self._clock() * 1_000_000)
def _clock_ms_boot(self) -> int:
return int(self._clock() * 1_000)
def _fdr_signing_event(self, *, kind: str, kv: dict[str, Any]) -> None:
record = FdrRecord(
schema_version=1,
ts=datetime.now(tz=timezone.utc).isoformat(),
producer_id="c8_fc_adapter",
kind="log",
payload={
"level": "INFO" if kind.endswith("_rotated") else "ERROR",
"component": "c8_fc_adapter",
"kind": kind,
"msg": kind,
"kv": kv,
},
)
try:
self._fdr_client.enqueue(record)
except Exception as exc:
self._log.debug(
f"c8.ap.fdr_enqueue_failed: {exc!r}",
extra={
"kind": "c8.ap.fdr_enqueue_failed",
"kv": {"error": repr(exc), "downstream_kind": kind},
},
)