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.
Times below are monotonic receive times, never the flight controller's boot clock.
"""

import logging
import math
import time
from typing import Any

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
STALE_SECONDS = 5.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


def _enum_label(enum: str, value: int, prefix: str) -> str:
    entry = VKFly.enums.get(enum, {}).get(value)
    return entry.name.removeprefix(prefix) if entry else f"UNKNOWN_{value}"


def _distance_m(lat1: float, lon1: float, lat2: float, lon2: float) -> float:
    r1, r2 = math.radians(lat1), math.radians(lat2)
    dr = r2 - r1
    dl = math.radians(lon2 - lon1)
    a = math.sin(dr / 2) ** 2 + math.cos(r1) * math.cos(r2) * math.sin(dl / 2) ** 2
    return 6371000.0 * 2 * math.asin(min(1.0, math.sqrt(a)))


def _put_finite(data: dict[str, Any], name: str, value: float) -> None:
    if math.isfinite(value):
        data[name] = value


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

    Neither the parser nor state 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.source: tuple[int, int] | None = None
        self.latest: dict[str, tuple[Any, float]] = {}
        self.batteries: dict[int, tuple[Any, float]] = {}
        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

    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 _handle(self, message: Any, now: float) -> list[tuple[int, Any]]:
        kind = message.get_type()
        src = (message.get_srcSystem(), message.get_srcComponent())
        if self.source is None:
            if (kind != "HEARTBEAT" or src[1] != _AUTOPILOT_COMPONENT
                    or message.autopilot == _INVALID_AUTOPILOT):
                return []
            self.source = src
            LOG.info("locked MAVLink flight controller sys=%s comp=%s", *src)
        if src != self.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.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.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})]
        return []

    def _fresh(self, kind: str, now: float) -> Any | None:
        entry = self.latest.get(kind)
        return entry[0] if entry and 0 <= now - entry[1] <= STALE_SECONDS else None

    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]] = []
        heartbeat = self._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, self.flight_data(now, heartbeat)))
            self.last_flight_report = now
            self._log_missing(now)
        if self.battery_dirty and (self.last_battery_report is None
                                   or now - self.last_battery_report >= REPORT_SECONDS):
            battery = self.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, 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 self._fresh(name, now) is None]
        if missing:
            LOG.debug("missing/stale MAVLink telemetry: %s", ", ".join(missing))
        self.last_missing_log = now

    def flight_data(self, now: float, heartbeat: Any) -> dict[str, Any]:
        data: dict[str, Any] = {
            "SystemID": self.config.system_id,
            "Armed": bool(heartbeat.base_mode & _ARMED_FLAG),
            "IsFlying": bool(heartbeat.base_mode & _ARMED_FLAG),
            "DroneAutopilot": int(heartbeat.autopilot),
            "UavType": int(heartbeat.type),
            "FlightMode": _enum_label("VKFLY_CUSTOM_MODE", heartbeat.custom_mode,
                                      "VKFLY_CUSTOM_MODE_"),
        }
        attitude = self._fresh("ATTITUDE", now)
        if attitude:
            _put_finite(data, "Roll", math.degrees(attitude.roll))
            _put_finite(data, "Pitch", math.degrees(attitude.pitch))
            _put_finite(data, "Yaw", math.degrees(attitude.yaw) % 360)
        position = self._fresh("GLOBAL_POSITION_INT", now)
        if position:
            if abs(position.lat) <= 900000000 and abs(position.lon) <= 1800000000:
                data.update(Latitude=position.lat / 1e7, Longitude=position.lon / 1e7)
            data.update(Altitude=position.relative_alt / 1000,
                        VelocityX=position.vx / 100, VelocityY=position.vy / 100,
                        VelocityZ=position.vz / 100)
        hud = self._fresh("VFR_HUD", now)
        if hud:
            for key, value in (("AirSpeed", hud.airspeed), ("GroundSpeed", hud.groundspeed),
                               ("ClimbRate", hud.climb)):
                _put_finite(data, key, value)
        home = self._fresh("HOME_POSITION", now)
        if home and abs(home.latitude) <= 900000000 and abs(home.longitude) <= 1800000000:
            data.update(HomeLatitude=home.latitude / 1e7, HomeLongitude=home.longitude / 1e7,
                        HomeAltitude=home.altitude / 1000, IsHomeLocationSet=True)
            if position and "Latitude" in data:
                distance = _distance_m(data["Latitude"], data["Longitude"],
                                       data["HomeLatitude"], data["HomeLongitude"])
                data["DistanceToHome"] = distance
                data["SpaceDistToHome"] = math.hypot(
                    distance, position.alt / 1000 - data["HomeAltitude"])
        gps = self._fresh("GPS_RAW_INT", now)
        if gps:
            info: dict[str, Any] = {
                "Type": 15,
                "FixType": _enum_label("GPS_FIX_TYPE", gps.fix_type, "GPS_FIX_TYPE_"),
            }
            if gps.fix_type >= 2:
                info["AbsoluteAlt"] = gps.alt / 1000
            if gps.eph != 65535:
                info["LevelDivisor"] = gps.eph / 100
            if gps.epv != 65535:
                info["VerticalDivisor"] = gps.epv / 100
            if gps.satellites_visible != 255:
                info["SatelliteCount"] = gps.satellites_visible
            data["Gps"] = info
        return data

    def battery_data(self, now: float) -> dict[str, Any] | None:
        packs = {id_: item[0] for id_, item in self.batteries.items()
                 if 0 <= now - item[1] <= STALE_SECONDS}
        if not packs:
            return None
        data: dict[str, Any] = {"SystemID": self.config.system_id}
        for id_, pack in packs.items():
            side = "Left" if id_ == 0 else "Right"
            if pack.voltage:
                data["Battery_Voltage" + side] = pack.voltage / 1000
            data["Battery_Current" + side] = pack.current / 100
            if 0 <= pack.cap_percent <= 100:
                data["Battery_Remaining" + side] = pack.cap_percent
        if len(packs) == 1:
            pack = next(iter(packs.values()))
            if pack.voltage:
                data["Battery_Voltage"] = pack.voltage / 1000
            data["Battery_Current"] = pack.current / 100
            if 0 <= pack.cap_percent <= 100:
                data["Battery_Remaining"] = pack.cap_percent
            data["Battery_Temperature"] = pack.temperature
            data["cycleIndex"] = pack.cyc_cnt
            data["flags"] = pack.err_code
        return data