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