Add platform command forwarding callbacks

README.md

@@ -32,6 +32,148 @@ export MOTIAN_MQTT_PASSWORD='由部署环境提供的密码'
 
 扩展飞控业务时，通过 `Bridge.register_handler(data_type, handler)` 注册非 526 命令回调。回调收到 `BaseMessage` 和桥实例，可调用 `bridge.publish_report(data_type, data)` 向 `/RECEIVE/<SystemID>` 上报。透传上行通过同一方法的 `passthrough=True` 发布到 `/PUBLISH/<SystemID>`。回调应自行处理飞控交互和业务 ACK，不要把未实现的命令视为成功。
 
+## 已实现的平台控制指令
+
+Bridge 在收到飞控 HEARTBEAT 并锁定目标 system/component 后，支持把以下 `/OBTAIN/<SystemID>` 指令转换为 VKFly MAVLink：
+
+| DataType | 平台指令 | MAVLink |
+| ---: | --- | --- |
+| 281 | 获取飞机底层参数 | `PARAM_REQUEST_LIST` |
+| 282 | 配置飞机底层参数 | `PARAM_SET`，按下面的参数映射发送 |
+| 2050 | 无人机解锁 | 当前按平台文档实现：`Data=false` 发送解锁；`Data=true` 暂留 TODO，不写串口 |
+| 2054 | 无人机起飞 | `COMMAND_LONG / MAV_CMD_NAV_TAKEOFF`，高度 `NaN` 表示使用飞控默认值 |
+| 2055 | 无人机降落 | `COMMAND_LONG / MAV_CMD_NAV_LAND` |
+| 2070 | 指点飞行 | `COMMAND_INT / MAV_CMD_DO_REPOSITION`，相对高度坐标系 |
+
+#282 已确认的参数映射：
+
+| 平台参数名 | 飞控 param_id |
+| --- | --- |
+| `battery_low_threshold` | `VCAP1_LOW_VAL` |
+| `battery_critical_threshold` | `VCAP2_LOW_VAL` |
+| `height_limit` | `ALT_LIM_UP1` |
+| `distance_limit` | `MAX_HOR_DIST` |
+| `return_height` | `RTL_ALT_M` |
+| `return_speed` | `MC_XY_CRUISE` |
+| `obstacle_avoidance_enable` | `OBAVOID_ACT`，开=悬停避障、关=关闭 |
+| `obstacle_avoidance_switch` | `OBAVOID_ACT`，与上项相同 |
+| `obstacle_avoidance_distance` | `OBAVOID_DIST` |
+| `return_type` | `RTL_EXEC_MODE`，直线/正序/逆序分别为 0/1/2 |
+| `precision_land_mode` | `RTL_PLD_EN`，关闭/二维码引导分别为 0/1 |
+| `low_battery_action` | 修改 `FS_CONF_A` 的低两级电量保护位；必须先执行 #281 并收到当前 `FS_CONF_A`，以保留其他保护位 |
+
+以下参数映射尚未由飞控协议明确，代码仅记录 TODO 并拒绝写串口：`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`、`rtk_switch`。未知参数同样不会直接作为 param_id 发送。
+
+这些业务指令不会在尚未收到飞控 HEARTBEAT 时发送，以避免使用错误的目标 system/component。当前只负责下发帧；COMMAND_ACK、PARAM_VALUE 到平台业务应答的关联与超时重试尚未实现。
+
+## 没有远程平台时的串口实测
+
+可以先在本机启动一个临时 MQTT Broker，把它当作平台。桥接程序不需要远程平台才能解析串口和产生状态报文。
+
+### 1. 确认飞控串口
+
+插入飞控后查看设备名：
+
+```bash
+ls -l /dev/serial/by-id/
+# 或
+dmesg | tail -30
+```
+
+优先使用 `/dev/serial/by-id/...`，不要直接依赖会变化的 `/dev/ttyUSB0`。确认串口波特率与飞控配置一致。不要同时让其他地面站占用同一个串口。
+
+### 2. 安装并启动本地 Broker
+
+项目提供了基于 Python aMQTT 的本地测试脚本。aMQTT 是开发/测试依赖，不属于桥接服务的生产依赖：
+
+```bash
+.venv/bin/python -m pip install "amqtt>=0.12,<0.13"
+.venv/bin/python tools/local_mqtt_broker.py
+```
+
+脚本默认监听 `127.0.0.1:1883`，无认证、无 TLS，仅供本机测试。不要把它暴露到公网。如果 1883 已占用，可以执行：
+
+```bash
+.venv/bin/python tools/local_mqtt_broker.py --port 1884
+```
+
+并同步修改桥接配置中的 `mqtt_port`。也可以使用 Mosquitto 或其他兼容 MQTT 3.1.1 的 Broker。
+
+### 3. 准备本地测试配置
+
+复制示例配置后，至少修改以下字段：
+
+```bash
+cp config.example.json config.local.json
+```
+
+```json
+{
+  "system_id": "UAV000001",
+  "uav_type": 0,
+  "serial_port": "/dev/serial/by-id/你的飞控串口",
+  "serial_baudrate": 57600,
+  "mqtt_host": "127.0.0.1",
+  "mqtt_port": 1883,
+  "mqtt_client_id": "local-motian-test",
+  "mqtt_username": null,
+  "mqtt_password_env": null,
+  "mqtt_tls": false,
+  "publish_raw_mavlink": true
+}
+```
+
+配置文件中未列出的选项会使用默认值。`uav_type` 在没有平台分配值时可以先使用 `0` 做本地联调；正式接入平台时必须换成平台分配值。
+
+### 4. 启动桥接服务
+
+```bash
+.venv/bin/motian-mavlink-bridge --config config.local.json --debug
+```
+
+看到 `serial connected` 后，保持该终端运行。程序会被动解析串口上已有的 MAVLink 消息：它不会主动向飞控请求消息流。如果日志出现 `missing/stale MAVLink telemetry`，说明飞控没有在这个串口广播对应消息，需在飞控端打开相应 MAVLink stream 或换用正确的串口。
+
+### 5. 另开终端观察 MQTT 报文
+
+观察结构化状态：
+
+```bash
+mosquitto_sub -h 127.0.0.1 -p 1883 \
+  -t '/RECEIVE/UAV000001' -v
+```
+
+观察原始 MAVLink HEX 透传：
+
+```bash
+mosquitto_sub -h 127.0.0.1 -p 1883 \
+  -t '/PUBLISH/UAV000001' -v
+```
+
+预期在 `/RECEIVE/UAV000001` 看到：
+
+- `DataType: 258`：每秒一条左右；至少收到 HEARTBEAT 后就可能出现，位置、姿态等字段取决于飞控是否发送对应消息；
+- `DataType: 257`：只有收到厂商 `VK_BMS_STATUS` 才会出现；
+- `DataType: 260`：飞控发送 `STATUSTEXT` 时出现；
+- `DataType: 274`：首次收到 HEARTBEAT 以及解锁状态改变时出现，`true` 表示解锁、`false` 表示上锁；
+- `/PUBLISH/...` 中的 `DataType: 526`：原始串口读取块的 HEX 内容。
+
+如果本机没有 `mosquitto_sub`，可以使用 aMQTT 自带的 `amqtt_sub`：
+
+```bash
+.venv/bin/amqtt_sub --url mqtt://127.0.0.1:1883 \
+  -t '/RECEIVE/UAV000001'
+```
+
+也可以使用任意 MQTT 客户端订阅这两个主题。桥接程序发布 QoS 0 且不保留消息，因此订阅端要先启动再启动桥接程序，避免错过实时数据。
+
+### 6. 建议的安全测试顺序
+
+1. 先只接收观察，确认 HEARTBEAT、位置和姿态数据正常；
+2. 确认 `DataType=258` 的字段单位和数值符合预期；
+3. 再测试解锁状态变化，确认 #274；
+4. 确认无误后才测试下行 #526。下行会直接写飞控串口，严禁在桨叶安装、电机通电或飞行状态下盲目发送控制帧；
+5. 测试结束后停止桥接程序和临时 Broker。
+
 ## 本地检查
 
 ```bash

doc/.gitignore

@@ -0,0 +1,2 @@
+qa.md
+

doc/实施方案.md

@@ -25,3 +25,10 @@
 - `/RECEIVE/<SystemID>` 增加 #258 飞行状态（1 秒合并）、#257 厂商 BMS 电池（有更新时至多每秒一次）、#260 STATUSTEXT 提示、#274 解锁/上锁布尔事件。只上报有效的已知字段；心跳或其他来源超过 5 秒即停止使用旧数据。
 - 保持下行 #526 不变；原始串口字节上行 #526 保留，增加 `publish_raw_mavlink` 配置开关，默认为开启。
 - 使用生成的真实 MAVLink 帧验证分块、CRC、来源、单位、超时及双电池；平台对部分字段的接受情况仍需实际联调。#274 的“飞行”在此阶段按解锁解释，不代表物理离地。
+
+## 第三阶段：平台飞控命令回调
+
+- 实现 #281 参数表读取、#282 已确认参数映射、#2054 起飞、#2055 降落和 #2070 指点飞行，目标 system/component 来自飞控 HEARTBEAT。
+- #2050 按平台文档约定仅在 `Data=false` 时发送解锁；`Data=true` 的“接管”语义保留 TODO，不向串口发送。
+- 第三级电量阈值和其他无法从 VKFly 参数表确认的配置保留 TODO 并拒绝发送；低电量动作通过先读取再按位更新 `FS_CONF_A`。
+- 当前阶段只发送 MAVLink 帧，尚未实现 COMMAND_ACK/PARAM_VALUE 到平台 #270/#303 的完整事务关联、超时和重试。

motian_bridge/bridge.py

@@ -90,6 +90,9 @@ class Bridge:
         self._serial: Any = None
         self._thread: threading.Thread | None = None
         self._telemetry = Telemetry(config)
+        from .commands import FlightCommandHandlers
+        self._flight_commands = FlightCommandHandlers(self)
+        self._flight_commands.install()
         self.mqtt.on_connect = self._on_connect
         self.mqtt.on_disconnect = self._on_disconnect
         self.mqtt.on_message = self._on_message
@@ -100,6 +103,19 @@ class Bridge:
             raise ValueError("handler requires a non-526 integer DataType and a callable")
         self._handlers[data_type] = handler
 
+    @property
+    def mavlink_target(self) -> tuple[int, int] | None:
+        """Flight-controller system/component selected from its HEARTBEAT."""
+        return self._telemetry.source
+
+    def mavlink_param(self, param_id: str) -> float | None:
+        """Return the latest parameter value learned from PARAM_VALUE."""
+        return self._telemetry.params.get(param_id)
+
+    def write_mavlink(self, frame: bytes) -> None:
+        """Write a generated MAVLink frame to the flight-controller serial port."""
+        self._write_serial(frame)
+
     def publish_report(self, data_type: int, data: Any, *, passthrough: bool = False) -> bool:
         """Publish future reports to /RECEIVE, or raw data to /PUBLISH."""
         if not self._mqtt_connected.is_set():
@@ -216,6 +232,8 @@ class Bridge:
             try:
                 data = current.read(self.config.max_chunk_bytes)
             except Exception as exc:
+                if self._stop.is_set():
+                    break
                 LOG.warning("serial read failed: %s", exc)
                 with self._serial_lock:
                     if self._serial is current:

motian_bridge/commands.py

@@ -0,0 +1,235 @@
+"""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)
+
+    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._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 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 _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]

motian_bridge/telemetry.py

@@ -55,6 +55,7 @@ class Telemetry:
         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.battery_dirty = False
         self.last_flight_report: float | None = None
         self.last_battery_report: float | None = None
@@ -80,8 +81,11 @@ class Telemetry:
         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):
+            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)
@@ -107,11 +111,16 @@ class Telemetry:
             # 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]]
+            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":
+            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)
         return []
 
     def _fresh(self, kind: str, now: float) -> Any | None:
@@ -122,13 +131,17 @@ class Telemetry:
         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):
+        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):
+        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:
@@ -139,9 +152,18 @@ class Telemetry:
     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]
+        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
@@ -153,8 +175,9 @@ class Telemetry:
             "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_"),
+            "FlightMode": _enum_label(
+                "VKFLY_CUSTOM_MODE", heartbeat.custom_mode, "VKFLY_CUSTOM_MODE_"
+            ),
         }
         attitude = self._fresh("ATTITUDE", now)
         if attitude:
@@ -165,24 +188,43 @@ class Telemetry:
         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)
+            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)):
+            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 (
+            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"])
+                distance = _distance_m(
+                    data["Latitude"],
+                    data["Longitude"],
+                    data["HomeLatitude"],
+                    data["HomeLongitude"],
+                )
                 data["DistanceToHome"] = distance
                 data["SpaceDistToHome"] = math.hypot(
-                    distance, position.alt / 1000 - data["HomeAltitude"])
+                    distance, position.alt / 1000 - data["HomeAltitude"]
+                )
         gps = self._fresh("GPS_RAW_INT", now)
         if gps:
             info: dict[str, Any] = {
@@ -201,8 +243,11 @@ class Telemetry:
         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}
+        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}

tests/test_commands.py

@@ -0,0 +1,171 @@
+import math
+import unittest
+
+from motian_bridge import Bridge
+from motian_bridge.dialects.v20 import VKFly
+from test_bridge import FakeMqtt, FakeSerial, command, config
+
+
+def heartbeat(system=1, component=1):
+    mav = VKFly.MAVLink(None, srcSystem=system, srcComponent=component)
+    return mav.heartbeat_encode(
+        VKFly.MAV_TYPE_QUADROTOR,
+        VKFly.MAV_AUTOPILOT_GENERIC,
+        0,
+        VKFly.VKFLY_CUSTOM_MODE_POSHOLD,
+        VKFly.MAV_STATE_ACTIVE,
+    ).pack(mav)
+
+
+def decode(frame):
+    parser = VKFly.MAVLink(None)
+    result = None
+    for byte in frame:
+        message = parser.parse_char(bytes((byte,)))
+        if message is not None:
+            result = message
+    return result
+
+
+class CommandHandlerTests(unittest.TestCase):
+    def setUp(self):
+        self.cfg = config()
+        self.mqtt = FakeMqtt()
+        self.serial = FakeSerial()
+        self.bridge = Bridge(self.cfg, self.mqtt, lambda *args, **kwargs: self.serial)
+        self.bridge._serial = self.serial
+        self.bridge._on_connect(self.mqtt, None, None, 0, None)
+        self.bridge._telemetry.feed(heartbeat(42, 1), now=1)
+
+    def deliver(self, data_type, data):
+        self.bridge._on_message(self.mqtt, None, type("Message", (), {
+            "topic": self.cfg.obtain_topic,
+            "retain": False,
+            "payload": command(data_type, data),
+        })())
+
+    def last(self):
+        return decode(self.serial.written[-1])
+
+    def test_get_all_params(self):
+        self.deliver(281, None)
+        message = self.last()
+        self.assertEqual(message.get_type(), "PARAM_REQUEST_LIST")
+        self.assertEqual((message.target_system, message.target_component), (42, 1))
+
+    def test_direct_parameter_mappings(self):
+        cases = [
+            ("battery_low_threshold", 30, "VCAP1_LOW_VAL", VKFly.MAV_PARAM_TYPE_UINT8, 30),
+            ("battery_critical_threshold", 20, "VCAP2_LOW_VAL", VKFly.MAV_PARAM_TYPE_UINT8, 20),
+            ("height_limit", 120, "ALT_LIM_UP1", VKFly.MAV_PARAM_TYPE_UINT16, 120),
+            ("distance_limit", 5000, "MAX_HOR_DIST", VKFly.MAV_PARAM_TYPE_UINT32, 5000),
+            ("return_height", 80, "RTL_ALT_M", VKFly.MAV_PARAM_TYPE_REAL32, 80),
+            ("return_speed", 8, "MC_XY_CRUISE", VKFly.MAV_PARAM_TYPE_REAL32, 8),
+            ("obstacle_avoidance_distance", 5, "OBAVOID_DIST",
+             VKFly.MAV_PARAM_TYPE_REAL32, 5),
+            ("obstacle_avoidance_enable", 1, "OBAVOID_ACT",
+             VKFly.MAV_PARAM_TYPE_UINT8, 1),
+            ("obstacle_avoidance_switch", 0, "OBAVOID_ACT",
+             VKFly.MAV_PARAM_TYPE_UINT8, 0),
+        ]
+        for name, value, expected_id, expected_type, expected_value in cases:
+            with self.subTest(name=name):
+                self.serial.written.clear()
+                self.deliver(282, {"name": name, "type": 1, "value": value})
+                message = self.last()
+                self.assertEqual(message.get_type(), "PARAM_SET")
+                self.assertEqual(message.param_id, expected_id)
+                self.assertEqual(message.param_type, expected_type)
+                self.assertEqual(message.param_value, expected_value)
+
+    def test_string_parameter_mappings(self):
+        self.deliver(282, {"name": "return_type", "type": 2, "value": "MissionReverse"})
+        self.assertEqual(self.last().param_id, "RTL_EXEC_MODE")
+        self.assertEqual(self.last().param_value, 2)
+        self.deliver(282, {"name": "precision_land_mode", "type": 2,
+                           "value": "AprilTag"})
+        self.assertEqual(self.last().param_id, "RTL_PLD_EN")
+        self.assertEqual(self.last().param_value, 1)
+
+    def test_todo_and_third_threshold_do_not_write(self):
+        for name in ("battery_emergency_threshold", "geofence_action", "rtk_switch",
+                     "visual_sensor_enable", "unknown"):
+            with self.subTest(name=name):
+                before = len(self.serial.written)
+                self.deliver(282, {"name": name, "type": 1, "value": 1})
+                self.assertEqual(len(self.serial.written), before)
+
+    def test_low_battery_action_preserves_other_bits(self):
+        self.bridge._telemetry.params["FS_CONF_A"] = float(0xAB00)
+        self.deliver(282, {"name": "low_battery_action", "type": 2,
+                           "value": "ReturnHome"})
+        message = self.last()
+        self.assertEqual(message.param_id, "FS_CONF_A")
+        self.assertEqual(message.param_type, VKFly.MAV_PARAM_TYPE_UINT32)
+        self.assertEqual(message.param_value, 0xAB22)
+
+    def test_low_battery_action_requires_parameter_download(self):
+        before = len(self.serial.written)
+        self.deliver(282, {"name": "low_battery_action", "type": 2,
+                           "value": "ReturnHome"})
+        self.assertEqual(len(self.serial.written), before)
+
+    def test_param_value_is_cached(self):
+        mav = VKFly.MAVLink(None, srcSystem=42, srcComponent=1)
+        frame = mav.param_value_encode(
+            b"FS_CONF_A", 4660, VKFly.MAV_PARAM_TYPE_UINT32, 1, 0
+        ).pack(mav)
+        self.bridge._telemetry.feed(frame, now=2)
+        self.assertEqual(self.bridge.mavlink_param("FS_CONF_A"), 4660)
+
+    def test_2050_false_arms_and_true_is_todo(self):
+        self.deliver(2050, False)
+        message = self.last()
+        self.assertEqual(message.get_type(), "COMMAND_LONG")
+        self.assertEqual(message.command, VKFly.MAV_CMD_COMPONENT_ARM_DISARM)
+        self.assertEqual(message.param1, 1)
+        before = len(self.serial.written)
+        self.deliver(2050, True)
+        self.assertEqual(len(self.serial.written), before)
+
+    def test_takeoff_and_land(self):
+        self.deliver(2054, None)
+        takeoff = self.last()
+        self.assertEqual(takeoff.command, VKFly.MAV_CMD_NAV_TAKEOFF)
+        self.assertTrue(math.isnan(takeoff.param7))
+        self.deliver(2055, None)
+        land = self.last()
+        self.assertEqual(land.command, VKFly.MAV_CMD_NAV_LAND)
+
+    def test_reposition_command_int(self):
+        self.deliver(2070, {
+            "latitude": 33.3180863,
+            "longitude": 120.0978796,
+            "altitude": 61.52,
+            "speed": 5.0,
+            "heading": 30,
+        })
+        message = self.last()
+        self.assertEqual(message.get_type(), "COMMAND_INT")
+        self.assertEqual(message.command, VKFly.MAV_CMD_DO_REPOSITION)
+        self.assertEqual(message.frame, VKFly.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT)
+        self.assertEqual(message.param2, VKFly.MAV_DO_REPOSITION_FLAGS_CHANGE_MODE)
+        self.assertEqual(message.x, 333180863)
+        self.assertEqual(message.y, 1200978796)
+        self.assertAlmostEqual(message.z, 61.52, places=4)
+
+    def test_commands_wait_for_heartbeat(self):
+        bridge = Bridge(self.cfg, self.mqtt, lambda *args, **kwargs: self.serial)
+        bridge._serial = self.serial
+        bridge._on_connect(self.mqtt, None, None, 0, None)
+        before = len(self.serial.written)
+        bridge._on_message(self.mqtt, None, type("Message", (), {
+            "topic": self.cfg.obtain_topic,
+            "retain": False,
+            "payload": command(2054, None),
+        })())
+        self.assertEqual(len(self.serial.written), before)
+
+
+if __name__ == "__main__":
+    unittest.main()

tools/local_mqtt_broker.py

@@ -0,0 +1,57 @@
+"""Start an anonymous MQTT broker for local bridge testing.
+
+This is intentionally a development-only helper. It binds to loopback and
+does not configure authentication or TLS.
+"""
+
+import argparse
+import asyncio
+import logging
+
+from amqtt.broker import Broker
+
+
+def build_config(host: str, port: int) -> dict:
+    return {
+        "listeners": {
+            "default": {
+                "type": "tcp",
+                "bind": f"{host}:{port}",
+            }
+        },
+        "auth": {
+            "allow-anonymous": True,
+        },
+        "topic-check": {
+            "enabled": False,
+        },
+    }
+
+
+async def run(host: str, port: int) -> None:
+    broker = Broker(build_config(host, port))
+    await broker.start()
+    logging.info("local MQTT broker listening on mqtt://%s:%s", host, port)
+    try:
+        await asyncio.Event().wait()
+    finally:
+        await broker.shutdown()
+
+
+def main() -> None:
+    parser = argparse.ArgumentParser(description="Local development MQTT broker")
+    parser.add_argument("--host", default="127.0.0.1")
+    parser.add_argument("--port", default=1883, type=int)
+    args = parser.parse_args()
+    logging.basicConfig(
+        level=logging.INFO,
+        format="%(asctime)s %(levelname)s %(name)s: %(message)s",
+    )
+    try:
+        asyncio.run(run(args.host, args.port))
+    except KeyboardInterrupt:
+        pass
+
+
+if __name__ == "__main__":
+    main()