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