|
|
|
@ -602,7 +602,7 @@ class DroneMonitor(Node):
|
|
|
|
# ================================================================================
|
|
|
|
# ================================================================================
|
|
|
|
|
|
|
|
|
|
|
|
async def set_mode(self, drone_id, mode_name):
|
|
|
|
async def set_mode(self, drone_id, mode_name):
|
|
|
|
"""使用 CommandLongClient 切換無人機飛行模式"""
|
|
|
|
"""使用 CommandLongClient 切換無人機飛行模式(使用非阻塞的 async 方法)"""
|
|
|
|
try:
|
|
|
|
try:
|
|
|
|
# 解析 drone_id 提取 sysid
|
|
|
|
# 解析 drone_id 提取 sysid
|
|
|
|
parts = drone_id.split('_')
|
|
|
|
parts = drone_id.split('_')
|
|
|
|
@ -619,30 +619,24 @@ class DroneMonitor(Node):
|
|
|
|
|
|
|
|
|
|
|
|
print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
|
|
|
|
print(f"\n📢 [SET_MODE] {drone_id} → {mode_name} (custom_mode={custom_mode})")
|
|
|
|
|
|
|
|
|
|
|
|
# 在線程池中調用 CommandLongClient
|
|
|
|
if not self.command_long_client:
|
|
|
|
loop = asyncio.get_event_loop()
|
|
|
|
print(f"❌ [SET_MODE] CommandLongClient 未初始化")
|
|
|
|
executor = ThreadPoolExecutor(max_workers=1)
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
|
|
def _call_set_mode():
|
|
|
|
# 直接調用 async 方法,無需線程池(避免嵌套執行器衝突)
|
|
|
|
client = CommandLongClient() if CommandLongClient else None
|
|
|
|
result = await self.command_long_client.change_mode_async(
|
|
|
|
if not client:
|
|
|
|
target_sysid=sysid,
|
|
|
|
return False
|
|
|
|
custom_mode=float(custom_mode),
|
|
|
|
result = client.change_mode(
|
|
|
|
target_compid=0,
|
|
|
|
target_sysid=sysid,
|
|
|
|
base_mode=1.0,
|
|
|
|
custom_mode=float(custom_mode),
|
|
|
|
timeout_sec=2.0,
|
|
|
|
target_compid=0,
|
|
|
|
)
|
|
|
|
base_mode=1.0,
|
|
|
|
|
|
|
|
timeout_sec=2.0,
|
|
|
|
if result and result.success:
|
|
|
|
)
|
|
|
|
|
|
|
|
client.destroy_node()
|
|
|
|
|
|
|
|
return result.success if result else False
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
result = await loop.run_in_executor(executor, _call_set_mode)
|
|
|
|
|
|
|
|
if result:
|
|
|
|
|
|
|
|
print(f"✅ [SET_MODE] 模式切換成功")
|
|
|
|
print(f"✅ [SET_MODE] 模式切換成功")
|
|
|
|
return True
|
|
|
|
return True
|
|
|
|
else:
|
|
|
|
else:
|
|
|
|
print(f"❌ [SET_MODE] 模式切換失敗")
|
|
|
|
print(f"❌ [SET_MODE] 模式切換失敗 (message={result.message if result else 'None'})")
|
|
|
|
return False
|
|
|
|
return False
|
|
|
|
except Exception as e:
|
|
|
|
except Exception as e:
|
|
|
|
print(f"❌ [SET_MODE] 例外錯誤: {e}")
|
|
|
|
print(f"❌ [SET_MODE] 例外錯誤: {e}")
|
|
|
|
|