From fa0a2d08315e527037599f89ce28814e68084f4c Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Fri, 16 Jan 2026 12:58:23 +0800 Subject: [PATCH] =?UTF-8?q?(Tested)=20=E6=96=B0=E5=A2=9E=E5=88=AA=E9=99=A4?= =?UTF-8?q?=E8=BC=89=E5=85=B7=E5=8A=9F=E8=83=BD=20=E4=BB=A3=E7=A2=BC?= =?UTF-8?q?=E5=84=AA=E5=8C=96=20=E8=A9=B3=E8=A6=8B=E6=94=B9=E7=89=88?= =?UTF-8?q?=E8=A8=98=E9=8C=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../fc_network_adapter/fc_network_adapter.md | 28 ++++++---- .../fc_network_adapter/mainOrchestrator.py | 45 ++++++++++++---- .../fc_network_adapter/mavlinkObject.py | 51 ++++++++----------- .../fc_network_adapter/mavlinkVehicleView.py | 28 +++++++--- 4 files changed, 98 insertions(+), 54 deletions(-) diff --git a/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md index 98dba3a..ace76ac 100644 --- a/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md +++ b/src/fc_network_adapter/fc_network_adapter/fc_network_adapter.md @@ -84,16 +84,20 @@ 唯一實例 實際去解析 mavlink 封包的地方 接收 stream_bridge_ring 與 return_packet_ring 的資料 + 這邊是比較偏自動化 不會被操作的 - *self.thread* 自己的執行緒 --- - *_run_thread()* 核心方法 - *_handle_XXXXX()* 每一種單項 mavlink 封包的解析 +- *send_message()* 是 _send_to_socket() 的高階包裝 跟 ros2 介面做互動的方法 +- *_send_to_socket()* 把要傳送的封包 丟給 mavlink 去處理 ### **[Class]** async_io_manager 唯一實例 異步 event loop - 管理 mavlink_object 的地方 沒有核心方法 + 這邊主要是管理 mavlink_object 的地方 (但不會對於某個 mavlink_object 內部需求做操作) + - *self.thread* 自己的執行緒 - *self.managed_objects* 資料結構 socket_id: mavlink_object --- @@ -110,7 +114,7 @@ --- - *process_data()* [async method] 核心方法 - *remove_target_socket()* *add_target_socket()* -- *send_message()* +- *message_put_queue()* 把要傳送的封包放到自己這個物件的暫存區 會由 process_data() 依照異步流程被實際丟出 ## serialManager.py 看這個檔案的重點再於要搞清楚 端口物件 還是 傳輸物件 @@ -120,7 +124,6 @@ 管理 mavlink_object 的地方 - *self.thread* 自己的執行緒 - *self.loop* 自己的事件迴圈 -- *self.serial_objects* 存放管理的物件 serial id num : serial_object --- - *create_serial_link()* [call method] 把 serial 端口跟 UDP 端口打通 - *_async_create_serial_link()* [async method] 把兩種端口接起來的重點程序 @@ -157,10 +160,15 @@ ## 已實現功能 1. mavlink 分流解析 2. mavlink socket 建立 -3. mavlink socket 轉拋 -4. 連結 Serial 轉 UDP -5. 各單元模組化 -6. 終端機介面控制 -7. 基礎載具流量觀測 - - +3. mavlink socket 轉拋 proxy +4. 建立 Serial 轉 UDP 連結 並管理 +5. 建立 serial 連線 +6. 各單元模組化 +7. 終端機介面控制 +8. 基礎載具流量觀測 +9. 載具狀態收集與彙整 + +### 待開發功能 +5-1. 建立 serial 連線 並可以對接收器下達AT指令 +5-2. 模組化 serial 連線機制 以利後期擴容其他模組 +10-1. ros2 應用開發介面 \ No newline at end of file diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index f77d1ad..c7b573f 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -28,7 +28,7 @@ from .utils import acquireSerial, acquirePort from .utils.acquirePort import find_available_port logger = setup_logger(os.path.basename(__file__)) -VERSION_NO = "v0.56" +VERSION_NO = "v0.58" class PanelState: def __init__(self): @@ -1088,12 +1088,17 @@ class ControlPanel: # 確保跳過顯示區域 line = line + 6 - dialog_win.addstr(dialog_height - 2, 2, "Press any key to return... | Auto-refresh: 1.0s") + dialog_win.addstr(dialog_height - 2, 2, "Press R: Reset Stats, C: Unregister, other key to return...") dialog_win.refresh() # 檢查是否有按鍵（非阻塞） ch = dialog_win.getch() - if ch != -1: # 有按鍵則退出 + if ch in (ord('R'), ord('r')): + cmd_q.put(("RESET_VEHICLE_STATISTICS", sysid, compid)) + elif ch in (ord('C'), ord('c')): + cmd_q.put(("UNREGISTER_VEHICLE", sysid)) + break + elif ch != -1: # 有按鍵則退出 break # 短暫延遲 @@ -1197,7 +1202,7 @@ class Orchestrator: self.remove_target_from_object(source_id, target_id) elif action == "INSPECT_MAV_OBJECT": socket_id = cmd[1] - mav_obj = self.manager.managed_objects.get(socket_id, None) + mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id, None) if mav_obj: self.panelState.socket_info_single["socket_type"] = mav_obj.socket_type self.panelState.socket_info_single["socket_state"] = mav_obj.state.name @@ -1223,9 +1228,17 @@ class Orchestrator: 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 action == "UPDATE_VEHICLES_LIST": # TODO 這個擺這邊 不知道為何可以有作用 先不動 也許後面有bug? + # logger.debug("Orchestrator: Updating vehicles list upon request") + # self._update_vehicles_list() + elif action == "RESET_VEHICLE_STATISTICS": + sysid, compid = cmd[1], cmd[2] + vehicle_sys = self.vehicle_registry.get(sysid) + vehicle_sys.reset_component_stats(compid) + elif action == "UNREGISTER_VEHICLE": + sysid = cmd[1] + self.vehicle_registry.unregister(sysid) + elif cmd == "CREATE_UDP_INBOUND": self.panelState.udp_info_temp["direction"] = "inbound" self.create_udp_object() @@ -1240,12 +1253,12 @@ class Orchestrator: self.manager.shutdown() elif cmd == "SHUTDOWN_SERIAL_MANAGER": self.plumber.shutdown() + except queue.Empty: pass except Exception as e: logger.error(f"Error processing command: {e}") - time.sleep(0.1) except KeyboardInterrupt: @@ -1321,6 +1334,14 @@ class Orchestrator: def delete_udp_object(self, socket_id): """移除指定的 mavlink_object""" + + mavlink_obj = mo.mavlink_object.mavlinkObjects[socket_id] + connection_string = mavlink_obj.mavlink_socket.address + strings = connection_string.split(':') + ip = strings[0] + port = int(strings[1]) + self.occupied_ip_ports[ip].remove(port) + self.manager.remove_mavlink_object(socket_id) def add_target_to_object(self, source_id, target_id): @@ -1414,7 +1435,7 @@ class Orchestrator: # 更新 vehicle_info_single socket_type = "N/A" - socket_obj = self.manager.managed_objects.get(socket_id, None) + socket_obj =mo.mavlink_object.mavlinkObjects.get(socket_id, None) if socket_obj: socket_type = socket_obj.socket_type @@ -1534,5 +1555,11 @@ if __name__ == "__main__": 4. 修改 避免 serial 與 ip port 重複建立相同的通道 5. 修改 show_object_info 與 show_linked_serial_info 改變檢核 Ready 方式 避免卡死 +2025.01.16 +1. 新增 "移除載具" 與 "重置載具統計" 功能 +2. 修正 udp port 在移除後仍被記錄為佔用的問題 +3. 因應 mvalinkObject.py 中 mavlinkObjects 修正變數存取方式 +4. 註解掉無效代碼 action == "UPDATE_VEHICLES_LIST" 區塊 + ''' diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 7b321f1..82dccce 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -323,7 +323,7 @@ class mavlink_bridge: return False mav_obj = mavlink_object.mavlinkObjects[socket_id] - return mav_obj.send_message(message_bytes) + return mav_obj.message_put_queue(message_bytes) # 定義 mavlink_object 的狀態 class MavlinkObjectState(Enum): @@ -501,7 +501,7 @@ class mavlink_object: logger.error(f"Invalid return message types: {msg_types}") return False - def send_message(self, message_bytes): + def message_put_queue(self, message_bytes): """ 從主線程向此 mavlink_object 的 socket 發送數據 將數據添加到簡單的列表中，由 asyncio 任務處理 @@ -568,9 +568,7 @@ class async_io_manager: self.loop = None self.running = False # self.main_task = None - self.managed_objects = {} # socket_id: mavlink_object self.thread = None - self._stop_event = threading.Event() def __del__(self): self.loop = None @@ -586,7 +584,6 @@ class async_io_manager: return self.running = True - self._stop_event.clear() # 啟動獨立線程 命名為 AsyncIOManager self.thread = threading.Thread( @@ -618,12 +615,11 @@ class async_io_manager: return # 停止所有被管理的 mavlink_object 所屬的 task - for socket_id in list(self.managed_objects.keys()): + for socket_id in list(mavlink_object.mavlinkObjects.keys()): self.remove_mavlink_object(socket_id) # 停止自己的 task self.running = False - self._stop_event.set() # 解開事件循環的阻塞 self.loop.call_soon_threadsafe(self.loop.stop) @@ -662,15 +658,6 @@ class async_io_manager: self.loop = None self.running = False logger.info("async_io_manager event loop END!") - - # async def _main_task(self): # 當初想說可能要一個額外的 task 來管理 但是目前好像用不掉 先放著不管 - # """主任務協程 讓 async_io_manager 在執行緒中持續運作""" - # logger.info("async_io_manager main task started") - - # while self.running and not self._stop_event.is_set(): - # await asyncio.sleep(0.1) - - # logger.info("async_io_manager main task ended") def add_mavlink_object(self, mavlink_obj: mavlink_object): """添加 mavlink_object""" @@ -681,9 +668,12 @@ class async_io_manager: socket_id = mavlink_obj.socket_id - if socket_id in self.managed_objects: - logger.warning(f"mavlink_object {socket_id} already managed") - return False + # 檢查該對象是否已經在運行中 + if socket_id in mavlink_object.mavlinkObjects: + existing_obj = mavlink_object.mavlinkObjects[socket_id] + if existing_obj.state == MavlinkObjectState.RUNNING: + logger.warning(f"mavlink_object {socket_id} already managed") + return False # 使用 run_coroutine_threadsafe 執行協程並獲取結果 future = asyncio.run_coroutine_threadsafe( @@ -708,7 +698,6 @@ class async_io_manager: try: task = asyncio.create_task(mavlink_obj.process_data()) - self.managed_objects[socket_id] = mavlink_obj mavlink_obj.task = task mavlink_obj.state = MavlinkObjectState.RUNNING mavlink_obj.outgoing_msgs.clear() @@ -743,11 +732,11 @@ class async_io_manager: async def _async_remove_mavlink_object(self, socket_id): """在事件循環線程中同步執行""" - if socket_id not in self.managed_objects: - logger.warning(f"mavlink_object {socket_id} not managed") - return + if socket_id not in mavlink_object.mavlinkObjects: + logger.warning(f"mavlink_object {socket_id} not found") + return False - mavlink_obj = self.managed_objects[socket_id] + mavlink_obj = mavlink_object.mavlinkObjects[socket_id] mavlink_obj.state = MavlinkObjectState.SHUTTINGDOWN if not mavlink_obj.task.done(): @@ -761,9 +750,8 @@ class async_io_manager: break await asyncio.sleep(0.1) - # 如果正常結束 則移除 + # 如果正常結束 則設置為關閉狀態（物件的清理由 __del__ 處理） if mavlink_obj.task.done(): - del self.managed_objects[socket_id] mavlink_obj.state = MavlinkObjectState.CLOSED logger.info(f"Removed mavlink_object {socket_id} from manager.") return True @@ -773,8 +761,9 @@ class async_io_manager: return False def get_managed_objects(self): - """獲取所有被管理的對象列表""" - return list(self.managed_objects.keys()) + """獲取所有被管理的對象列表（狀態為 RUNNING 的對象）""" + return [socket_id for socket_id, obj in mavlink_object.mavlinkObjects.items() + if obj.state == MavlinkObjectState.RUNNING] # ====================== 分割線 ===================== @@ -807,9 +796,13 @@ if __name__ == '__main__': 3. mavlink_bridge 的主要迴圈 增加 send_message 功能 可指定目標 sysid 或 socket_id 發送 mavlink 封包 4. async_io_manager 循環邏輯大改動 優化 mavlink_object 加入與移除的邏輯 並使得 task 與 evenlt loop 分層更清楚 5. mavlink_object 移除不必要的 start 與 stop 方法 由 async_io_manager 統一管理其生命週期 -6. mavlink_object 優化 send_message 方法 避免無效判斷 與 增加一些防呆檢驗 並與 mavlink_bridge 連動工作 +6. mavlink_object 優化 message_put_queue 方法 避免無效判斷 與 增加一些防呆檢驗 並與 mavlink_bridge 連動工作 7. 移除迴圈內的 try except 堆疊 增加效能 8. 移除對於 mavlinkDevice 的依賴 改用 vehicle_registry 來管理所有的載具 +2026年 01月 15日 +1. async_io_manager.managed_objects 與 mavlink_object.mavlinkObjects 功能重複整合 保留 mavlink_object.mavlinkObjects +2. async_io_manager 的 _stop_event 無效變數移除 + ''' diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py index f59eb17..aeae274 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py @@ -222,15 +222,15 @@ class VehicleComponent: def reset_packet_stats(self) -> None: """重置封包統計""" self.packet_stats = PacketStats() - + def set_parameter(self, param_name: str, param_value: Any) -> None: """設定參數 (手動餵入)""" self.parameters[param_name] = param_value - + def get_parameter(self, param_name: str, default: Any = None) -> Any: """取得參數""" return self.parameters.get(param_name, default) - + def __str__(self) -> str: return (f"Component(id={self.component_id}, type={self.type.value}, " f"mav_type={self.mav_type}, received={self.packet_stats.received_count}, " @@ -323,7 +323,7 @@ class VehicleView: """ if component_id not in self.components: self.components[component_id] = VehicleComponent(component_id, comp_type) - logger.info(f"Added component {component_id} to system {self.sysid}") + # logger.info(f"Added component {component_id} to system {self.sysid}") return self.components[component_id] def get_component(self, component_id: int) -> Optional[VehicleComponent]: @@ -334,10 +334,17 @@ class VehicleView: """移除組件""" if component_id in self.components: del self.components[component_id] - logger.info(f"Removed component {component_id} from system {self.sysid}") + # logger.info(f"Removed component {component_id} from system {self.sysid}") return True return False - + + def reset_component_stats(self, component_id: int) -> None: + """重置指定組件的封包統計""" + component = self.get_component(component_id) + if component: + component.reset_packet_stats() + # logger.info(f"Reset packet stats for component {component_id} in system {self.sysid}") + def set_rf_module(self, rf_type: RFModuleType) -> RFModule: """設定RF模組""" self.rf_module = RFModule(rf_type) @@ -435,3 +442,12 @@ class VehicleViewRegistry: # 全域註冊表實例 vehicle_registry = VehicleViewRegistry() + +''' +================= 改版記錄 ============================ + +2026.01.16 +1. 新增 重置指定組件的封包統計 功能 + +''' +