diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index 4a9f16a..ed78fbd 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -55,6 +55,22 @@ class PanelState: "serial_port": "", "baudrate": "", "receiver_type": "", "target_port": "", "InfoReady": False} # 暫存單一 serial 連結的資訊 + # 關於顯示載具資訊 + self.connected_vehicles_dict = {} # {(sysid, compid): {...基本資訊...}} + self.vehicle_info_single = { + "sysid": 0, + "compid": 0, + "vehicle_type": "", + "component_type": "", + "mav_autopilot": "", + "socket_id": None, + "connection_type": "", + "packet_stats": {}, + "msg_type_counts": {}, + "prev_stats": {}, # 用於計算變化率 + "InfoReady": False + } + def intoSTART(self): self.panel_status = "Running" @@ -439,7 +455,9 @@ class ControlPanel: # ================ 關於載具檢視的部份 =================== def create_vehicles_list_menu(self, state: PanelState, page=0, items_per_page=8): - """動態創建 已連線載具 列表選單（支持分頁）""" + """動態創建 已連線載具 列表選單（支持分頁） + 每個 vehicle-component 組合都是獨立的選單項目 + """ children = [] if not state.connected_vehicles_dict: @@ -450,16 +468,19 @@ class ControlPanel: start_idx = page * items_per_page end_idx = min(start_idx + items_per_page, total_items) - vehicle_id_list = list(state.connected_vehicles_dict.keys()) + # vehicle_id_list 現在是 (sysid, compid) 的元組列表 + vehicle_comp_list = list(state.connected_vehicles_dict.keys()) + # 顯示當前頁的物件 - for vehicle_id in vehicle_id_list[start_idx:end_idx]: - vehicle_menu = MenuNode(f"Vehicle #{vehicle_id}", f"載具 {vehicle_id}", None, children=[ - MenuNode("Info", "查看詳細資訊", "INSPECT_VEHICLE"), - MenuNode("GoUp", "回到列表", "BACK"), - ]) - # 將 vehicle_id 附加到每個子選單項目上 - for child in vehicle_menu.children: - child.vehicle_id = vehicle_id + for sysid, compid in vehicle_comp_list[start_idx:end_idx]: + # 建立顯示名稱 + display_name = f"Vehicle #{sysid} - Comp #{compid}" + desc = f"載具 {sysid} 組件 {compid}" + + vehicle_menu = MenuNode(display_name, desc, "INSPECT_VEHICLE") + # 將 sysid 和 compid 附加到選單項目上 + vehicle_menu.sysid = sysid + vehicle_menu.compid = compid children.append(vehicle_menu) # 添加分頁控制 @@ -479,6 +500,172 @@ class ControlPanel: menu.current_page = page return menu + def show_vehicle_info(self, stdscr, sysid, compid, cmd_q: queue.Queue, state: PanelState): + """顯示載具組件詳細資訊（動態更新，顯示變化率）""" + + # 等待資訊準備 + start = time.time() + while not state.vehicle_info_single.get('InfoReady', False): + if time.time() - start > 2: + state.panel_info_msg_list.append(("Fail! Vehicle Info NOT Acquired!", time.time())) + return + time.sleep(0.05) + + info = state.vehicle_info_single + height, width = stdscr.getmaxyx() + dialog_height = min(22, height - 4) + dialog_width = min(70, width - 4) + start_y = (height - dialog_height) // 2 + start_x = (width - dialog_width) // 2 + + dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x) + dialog_win.nodelay(True) # 非阻塞模式，允許動態更新 + dialog_win.keypad(True) + + # MAV_TYPE 名稱對應 + MAV_TYPE_NAMES = { + 0: "Generic", 1: "Fixed Wing", 2: "Quadrotor", 3: "Helicopter", + 4: "Antenna Tracker", 5: "GCS", 6: "Airship", 10: "Ground Rover", + 12: "Boat", 13: "Submarine", 26: "Gimbal", 30: "Camera" + } + + # 動態更新迴圈 + last_update = time.time() + while True: + current_time = time.time() + + # 每 1 秒重新請求資料 + if current_time - last_update >= 1.0: + # 觸發資料更新（透過 Orchestrator） + cmd_q.put(("INSPECT_VEHICLE", sysid, compid)) + # 等待新資料準備好 + wait_start = time.time() + state.vehicle_info_single['InfoReady'] = False + while not state.vehicle_info_single.get('InfoReady', False): + if time.time() - wait_start > 0.5: # 最多等 0.5 秒 + break + time.sleep(0.01) + # 更新 info 參照 + info = state.vehicle_info_single + last_update = current_time + + dialog_win.clear() + dialog_win.border() + dialog_win.addstr(0, 2, f" Vehicle #{info['sysid']} - Component #{info['compid']} ", curses.A_BOLD) + + # === 基礎資訊 === + dialog_win.addstr(2, 2, "[Identity]", curses.A_UNDERLINE) + dialog_win.addstr(2, 32, "[Connection]", curses.A_UNDERLINE) + + # # MAV Type # 這個用不到 + # mav_type = info.get('vehicle_type', 'N/A') + # mav_type_str = f"{mav_type} ({MAV_TYPE_NAMES.get(mav_type, 'Unknown')})" if isinstance(mav_type, int) else str(mav_type) + # dialog_win.addstr(3, 2, f"MAV Type : {mav_type_str}") + + # Component Type + dialog_win.addstr(3, 2, f"Component Type : {info.get('component_type', 'N/A')}") + + # Autopilot Type + if info.get('mav_autopilot') is not None: + dialog_win.addstr(4, 2, f"Autopilot : {info.get('mav_autopilot', 'N/A')}") + + # Connection Info + dialog_win.addstr(3, 32, f"Connection : {info.get('connection_type', 'N/A')}") + dialog_win.addstr(4, 32, f"Socket ID : #{info.get('socket_id', 'N/A')}") + + # === 封包統計 === + stats = info.get('packet_stats', {}) + dialog_win.addstr(7, 2, "[Packet Statistics]", curses.A_UNDERLINE) + + received = stats.get('received', 0) + lost = stats.get('lost', 0) + loss_rate = stats.get('loss_rate', 0.0) + last_seq = stats.get('last_seq', 'N/A') + + # 計算變化 + received_delta = stats.get('received_delta', 0) + lost_delta = stats.get('lost_delta', 0) + + # 顯示變化率 + recv_str = f"{received:6d}" + if received_delta > 0: + recv_str += f" (+{received_delta}↑)" + + lost_str = f"{lost:4d}" + if lost_delta > 0: + lost_str += f" (+{lost_delta}↑)" + + dialog_win.addstr(8, 2, f"Received : {recv_str}") + dialog_win.addstr(8, 32, f"Lost : {lost_str}") + dialog_win.addstr(9, 2, f"Loss Rate : {loss_rate:.2f}%") + dialog_win.addstr(9, 32, f"Last Seq : {last_seq}") + + # 最後接收時間 + last_msg_time = stats.get('last_msg_time') + if last_msg_time: + time_str = time.strftime("%H:%M:%S", time.localtime(last_msg_time)) + elapsed = current_time - last_msg_time + dialog_win.addstr(10, 2, f"Last Time : {time_str}") + dialog_win.addstr(10, 32, f"Elapsed : {elapsed:.1f}s") + else: + dialog_win.addstr(10, 2, "Last Time : N/A") + + # === 訊息類型分佈 === + dialog_win.addstr(12, 2, "[Message Types] (Top 12)", curses.A_UNDERLINE) + + msg_counts = info.get('msg_type_counts', {}) + + # MAVLink 訊息名稱對應（縮寫版本） + MSG_NAMES = { + 0: "HB", 1: "SYS_ST", 24: "GPS_RAW", 27: "RAW_IMU", + 30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL", + 74: "VFR_HUD", 147: "BATT_ST" + } + + # 顯示前 12 個最常見的訊息類型（兩列各 6 個） + msg_items = list(msg_counts.items())[:12] + line = 13 + for i, (msg_id, count) in enumerate(msg_items): + msg_name = MSG_NAMES.get(msg_id, "???") + delta = stats.get(f'msg_delta_{msg_id}', 0) + + # 格式化數據 + if delta > 0: + data_str = f"{count}(+{delta}↑)" + else: + data_str = f"{count}" + + # 格式化顯示：[ID]NAME DATA (ID固定3字符寬度，右對齊) + display_str = f"[{msg_id:3d}]{msg_name:8s} {data_str}" + + # 左列(偶數索引)或右列(奇數索引) + col = 2 if i % 2 == 0 else 36 + row = line + (i // 2) + + if row >= dialog_height - 3: # 避免超出邊界 + break + + dialog_win.addstr(row, col, display_str) + + # 確保跳過顯示區域 + line = line + 6 + + dialog_win.addstr(dialog_height - 2, 2, "Press any key to return... | Auto-refresh: 1.0s") + dialog_win.refresh() + + # 檢查是否有按鍵（非阻塞） + ch = dialog_win.getch() + if ch != -1: # 有按鍵則退出 + break + + # 短暫延遲 + time.sleep(0.1) + + state.vehicle_info_single['InfoReady'] = False + del dialog_win + stdscr.clear() + stdscr.refresh() + # ================ 關於 主要選單 的部份 =================== def menu_tree(self): @@ -821,6 +1008,8 @@ class ControlPanel: created_list_menu = self.create_object_list_menu(state, page=selected.page) elif current_list_menu.name == "Linked Serial List": created_list_menu = self.create_linked_serial_menu(state, page=selected.page) + elif current_list_menu.name == "Connected Vehicles": + created_list_menu = self.create_vehicles_list_menu(state, page=selected.page) else: # 不支援的選單類型，回到原本的選單 menu_stack.append(current_list_menu) @@ -892,6 +1081,19 @@ class ControlPanel: continue # 只有在工程模式下才能操作 cmd_q.put("SHUTDOWN_SERIAL_MANAGER") + elif selected.action == "INSPECT_VEHICLES": + # 進入載具檢視選單 + cmd_q.put("UPDATE_VEHICLES_LIST") + created_list_menu = self.create_vehicles_list_menu(state, page=0) + menu_stack.append(created_list_menu) + idx_stack.append(0) + + elif selected.action == "INSPECT_VEHICLE": + # 顯示載具詳細資訊 + if hasattr(selected, 'sysid') and hasattr(selected, 'compid'): + cmd_q.put(("INSPECT_VEHICLE", selected.sysid, selected.compid)) + self.show_vehicle_info(stdscr, selected.sysid, selected.compid, cmd_q, state) + elif callable(selected.action): # 執行函式 cmd_q.put(selected.action) @@ -968,6 +1170,9 @@ class Orchestrator: linked_serial_dict = self.plumber.get_serial_link() self.panelState.linked_serial_dict = linked_serial_dict + # B. 更新載具列表（從 vehicle_registry 獲取） + self._update_vehicles_list() + # 取出面板丟過來的「動作」 try: cmd = self.cmd_q.get_nowait() @@ -1018,6 +1223,12 @@ class Orchestrator: serial_id = cmd[1] self.plumber.remove_serial_link(serial_id) + elif action == "INSPECT_VEHICLE": + sysid, compid = cmd[1], cmd[2] + self._prepare_vehicle_info(sysid, compid) + elif action == "UPDATE_VEHICLES_LIST": + self._update_vehicles_list() + elif cmd == "CREATE_UDP_INBOUND": self.panelState.udp_info_temp["direction"] = "inbound" self.create_udp_object() @@ -1071,7 +1282,7 @@ class Orchestrator: # =============== 面板動作 - Mavlink Object =============== - def create_udp_object(self): + def create_udp_object(self, socket_type:str = ""): # 監聽部分 if self.panelState.udp_info_temp["direction"] == "inbound": connection_string = f"udp:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}" @@ -1097,11 +1308,18 @@ class Orchestrator: except Exception as e: self.panelState.panel_info_msg_list.append((f"Failed to create UDP {self.panelState.udp_info_temp['direction']} object: {e}", time.time()-1)) return - # mavlink 連結建立成功 把他丟到 mavlink_object + + # mavlink 連結建立成功 把他丟到 mavlink_object # 重點句 mavlink_object = mo.mavlink_object(mavlink_socket) - mavlink_object.socket_type = "UDP " + self.panelState.udp_info_temp['direction'].capitalize() - # 再把 mavlink_object 丟到 manager 的 event loop 裡面去管理與執行 + # 再把 mavlink_object 丟到 manager 的 event loop 裡面去管理與執行 # 重點句 self.manager.add_mavlink_object(mavlink_object) + + # 設定一下 mavlink_object 的類型描述 + if socket_type == "": + mavlink_object.socket_type = "UDP " + self.panelState.udp_info_temp['direction'].capitalize() + else: + mavlink_object.socket_type = socket_type + self.panelState.panel_info_msg_list.append((f"Created UDP {self.panelState.udp_info_temp['direction']} object: {connection_string}", time.time())) def delete_mavlink_object(self, socket_id): @@ -1134,6 +1352,103 @@ class Orchestrator: else: self.panelState.panel_info_msg_list.append((f"Fail Removing target {target_id} from socket {source_id}", time.time())) + # =============== 面板動作 - Vehicle Inspector =============== + + def _update_vehicles_list(self): + """更新已連線載具列表（從 vehicle_registry 獲取）""" + vehicles_dict = {} + + # 從 vehicle_registry 獲取所有載具 + all_vehicles = self.vehicle_registry.get_all() + + for sysid, vehicle in all_vehicles.items(): + # 遍歷每個載具的所有組件 + for compid, component in vehicle.components.items(): + # 使用 (sysid, compid) 作為 key + vehicles_dict[(sysid, compid)] = { + 'sysid': sysid, + 'compid': compid, + 'vehicle_type': vehicle.vehicle_type, + 'component_type': component.type.value, + 'connection_via': vehicle.connected_via.value, + 'socket_id': vehicle.custom_meta.get('socket_id', 'N/A') + } + + self.panelState.connected_vehicles_dict = vehicles_dict + + def _prepare_vehicle_info(self, sysid, compid): + """準備載具組件的詳細資訊（包含變化率計算）""" + vehicle = self.vehicle_registry.get(sysid) + if not vehicle: + logger.warning(f"Vehicle {sysid} not found") + return + + socket_id = vehicle.custom_meta.get('socket_id', 'N/A') + + component = vehicle.get_component(compid) + if not component: + logger.warning(f"Component {compid} not found in vehicle {sysid}") + return + + stats = component.packet_stats + + # 取得之前的統計資料（用於計算變化） + prev_stats = self.panelState.vehicle_info_single.get('prev_stats', {}) + prev_received = prev_stats.get('received', 0) + prev_lost = prev_stats.get('lost', 0) + prev_msg_counts = prev_stats.get('msg_counts', {}) + + # 計算變化率 + received_delta = stats.received_count - prev_received + lost_delta = stats.lost_count - prev_lost + + # 準備訊息類型計數（排序後取前幾個） + sorted_msg_counts = dict(sorted( + stats.msg_type_count.items(), + key=lambda x: x[1], + reverse=True + )[:12]) # 取前 12 個最常見的 + + # 計算每種訊息類型的變化 + msg_deltas = {} + for msg_id, count in sorted_msg_counts.items(): + prev_count = prev_msg_counts.get(msg_id, 0) + msg_deltas[f'msg_delta_{msg_id}'] = count - prev_count + + # 更新 vehicle_info_single + socket_type = "N/A" + socket_obj = self.manager.managed_objects.get(socket_id, None) + if socket_obj: + socket_type = socket_obj.socket_type + + self.panelState.vehicle_info_single = { + "sysid": sysid, + "compid": compid, + # "vehicle_type": vehicle.vehicle_type, # 這個用不到 + "component_type": component.type.value, + "mav_autopilot": component.mav_autopilot, + "socket_id": socket_id, + "connection_type": socket_type, + "packet_stats": { + "received": stats.received_count, + "lost": stats.lost_count, + "loss_rate": (stats.lost_count / stats.received_count * 100 + if stats.received_count > 0 else 0), + "last_seq": stats.last_seq, + "last_msg_time": stats.last_msg_time, + "received_delta": received_delta, + "lost_delta": lost_delta, + **msg_deltas # 展開訊息類型的變化 + }, + "msg_type_counts": sorted_msg_counts, + "prev_stats": { # 保存當前數據用於下次計算變化 + "received": stats.received_count, + "lost": stats.lost_count, + "msg_counts": dict(stats.msg_type_count) + }, + "InfoReady": True + } + # =============== 面板動作 - Serial Manager =============== def create_serial_port_object(self): @@ -1189,7 +1504,7 @@ class Orchestrator: self.panelState.udp_info_temp['IP'] = "127.0.0.1" self.panelState.udp_info_temp['Port'] = str(udp_port_tmp) self.panelState.udp_info_temp['direction'] = "inbound" - self.create_udp_object() + self.create_udp_object("SERIAL_XBEE") def main(): diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py index 86c0478..f59eb17 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py @@ -292,6 +292,8 @@ class VehicleView: 4. 可擴充 (支援 RF 模組狀態) """ + # TODO: connected_via 這個值可能用不到 之後可能要移除 不要用它再加功能了 + def __init__(self, sysid: int): # Meta 資訊 self.sysid = sysid