chiyu1468
/
AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Tested

Browse Source 
(modify) 1. 新增已連線載具的基礎資訊功能
chiyu
Chiyu Chen 4 months ago
parent e00880c6de
commit f8f5ff5a15
2 changed files with 332 additions and 15 deletions
Show Stats Download Patch File Download Diff File

345
src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
Unescape Escape View File

@ -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():

2
src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py
Unescape Escape View File

@ -292,6 +292,8 @@ class VehicleView:
4. 可擴充 (支援 RF 模組狀態)
"""
# TODO: connected_via 這個值可能用不到 之後可能要移除 不要用它再加功能了
def __init__(self, sysid: int):
# Meta 資訊
self.sysid = sysid

Write Preview
Loading…
Cancel
Save