From ebd219c88e978f8e794a3a3edff0130afcd848e7 Mon Sep 17 00:00:00 2001 From: ken910606 Date: Tue, 18 Aug 2026 15:56:16 +0800 Subject: [PATCH] 2.8.0 xbee and ws command --- src/GUI/communication.py | 223 +++++++++++++++++++++++++--- src/GUI/drone_panel.py | 18 +-- src/GUI/gui.py | 280 +++++++++++++++++++++++++++--------- src/GUI/mission_group.py | 45 +++++- src/GUI/overview_table.py | 6 +- src/GUI/websocket_client.py | 155 ++++++++++++++++++++ src/GUI/websocket_server.py | 179 +++++++++++++++++++++++ src/GUI/xbee_gcs.py | 127 ++++++++++++++++ src/GUI/xbee_uav.py | 141 ++++++++++++++++++ 9 files changed, 1077 insertions(+), 97 deletions(-) create mode 100644 src/GUI/websocket_client.py create mode 100644 src/GUI/websocket_server.py create mode 100644 src/GUI/xbee_gcs.py create mode 100644 src/GUI/xbee_uav.py diff --git a/src/GUI/communication.py b/src/GUI/communication.py index 83d65f3..1975141 100644 --- a/src/GUI/communication.py +++ b/src/GUI/communication.py @@ -29,6 +29,28 @@ def _log(level, message): print(f"[{level}] {message}") +def build_xbee_json_command(system_id, command, position=None): + """Build the shared newline-free XBee command used by Serial and WebSocket.""" + if isinstance(system_id, bool) or not isinstance(system_id, int): + raise ValueError("XBee system_id 必須為整數") + if not isinstance(command, str): + raise ValueError("XBee 指令必須為大寫字串") + if command not in {'START', 'STOP', 'GOTO'}: + raise ValueError(f"XBee 指令必須為大寫 START、STOP 或 GOTO: {command}") + + payload_data = { + 's': system_id, + 'c': command, + } + if command == 'GOTO': + if not isinstance(position, (list, tuple)) or len(position) < 2: + raise ValueError("GOTO 缺少有效的經緯度") + payload_data['p'] = [float(position[0]), float(position[1])] + + return json.dumps( + payload_data, ensure_ascii=False, separators=(',', ':')) + + # 確保 src 目錄在 Python 路徑中（用於 fc_network_apps 導入） _src_path = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) if _src_path not in sys.path: @@ -95,7 +117,9 @@ class JsonTelemetryProcessor: "position": {"lat": 24.0, "lon": 120.0}, "heading": 90 } - Serial JSON also accepts the compact UAV.py shape: + Serial and WebSocket JSON also accept compact UAV status shapes: + {"s": 1, "m": "GUIDED", "b": 85, "p": [24.0, 120.0], "y": 90.0} + or the full shape: {"s": 1, "m": "GUIDED", "a": 1, "b": 85, "h": 10.0, "v": 4.2, "p": [24.0, 120.0], "ypr": [90.0, 0.0, 0.0], "g": 3, "d": [0.8, 1.2]} @@ -118,6 +142,20 @@ class JsonTelemetryProcessor: if not isinstance(data, dict): return + # XBee command acknowledgement does not carry a UAV system id. + # Route it to the GUI log/status area instead of silently dropping it. + if all(key in data for key in ('result', 'command', 'message')): + self.signals.update_signal.emit( + 'xbee_ack', + self.connection_name, + { + 'result': data.get('result'), + 'command': data.get('command'), + 'message': data.get('message'), + } + ) + return + system_id = data.get('system_id', data.get('sysid', data.get('s'))) if system_id is None: return @@ -125,6 +163,13 @@ class JsonTelemetryProcessor: drone_id = f"s{self.socket_id}_{system_id}" self._emit_json_connection_type(drone_id) + # Compact XBee/UAV packets are complete snapshots. Emit every field + # group, using None for omitted values, so the GUI can show '-' + # instead of inventing zero-valued telemetry or retaining stale data. + if 's' in data and 'c' not in data: + self._process_compact_uav_telemetry(data, drone_id) + return + mode = data.get('mode', data.get('mode_name', data.get('m'))) state = {} if mode is not None: @@ -301,6 +346,82 @@ class JsonTelemetryProcessor: except Exception as e: print(f"{self.source_type} JSON telemetry processing error: {e}") + def _process_compact_uav_telemetry(self, data, drone_id): + """Convert one compact XBee UAV snapshot into existing GUI events.""" + snapshot = {'_snapshot': True} + + self.signals.update_signal.emit('state', drone_id, { + **snapshot, + 'mode': data.get('m'), + 'armed': bool(data.get('a')) if data.get('a') is not None else None, + }) + self.signals.update_signal.emit('battery', drone_id, { + **snapshot, + 'percentage': data.get('b'), + 'voltage': None, + }) + + position = data.get('p') + has_position = isinstance(position, (list, tuple)) and len(position) >= 2 + dop = data.get('d') + has_dop = isinstance(dop, (list, tuple)) + ypr = data.get('ypr') + has_ypr = isinstance(ypr, (list, tuple)) and len(ypr) >= 3 + has_yaw = data.get('y') is not None + # In the legacy compact shape, a lone 'h' is heading. In the newer + # y/ypr shapes it is height, matching the original ground-station parser. + height = data.get('h') if has_ypr or has_yaw else None + yaw = ypr[0] if has_ypr else data.get('y', data.get('h')) + pitch = ypr[1] if has_ypr else None + roll = ypr[2] if has_ypr else None + + self.signals.update_signal.emit('gps', drone_id, { + **snapshot, + 'lat': position[0] if has_position else None, + 'lon': position[1] if has_position else None, + 'alt': height, + 'fix_type': data.get('g'), + 'eph': dop[0] if has_dop and len(dop) >= 1 else None, + 'epv': dop[1] if has_dop and len(dop) >= 2 else None, + 'satellites_visible': None, + }) + + self.signals.update_signal.emit('altitude', drone_id, { + **snapshot, + 'altitude': height, + }) + self.signals.update_signal.emit('local_pose', drone_id, { + **snapshot, + 'x': None, + 'y': None, + 'z': height, + }) + + # Compact 'v' is scalar ground speed, not an XY velocity vector. + self.signals.update_signal.emit('velocity', drone_id, { + **snapshot, + 'vx': None, + 'vy': None, + 'vz': None, + }) + + self.signals.update_signal.emit('attitude', drone_id, { + **snapshot, + 'roll': roll, + 'pitch': pitch, + 'yaw': yaw, + 'rates': None, + }) + self.signals.update_signal.emit('hud', drone_id, { + **snapshot, + 'heading': yaw, + 'groundspeed': data.get('v'), + 'airspeed': None, + 'throttle': None, + 'alt': height, + 'climb': None, + }) + class UDPMavlinkReceiver(threading.Thread): """UDP MAVLink 接收器""" def __init__(self, ip, port, signals, connection_name, monitor=None): @@ -448,6 +569,7 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor): self.source_type = 'Serial' self.running = False self.serial_conn = None + self.serial_write_lock = Lock() self.mav_parser = mavutil.mavlink.MAVLink(None) self.mav_parser.robust_parsing = True self.json_buffer = [] @@ -490,11 +612,24 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor): finally: if self.serial_conn: try: - self.serial_conn.close() + with self.serial_write_lock: + self.serial_conn.close() except Exception: pass self._release_socket_id() + def send_json_command(self, system_id, command, position=None): + """Send one newline-delimited XBee JSON command on this serial link.""" + payload = build_xbee_json_command(system_id, command, position) + '\n' + with self.serial_write_lock: + if not self.running or self.serial_conn is None: + raise RuntimeError("Serial 連線尚未啟動") + if hasattr(self.serial_conn, 'is_open') and not self.serial_conn.is_open: + raise RuntimeError("Serial 連線已關閉") + self.serial_conn.write(payload.encode('utf-8')) + self.serial_conn.flush() + return payload.strip() + def _process_mavlink_byte(self, byte): try: msg = self.mav_parser.parse_char(byte) @@ -651,7 +786,8 @@ class SerialMavlinkReceiver(threading.Thread, JsonTelemetryProcessor): self.running = False if self.serial_conn: try: - self.serial_conn.close() + with self.serial_write_lock: + self.serial_conn.close() except Exception: pass self._release_socket_id() @@ -675,12 +811,19 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor): self.running = False self.max_retries = 5 self.base_delay = 1.0 + self.loop = None + self.websocket = None + self.websocket_lock = Lock() def run(self): """執行 WebSocket 接收循環""" self.running = True - asyncio.set_event_loop(asyncio.new_event_loop()) - asyncio.get_event_loop().run_until_complete(self.ws_client_loop()) + self.loop = asyncio.new_event_loop() + asyncio.set_event_loop(self.loop) + try: + self.loop.run_until_complete(self.ws_client_loop()) + finally: + self.loop.close() async def ws_client_loop(self): """WebSocket 連接的主循環""" @@ -691,22 +834,28 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor): while self.running and retry_count < self.max_retries: try: async with websockets.connect(self.url) as websocket: + with self.websocket_lock: + self.websocket = websocket print(f"WebSocket {self.connection_name} connected to {self.url}") retry_count = 0 # 重置重試計數 - - async for message in websocket: - if not self.running: - break - - try: - data = json.loads(message) - if isinstance(data, dict): - data['_connection_source'] = self.connection_name - self.process_websocket_message(data) - except json.JSONDecodeError as e: - print(f"WebSocket {self.connection_name} JSON decode error: {e}") - except Exception as e: - print(f"WebSocket {self.connection_name} message processing error: {e}") + try: + async for message in websocket: + if not self.running: + break + + try: + data = json.loads(message) + if isinstance(data, dict): + data['_connection_source'] = self.connection_name + self.process_websocket_message(data) + except json.JSONDecodeError as e: + print(f"WebSocket {self.connection_name} JSON decode error: {e}") + except Exception as e: + print(f"WebSocket {self.connection_name} message processing error: {e}") + finally: + with self.websocket_lock: + if self.websocket is websocket: + self.websocket = None except websockets.exceptions.ConnectionClosedError: print(f"WebSocket {self.connection_name} connection closed") @@ -729,6 +878,37 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor): print(f"WebSocket client {self.connection_name} stopped") self._release_socket_id() + + def send_json_command(self, system_id, command, position=None): + """Schedule an XBee JSON command on this receiver's WebSocket loop.""" + payload = build_xbee_json_command(system_id, command, position) + with self.websocket_lock: + loop = self.loop + websocket = self.websocket + + if not self.running or loop is None or not loop.is_running(): + raise RuntimeError("WebSocket 連線尚未啟動") + if websocket is None: + raise RuntimeError("WebSocket 尚未連線") + + async def _send_on_current_connection(): + with self.websocket_lock: + if self.websocket is not websocket: + raise RuntimeError("WebSocket 連線已變更") + await websocket.send(payload) + + future = asyncio.run_coroutine_threadsafe( + _send_on_current_connection(), loop) + + def _report_send_error(done_future): + try: + done_future.result() + except Exception as e: + print( + f"WebSocket {self.connection_name} command send error: {e}") + + future.add_done_callback(_report_send_error) + return payload def process_websocket_message(self, data): """處理 WebSocket 訊息""" @@ -737,6 +917,11 @@ class WebSocketMavlinkReceiver(threading.Thread, JsonTelemetryProcessor): def stop(self): """停止接收器""" self.running = False + with self.websocket_lock: + loop = self.loop + websocket = self.websocket + if loop is not None and loop.is_running() and websocket is not None: + asyncio.run_coroutine_threadsafe(websocket.close(), loop) self._release_socket_id() def _release_socket_id(self): diff --git a/src/GUI/drone_panel.py b/src/GUI/drone_panel.py index b7ac474..14fb8a7 100644 --- a/src/GUI/drone_panel.py +++ b/src/GUI/drone_panel.py @@ -222,11 +222,11 @@ class DronePanel(QWidget): status_title = QLabel("狀態：") _set_scaled_stylesheet(status_title, "color: #888; min-width: 50px;") - self.mode_label = QLabel("--") + self.mode_label = QLabel("-") self.mode_label.setObjectName(f"{self.drone_id}_mode") _set_scaled_stylesheet(self.mode_label, "color: #DDD;") - self.armed_label = QLabel("--") + self.armed_label = QLabel("-") self.armed_label.setObjectName(f"{self.drone_id}_armed") _set_scaled_stylesheet(self.armed_label, "color: #DDD;") @@ -284,7 +284,7 @@ class DronePanel(QWidget): battery_title = QLabel("電池：") _set_scaled_stylesheet(battery_title, "color: #888; min-width: 50px;") - self.battery_pct_label = QLabel("--") + self.battery_pct_label = QLabel("-") self.battery_pct_label.setObjectName(f"{self.drone_id}_battery_pct") _set_scaled_stylesheet(self.battery_pct_label, "color: #DDD;") @@ -294,7 +294,7 @@ class DronePanel(QWidget): _set_scaled_stylesheet(separator1, "color: #DDD;") # 顯示電壓 - self.battery_vol_label = QLabel("--") + self.battery_vol_label = QLabel("-") self.battery_vol_label.setObjectName(f"{self.drone_id}_battery_vol") _set_scaled_stylesheet(self.battery_vol_label, "color: #DDD;") @@ -304,7 +304,7 @@ class DronePanel(QWidget): _set_scaled_stylesheet(separator2, "color: #DDD;") # 顯示電池節數 (S count) - self.battery_cells_label = QLabel("--") + self.battery_cells_label = QLabel("-") self.battery_cells_label.setObjectName(f"{self.drone_id}_battery_cells") _set_scaled_stylesheet(self.battery_cells_label, "color: #DDD;") @@ -327,14 +327,14 @@ class DronePanel(QWidget): altitude_title = QLabel("高度：") _set_scaled_stylesheet(altitude_title, "color: #888; min-width: 50px;") - self.altitude_label = QLabel("--") + self.altitude_label = QLabel("-") self.altitude_label.setObjectName(f"{self.drone_id}_altitude") _set_scaled_stylesheet(self.altitude_label, "color: #DDD;") speed_title = QLabel("速度：") _set_scaled_stylesheet(speed_title, "color: #888; margin-left: 10px;") - self.speed_label = QLabel("--") + self.speed_label = QLabel("-") self.speed_label.setObjectName(f"{self.drone_id}_speed") _set_scaled_stylesheet(self.speed_label, "color: #DDD;") @@ -355,14 +355,14 @@ class DronePanel(QWidget): fix_title = QLabel("定位：") _set_scaled_stylesheet(fix_title, "color: #888; min-width: 50px;") - self.fix_type_label = QLabel("--") + self.fix_type_label = QLabel("-") self.fix_type_label.setObjectName(f"{self.drone_id}_fix_type_label") _set_scaled_stylesheet(self.fix_type_label, "color: #DDD;") error_title = QLabel("誤差(H/V)：") _set_scaled_stylesheet(error_title, "color: #888; margin-left: 8px;") - self.gnss_error_label = QLabel("--") + self.gnss_error_label = QLabel("-") self.gnss_error_label.setObjectName(f"{self.drone_id}_gnss_error") _set_scaled_stylesheet(self.gnss_error_label, "color: #DDD;") diff --git a/src/GUI/gui.py b/src/GUI/gui.py index 1a5896e..25da1f4 100644 --- a/src/GUI/gui.py +++ b/src/GUI/gui.py @@ -147,7 +147,7 @@ class ToggleSwitch(QWidget): class ControlStationUI(QMainWindow): planning_finished = pyqtSignal(object) - VERSION = '2.7.1' + VERSION = '2.8.0' FONT_SCALE_MIN = 70 FONT_SCALE_MAX = 180 FONT_SCALE_DEFAULT = 100 @@ -449,7 +449,7 @@ class ControlStationUI(QMainWindow): self.right_vertical_splitter.setChildrenCollapsible(False) self.right_vertical_splitter.setStretchFactor(0, 0) self.right_vertical_splitter.setStretchFactor(1, 1) - self.right_vertical_splitter.setSizes([180, 640]) + self.right_vertical_splitter.setSizes([210, 640]) right_layout.addWidget(self.right_vertical_splitter) # 添加地圖 @@ -873,6 +873,16 @@ class ControlStationUI(QMainWindow): 'drone', f"/fc_network/vehicle/{sys_id}/status_text " f"[{label}] {data.get('text', '')}") + def _record_xbee_ack(self, connection_name, data): + """將 XBee JSON ACK 顯示於既有無人機紀錄與狀態列。""" + command = data.get('command', '-') + result = data.get('result', '-') + message = data.get('message', '-') + text = (f"{connection_name} XBee ACK｜命令:{command} " + f"狀態:{result} 訊息:{message}") + self._append_history('drone', text) + self.statusBar().showMessage(text, 5000) + def _record_sys_diags_warning(self, sys_id, data): """sys_diags 只在診斷值異常或內容改變時顯示警告。""" installed = int(data.get('sensors_install_mask', 0)) @@ -1062,8 +1072,11 @@ class ControlStationUI(QMainWindow): def handle_serial_connection_added(self, port, baudrate): conn = {'name': 'Serial', 'port': port, 'baudrate': baudrate, 'enabled': False, 'receiver': None} self.serial_connections.append(conn) - self.comm_panel.add_serial_panel(conn) - self.statusBar().showMessage(f"已添加 Serial 連接: {port} @ {baudrate}", 3000) + panel = self.comm_panel.add_serial_panel(conn) + # Serial 新增後立即啟動；若啟動失敗，面板仍保留為停止狀態， + # 使用者可在排除連線問題後手動重試。 + self.toggle_serial_connection( + conn, panel.toggle_btn, panel.status_label) def toggle_serial_connection(self, conn, btn, status_label): if conn.get('enabled', False): @@ -1278,6 +1291,7 @@ class ControlStationUI(QMainWindow): panel.arm_requested.connect(self._handle_group_arm) panel.takeoff_requested.connect(self._handle_group_takeoff) panel.reboot_requested.connect(self._handle_group_reboot) + panel.xbee_command_requested.connect(self._handle_xbee_command) panel.box_select_requested.connect(self._handle_box_select) panel.select_all_requested.connect(self._handle_select_all_for_group) panel.clear_group_requested.connect(self._handle_clear_group) @@ -1552,6 +1566,7 @@ class ControlStationUI(QMainWindow): if group.executor: group.executor.stop() group.planned_waypoints = None + group.mission_target = None self.drone_map.clear_mission_plan_for_group(group_id) panel = self.group_panels.get(group_id) if panel: @@ -1666,6 +1681,81 @@ class ControlStationUI(QMainWindow): future = self.monitor.reboot_drone(drone_id) loop.create_task(self.handle_service_response(future, f"重啟飛控 {drone_id}")) + def _handle_xbee_command(self, group_id, command): + """依 UAV 所屬連線透過 Serial 或 WebSocket 發送 XBee JSON 指令。""" + group = self.mission_groups.get(group_id) + if not group: + self.statusBar().showMessage(f"Group {group_id} 不存在", 3000) + return + + selected = sorted(group.selected_drone_ids) + if not selected: + self.statusBar().showMessage( + f"Group {group_id}: 請先分配無人機", 3000) + return + + command = str(command).upper() + position = None + if command == 'GOTO': + position = group.mission_target + if position is None: + self.statusBar().showMessage( + f"Group {group_id}: 請先在地圖點選目標點", 4000) + return + + receivers_by_socket = {} + for conn in self.serial_connections: + receiver = conn.get('receiver') + if conn.get('enabled') and receiver is not None: + receivers_by_socket[str(receiver.socket_id)] = ( + receiver, 'Serial') + for conn in self.ws_connections: + receiver = conn.get('receiver') + if conn.get('enabled') and receiver is not None: + receivers_by_socket[str(receiver.socket_id)] = ( + receiver, 'WebSocket') + + sent = [] + failed = [] + for drone_id in selected: + match = re.fullmatch(r's(\d+)_(\d+)', drone_id) + if not match: + failed.append(f"{drone_id}(ID格式錯誤)") + continue + + socket_id, system_id = match.groups() + route = receivers_by_socket.get(socket_id) + if route is None: + failed.append(f"{drone_id}(無可用Serial/WebSocket)") + continue + + receiver, transport = route + try: + payload = receiver.send_json_command( + int(system_id), command, position=position) + sent.append(drone_id) + self._append_history( + 'gui', f"{drone_id} XBee {transport} TX｜{payload}") + except Exception as e: + failed.append(f"{drone_id}({e})") + + if sent and not failed: + target_text = ( + f" → {position[0]:.7f}, {position[1]:.7f}" + if position is not None else "" + ) + self.statusBar().showMessage( + f"Group {group_id}: XBee {command} 已送至 " + f"{len(sent)} 台{target_text}", 4000) + elif sent: + self.statusBar().showMessage( + f"Group {group_id}: XBee {command} 已送 {len(sent)} 台；" + f"失敗 {len(failed)} 台：{', '.join(failed)}", 6000) + else: + self.statusBar().showMessage( + f"Group {group_id}: XBee {command} 未送出：" + f"{', '.join(failed)}", 6000) + def _handle_box_select(self, group_id): """觸發地圖框選 → 框選完成後直接分配到該群組""" self._pending_box_assign = group_id @@ -1754,6 +1844,7 @@ class ControlStationUI(QMainWindow): group.selected_drone_ids.clear() group.planned_waypoints = None + group.mission_target = None if group.executor: group.executor.stop() self.drone_map.clear_mission_plan_for_group(group_id) @@ -1837,6 +1928,9 @@ class ControlStationUI(QMainWindow): def update_ui(self, msg_type, drone_id, data): """只做數據快取，不在這裡更新 UI""" + if msg_type == 'xbee_ack': + self._record_xbee_ack(drone_id, data) + return if msg_type == 'status_text': self._record_drone_status(drone_id, data) return @@ -1859,7 +1953,6 @@ class ControlStationUI(QMainWindow): if drone_id not in self.drones: self.add_drone(drone_id) - return # 只做資料快取，不更新 UI - 所有 UI 更新都在 _update_panel_and_map 中進行 if drone_id not in self._message_cache: @@ -2133,6 +2226,7 @@ class ControlStationUI(QMainWindow): center_lat, center_lon, _ = center_origin target_lat, target_lon = context['draw_target'] + group.mission_target = (float(target_lat), float(target_lon)) self.drone_map.draw_mission_plan_for_group( group_id, group.color, center_lat, center_lon, target_lat, target_lon @@ -2171,6 +2265,9 @@ class ControlStationUI(QMainWindow): if mission_type is None: return # Grid Sweep / Leader-Follower 由各自的觸發方式處理 + # XBee GOTO 直接使用該 Group 最近一次點擊的地圖座標，與文字欄位解耦。 + group.mission_target = (float(lat), float(lon)) + selected_drones = self._get_group_drones(group) if len(selected_drones) == 0: self.statusBar().showMessage(f"Group {group.group_id}: 請先分配無人機", 3000) @@ -2441,7 +2538,7 @@ class ControlStationUI(QMainWindow): def _format_gnss_fix_type(fix_type): fix_labels = { 0: "No GPS", - 1: "No GPS", + 1: "No Fix", 2: "2D Fix", 3: "3D Fix", 4: "DGPS", @@ -2451,7 +2548,7 @@ class ControlStationUI(QMainWindow): try: return fix_labels.get(int(fix_type), f"Fix {fix_type}") except (TypeError, ValueError): - return "--" + return "-" def update_overview_table(self, drone_id=None, field=None, value=None): if not hasattr(self, 'overview_table') or self.overview_table is None: return @@ -2687,15 +2784,21 @@ class ControlStationUI(QMainWindow): # 處理所有快取的消息類型 for msg_type, data in cached_data.items(): if msg_type == 'state': - mode = data.get('mode', 'UNKNOWN') + mode_value = data.get('mode') + mode = str(mode_value) if mode_value is not None else '-' armed = data.get('armed', None) - mode_color = '#FF5555' if any(x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY']) else '#55FF55' + if mode == '-': + mode_color = '#AAAAAA' + else: + mode_color = '#FF5555' if any( + x in mode.upper() for x in ['RTL', '返航', 'EMERGENCY'] + ) else '#55FF55' if armed is True: arm_text, arm_color = "ARMED", '#55FF55' elif armed is False: arm_text, arm_color = "DISARMED", '#FF5555' else: - arm_text, arm_color = "--", '#AAAAAA' + arm_text, arm_color = "-", '#AAAAAA' self.update_field(panel, drone_id, 'mode', mode, mode_color) self.update_field(panel, drone_id, 'armed', arm_text, arm_color) self.queue_overview_update(drone_id, 'mode', mode) @@ -2703,42 +2806,59 @@ class ControlStationUI(QMainWindow): elif msg_type == 'battery': voltage = data.get('voltage') - cells = round(voltage / 3.95) if voltage is not None else None - percentage = ( + cells = round(voltage / 3.95) if isinstance(voltage, (int, float)) else None + calculated_percentage = ( (voltage / cells - 3.7) / 0.5 * 100 - if voltage is not None and cells and cells > 0 - else data.get('percentage', 0) + if isinstance(voltage, (int, float)) and cells and cells > 0 + else None ) - if percentage < 20: voltage_color = '#FF6464' - elif percentage < 50: voltage_color = '#FFA500' - else: voltage_color = '#FFFFFF' - percentage = data.get('percentage', percentage) - self.update_field(panel, drone_id, 'battery_pct', f"{percentage:.0f}%", voltage_color) - if voltage is not None: + percentage = data.get('percentage') + if not isinstance(percentage, (int, float)): + percentage = calculated_percentage + if isinstance(percentage, (int, float)): + if percentage < 20: voltage_color = '#FF6464' + elif percentage < 50: voltage_color = '#FFA500' + else: voltage_color = '#FFFFFF' + percentage_text = f"{percentage:.0f}%" + else: + voltage_color = '#AAAAAA' + percentage_text = '-' + self.update_field(panel, drone_id, 'battery_pct', percentage_text, voltage_color) + if isinstance(voltage, (int, float)): self.update_field(panel, drone_id, 'battery_sep1', " - ") self.update_field(panel, drone_id, 'battery_vol', f"{voltage:.2f}V") self.update_field(panel, drone_id, 'battery_sep2', " - ") self.update_field(panel, drone_id, 'battery_cells', f"{cells}S") self.queue_overview_update(drone_id, 'battery', f"{voltage:.2f}V") + elif data.get('_snapshot'): + self.update_field(panel, drone_id, 'battery_sep1', " - ") + self.update_field(panel, drone_id, 'battery_vol', "-") + self.update_field(panel, drone_id, 'battery_sep2', " - ") + self.update_field(panel, drone_id, 'battery_cells', "-") + self.queue_overview_update(drone_id, 'battery', percentage_text) else: self.update_field(panel, drone_id, 'battery_sep1', "") self.update_field(panel, drone_id, 'battery_vol', "") self.update_field(panel, drone_id, 'battery_sep2', "") self.update_field(panel, drone_id, 'battery_cells', "") - self.queue_overview_update(drone_id, 'battery', f"{percentage:.0f}%") + self.queue_overview_update(drone_id, 'battery', percentage_text) elif msg_type == 'altitude': - altitude = data.get('altitude', 0) - text = f"{altitude:.1f} m" + altitude = data.get('altitude') + text = f"{altitude:.1f} m" if isinstance(altitude, (int, float)) else '-' self.update_field(panel, drone_id, 'altitude', text) self.queue_overview_update(drone_id, 'altitude', text) elif msg_type == 'local_pose': - x, y = data.get('x', 0), data.get('y', 0) - if not hasattr(self.monitor, 'drone_local'): - self.monitor.drone_local = {} - self.monitor.drone_local[drone_id] = {'x': x, 'y': y} - self.queue_overview_update(drone_id, 'local', f"{x:.1f}, {y:.1f}") + x, y = data.get('x'), data.get('y') + if isinstance(x, (int, float)) and isinstance(y, (int, float)): + if not hasattr(self.monitor, 'drone_local'): + self.monitor.drone_local = {} + self.monitor.drone_local[drone_id] = {'x': x, 'y': y} + local_text = f"{x:.1f}, {y:.1f}" + else: + local_text = '-' + self.queue_overview_update(drone_id, 'local', local_text) elif msg_type == 'loss_rate': text = f"{data.get('loss_rate', 0):.1f}%" @@ -2751,24 +2871,47 @@ class ControlStationUI(QMainWindow): self.queue_overview_update(drone_id, 'ping', text) elif msg_type == 'velocity': - self.queue_overview_update(drone_id, 'velocity', f"{data['vx']:.1f}, {data['vy']:.1f}") + vx, vy = data.get('vx'), data.get('vy') + velocity_text = ( + f"{vx:.1f}, {vy:.1f}" + if isinstance(vx, (int, float)) and isinstance(vy, (int, float)) + else '-' + ) + self.queue_overview_update(drone_id, 'velocity', velocity_text) elif msg_type == 'attitude': - roll, pitch, yaw = data.get('roll', 0), data.get('pitch', 0), data.get('yaw', 0) - self.queue_overview_update(drone_id, 'roll', f"{roll:.1f}°") - self.queue_overview_update(drone_id, 'pitch', f"{pitch:.1f}°") - self.queue_overview_update(drone_id, 'yaw', f"{yaw:.1f}°") + roll, pitch, yaw = data.get('roll'), data.get('pitch'), data.get('yaw') + self.queue_overview_update( + drone_id, 'roll', + f"{roll:.1f}°" if isinstance(roll, (int, float)) else '-' + ) + self.queue_overview_update( + drone_id, 'pitch', + f"{pitch:.1f}°" if isinstance(pitch, (int, float)) else '-' + ) + self.queue_overview_update( + drone_id, 'yaw', + f"{yaw:.1f}°" if isinstance(yaw, (int, float)) else '-' + ) elif msg_type == 'gps': gps_data = data - lat, lon = gps_data.get('lat', 0), gps_data.get('lon', 0) - self.drone_positions[drone_id] = (lat, lon) - self._map_dirty_drones.add(drone_id) - if not hasattr(self.monitor, 'drone_gps'): - self.monitor.drone_gps = {} - self.monitor.drone_gps[drone_id] = gps_data.copy() - self.queue_overview_update(drone_id, 'latitude', f"{lat:.6f}°") - self.queue_overview_update(drone_id, 'longitude', f"{lon:.6f}°") + lat, lon = gps_data.get('lat'), gps_data.get('lon') + has_position = ( + isinstance(lat, (int, float)) and isinstance(lon, (int, float)) + ) + if has_position: + self.drone_positions[drone_id] = (lat, lon) + self._map_dirty_drones.add(drone_id) + if not hasattr(self.monitor, 'drone_gps'): + self.monitor.drone_gps = {} + self.monitor.drone_gps[drone_id] = gps_data.copy() + self.queue_overview_update( + drone_id, 'latitude', f"{lat:.6f}°" if has_position else '-' + ) + self.queue_overview_update( + drone_id, 'longitude', f"{lon:.6f}°" if has_position else '-' + ) fix_type = gps_data.get('fix_type') satellites_visible = gps_data.get('satellites_visible') eph = gps_data.get('eph') @@ -2778,7 +2921,7 @@ class ControlStationUI(QMainWindow): if isinstance(eph, (int, float)) and isinstance(epv, (int, float)): error_text = f"{eph:.1f}/{epv:.1f}" else: - error_text = "--" + error_text = "-" self.update_field(panel, drone_id, 'gnss_error', error_text) self.queue_overview_update( drone_id, 'fix_type', @@ -2786,38 +2929,41 @@ class ControlStationUI(QMainWindow): ) self.queue_overview_update( drone_id, 'satellites_visible', - str(satellites_visible) if satellites_visible is not None else "--" + str(satellites_visible) if satellites_visible is not None else "-" ) self.queue_overview_update( drone_id, 'eph', - f"{eph:.2f}" if isinstance(eph, (int, float)) else "--" + f"{eph:.2f}" if isinstance(eph, (int, float)) else "-" ) self.queue_overview_update( drone_id, 'epv', - f"{epv:.2f}" if isinstance(epv, (int, float)) else "--" + f"{epv:.2f}" if isinstance(epv, (int, float)) else "-" ) elif msg_type == 'hud': hud_data = data - heading = hud_data.get('heading', 0) - self.drone_headings[drone_id] = heading - self._map_dirty_drones.add(drone_id) - groundspeed = hud_data.get('groundspeed', 0) - airspeed = hud_data.get('airspeed', 0) - throttle = hud_data.get('throttle', 0) - hud_alt = hud_data.get('alt', 0) - climb = hud_data.get('climb', 0) + heading = hud_data.get('heading') + if isinstance(heading, (int, float)): + self.drone_headings[drone_id] = heading + self._map_dirty_drones.add(drone_id) + groundspeed = hud_data.get('groundspeed') + airspeed = hud_data.get('airspeed') + throttle = hud_data.get('throttle') + hud_alt = hud_data.get('alt') + climb = hud_data.get('climb') - self.queue_overview_update(drone_id, 'heading', f"{heading:.1f}°") - self.queue_overview_update(drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--") - self.queue_overview_update(drone_id, 'airspeed', f"{airspeed:.1f} m/s" if isinstance(airspeed, (int, float)) else "--") - self.queue_overview_update(drone_id, 'throttle', f"{throttle:.0f}%" if isinstance(throttle, (int, float)) else "--") - self.queue_overview_update(drone_id, 'hud_alt', f"{hud_alt:.1f} m" if isinstance(hud_alt, (int, float)) else "--") - self.queue_overview_update(drone_id, 'climb', f"{climb:.1f} m/s" if isinstance(climb, (int, float)) else "--") + heading_text = f"{heading:.1f}°" if isinstance(heading, (int, float)) else '-' + groundspeed_text = f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else '-' + self.queue_overview_update(drone_id, 'heading', heading_text) + self.queue_overview_update(drone_id, 'groundspeed', groundspeed_text) + self.queue_overview_update(drone_id, 'airspeed', f"{airspeed:.1f} m/s" if isinstance(airspeed, (int, float)) else "-") + self.queue_overview_update(drone_id, 'throttle', f"{throttle:.0f}%" if isinstance(throttle, (int, float)) else "-") + self.queue_overview_update(drone_id, 'hud_alt', f"{hud_alt:.1f} m" if isinstance(hud_alt, (int, float)) else "-") + self.queue_overview_update(drone_id, 'climb', f"{climb:.1f} m/s" if isinstance(climb, (int, float)) else "-") - self.update_field(panel, drone_id, 'heading', f"{heading:.1f}°") - self.update_field(panel, drone_id, 'groundspeed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--") - self.update_field(panel, drone_id, 'speed', f"{groundspeed:.1f} m/s" if isinstance(groundspeed, (int, float)) else "--") + self.update_field(panel, drone_id, 'heading', heading_text) + self.update_field(panel, drone_id, 'groundspeed', groundspeed_text) + self.update_field(panel, drone_id, 'speed', groundspeed_text) elapsed = (time.time() - start_time) * 1000 @@ -2857,9 +3003,13 @@ class ControlStationUI(QMainWindow): if not panel or not hasattr(panel, 'update_attitude'): continue - roll = data.get('roll', 0) - pitch = data.get('pitch', 0) - yaw = data.get('yaw', 0) + roll = data.get('roll') + pitch = data.get('pitch') + yaw = data.get('yaw') + # The ADI is graphical and cannot render '-'. Keep its last valid + # pose while the text/table fields show missing snapshot values. + if not all(isinstance(value, (int, float)) for value in (roll, pitch, yaw)): + continue heading = self.drone_headings.get(drone_id, yaw) panel._last_roll = roll diff --git a/src/GUI/mission_group.py b/src/GUI/mission_group.py index 2a5122b..d7d766d 100644 --- a/src/GUI/mission_group.py +++ b/src/GUI/mission_group.py @@ -57,6 +57,7 @@ class MissionGroup: self.planned_waypoints = None # 規劃結果 dict self.executor = None # MissionExecutor 實例（延遲建立） self.center_origin = None # 規劃原點 + self.mission_target = None # (lat, lon)，最近點擊的地圖目標供 XBee GOTO 使用 self.leader_drone_id = None # LEADER_FOLLOWER 專用：指定的領隊無人機 ID @property @@ -163,6 +164,7 @@ class GroupPanel(QWidget): arm_requested = pyqtSignal(str) # group_id takeoff_requested = pyqtSignal(str, float) # group_id, altitude reboot_requested = pyqtSignal(str) # group_id + xbee_command_requested = pyqtSignal(str, str) # group_id, START/STOP/GOTO box_select_requested = pyqtSignal(str) # group_id — 框選直接分配 select_all_requested = pyqtSignal(str) # group_id — 全選直接分配 clear_group_requested = pyqtSignal(str) # group_id — 清除分組 @@ -394,6 +396,43 @@ class GroupPanel(QWidget): reboot_col.addWidget(reboot_btn) reboot_col.addStretch() + # ============================ + # XBee JSON 指令（GOTO 使用最近點擊的地圖目標點） + # ============================ + xbee_col = QVBoxLayout() + xbee_col.setSpacing(3) + + xbee_title = QLabel("XBee/WS 指令") + xbee_title.setStyleSheet(TITLE) + xbee_col.addWidget(xbee_title) + + xbee_buttons = QVBoxLayout() + xbee_buttons.setSpacing(3) + start_xbee_btn = QPushButton("START") + start_xbee_btn.setStyleSheet( + BTN.format(bg='#2E7D32', fg='white', hover='#388E3C')) + start_xbee_btn.clicked.connect( + lambda: self.xbee_command_requested.emit( + self.group.group_id, 'START')) + stop_xbee_btn = QPushButton("STOP") + stop_xbee_btn.setStyleSheet( + BTN.format(bg='#C62828', fg='white', hover='#D32F2F')) + stop_xbee_btn.clicked.connect( + lambda: self.xbee_command_requested.emit( + self.group.group_id, 'STOP')) + goto_xbee_btn = QPushButton("GOTO") + goto_xbee_btn.setToolTip("前往目前 Group 最近一次點擊的地圖目標點") + goto_xbee_btn.setStyleSheet( + BTN.format(bg='#1565C0', fg='white', hover='#1976D2')) + goto_xbee_btn.clicked.connect( + lambda: self.xbee_command_requested.emit( + self.group.group_id, 'GOTO')) + xbee_buttons.addWidget(start_xbee_btn) + xbee_buttons.addWidget(stop_xbee_btn) + xbee_buttons.addWidget(goto_xbee_btn) + xbee_col.addLayout(xbee_buttons) + xbee_col.addStretch() + # ============================ # 第四欄：任務參數 # ============================ @@ -500,7 +539,7 @@ class GroupPanel(QWidget): # 當任務類型切換時更新參數顯示 self.type_combo.currentTextChanged.connect(self._update_param_visibility) - # ── 組裝欄位：控制指令 > 任務規劃 > 任務參數 > 選取與分組 > 飛控操作 ── + # ── 組裝欄位：控制指令 > 任務規劃 > 任務參數 > 選取與分組 > 飛控操作 > XBee ── # 使用伸展因子 0 讓列根據內容自動調整寬度，而不是均等分配 cols.addLayout(left, 0) cols.addWidget(self._make_sep()) @@ -511,6 +550,8 @@ class GroupPanel(QWidget): cols.addLayout(right, 0) cols.addWidget(self._make_sep()) cols.addLayout(reboot_col, 0) + cols.addWidget(self._make_sep()) + cols.addLayout(xbee_col, 0) cols.addStretch() # 填充剩餘空間，使欄位置左 layout.addLayout(cols) @@ -619,6 +660,7 @@ class GroupPanel(QWidget): def update_mission_info(self, center_lat, center_lon, target_lat, target_lon): """更新中心點 / 目標點顯示""" + self.group.mission_target = (float(target_lat), float(target_lon)) info_style = f"color: {self.group.color}; font-size: 11px; font-weight: bold;" self.center_label.setText(f"中心: {center_lat:.6f}°, {center_lon:.6f}°") self.center_label.setStyleSheet(info_style) @@ -627,6 +669,7 @@ class GroupPanel(QWidget): def clear_mission_info(self): """清除中心點 / 目標點顯示""" + self.group.mission_target = None self.center_label.setText("中心: --") self.center_label.setStyleSheet("color: #AAA; font-size: 11px;") self.target_label.setText("目標: --") diff --git a/src/GUI/overview_table.py b/src/GUI/overview_table.py index 4ee925e..3557e97 100644 --- a/src/GUI/overview_table.py +++ b/src/GUI/overview_table.py @@ -115,11 +115,11 @@ class OverviewTable(QTableWidget): panel = self.drones[did] for field, row in self.info_type_map.items(): if field == 'mission': - # 任務狀態非遙測，取自獨立保存，避免被 "--" 蓋掉 - val = self.mission_status.get(did, "--") + # 任務狀態非遙測，取自獨立保存，避免被 "-" 蓋掉 + val = self.mission_status.get(did, "-") else: lbl = panel.findChild(QLabel, f"{did}_{field}") - val = lbl.text() if lbl else "--" + val = lbl.text() if lbl else "-" val_item = QTableWidgetItem(val) val_item.setTextAlignment(Qt.AlignmentFlag.AlignCenter) self.setItem(row, col, val_item) diff --git a/src/GUI/websocket_client.py b/src/GUI/websocket_client.py new file mode 100644 index 0000000..eeb13d2 --- /dev/null +++ b/src/GUI/websocket_client.py @@ -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()) diff --git a/src/GUI/websocket_server.py b/src/GUI/websocket_server.py new file mode 100644 index 0000000..f9c7764 --- /dev/null +++ b/src/GUI/websocket_server.py @@ -0,0 +1,179 @@ +import asyncio +import websockets +import json +from urllib.parse import urlparse + +# 可切換為 "essential" 或 "full"；預設廣播完整狀態。 +STATUS_FORMAT = "full" +SYSTEM_ID = 2 + +# WebSocket 伺服器位址；修改此參數即可更換監聽 IP/port。 +WS_URL = "ws://0.0.0.0:8765" + + +def parse_ws_url(ws_url): + """將 ws://host:port 解析為 websockets.serve 所需參數。""" + parsed = urlparse(ws_url) + if parsed.scheme != "ws": + raise ValueError("WS_URL 必須使用 ws:// 格式") + if not parsed.hostname: + raise ValueError("WS_URL 缺少主機位址") + try: + port = parsed.port + except ValueError as e: + raise ValueError(f"WS_URL port 格式錯誤: {e}") from e + if port is None: + raise ValueError("WS_URL 缺少 port") + return parsed.hostname, port + +# 啟動伺服器 +async def main(ws_url=WS_URL): + host, port = parse_ws_url(ws_url) + async with websockets.serve(client_handler, host, port): + print(f"WebSocket server started at {ws_url}") + await asyncio.Future() # run forever + +# 模擬資料 +def get_uav_status(status_format=None): + status_format = status_format or STATUS_FORMAT + essential_status = { + "s": SYSTEM_ID, # system ID + "m": "GUIDED", # mode + "b": 78, # battery (%) + "p": [24.1199660, 120.677230], # [lat, lon] (deg) + "y": 178.5, # yaw (deg) + } + + full_status = { + "s": SYSTEM_ID, # system ID + "m": "GUIDED", # mode + "a": 1, # 1=armed, 0=disarmed + "b": 78, # battery (%) + "h": 12.3, # height (m) + "v": 4.2, # velocity (m/s) + "p": [24.1199660, 120.677230], # [lat, lon] (deg) + "ypr": [178.5, -0.1, 0.0], # yaw, pitch, roll (deg) + "g": 3, # GPS fix type + "d": [0.8, 1.2], # HDOP, VDOP + } + + if status_format == "essential": + return essential_status + if status_format == "full": + return full_status + raise ValueError( + f"不支援的 STATUS_FORMAT: {status_format}，請使用 essential 或 full") + +# 處理 client 傳來的訊息 +async def handle_client_messages(websocket): + try: + async for message in websocket: + try: + data = json.loads(message) + print(f"Received from client: {data}") + system_id = data.get("s") + cmd = data.get("c") + # XBee 命令格式: + # {"s":2,"c":"START"} + # {"s":2,"c":"STOP"} + # {"s":2,"c":"GOTO","p":[lat,lon]} + if system_id is None: + ack = { + "result": "ERROR", + "command": cmd, + "message": "缺少 system_id 欄位 s", + } + await websocket.send(json.dumps(ack, ensure_ascii=False)) + print(f"Sent ack: {ack}") + elif (isinstance(system_id, bool) + or not isinstance(system_id, int) + or system_id != SYSTEM_ID): + ack = { + "result": "ERROR", + "command": cmd, + "message": ( + f"指令目標 system_id={system_id} 不符，" + f"本 UAV system_id={SYSTEM_ID}" + ), + } + await websocket.send(json.dumps(ack, ensure_ascii=False)) + print(f"Sent ack: {ack}") + elif isinstance(cmd, str) and cmd in {"START", "STOP", "GOTO"}: + if cmd == "START": + print(f"收到 START 指令: system_id={system_id}") + ack = { + "result": "SUCCESS", + "command": cmd, + "message": "已接收 START 指令", + } + elif cmd == "STOP": + print(f"收到 STOP 指令: system_id={system_id}") + ack = { + "result": "SUCCESS", + "command": cmd, + "message": "已接收 STOP 指令", + } + elif cmd == "GOTO": + position = data.get("p") + if isinstance(position, (list, tuple)) and len(position) >= 2: + lat, lon = position[0], position[1] + print( + f"收到 GOTO 指令: system_id={system_id}, " + f"lat={lat}, lon={lon}" + ) + ack = { + "result": "SUCCESS", + "command": cmd, + "message": "已接收 GOTO 指令", + } + else: + print("GOTO 指令缺少位置 p=[lat,lon]") + ack = { + "result": "ERROR", + "command": cmd, + "message": "GOTO 指令缺少位置 p=[lat,lon]", + } + + await websocket.send(json.dumps(ack, ensure_ascii=False)) + print(f"Sent ack: {ack}") + else: + ack = { + "result": "ERROR", + "command": cmd, + "message": "指令 c 必須為大寫 START、STOP 或 GOTO", + } + await websocket.send(json.dumps(ack, ensure_ascii=False)) + print(f"Sent ack: {ack}") + except json.JSONDecodeError: + print("❌ JSON 格式錯誤") + except websockets.ConnectionClosed: + print("❌ Client connection closed while receiving") + +# 持續廣播 UAV 資訊 +async def broadcast_uav_status(websocket): + try: + while True: + msg = get_uav_status() + await websocket.send(json.dumps(msg)) + await asyncio.sleep(1) # 每秒發送一次 + except websockets.ConnectionClosed: + print("❌ Client connection closed while sending") + +# 每個 client 的處理流程 +async def client_handler(websocket, path=None): + print(f"🔗 Client connected: {websocket.remote_address}") + receive_task = asyncio.create_task(handle_client_messages(websocket)) + send_task = asyncio.create_task(broadcast_uav_status(websocket)) + + done, pending = await asyncio.wait( + [receive_task, send_task], + return_when=asyncio.FIRST_COMPLETED, + ) + + for task in pending: + task.cancel() + + print(f"🔌 Client disconnected: {websocket.remote_address}") + +if __name__ == "__main__": + asyncio.run(main(WS_URL)) diff --git a/src/GUI/xbee_gcs.py b/src/GUI/xbee_gcs.py new file mode 100644 index 0000000..8ff966f --- /dev/null +++ b/src/GUI/xbee_gcs.py @@ -0,0 +1,127 @@ +import serial +import json +import threading +import time +from pynput import keyboard + +SYSTEM_ID = 2 + +try: + ser = serial.Serial('/dev/ttyUSB1', 115200, timeout=0.1) + print("XBee 地面站啟動") + print("[1] START [2] STOP [3] GOTO [Esc] 退出程式") +except Exception as e: + print(f"無法開啟序列埠: {e}") + exit() + +GPS_TYPE_LABELS = { + 0: "no GPS", + 1: "no fix", + 2: "2D", + 3: "3D", + 4: "DGPS", + 5: "RTK float", + 6: "RTK fixed", +} + +def format_uav_data(uav_data): + system_id = uav_data.get("s", "?") + mode = uav_data.get("m", "?") + battery = uav_data.get("b", "?") + position = uav_data.get("p", "?") + + parts = [ + f"[UAV] ID:{system_id}", + f"Mode:{mode}", + f"Bat:{battery}%", + f"Pos:{position}", + ] + + if "a" in uav_data: + armed_text = "ARMED" if int(uav_data.get("a", 0)) == 1 else "DISARMED" + parts.append(f"Arm:{armed_text}") + + ypr = uav_data.get("ypr") + if isinstance(ypr, list) and len(ypr) >= 3: + parts.append(f"Height:{uav_data.get('h', '?')}m") + parts.append(f"Yaw:{ypr[0]}deg") + parts.append(f"Pitch:{ypr[1]}deg") + parts.append(f"Roll:{ypr[2]}deg") + elif "y" in uav_data: + parts.append(f"Height:{uav_data.get('h', '?')}m") + parts.append(f"Yaw:{uav_data.get('y')}deg") + elif "h" in uav_data: + parts.append(f"Heading:{uav_data.get('h')}") + + if "v" in uav_data: + parts.append(f"Vel:{uav_data.get('v')}m/s") + + if "g" in uav_data: + gps_type = uav_data.get("g") + parts.append(f"GPS:{gps_type}({GPS_TYPE_LABELS.get(gps_type, 'unknown')})") + + dop = uav_data.get("d") + if isinstance(dop, list) and len(dop) >= 2: + parts.append(f"DOP(H/V):{dop[0]}/{dop[1]}") + + return " | ".join(parts) + +def receive_thread(): + while True: + try: + if ser.in_waiting > 0: + line = ser.readline().decode('utf-8', errors='ignore').strip() + if line: + try: + data = json.loads(line) + # 判斷是確認訊息（ack）還是 UAV 狀態 + if "result" in data and "command" in data and "message" in data: + # 這是確認訊息 + result = data.get("result") + cmd = data.get("command") + msg = data.get("message") + print(f"\n[確認] 命令:{cmd} 狀態:{result} 訊息:{msg}", flush=True) + else: + # 這是 UAV 狀態訊息 + print(format_uav_data(data), flush=True) + except json.JSONDecodeError: + pass + except Exception as e: + print(f"\n[接收錯誤] {e}") + time.sleep(0.01) + +t = threading.Thread(target=receive_thread, daemon=True) +t.start() + +def send_command(command, position=None): + try: + if command not in {"START", "STOP", "GOTO"}: + raise ValueError("指令必須為大寫 START、STOP 或 GOTO") + cmd = { + "s": SYSTEM_ID, + "c": command + } + if position is not None: + cmd["p"] = position + + payload = json.dumps(cmd, separators=(',', ':')) + "\n" + ser.write(payload.encode('utf-8')) + print(f"\n>> [指令已發送] {payload.strip()}") + except Exception as e: + print(f"\n發送失敗: {e}") + +def on_press(key): + try: + if key.char == "1": + send_command("START") + elif key.char == "2": + send_command("STOP") + elif key.char == "3": + send_command("GOTO", [24.1199660, 120.677230]) + except AttributeError: + if key == keyboard.Key.esc: + print("\n程式結束") + return False + +with keyboard.Listener(on_press=on_press) as listener: + listener.join() diff --git a/src/GUI/xbee_uav.py b/src/GUI/xbee_uav.py new file mode 100644 index 0000000..57fd778 --- /dev/null +++ b/src/GUI/xbee_uav.py @@ -0,0 +1,141 @@ +import serial +import json +import math +import time + +PORT = '/dev/ttyUSB0' # 替換為實際的序列埠名稱 +BAUD_RATE = 115200 +SYSTEM_ID = 2 + +# 可切換為 "essential" 或 "full"；預設發送完整狀態。 +STATUS_FORMAT = "full" + +try: + ser = serial.Serial(PORT, BAUD_RATE, timeout=0.1) + print("XBee 無人機啟動...") +except Exception as e: + print(f"無法開啟序列埠: {e}") + exit() + +def get_uav_status(status_format=None): + status_format = status_format or STATUS_FORMAT + essential_status = { + "s": SYSTEM_ID, # system ID + "m": "GUIDED", # mode + "b": 78, # battery (%) + "p": [24.1199660, 120.677230], # [lat, lon] (deg) + "y": 178.5, # yaw (deg) + } + full_status = { + "s": SYSTEM_ID, # system ID + "m": "GUIDED", # mode + "a": 1, # 1=armed, 0=disarmed + "b": 78, # battery (%) + "h": 12.3, # height (m) + "v": 4.2, # velocity (m/s) + "p": [24.1199660, 120.677230], # position: [lat, lon] (deg) + "ypr": [178.5, -0.1, 0.0], # yaw, pitch, roll (deg) + "g": 3, # GPS type {0: no GPS, 1: no fix, 2: 2D, 3: 3D, 4: DGPS, 5: RTK float, 6: RTK fixed} + "d": [0.8, 1.2], # DOP: [hdop, vdop] + } + if status_format == "essential": + return essential_status + if status_format == "full": + return full_status + raise ValueError( + f"不支援的 STATUS_FORMAT: {status_format}，請使用 essential 或 full") + +def handle_command(cmd_obj): + # 統一指令格式: {"s": , "c": "START|STOP|GOTO"} + target_s = cmd_obj.get("s") + command = cmd_obj.get("c") + ack = None + + if (isinstance(target_s, bool) + or not isinstance(target_s, int) + or target_s != SYSTEM_ID): + ack = { + "result": "ERROR", + "command": command, + "message": ( + f"指令目標 system_id={target_s} 不符，" + f"本 UAV system_id={SYSTEM_ID}" + ), + } + elif not isinstance(command, str) or command not in {"START", "STOP", "GOTO"}: + print(f"\n[接收指令] 未知或格式錯誤的指令: {command}") + ack = { + "result": "ERROR", + "command": command, + "message": "指令 c 必須為大寫 START、STOP 或 GOTO", + } + elif command == "START": + print("\n[接收指令] 開始任務") + ack = {"result": "SUCCESS", "command": "START", "message": "成功"} + elif command == "STOP": + print("\n[接收指令] 停止任務") + ack = {"result": "SUCCESS", "command": "STOP", "message": "成功"} + elif command == "GOTO": + # 優先使用 XBee 格式 p=[lat, lon]，並兼容舊版 lat/lon 欄位。 + position = cmd_obj.get("p") + if isinstance(position, (list, tuple)) and len(position) >= 2: + lat, lon = position[0], position[1] + else: + lat, lon = cmd_obj.get("lat"), cmd_obj.get("lon") + + try: + if isinstance(lat, bool) or isinstance(lon, bool): + raise ValueError + lat = float(lat) + lon = float(lon) + if not math.isfinite(lat) or not math.isfinite(lon): + raise ValueError + if not -90.0 <= lat <= 90.0 or not -180.0 <= lon <= 180.0: + raise ValueError + except (TypeError, ValueError): + print("\n[接收指令] GOTO 目標無效，需要有效的 p=[lat,lon]") + ack = { + "result": "ERROR", + "command": "GOTO", + "message": "GOTO 指令缺少或包含無效的 p=[lat,lon]", + } + else: + target = [lat, lon] + print(f"\n[接收指令] 導航任務 目標位置: {target}") + ack = { + "result": "SUCCESS", + "command": "GOTO", + "p": target, + "message": "成功", + } + # 發送回覆（ack）到 GCS via serial + if ack is not None: + try: + payload = json.dumps(ack, ensure_ascii=False) + "\n" + ser.write(payload.encode('utf-8')) + print(f"已發送回覆: {payload.strip()}") + except Exception as e: + print(f"發送回覆失敗: {e}") + +try: + while True: + data = get_uav_status() + json_payload = json.dumps(data, separators=(',', ':')) + "\n" + ser.write(json_payload.encode('utf-8')) + print(f"發送狀態: {json_payload.strip()}") + for _ in range(10): + if ser.in_waiting > 0: + try: + cmd_line = ser.readline().decode('utf-8').strip() + if cmd_line: + cmd_obj = json.loads(cmd_line) + handle_command(cmd_obj) + + except json.JSONDecodeError: + pass + time.sleep(0.1) + +except KeyboardInterrupt: + print("發送停止") +finally: + ser.close()