motian_mavbridge/motian_bridge/telemetry.py

"""State derived from checksummed VKFly MAVLink messages.

The wire parser and cached message sources live on the serial reader thread only.
Observations are grouped per flight controller in an
:class:`~motian_bridge.aircraft.Aircraft`. Times below are monotonic receive
times, never the flight controller's boot clock.
"""

import logging
import time
from typing import Any

from .aircraft import Aircraft, ParamTable
from .config import Config
from .dialects.v20 import VKFly


LOG = logging.getLogger(__name__)
# The generated module honors MAV_IGNORE_CRC from the environment by default.
# Never permit that override for telemetry used as flight state.
VKFly.MAVLINK_IGNORE_CRC = 0
REPORT_SECONDS = 1.0
_INVALID_AUTOPILOT = VKFly.MAV_AUTOPILOT_INVALID
_AUTOPILOT_COMPONENT = VKFly.MAV_COMP_ID_AUTOPILOT1
_ARMED_FLAG = VKFly.MAV_MODE_FLAG_SAFETY_ARMED


class Telemetry:
    """Parse MAVLink v1/v2 bytes and emit (DataType, Data) reports.

    Neither the parser nor the current :class:`Aircraft` is shared across serial
    reconnects. Non-flight components and other systems never contribute to the
    selected UAV's state.
    """

    def __init__(self, config: Config):
        self.config = config
        self.parser = VKFly.MAVLink(None)
        self.aircraft: Aircraft | None = None
        self._empty_params = ParamTable()
        self.battery_dirty = False
        self.last_flight_report: float | None = None
        self.last_battery_report: float | None = None
        self.last_armed: bool | None = None
        self.last_missing_log: float | None = None

    # Backwards-compatible views onto the currently locked aircraft. These are
    # empty until the first valid flight-controller HEARTBEAT arrives.
    @property
    def source(self) -> tuple[int, int] | None:
        return self.aircraft.source if self.aircraft is not None else None

    @property
    def latest(self) -> dict[str, tuple[Any, float]]:
        return self.aircraft.latest if self.aircraft is not None else {}

    @property
    def batteries(self) -> dict[int, tuple[Any, float]]:
        return self.aircraft.batteries if self.aircraft is not None else {}

    @property
    def params(self) -> ParamTable:
        return self.aircraft.params if self.aircraft is not None else self._empty_params

    def param_value(self, param_id: str) -> float | None:
        return self.params.get(param_id)

    def feed(self, chunk: bytes, now: float | None = None) -> list[tuple[int, Any]]:
        now = time.monotonic() if now is None else now
        reports: list[tuple[int, Any]] = []
        # parse_char returns at most one message, so feed bytes individually.
        # Splitting and coalescing serial reads does not affect message framing.
        for byte in chunk:
            try:
                message = self.parser.parse_char(bytes((byte,)))
            except VKFly.MAVError as exc:
                LOG.debug("discarded invalid MAVLink frame: %s", exc)
                continue
            if message is not None:
                reports.extend(self._handle(message, now))
        return reports

    def _lock(self, message: Any, src: tuple[int, int]) -> None:
        self.aircraft = Aircraft(
            self.config, src[0], src[1], int(message.autopilot), int(message.type)
        )
        LOG.info("locked MAVLink flight controller sys=%s comp=%s", *src)

    def _handle(self, message: Any, now: float) -> list[tuple[int, Any]]:
        kind = message.get_type()
        src = (message.get_srcSystem(), message.get_srcComponent())
        if self.aircraft is None:
            if (
                kind != "HEARTBEAT"
                or src[1] != _AUTOPILOT_COMPONENT
                or message.autopilot == _INVALID_AUTOPILOT
            ):
                return []
            self._lock(message, src)
        if src != self.aircraft.source:
            return []

        # A BMS frame with an unexpected ID must not appear as a usable pack.
        if kind == "VK_BMS_STATUS" and message.bat_id not in (0, 1):
            return []
        self.aircraft.latest[kind] = (message, now)
        if kind == "HEARTBEAT":
            armed = bool(message.base_mode & _ARMED_FLAG)
            if armed != self.last_armed:
                self.last_armed = armed
                # Platform convention: "flying" means armed, not airborne.
                return [(274, armed)]
        elif kind == "VK_BMS_STATUS" and message.bat_id in (0, 1):
            self.aircraft.batteries[message.bat_id] = (message, now)
            self.battery_dirty = True
        elif kind == "STATUSTEXT":
            # Generated Python text decoding assumes ASCII and irreversibly
            # replaces UTF-8 bytes. Read the already CRC-validated frame's
            # severity + char[50] wire payload instead.
            frame = bytes(message.get_msgbuf())
            header_size = 6 if frame[0] == VKFly.PROTOCOL_MARKER_V1 else 10
            payload = frame[header_size : header_size + frame[1]]
            text = payload[1:51].split(b"\x00", 1)[0]
            text = text.decode("utf-8", "replace")
            if text:
                return [(260, {"severity": int(message.severity), "msg": text})]
        elif kind == "PARAM_VALUE":
            self.aircraft.params.update(message)
        return []

    def tick(self, now: float | None = None) -> list[tuple[int, Any]]:
        now = time.monotonic() if now is None else now
        reports: list[tuple[int, Any]] = []
        aircraft = self.aircraft
        if aircraft is None:
            return reports
        heartbeat = aircraft.fresh("HEARTBEAT", now)
        if heartbeat is not None and (
            self.last_flight_report is None
            or now - self.last_flight_report >= REPORT_SECONDS
        ):
            reports.append((258, aircraft.flight_data(now, heartbeat)))
            self.last_flight_report = now
            self._log_missing(aircraft, now)
        if self.battery_dirty and (
            self.last_battery_report is None
            or now - self.last_battery_report >= REPORT_SECONDS
        ):
            battery = aircraft.battery_data(now)
            self.battery_dirty = False
            if battery is not None:
                reports.append((257, battery))
                self.last_battery_report = now
        return reports

    def _log_missing(self, aircraft: Aircraft, now: float) -> None:
        if self.last_missing_log is not None and now - self.last_missing_log < 60:
            return
        missing = [
            name
            for name in (
                "GLOBAL_POSITION_INT",
                "ATTITUDE",
                "VFR_HUD",
                "GPS_RAW_INT",
                "HOME_POSITION",
                "VK_BMS_STATUS",
            )
            if aircraft.fresh(name, now) is None
        ]
        if missing:
            LOG.debug("missing/stale MAVLink telemetry: %s", ", ".join(missing))
        self.last_missing_log = now