|
|
@@ -1,14 +1,16 @@
|
|
|
"""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.
|
|
|
+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 math
|
|
|
import time
|
|
|
from typing import Any
|
|
|
|
|
|
+from .aircraft import Aircraft, ParamTable
|
|
|
from .config import Config
|
|
|
from .dialects.v20 import VKFly
|
|
|
|
|
|
@@ -17,51 +19,52 @@ 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.
|
|
|
+ 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.source: tuple[int, int] | None = None
|
|
|
- self.latest: dict[str, tuple[Any, float]] = {}
|
|
|
- self.batteries: dict[int, tuple[Any, float]] = {}
|
|
|
- self.params: dict[str, float] = {}
|
|
|
+ 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]] = []
|
|
|
@@ -77,25 +80,30 @@ class Telemetry:
|
|
|
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.source is None:
|
|
|
+ if self.aircraft 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:
|
|
|
+ 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.latest[kind] = (message, now)
|
|
|
+ self.aircraft.latest[kind] = (message, now)
|
|
|
if kind == "HEARTBEAT":
|
|
|
armed = bool(message.base_mode & _ARMED_FLAG)
|
|
|
if armed != self.last_armed:
|
|
|
@@ -103,7 +111,7 @@ class Telemetry:
|
|
|
# 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.aircraft.batteries[message.bat_id] = (message, now)
|
|
|
self.battery_dirty = True
|
|
|
elif kind == "STATUSTEXT":
|
|
|
# Generated Python text decoding assumes ASCII and irreversibly
|
|
|
@@ -117,39 +125,35 @@ class Telemetry:
|
|
|
if text:
|
|
|
return [(260, {"severity": int(message.severity), "msg": text})]
|
|
|
elif kind == "PARAM_VALUE":
|
|
|
- param_id = message.param_id
|
|
|
- if isinstance(param_id, bytes):
|
|
|
- param_id = param_id.decode("ascii", "replace")
|
|
|
- self.params[param_id.rstrip("\x00")] = float(message.param_value)
|
|
|
+ self.aircraft.params.update(message)
|
|
|
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)
|
|
|
+ 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, self.flight_data(now, heartbeat)))
|
|
|
+ reports.append((258, aircraft.flight_data(now, heartbeat)))
|
|
|
self.last_flight_report = now
|
|
|
- self._log_missing(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 = self.battery_data(now)
|
|
|
+ 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, now: float) -> None:
|
|
|
+ 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 = [
|
|
|
@@ -162,110 +166,8 @@ class Telemetry:
|
|
|
"HOME_POSITION",
|
|
|
"VK_BMS_STATUS",
|
|
|
)
|
|
|
- if self._fresh(name, now) is None
|
|
|
+ if aircraft.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
|