|
|
|
|
@ -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)
|
|
|
|
|
@ -663,15 +659,6 @@ class async_io_manager:
|
|
|
|
|
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"""
|
|
|
|
|
# 一個防呆 確保有 event loop 與 _main_task 正在運作
|
|
|
|
|
@ -681,7 +668,10 @@ class async_io_manager:
|
|
|
|
|
|
|
|
|
|
socket_id = mavlink_obj.socket_id
|
|
|
|
|
|
|
|
|
|
if socket_id in self.managed_objects:
|
|
|
|
|
# 檢查該對象是否已經在運行中
|
|
|
|
|
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
|
|
|
|
|
|
|
|
|
|
@ -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 無效變數移除
|
|
|
|
|
|
|
|
|
|
'''
|
|
|
|
|
|
|
|
|
|
|
