motian_mavbridge/motian_bridge/commands.py

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