gps-denied-onboard/src/gps_denied_onboard/helpers/altitude_provider.py

"""Above-Ground-Level (AGL) provider abstraction for C1 VIO scale recovery.

The monocular KLT/RANSAC strategy (AZ-334) recovers metric scale from the
unit-length translation that ``cv2.recoverPose`` emits by using ground
sample distance (GSD), which requires the current drone height above the
ground plane. The AGL signal lives in the C5 state estimator's nominal
position; this module wraps that read so the C1 strategy does not import
or hold a direct reference to the C5 estimator (which is built later in
the composition-root topological order).

AZ-919 introduces only the interface + plumbing. The GSD scale-recovery
math lands in AZ-920, and the degraded-mode signal in AZ-921.
"""

from __future__ import annotations

from typing import TYPE_CHECKING, Protocol, runtime_checkable

from gps_denied_onboard._types.state import IsamState

if TYPE_CHECKING:
    from collections.abc import Callable

    from gps_denied_onboard.components.c5_state.eskf_baseline import (
        EskfStateEstimator,
    )

__all__ = [
    "AltitudeProvider",
    "EskfNominalAltitudeProvider",
]


@runtime_checkable
class AltitudeProvider(Protocol):
    """Read the drone's current AGL height in metres, or ``None``.

    Producers MUST return ``None`` whenever the local-ENU origin has not
    yet been anchored (pre cold-start) or the underlying estimator is
    in :class:`IsamState.LOST`. Consumers MUST treat ``None`` as "no
    reliable AGL" and fall back to a non-scale-recovery code path
    (AZ-921 formalises that fallback as a degraded VIO output).
    """

    def agl_m(self, now_ns: int) -> float | None:  # pragma: no cover - Protocol
        """Return AGL in metres at ``now_ns`` (monotonic), or ``None``.

        ``now_ns`` is the same monotonic timebase used by the C1 strategy
        for ``VioOutput.emitted_at_ns``. It is currently advisory — the
        ESKF impl does not interpolate — but the parameter is in the
        Protocol so future implementations (e.g. an LPF-smoothed AGL or
        a DEM-aware provider) can interpolate or extrapolate without a
        breaking change.
        """
        ...


class EskfNominalAltitudeProvider:
    """Concrete :class:`AltitudeProvider` backed by the C5 ESKF estimator.

    Reads AGL as the Z component of the ESKF nominal-position vector in
    local-ENU. The takeoff origin is anchored at local-ENU ``(0, 0, 0)``
    when ``set_takeoff_origin`` lands, so ``nominal_pos_z`` IS the AGL
    once the origin has been set — no separate cold-start-altitude
    subtraction is needed.

    The estimator instance is supplied through a callable rather than
    held directly because the composition root builds C1 (where this
    provider is wired) before C5. The callable closes over the
    composition root's mutable ``constructed`` dict and resolves the
    estimator at every ``agl_m`` call, which is the same time the C1
    strategy actually consumes the AGL signal (well after the topo
    order has built C5).
    """

    def __init__(
        self,
        estimator_supplier: Callable[[], EskfStateEstimator | None],
    ) -> None:
        self._estimator_supplier = estimator_supplier

    def agl_m(self, now_ns: int) -> float | None:
        estimator = self._estimator_supplier()
        if estimator is None:
            return None
        if getattr(estimator, "_takeoff_origin_set", None) is None:
            return None
        if getattr(estimator, "_isam2_state", None) == IsamState.LOST:
            return None
        nominal_pos = getattr(estimator, "_nominal_pos", None)
        if nominal_pos is None:
            return None
        return float(nominal_pos[2])