[AZ-385] C5 SourceLabelStateMachine + spoof-promotion gate

Implements Invariants 5 + 8 + AC-NEW-2 / AC-NEW-8: the
EstimatorOutput.source_label now reflects a real state machine
(DEAD_RECKONED → SATELLITE_ANCHORED ↔ VISUAL_PROPAGATED) governed by
a spoof-promotion gate that latches closed on FC SPOOFED GPS health
and re-opens only when BOTH conditions hold — ≥10 s
STABLE_NON_SPOOFED AND next anchor within
spoof_promotion_visual_consistency_tol_m.

Every reject emits a c5.state.spoof_rejected FDR record plus a
subscriber-fan-out STATUSTEXT (severity WARNING, 50-char cap per
MAVLink). FDR and subscriber paths bypass the standard logger so
silencing logs cannot suppress the spoof trail (R07 / AC-6).

GtsamIsam2StateEstimator now eagerly builds the SM from C5StateConfig
in __init__; new public methods notify_gps_health() (delegates to
SM, called by composition root from C8 inbound) and
subscribe_spoof_rejection() (composition root attaches C8's
QgcTelemetryAdapter here). health_snapshot.spoof_promotion_blocked
+ current_estimate.source_label now flow from the live SM.

25 new unit tests across all 12 ACs plus cancellation, subscriber
exception isolation, and estimator wire-up integration cases. One
AZ-384 test renamed + updated to expect DEAD_RECKONED before any
anchor (was VISUAL_PROPAGATED placeholder pre-AZ-385).

Full suite: 632 passed, 2 skipped.

Co-authored-by: Cursor <cursoragent@cursor.com>

src/gps_denied_onboard/components/c5_state/_source_label_sm.py

