123 lines
4.9 KiB
Python
123 lines
4.9 KiB
Python
import time
|
||
from pymavlink import mavutil
|
||
from pymavlink.dialects.v20.all import MAVLink_sys_status_message
|
||
|
||
from agent.device import controller
|
||
from agent.tools.base_tool import BaseTool, ToolResult
|
||
|
||
|
||
class Tool(BaseTool):
|
||
name = "uav_self_check"
|
||
description = ("对无人机设备进行使用前自检")
|
||
parameters = {}
|
||
|
||
def execute(self, **kwargs) -> ToolResult:
|
||
"""
|
||
MAVLink 飞控系统健康自检
|
||
解决：Arming denied: Resolve system health failures first
|
||
"""
|
||
# 等待健康数据
|
||
|
||
assert controller.connect(), "连接应成功"
|
||
try:
|
||
connection = controller.adapter.get_connection()
|
||
check_list = []
|
||
|
||
health = connection.wait_heartbeat(timeout=5)
|
||
if not health:
|
||
return ToolResult(success=False, output="❌ 无法获取飞控状态，连接异常")
|
||
|
||
# 2. 获取系统健康状态（关键：判断解锁失败原因）
|
||
check_list.append("\n===== 系统健康自检结果 =====")
|
||
system_status = health.system_status
|
||
check_list.append(f"系统状态码：{system_status}")
|
||
|
||
# 系统状态解释
|
||
status_map = {
|
||
0: "未初始化",
|
||
1: "初始化中",
|
||
2: "测试模式",
|
||
3: "待机",
|
||
4: "活动",
|
||
5: "临界",
|
||
6: "紧急",
|
||
7: "故障"
|
||
}
|
||
check_list.append(f"系统状态：{status_map.get(system_status, '未知')}")
|
||
# 3. 读取详细传感器健康（核心自检）
|
||
check_list.append("\n===== 传感器健康检测 =====")
|
||
for _ in range(30):
|
||
msg = connection.recv_match(type="SYS_STATUS", blocking=True)
|
||
if msg:
|
||
# 传感器是否存在
|
||
sensors_present = msg.onboard_control_sensors_present
|
||
sensors_enable = msg.onboard_control_sensors_enabled
|
||
|
||
# 传感器列表
|
||
sensor_list = [
|
||
("3D陀螺仪", 0),
|
||
("3D加速度计", 1),
|
||
("3D磁力计", 2),
|
||
("气压计", 3),
|
||
("GPS", 4),
|
||
("光学流量", 5),
|
||
("视觉定位", 6),
|
||
("测距传感器", 7),
|
||
("电机控制", 8),
|
||
("电池", 9),
|
||
("遥控信号", 10),
|
||
("飞行控制", 11),
|
||
("避障系统", 12),
|
||
("云台", 13),
|
||
]
|
||
|
||
# 逐个检测
|
||
for name, bit in sensor_list:
|
||
present = (sensors_present >> bit) & 1
|
||
enable = (sensors_enable >> bit) & 1
|
||
|
||
if present:
|
||
if not enable:
|
||
check_list.append(f"❌ {name}：故障")
|
||
else:
|
||
check_list.append(f"✅ {name}：正常")
|
||
else:
|
||
check_list.append(f"ℹ️ {name}：未安装")
|
||
break
|
||
time.sleep(0.1)
|
||
|
||
# 4. 解锁能力判断（解决 Arming denied）
|
||
check_list.append("\n===== 解锁能力检测 =====")
|
||
can_arm = False
|
||
for _ in range(20):
|
||
msg = connection.recv_match(type="HEARTBEAT", blocking=True)
|
||
if msg:
|
||
base_mode = msg.base_mode
|
||
custom_mode = msg.custom_mode
|
||
|
||
# 0b10000000 = 已解锁
|
||
if (base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0:
|
||
check_list.append("✅ 当前状态：已解锁")
|
||
can_arm = True
|
||
else:
|
||
check_list.append("⚠️ 当前状态：未解锁")
|
||
|
||
# 判断是否禁止解锁
|
||
if system_status in [5, 6, 7]:
|
||
check_list.append("❌ 禁止解锁原因：系统处于临界/紧急/故障状态")
|
||
else:
|
||
check_list.append("✅ 系统允许解锁，可执行 arm 指令")
|
||
can_arm = True
|
||
break
|
||
|
||
print("\n===== 自检完成 =====")
|
||
if can_arm:
|
||
check_list.append("🎉 结论：无健康故障，可以执行解锁（arm）")
|
||
else:
|
||
check_list.append("⚠️ 结论：存在系统故障，必须修复后才能 arm")
|
||
|
||
return ToolResult(success=True, output="\n".join(check_list))
|
||
except Exception as e:
|
||
return ToolResult(success=False, output=str(e))
|
||
finally:
|
||
controller.disconnect() |