2.8.0 xbee and ws command

2026-08-18 15:56:16 +08:00
parent 1db3445e86
commit ebd219c88e
9 changed files with 1077 additions and 97 deletions
@@ -0,0 +1,155 @@
+import asyncio
+import websockets
+import json
+import sys
+
+WS_URL = "ws://127.0.0.1:8765"
+SYSTEM_ID = 2
+GOTO_POSITION = [24.1234567, 120.1234567]
+
+
+class WebSocketClient:
+    def __init__(self, ws_url=WS_URL, system_id=SYSTEM_ID):
+        self.ws_url = ws_url
+        if isinstance(system_id, bool) or not isinstance(system_id, int):
+            raise ValueError("system_id 必須為整數")
+        self.system_id = system_id
+        
+    def process_websocket_message(self, data):
+        """解析 server 的 Essential／Full UAV 狀態或命令 ACK。"""
+        if isinstance(data, str):
+            try:
+                data = json.loads(data)
+            except json.JSONDecodeError as e:
+                print(f"JSON 格式錯誤: {e}")
+                return
+
+        if not isinstance(data, dict):
+            print(f"未知訊息格式: {data}")
+            return
+
+        if all(key in data for key in ("result", "command", "message")):
+            print(
+                f"ACK | Command:{data.get('command', '-')} | "
+                f"Result:{data.get('result', '-')} | "
+                f"Message:{data.get('message', '-')}"
+            )
+            return
+
+        if "s" not in data:
+            print(f"未知 JSON 訊息: {data}")
+            return
+
+        position = data.get("p")
+        if isinstance(position, (list, tuple)) and len(position) >= 2:
+            position_text = f"{position[0]}, {position[1]}"
+        else:
+            position_text = "-"
+
+        ypr = data.get("ypr")
+        if isinstance(ypr, (list, tuple)) and len(ypr) >= 3:
+            yaw, pitch, roll = ypr[0], ypr[1], ypr[2]
+        else:
+            yaw, pitch, roll = data.get("y"), None, None
+
+        dop = data.get("d")
+        if isinstance(dop, (list, tuple)) and len(dop) >= 2:
+            dop_text = f"{dop[0]}/{dop[1]}"
+        else:
+            dop_text = "-"
+
+        armed = data.get("a")
+        if armed is None:
+            armed_text = "-"
+        else:
+            armed_text = "ARMED" if bool(armed) else "DISARMED"
+
+        def value_or_dash(value, suffix=""):
+            return f"{value}{suffix}" if value is not None else "-"
+
+        print(
+            f"UAV | ID:{data.get('s', '-')} | "
+            f"Mode:{value_or_dash(data.get('m'))} | "
+            f"Arm:{armed_text} | "
+            f"Battery:{value_or_dash(data.get('b'), '%')} | "
+            f"Position:{position_text} | "
+            f"Height:{value_or_dash(data.get('h'), 'm')} | "
+            f"Velocity:{value_or_dash(data.get('v'), 'm/s')} | "
+            f"Yaw:{value_or_dash(yaw, 'deg')} | "
+            f"Pitch:{value_or_dash(pitch, 'deg')} | "
+            f"Roll:{value_or_dash(roll, 'deg')} | "
+            f"GPS:{value_or_dash(data.get('g'))} | DOP(H/V):{dop_text}"
+        )
+
+    async def send_commands_loop(self, websocket):
+        """從 stdin 讀取按鍵 (輸入後按 Enter) 並發送對應的 JSON 指令"""
+        loop = asyncio.get_event_loop()
+        print("輸入 1 = START, 2 = STOP, 3 = GOTO (然後按 Enter)")
+        while True:
+            # readline 會包含換行
+            try:
+                line = await loop.run_in_executor(None, sys.stdin.readline)
+            except Exception:
+                return
+            if not line:
+                # EOF
+                return
+            cmd = line.strip()
+            if cmd == '1':
+                msg = {"s": self.system_id, "c": "START"}
+            elif cmd == '2':
+                msg = {"s": self.system_id, "c": "STOP"}
+            elif cmd == '3':
+                msg = {
+                    "s": self.system_id,
+                    "c": "GOTO",
+                    "p": list(GOTO_POSITION),
+                }
+            else:
+                print(f"未知指令: {cmd}")
+                continue
+
+            try:
+                await websocket.send(json.dumps(msg))
+                print(f"已發送: {msg}")
+            except Exception as e:
+                print(f"發送失敗: {e}")
+                return
+
+    async def ws_client_loop(self):
+        retry_count = 0
+        max_retries = 5
+        base_delay = 1.0
+        while retry_count < max_retries:
+            try:
+                async with websockets.connect(self.ws_url) as websocket:
+                    print(f"WebSocket connected: {self.ws_url}")
+                    retry_count = 0
+                    # 啟動後台任務來接收 stdin 指令並發送
+                    send_task = asyncio.create_task(self.send_commands_loop(websocket))
+                    try:
+                        async for message in websocket:
+                            # message 已經是 string
+                            self.process_websocket_message(message)
+                    finally:
+                        send_task.cancel()
+                        await asyncio.gather(send_task, return_exceptions=True)
+                            
+            except websockets.exceptions.ConnectionClosedError:
+                print("WebSocket connection closed")
+                break
+            except Exception as e:
+                retry_count += 1
+                delay = base_delay * (2 ** min(retry_count, 4))
+                print(f"WebSocket connection error: {e}, retrying in {delay}s (attempt {retry_count}/{max_retries})")
+                await asyncio.sleep(delay)
+        
+        print("WebSocket client stopped after maximum retries")
+
+    async def start(self):
+        """啟動 WebSocket 客戶端"""
+        await self.ws_client_loop()
+
+if __name__ == "__main__":
+    client = WebSocketClient()
+    asyncio.run(client.start())