@@ -0,0 +1,443 @@
+"""AZ-385 ``SourceLabelStateMachine`` — source-label + spoof-promotion gate.
+
+Per the C5 contract (Invariants 5 + 8 + AC-NEW-2 / AC-NEW-8) the
+``EstimatorOutput.source_label`` MUST reflect three real states:
+
+* ``SATELLITE_ANCHORED`` — the spoof-promotion gate is open AND a
+  recent satellite anchor exists.
+* ``VISUAL_PROPAGATED`` — anchor is stale OR the spoof-promotion gate
+  is closed.
+* ``DEAD_RECKONED`` — no satellite anchor has ever been observed.
+
+The gate latches CLOSED on the first FC-reported ``SPOOFED`` GPS
+health; it re-opens only when BOTH of these are true:
+
+1. FC GPS health has been ``STABLE_NON_SPOOFED`` for at least
+   ``spoof_promotion_min_stable_s`` (default 10 s).
+2. The next satellite anchor agrees with the FC GPS within
+   ``spoof_promotion_visual_consistency_tol_m`` metres (default 30 m).
+
+Every attempted promotion that fails either condition emits ONE
+``c5.state.spoof_rejected`` FDR record + fans out a STATUSTEXT
+callback to subscribers (composition root wires the C8 GCS adapter
+here). R07 / AC-6 — the FDR and subscriber paths bypass the standard
+logger, so silencing logs cannot suppress the reject trail.
+
+Threading: the state machine takes an explicit lock around every
+state mutation so the composition root's eventual C8-inbound-thread
+producer (``notify_gps_health``) does not race the C5 ingest-thread
+producer (``notify_satellite_anchor`` + reads from
+``current_estimate``). Single-writer thread per Invariant 1 is the
+deployed contract, but the lock is cheap and makes the unit-test
+matrix simpler.
+"""
+
+from __future__ import annotations
+
+import threading
+import time
+from collections.abc import Callable
+from datetime import datetime, timezone
+from typing import TYPE_CHECKING, Final, Protocol, runtime_checkable
+
+from gps_denied_onboard._types.fc import GpsStatus, Severity
+from gps_denied_onboard._types.state import PoseSourceLabel
+from gps_denied_onboard.fdr_client.records import FdrRecord
+from gps_denied_onboard.logging import get_logger
+
+if TYPE_CHECKING:
+    from gps_denied_onboard._types.fc import GpsHealth
+    from gps_denied_onboard.fdr_client.client import FdrClient
+
+__all__ = [
+    "RejectionCallback",
+    "RejectionSubscription",
+    "SourceLabelStateMachine",
+]
+
+
+# Subscriber signature — composition root receives
+# (reason, severity, statustext) on every reject. ``severity`` is
+# always ``WARNING`` per AC-12. ``statustext`` is pre-formatted and
+# capped at 50 chars (MAVLink ``STATUSTEXT.text`` max).
+RejectionCallback = Callable[[str, Severity, str], None]
+
+
+# AC-12 MAVLink STATUSTEXT max payload length (50 chars; longer texts
+# are silently truncated on the wire).
+_STATUSTEXT_MAX_LEN: Final[int] = 50
+
+# AC-12 STATUSTEXT severity — WARNING for spoof rejections.
+_REJECT_SEVERITY: Final[Severity] = Severity.WARNING
+
+# Anchor-staleness threshold for AC-3 ``VISUAL_PROPAGATED`` (ms).
+# Anchors older than this fall back to the visual-propagated label
+# even when the gate is open. The contract spec lists the threshold
+# as configurable in principle, but the only consumer (this state
+# machine) treats it as an implementation constant tied to the
+# D-C5-3 keyframe cadence (~3 Hz → 1 s between anchors at worst).
+_ANCHOR_STALENESS_THRESHOLD_MS: Final[int] = 1000
+
+# Reject reason codes — short tokens that fit into the 50-char
+# STATUSTEXT budget AFTER the ``"GPS spoof rejected: "`` prefix.
+_REASON_GPS_STILL_SPOOFED: Final[str] = "gps_spoofed"
+_REASON_DWELL_INSUFFICIENT: Final[str] = "dwell_short"
+_REASON_CONSISTENCY_VIOLATION: Final[str] = "consistency"
+_REASON_NO_GPS_OBSERVATION: Final[str] = "no_gps_obs"
+
+
+@runtime_checkable
+class RejectionSubscription(Protocol):
+    """Handle returned by :meth:`SourceLabelStateMachine.subscribe_rejection`.
+
+    Calling :meth:`cancel` removes the callback from the next
+    rejection dispatch. Subsequent cancels are no-ops.
+    """
+
+    def cancel(self) -> None: ...
+
+
+class _Subscription:
+    def __init__(
+        self,
+        registry: dict[int, RejectionCallback],
+        sub_id: int,
+        lock: threading.Lock,
+    ) -> None:
+        self._registry = registry
+        self._sub_id = sub_id
+        self._lock = lock
+
+    def cancel(self) -> None:
+        with self._lock:
+            self._registry.pop(self._sub_id, None)
+
+
+class SourceLabelStateMachine:
+    """Source-label + spoof-promotion state machine. One per estimator.
+
+    Public API:
+
+    * :meth:`notify_gps_health` — feed FC-reported GPS health; updates
+      the spoof-block latch + the stable-dwell timer.
+    * :meth:`notify_satellite_anchor` — feed a new satellite anchor;
+      may lift the spoof block (when BOTH AC-NEW-2 conditions hold)
+      or emit a reject record (otherwise, while blocked).
+    * :meth:`current_label` — pure read; returns the current label.
+    * :meth:`is_spoof_promotion_blocked` — pure read; returns the
+      gate state.
+    * :meth:`subscribe_rejection` — register a callback for the
+      STATUSTEXT mirror; returns a cancel handle.
+
+    The label is recomputed lazily on every read (``current_label``),
+    using the cached anchor timestamp + block state + clock. The
+    machine never mutates state from a read; only the two
+    ``notify_*`` methods change anything observable.
+    """
+
+    def __init__(
+        self,
+        *,
+        spoof_promotion_min_stable_s: float,
+        spoof_promotion_visual_consistency_tol_m: float,
+        fdr_client: FdrClient | None,
+        producer_id: str = "c5_state",
+        clock_ns: Callable[[], int] = time.monotonic_ns,
+    ) -> None:
+        if spoof_promotion_min_stable_s <= 0.0:
+            raise ValueError(
+                "SourceLabelStateMachine.spoof_promotion_min_stable_s must be > 0; "
+                f"got {spoof_promotion_min_stable_s}"
+            )
+        if spoof_promotion_visual_consistency_tol_m <= 0.0:
+            raise ValueError(
+                "SourceLabelStateMachine.spoof_promotion_visual_consistency_tol_m "
+                f"must be > 0; got {spoof_promotion_visual_consistency_tol_m}"
+            )
+        self._min_stable_ns: int = int(spoof_promotion_min_stable_s * 1_000_000_000)
+        self._consistency_tol_m: float = spoof_promotion_visual_consistency_tol_m
+        self._fdr_client: FdrClient | None = fdr_client
+        self._producer_id: str = producer_id
+        self._clock_ns: Callable[[], int] = clock_ns
+        self._log = get_logger("c5_state.source_label_sm")
+
+        self._lock = threading.Lock()
+        # Cached state — all writes go through the two ``notify_*``
+        # methods. Reads from ``current_label`` / ``is_spoof_promotion_blocked``
+        # snapshot under the same lock for consistency.
+        self._last_anchored_frame_ns: int | None = None
+        self._gps_status: GpsStatus | None = None
+        # Set when the FC reports STABLE_NON_SPOOFED; cleared on any
+        # other status. The dwell test is ``now - stable_since_ns >=
+        # min_stable_ns``.
+        self._gps_health_stable_since_ns: int | None = None
+        self._promotion_blocked: bool = False
+        self._last_label: PoseSourceLabel = PoseSourceLabel.DEAD_RECKONED
+
+        # Subscriber registry — composition root attaches C8's
+        # ``QgcTelemetryAdapter.send_statustext`` here so STATUSTEXT
+        # mirrors fire on the C8 outbound thread.
+        self._rejection_subs: dict[int, RejectionCallback] = {}
+        self._next_sub_id: int = 1
+
+    # ------------------------------------------------------------------
+    # Read API — pure; never mutates.
+
+    def current_label(self) -> PoseSourceLabel:
+        """Recompute + return the current source label.
+
+        Recomputation rules (per AC-1/2/3 + Invariant 5):
+
+        * No anchor ever observed → ``DEAD_RECKONED``.
+        * Spoof gate latched closed → ``VISUAL_PROPAGATED``.
+        * Last anchor older than ``_ANCHOR_STALENESS_THRESHOLD_MS``
+          → ``VISUAL_PROPAGATED``.
+        * Else → ``SATELLITE_ANCHORED``.
+        """
+        with self._lock:
+            label = self._recompute_label_locked()
+            self._last_label = label
+            return label
+
+    def is_spoof_promotion_blocked(self) -> bool:
+        """Return whether the spoof-promotion gate is currently closed."""
+        with self._lock:
+            return self._promotion_blocked
+
+    # ------------------------------------------------------------------
+    # Write API — both methods may transition state.
+
+    def notify_gps_health(self, gps_health: GpsHealth, now_ns: int | None = None) -> None:
+        """Feed a new ``GpsHealth`` observation.
+
+        On a SPOOFED → True transition the gate latches closed and one
+        ``c5.state.spoof_gate_engaged`` INFO log fires. On a transition
+        to ``STABLE_NON_SPOOFED`` the dwell timer starts; on any other
+        status the dwell timer clears.
+        """
+        ts = now_ns if now_ns is not None else self._clock_ns()
+        transition_info: tuple[bool, str, str] | None = None  # (block_engaged, from, to)
+        with self._lock:
+            prev_status = self._gps_status
+            self._gps_status = gps_health.status
+            if gps_health.status == GpsStatus.STABLE_NON_SPOOFED:
+                if prev_status != GpsStatus.STABLE_NON_SPOOFED:
+                    self._gps_health_stable_since_ns = ts
+            else:
+                self._gps_health_stable_since_ns = None
+            if gps_health.status == GpsStatus.SPOOFED and not self._promotion_blocked:
+                self._promotion_blocked = True
+                transition_info = (
+                    True,
+                    (prev_status.value if prev_status is not None else "init"),
+                    gps_health.status.value,
+                )
+
+        if transition_info is not None:
+            _engaged, prev, curr = transition_info
+            self._log.warning(
+                "c5.state.spoof_gate_engaged",
+                extra={
+                    "kind": "c5.state.spoof_gate_engaged",
+                    "kv": {"from": prev, "to": curr},
+                },
+            )
+
+    def notify_satellite_anchor(
+        self,
+        now_ns: int,
+        gps_consistency_delta_m: float | None,
+    ) -> None:
+        """Feed a new satellite anchor.
+
+        While the gate is open this is a pure bookkeeping call —
+        updates ``_last_anchored_frame_ns`` + may transition the
+        label from ``DEAD_RECKONED`` → ``SATELLITE_ANCHORED``.
+
+        While the gate is closed this is also a *promotion attempt*:
+        if both AC-NEW-2 conditions hold (≥10 s STABLE_NON_SPOOFED
+        AND consistency within tol_m), the gate re-opens and one
+        ``c5.state.spoof_gate_lifted`` INFO log fires; otherwise one
+        ``c5.state.spoof_rejected`` FDR record + STATUSTEXT mirror
+        fires (AC-6 — unsilenceable; bypasses the standard logger).
+        """
+        emit_reject: tuple[str, str, float, float | None] | None = None
+        emit_gate_lift: bool = False
+        emit_label_change: tuple[PoseSourceLabel, PoseSourceLabel] | None = None
+        with self._lock:
+            prev_label = self._last_label
+            self._last_anchored_frame_ns = now_ns
+            if self._promotion_blocked:
+                stable_ok = self._stable_long_enough_locked(now_ns)
+                consistency_ok = (
+                    gps_consistency_delta_m is not None
+                    and gps_consistency_delta_m <= self._consistency_tol_m
+                )
+                if stable_ok and consistency_ok:
+                    self._promotion_blocked = False
+                    self._gps_health_stable_since_ns = None
+                    emit_gate_lift = True
+                else:
+                    reason = self._classify_reject_reason_locked(
+                        stable_ok=stable_ok,
+                        consistency_ok=consistency_ok,
+                        gps_consistency_delta_m=gps_consistency_delta_m,
+                    )
+                    time_since_stable_s = self._time_since_stable_s_locked(now_ns)
+                    gps_status_str = (
+                        self._gps_status.value if self._gps_status is not None else "unknown"
+                    )
+                    emit_reject = (
+                        reason,
+                        gps_status_str,
+                        time_since_stable_s,
+                        gps_consistency_delta_m,
+                    )
+            new_label = self._recompute_label_locked()
+            if new_label != prev_label:
+                self._last_label = new_label
+                emit_label_change = (prev_label, new_label)
+
+        if emit_reject is not None:
+            self._emit_reject(*emit_reject)
+        if emit_gate_lift:
+            self._log.info(
+                "c5.state.spoof_gate_lifted",
+                extra={
+                    "kind": "c5.state.spoof_gate_lifted",
+                    "kv": {"consistency_tol_m": self._consistency_tol_m},
+                },
+            )
+        if emit_label_change is not None:
+            prev, curr = emit_label_change
+            self._log.info(
+                "c5.state.source_label_changed",
+                extra={
+                    "kind": "c5.state.source_label_changed",
+                    "kv": {
+                        "from": prev.value,
+                        "to": curr.value,
+                        "reason": "anchor_event",
+                    },
+                },
+            )
+
+    # ------------------------------------------------------------------
+    # Subscription API.
+
+    def subscribe_rejection(self, callback: RejectionCallback) -> RejectionSubscription:
+        """Register a callback invoked once per spoof-rejection event."""
+        with self._lock:
+            sub_id = self._next_sub_id
+            self._next_sub_id += 1
+            self._rejection_subs[sub_id] = callback
+        return _Subscription(self._rejection_subs, sub_id, self._lock)
+
+    # ------------------------------------------------------------------
+    # Internal helpers — all run under the lock unless suffixed
+    # ``_unlocked``.
+
+    def _recompute_label_locked(self) -> PoseSourceLabel:
+        if self._last_anchored_frame_ns is None:
+            return PoseSourceLabel.DEAD_RECKONED
+        if self._promotion_blocked:
+            return PoseSourceLabel.VISUAL_PROPAGATED
+        now_ns = self._clock_ns()
+        age_ms = (now_ns - self._last_anchored_frame_ns) / 1_000_000
+        if age_ms > _ANCHOR_STALENESS_THRESHOLD_MS:
+            return PoseSourceLabel.VISUAL_PROPAGATED
+        return PoseSourceLabel.SATELLITE_ANCHORED
+
+    def _stable_long_enough_locked(self, now_ns: int) -> bool:
+        if self._gps_health_stable_since_ns is None:
+            return False
+        return (now_ns - self._gps_health_stable_since_ns) >= self._min_stable_ns
+
+    def _time_since_stable_s_locked(self, now_ns: int) -> float:
+        if self._gps_health_stable_since_ns is None:
+            return 0.0
+        return (now_ns - self._gps_health_stable_since_ns) / 1_000_000_000
+
+    def _classify_reject_reason_locked(
+        self,
+        *,
+        stable_ok: bool,
+        consistency_ok: bool,
+        gps_consistency_delta_m: float | None,
+    ) -> str:
+        if self._gps_status == GpsStatus.SPOOFED:
+            return _REASON_GPS_STILL_SPOOFED
+        if gps_consistency_delta_m is None:
+            return _REASON_NO_GPS_OBSERVATION
+        if not stable_ok:
+            return _REASON_DWELL_INSUFFICIENT
+        if not consistency_ok:
+            return _REASON_CONSISTENCY_VIOLATION
+        return _REASON_DWELL_INSUFFICIENT
+
+    def _emit_reject(
+        self,
+        reason: str,
+        gps_status: str,
+        time_since_stable_s: float,
+        gps_consistency_delta_m: float | None,
+    ) -> None:
+        # AC-6: FDR + subscriber paths bypass the standard logger so
+        # silencing logs cannot suppress the spoof-rejection trail.
+        record = FdrRecord(
+            schema_version=1,
+            ts=datetime.now(tz=timezone.utc).isoformat(),
+            producer_id=self._producer_id,
+            kind="c5.state.spoof_rejected",
+            payload={
+                "reason": reason,
+                "gps_health": gps_status,
+                "time_since_stable_s": time_since_stable_s,
+                "visual_consistency_delta_m": gps_consistency_delta_m,
+            },
+        )
+        if self._fdr_client is not None:
+            try:
+                self._fdr_client.enqueue(record)
+            except Exception as exc:
+                self._log.debug(
+                    "c5.state.spoof_rejected_fdr_enqueue_failed",
+                    extra={
+                        "kind": "c5.state.spoof_rejected_fdr_enqueue_failed",
+                        "kv": {"error": repr(exc)},
+                    },
+                )
+        text = _format_statustext(reason)
+        with self._lock:
+            subs = list(self._rejection_subs.values())
+        for cb in subs:
+            try:
+                cb(reason, _REJECT_SEVERITY, text)
+            except Exception as exc:
+                self._log.debug(
+                    "c5.state.spoof_rejected_callback_failed",
+                    extra={
+                        "kind": "c5.state.spoof_rejected_callback_failed",
+                        "kv": {"error": repr(exc)},
+                    },
+                )
+        self._log.warning(
+            "c5.state.spoof_rejected",
+            extra={
+                "kind": "c5.state.spoof_rejected",
+                "kv": {
+                    "reason": reason,
+                    "gps_health": gps_status,
+                    "time_since_stable_s": time_since_stable_s,
+                    "visual_consistency_delta_m": gps_consistency_delta_m,
+                },
+            },
+        )
+
+
+def _format_statustext(reason: str) -> str:
+    """Build the 50-char MAVLink STATUSTEXT payload for a reject reason."""
+    text = f"GPS spoof rejected: {reason}"
+    if len(text) > _STATUSTEXT_MAX_LEN:
+        text = text[:_STATUSTEXT_MAX_LEN]
+    return text

