"""MQTT business commands translated to VKFly MAVLink messages.""" from __future__ import annotations import logging import math from typing import TYPE_CHECKING, Any, Callable from .dialects.v20 import VKFly if TYPE_CHECKING: from .bridge import BaseMessage, Bridge LOG = logging.getLogger(__name__) GCS_SYSTEM_ID = 255 GCS_COMPONENT_ID = VKFly.MAV_COMP_ID_MISSIONPLANNER def _plain(value: Any) -> bool: return type(value) in (int, float) and math.isfinite(float(value)) class FlightCommandHandlers: """Built-in handlers for the platform commands supported by VKFly.""" PARAMS: dict[str, tuple[str, int, Callable[[Any], float]]] = { "battery_low_threshold": ("VCAP1_LOW_VAL", VKFly.MAV_PARAM_TYPE_UINT8, float), "battery_critical_threshold": ("VCAP2_LOW_VAL", VKFly.MAV_PARAM_TYPE_UINT8, float), "height_limit": ("ALT_LIM_UP1", VKFly.MAV_PARAM_TYPE_UINT16, float), "distance_limit": ("MAX_HOR_DIST", VKFly.MAV_PARAM_TYPE_UINT32, float), "return_height": ("RTL_ALT_M", VKFly.MAV_PARAM_TYPE_REAL32, float), "return_speed": ("MC_XY_CRUISE", VKFly.MAV_PARAM_TYPE_REAL32, float), "obstacle_avoidance_enable": ( "OBAVOID_ACT", VKFly.MAV_PARAM_TYPE_UINT8, lambda value: float(1 if value else 0) ), "obstacle_avoidance_distance": ( "OBAVOID_DIST", VKFly.MAV_PARAM_TYPE_REAL32, float ), "obstacle_avoidance_switch": ( "OBAVOID_ACT", VKFly.MAV_PARAM_TYPE_UINT8, lambda value: float(1 if value else 0) ), "return_type": ( "RTL_EXEC_MODE", VKFly.MAV_PARAM_TYPE_UINT8, lambda value: float(_enum_value(value, { "normal": 0, "straight": 0, "direct": 0, "mission": 1, "missionsequence": 1, "sequence": 1, "missionreverse": 2, "reverse": 2, })), ), "precision_land_mode": ( "RTL_PLD_EN", VKFly.MAV_PARAM_TYPE_UINT32, lambda value: float(_enum_value(value, { "disabled": 0, "off": 0, "none": 0, "apriltag": 1, "qrcode": 1, "enabled": 1, "on": 1, })), ), } TODO_PARAMS = { # No third capacity threshold exists in the current VKFly parameter table. "battery_emergency_threshold", "geofence_action", "visual_sensor_enable", "april_tag_id", "data_link_loss_action", "rc_loss_action", "parachute_safety_enable", "four_g_lost_action", # NAV_DFLT_SATID selects a navigation source; it is not an RTK switch. "rtk_switch", } FAILSAFE_ACTIONS = { "none": 0, "noaction": 0, "hover": 1, "loiter": 1, "return": 2, "returnhome": 2, "rtl": 2, "rally": 3, "land": 4, "landing": 4, "disarm": 5, "lock": 5, "terminate": 5, } def __init__(self, bridge: Bridge): self.bridge = bridge self.encoder = VKFly.MAVLink( None, srcSystem=GCS_SYSTEM_ID, srcComponent=GCS_COMPONENT_ID ) def install(self) -> None: self.bridge.register_handler(281, self.get_params) self.bridge.register_handler(282, self.set_param) self.bridge.register_handler(2050, self.arm) self.bridge.register_handler(2054, self.takeoff) self.bridge.register_handler(2055, self.land) self.bridge.register_handler(2070, self.reposition) self.bridge.register_handler(2073, self.emergency_stop_with_parachute) self.bridge.register_handler(2074, self.emergency_stop_motors) def _target(self) -> tuple[int, int]: source = self.bridge.mavlink_target if source is None: raise ValueError("flight controller target is unknown; wait for HEARTBEAT") return source def _send(self, message: Any) -> None: self.bridge.write_mavlink(message.pack(self.encoder)) def get_params(self, command: BaseMessage, bridge: Bridge) -> None: if command.data is not None: raise ValueError("DataType 281 Data must be null") system, component = self._target() self.request_params(system, component) def request_params(self, system: int, component: int) -> None: """Send PARAM_REQUEST_LIST to download the flight controller's table.""" self._send(self.encoder.param_request_list_encode(system, component)) def set_param(self, command: BaseMessage, bridge: Bridge) -> None: data = command.data if not isinstance(data, dict): raise ValueError("DataType 282 Data must be an object") name = data.get("name") platform_type = data.get("type") if not isinstance(name, str) or not name: raise ValueError("parameter name must be a nonempty string") if type(platform_type) is not int or platform_type not in (1, 2): raise ValueError("parameter type must be 1 (number) or 2 (string)") if name in self.TODO_PARAMS: LOG.warning("TODO: platform parameter %s has no confirmed VKFly mapping", name) return if name == "low_battery_action": self._set_low_battery_action(data.get("value")) return mapping = self.PARAMS.get(name) if mapping is None: LOG.warning("TODO: unsupported platform parameter mapping: %s", name) return param_id, param_type, converter = mapping value = data.get("value") if platform_type == 1 and not _plain(value): raise ValueError(f"{name} requires a finite numeric value") try: converted = converter(value) except (TypeError, ValueError, KeyError) as exc: raise ValueError(f"invalid value for {name}: {value!r}") from exc self._send_param(param_id, converted, param_type) def _set_low_battery_action(self, value: Any) -> None: action = _enum_value(value, self.FAILSAFE_ACTIONS) current = self.bridge.mavlink_param("FS_CONF_A") if current is None: raise ValueError( "FS_CONF_A is unknown; send DataType 281 and wait for parameter download first" ) # VKFly: bits 0..3 and 4..7 are level-1/level-2 battery protection. updated = (int(current) & ~0xFF) | action | (action << 4) self._send_param("FS_CONF_A", float(updated), VKFly.MAV_PARAM_TYPE_UINT32) def _send_param(self, param_id: str, value: float, param_type: int) -> None: encoded = param_id.encode("ascii") if len(encoded) > 16: raise ValueError(f"MAVLink param_id is longer than 16 bytes: {param_id}") system, component = self._target() self._send(self.encoder.param_set_encode( system, component, encoded, float(value), param_type )) def arm(self, command: BaseMessage, bridge: Bridge) -> None: if type(command.data) is not bool: raise ValueError("DataType 2050 Data must be a boolean") if command.data: # TODO: platform document labels true as "takeover"; its intended # VKFly behavior must be confirmed before sending a flight command. LOG.warning("TODO: DataType 2050 Data=true takeover behavior is not implemented") return system, component = self._target() self._send(self.encoder.command_long_encode( system, component, VKFly.MAV_CMD_COMPONENT_ARM_DISARM, 0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, )) def emergency_stop_with_parachute(self, command: BaseMessage, bridge: Bridge) -> None: """DataType 2073: high-altitude emergency motor stop, then parachute release.""" if command.data is not None: raise ValueError("DataType 2073 Data must be null") system, component = self._target() # VKFly MAV_CMD_DO_PARACHUTE with PARACHUTE_RELEASE means # "Release parachute and kill motors", i.e. stop propellers and deploy. self._send(self.encoder.command_long_encode( system, component, VKFly.MAV_CMD_DO_PARACHUTE, 0, float(VKFly.PARACHUTE_RELEASE), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, )) def takeoff(self, command: BaseMessage, bridge: Bridge) -> None: if command.data is not None: raise ValueError("DataType 2054 Data must be null") self._send_command_long(VKFly.MAV_CMD_NAV_TAKEOFF, param7=math.nan) def land(self, command: BaseMessage, bridge: Bridge) -> None: if command.data is not None: raise ValueError("DataType 2055 Data must be null") self._send_command_long(VKFly.MAV_CMD_NAV_LAND) def emergency_stop_motors(self, command: BaseMessage, bridge: Bridge) -> None: """DataType 2074: ground emergency motor stop without parachute.""" if command.data is not None: raise ValueError("DataType 2074 Data must be null") system, component = self._target() # Forced disarm (MAV_CMD_COMPONENT_ARM_DISARM param2=21196) kills the # motors without triggering the parachute that DataType 2073 uses. self._send(self.encoder.command_long_encode( system, component, VKFly.MAV_CMD_COMPONENT_ARM_DISARM, 0, 0.0, 21196.0, 0.0, 0.0, 0.0, 0.0, 0.0, )) def _send_command_long(self, mav_command: int, *, param7: float = math.nan) -> None: system, component = self._target() nan = math.nan self._send(self.encoder.command_long_encode( system, component, mav_command, 0, nan, nan, nan, nan, nan, nan, param7, )) def reposition(self, command: BaseMessage, bridge: Bridge) -> None: data = command.data if not isinstance(data, dict): raise ValueError("DataType 2070 Data must be an object") required = ("latitude", "longitude", "altitude", "speed", "heading") if any(not _plain(data.get(key)) for key in required): raise ValueError("reposition requires finite latitude/longitude/altitude/speed/heading") latitude = float(data["latitude"]) longitude = float(data["longitude"]) altitude = float(data["altitude"]) speed = float(data["speed"]) heading = float(data["heading"]) if not -90 <= latitude <= 90 or not -180 <= longitude <= 180: raise ValueError("invalid reposition latitude or longitude") if speed < 0 or not 0 <= heading <= 360: raise ValueError("reposition speed/heading is out of range") switch_mode = data.get("SwitchModel", True) if type(switch_mode) is not bool: raise ValueError("SwitchModel must be a boolean when present") system, component = self._target() flags = VKFly.MAV_DO_REPOSITION_FLAGS_CHANGE_MODE if switch_mode else 0 self._send(self.encoder.command_int_encode( system, component, VKFly.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT, VKFly.MAV_CMD_DO_REPOSITION, 0, 0, speed, float(flags), math.nan, heading, round(latitude * 1e7), round(longitude * 1e7), altitude, )) def _enum_value(value: Any, names: dict[str, int]) -> int: if type(value) is int: if value in names.values(): return value raise ValueError(f"unsupported enum value: {value}") if not isinstance(value, str): raise ValueError(f"enum value must be a string or integer: {value!r}") key = "".join(character for character in value.strip().lower() if character.isalnum()) if key not in names: raise ValueError(f"unsupported enum value: {value}") return names[key]