src/gps_denied_onboard/components/c5_state/gtsam_isam2_estimator.py

@@ -58,6 +58,11 @@ from gps_denied_onboard.components.c5_state._isam2_handle import (
     ISam2GraphHandle,
     ISam2GraphHandleImpl,
 )
+from gps_denied_onboard.components.c5_state._source_label_sm import (
+    RejectionCallback,
+    RejectionSubscription,
+    SourceLabelStateMachine,
+)
 from gps_denied_onboard.components.c5_state.config import C5StateConfig
 from gps_denied_onboard.components.c5_state.errors import (
     EstimatorDegradedError,
@@ -69,6 +74,7 @@ from gps_denied_onboard.helpers.wgs_converter import WgsConverter
 from gps_denied_onboard.logging import get_logger
 
 if TYPE_CHECKING:
+    from gps_denied_onboard._types.fc import GpsHealth
     from gps_denied_onboard._types.nav import ImuWindow
     from gps_denied_onboard._types.pose import PoseEstimate
     from gps_denied_onboard._types.vio import VioOutput
@@ -194,11 +200,20 @@ class GtsamIsam2StateEstimator(StateEstimator):
         # origin via :meth:`set_enu_origin` from the first satellite
         # anchor.
         self._enu_origin: LatLonAlt | None = None
-        # Source-label state machine (AZ-385). When None,
-        # ``current_estimate`` emits ``VISUAL_PROPAGATED`` per the
-        # contract default and ``health_snapshot`` reports
-        # ``spoof_promotion_blocked=False``.
-        self._source_label_machine: Any | None = None
+        # Source-label state machine (AZ-385). Constructed eagerly
+        # so ``current_estimate`` + ``health_snapshot`` always have a
+        # real machine to query. The composition root feeds GPS
+        # health updates via :meth:`notify_gps_health` (sourced from
+        # C8 inbound, AZ-391) and subscribes the C8 GCS adapter to
+        # rejection events via :meth:`subscribe_spoof_rejection`.
+        # :meth:`attach_source_label_state_machine` remains available
+        # as an override for tests that need to inject a stub.
+        self._source_label_machine: SourceLabelStateMachine = SourceLabelStateMachine(
+            spoof_promotion_min_stable_s=block.spoof_promotion_min_stable_s,
+            spoof_promotion_visual_consistency_tol_m=block.spoof_promotion_visual_consistency_tol_m,
+            fdr_client=fdr_client,
+            producer_id="c5_state",
+        )
         # AC-NEW-8 rolling window of ``(ts_monotonic_ns, cov_norm)``
         # tuples for ``cov_norm_growing_for_s`` accounting.
         self._cov_norm_window: deque[tuple[int, float]] = deque()
@@ -270,14 +285,50 @@ class GtsamIsam2StateEstimator(StateEstimator):
         self._enu_origin = origin
 
     def attach_source_label_state_machine(self, machine: Any) -> None:
-        """Wire the AZ-385 source-label / spoof-promotion state machine.
+        """Override the AZ-385 source-label / spoof-promotion state machine.
+
+        Reserved for tests that need to inject a stub or alternative
+        implementation. In production the estimator constructs a
+        :class:`SourceLabelStateMachine` in ``__init__`` already; this
+        override REPLACES that instance. The composition root should
+        NOT call this in steady-state — it should drive the
+        pre-built machine via :meth:`notify_gps_health` and
+        :meth:`subscribe_spoof_rejection`.
 
         The injected object MUST expose ``current_label() -> PoseSourceLabel``
-        and ``is_spoof_promotion_blocked() -> bool``. AZ-384 only holds
-        the reference; AZ-385 owns the actual transition logic.
+        and ``is_spoof_promotion_blocked() -> bool``.
         """
         self._source_label_machine = machine
 
+    def notify_gps_health(self, gps_health: GpsHealth, now_ns: int | None = None) -> None:
+        """Forward an FC ``GpsHealth`` observation to the AZ-385 state machine.
+
+        Composition root wires C8 inbound's GPS-health subscription to
+        this method. The machine may transition the spoof-promotion
+        gate as a side-effect.
+        """
+        machine = self._source_label_machine
+        if isinstance(machine, SourceLabelStateMachine):
+            machine.notify_gps_health(gps_health, now_ns=now_ns)
+
+    def subscribe_spoof_rejection(self, callback: RejectionCallback) -> RejectionSubscription:
+        """Subscribe to AZ-385 spoof-rejection events.
+
+        Composition root attaches C8's ``QgcTelemetryAdapter`` here so
+        every reject mirrors as a MAVLink STATUSTEXT (severity
+        WARNING, capped at 50 chars per AC-12). See
+        :class:`SourceLabelStateMachine` for the exact callback
+        signature.
+        """
+        machine = self._source_label_machine
+        if not isinstance(machine, SourceLabelStateMachine):
+            raise StateEstimatorConfigError(
+                "subscribe_spoof_rejection requires the built-in "
+                "SourceLabelStateMachine; replace via attach_source_label_state_machine "
+                "with an instance that supports subscribe_rejection if needed."
+            )
+        return machine.subscribe_rejection(callback)
+
     # ------------------------------------------------------------------
     # AZ-388: AC-5.2 fallback public API.
 
@@ -447,6 +498,7 @@ class GtsamIsam2StateEstimator(StateEstimator):
                 raise EstimatorDegradedError(f"add_pose_anchor failed: {exc}") from exc
             self._reset_staging()
             self._record_committed_pose_key(pose_key)
+            self._notify_source_label_anchor(pose)
             self._log.debug(
                 "c5.state.add_pose_anchor_ok",
                 extra={
@@ -890,6 +942,33 @@ class GtsamIsam2StateEstimator(StateEstimator):
         except Exception:
             return (0.0, 0.0, 0.0)
 
+    def _notify_source_label_anchor(self, pose: PoseEstimate) -> None:
+        """Forward a successful satellite-anchor add to the AZ-385 SM.
+
+        The estimator does not currently compute the visual-vs-GPS
+        consistency delta itself (AZ-389 orthorectifier or the
+        composition root will supply it externally once available).
+        Until then we pass ``None``, which causes the state machine
+        to classify any in-block promotion attempt as
+        ``no_gps_observation`` — the conservative reject path.
+        """
+        machine = self._source_label_machine
+        if not isinstance(machine, SourceLabelStateMachine):
+            return
+        try:
+            machine.notify_satellite_anchor(
+                now_ns=time.monotonic_ns(),
+                gps_consistency_delta_m=None,
+            )
+        except Exception as exc:
+            self._log.error(
+                "c5.state.source_label_anchor_notify_failed",
+                extra={
+                    "kind": "c5.state.source_label_anchor_notify_failed",
+                    "kv": {"frame_id": str(pose.frame_id), "error": str(exc)},
+                },
+            )
+
     def _derive_source_label(self) -> PoseSourceLabel:
         """Read the source label from the AZ-385 state machine if wired.