2026.6.3小更新
This commit is contained in:
+134
-113
@@ -8,149 +8,176 @@ class MissionState(Enum):
|
||||
ARMING = 1 # 解锁中
|
||||
TAKEOFF = 2 # 自动起飞中
|
||||
NAVIGATING = 3 # 航点巡航导航
|
||||
HOLD_TASK = 4 # 到达航点后的原地任务悬停
|
||||
RETURNING = 5 # 返航或直接准备降落
|
||||
LANDING = 6 # 自动降落中
|
||||
FINISHED = 7 # 任务结束/已上锁
|
||||
HOLD_FINAL = 8 # 结束巡点后原地悬停（当 return_to_home 为 False 时使用）
|
||||
HOLD_TASK = 4 # 到达航点后的原地搜索/悬停
|
||||
VISUAL_TRACKING = 5 # 🎯 视觉追踪中（由视觉主循环接管控制权）
|
||||
RETURNING = 6 # 返航中
|
||||
LANDING = 7 # 自动降落中
|
||||
FINISHED = 8 # 任务结束/已上锁
|
||||
HOLD_FINAL = 9 # 结束巡点后原地悬停（当 return_to_home 为 False 时使用）
|
||||
|
||||
|
||||
class FailsafeTriggered(Exception):
|
||||
"""自定义安全异常：当安全员人工介入切出自控模式时抛出，用于暴力熔断主循环"""
|
||||
pass
|
||||
|
||||
|
||||
class MissionManager:
|
||||
def __init__(self, drone, waypoints, target_altitude=1.5, arrival_radius=0.3, hold_duration=5.0,
|
||||
return_to_home=True):
|
||||
"""
|
||||
:param drone: 实例化的 DroneController 底层驱动对象
|
||||
:param waypoints: 航点列表，格式为 [{'x': 1.0, 'y': 2.0, 'z': -1.5, 'yaw': 0}, ... ]
|
||||
注意：NED 坐标系中，Z轴向上为负，所以起飞或目标高度应为负值
|
||||
:param target_altitude: 初始起飞高度（米，正数，内部会自动转为NED负值）
|
||||
:param arrival_radius: 判定航点到达的三维欧氏距离阈值（米）
|
||||
:param hold_duration: 在每个航点到达后，执行任务（或悬停）的停留时间（秒）
|
||||
:param return_to_home: 巡点结束后是否返回起始点降落。如果为 False，则在最后一个航点原地悬停。
|
||||
:param waypoints: 航点列表 [{'x': 1.0, 'y': 2.0, 'z': -1.5, 'yaw': 0}, ... ]
|
||||
:param target_altitude: 默认起飞高度（米，NED中向上为负值）
|
||||
:param arrival_radius: 航点到达判定半径（米）
|
||||
:param hold_duration: 抵达巡点后，没有发现目标时的最大搜寻/悬停时间（秒）
|
||||
:param return_to_home: 航点巡完后是否返航，False则在最后一个点原地悬停
|
||||
"""
|
||||
self.drone = drone
|
||||
self.waypoints = waypoints
|
||||
self.target_altitude = -abs(target_altitude) # 强制转为 NED 负值
|
||||
self.target_altitude = target_altitude if target_altitude < 0 else -target_altitude
|
||||
self.arrival_radius = arrival_radius
|
||||
self.hold_duration = hold_duration
|
||||
self.return_to_home = return_to_home
|
||||
|
||||
self.state = MissionState.INIT
|
||||
self.current_wp_index = 0
|
||||
self.hold_start_time = 0.0
|
||||
|
||||
# 记录起始点坐标，用于返航
|
||||
# 计时器定义
|
||||
self.hold_start_time = 0.0
|
||||
self.tracking_start_time = 0.0
|
||||
|
||||
# 起始点（Home）坐标缓存
|
||||
self.home_x = 0.0
|
||||
self.home_y = 0.0
|
||||
self.home_z = self.target_altitude
|
||||
self.home_z = 0.0
|
||||
|
||||
def get_distance(self, target_wp):
|
||||
"""计算当前无人机局部 NED 坐标到目标航点的三维欧氏距离"""
|
||||
curr_x, curr_y, curr_z = self.drone.local_position
|
||||
dx = target_wp['x'] - curr_x
|
||||
dy = target_wp['y'] - curr_y
|
||||
dz = target_wp['z'] - curr_z
|
||||
return math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
|
||||
|
||||
def update(self):
|
||||
def _is_tracking_task_complete(self):
|
||||
"""
|
||||
状态机核心主循环，需由上位机主程序以 5Hz~10Hz 的高频流式调用。
|
||||
包含链路维持、安全熔断以及新增的原地悬停/返航分支。
|
||||
🛠️ 预留接口：判断视觉追踪任务是否完成
|
||||
你可以根据需求修改内部逻辑。例如：
|
||||
- 当前：假设持续追踪 5 秒视为完成任务。
|
||||
- 后续扩展：可以引入与目标的物理距离、传感器状态等。
|
||||
"""
|
||||
# 1. 专属安全熔断机制：一旦检测到场外安全员切回手动模式，立即熔断
|
||||
if self.drone.current_mode not in ['GUIDED', 'OFFBOARD'] and self.state != MissionState.INIT:
|
||||
print(f"\n⚠️ [安全熔断] 检测到飞控退出自动控制模式(当前模式: {self.drone.current_mode})！上位机状态机退出。")
|
||||
self.state = MissionState.FINISHED
|
||||
return False
|
||||
tracking_duration = time.time() - self.tracking_start_time
|
||||
target_hold_time = 5.0 # 默认设定5秒
|
||||
|
||||
# 2. 状态机逻辑分支
|
||||
if tracking_duration >= target_hold_time:
|
||||
return True
|
||||
return False
|
||||
|
||||
def update(self, target_detected=False):
|
||||
"""
|
||||
状态机高频核心业务逻辑，由外部主循环驱动
|
||||
:param target_detected: 当前帧是否发现了视觉追踪目标
|
||||
"""
|
||||
# 0. 越权检查（最强人肉兜底）：只要切出自主控制模式，立刻暴力熔断
|
||||
if self.state not in [MissionState.INIT, MissionState.FINISHED]:
|
||||
if self.drone.current_mode not in ['GUIDED', 'OFFBOARD']:
|
||||
print(f"\n⚠️ [Failsafe] 检测到飞控模式被切为: {self.drone.current_mode}! 外部人工强行接管，熔断任务！")
|
||||
self.state = MissionState.FINISHED
|
||||
raise FailsafeTriggered("安全员强行接管")
|
||||
|
||||
# 1. INIT: 等待飞控连接并获取当前位置作为 Home 点
|
||||
if self.state == MissionState.INIT:
|
||||
if self.drone.is_connected:
|
||||
print("\n[通信成功] 无人机已连接，准备记录起始点并解锁...")
|
||||
# 记录解锁时的局部位置作为 Home 点
|
||||
hx, hy, _ = self.drone.local_position
|
||||
self.home_x = hx
|
||||
self.home_y = hy
|
||||
self.state = MissionState.ARMING
|
||||
else:
|
||||
print("\r[等待中] 等待飞控心跳连接...", end="", flush=True)
|
||||
print("[INIT] 飞控已连上。获取当前坐标为起始点...")
|
||||
self.home_x, self.home_y, self.home_z = self.drone.local_position
|
||||
print(f"[INIT] 起始点坐标: ({self.home_x:.2f}, {self.home_y:.2f}, {self.home_z:.2f})")
|
||||
print("[INIT] 正在下发 GUIDED/OFFBOARD 模式并尝试解锁...")
|
||||
|
||||
elif self.state == MissionState.ARMING:
|
||||
print("\r[指令下发] 正在尝试解锁电机...", end="", flush=True)
|
||||
if self.drone.is_armed:
|
||||
print("\n[成功] 电机已解锁！准备切入控制模式并起飞。")
|
||||
# 引导无人机切入 GUIDED 或 OFFBOARD
|
||||
target_mode = 'OFFBOARD' if 'PX4' in getattr(self.drone, 'firmware_type', 'PX4') else 'GUIDED'
|
||||
target_mode = 'GUIDED' if 'guided' in self.drone.current_mode.lower() or True else 'OFFBOARD'
|
||||
self.drone.set_mode(target_mode)
|
||||
self.state = MissionState.TAKEOFF
|
||||
else:
|
||||
self.drone.arm()
|
||||
time.sleep(0.5)
|
||||
self.state = MissionState.ARMING
|
||||
|
||||
# 2. ARMING: 监测解锁状态
|
||||
elif self.state == MissionState.ARMING:
|
||||
if self.drone.is_armed and self.drone.current_mode in ['GUIDED', 'OFFBOARD']:
|
||||
print("\n[解锁成功] 正在下发自动起飞指令...")
|
||||
# 下发位置到期望高度
|
||||
self.drone.send_target_position(self.home_x, self.home_y, self.target_altitude, 0)
|
||||
self.state = MissionState.TAKEOFF
|
||||
|
||||
# 3. TAKEOFF: 自动起飞爬升判定
|
||||
elif self.state == MissionState.TAKEOFF:
|
||||
# 虚拟/流式起飞：高频下发起飞期望高度，维持链路活性
|
||||
current_z = self.drone.local_position[2]
|
||||
# 持续发送起飞点位置，激活控制链路
|
||||
self.drone.send_target_position(self.home_x, self.home_y, self.target_altitude, 0)
|
||||
|
||||
# 判定是否到达起飞高度
|
||||
_, _, curr_z = self.drone.local_position
|
||||
if abs(curr_z - self.target_altitude) < self.arrival_radius:
|
||||
print(f"\n[完成] 已到达安全起飞高度: {abs(curr_z):.2f}m。开始巡航任务...")
|
||||
if len(self.waypoints) > 0:
|
||||
self.current_wp_index = 0
|
||||
self.state = MissionState.NAVIGATING
|
||||
else:
|
||||
print("⚠️ 警告: 航点列表为空！")
|
||||
if self.return_to_home:
|
||||
self.state = MissionState.RETURNING
|
||||
else:
|
||||
self.state = MissionState.HOLD_FINAL
|
||||
else:
|
||||
print(f"\r[起飞中] 当前高度: {abs(curr_z):.2f}m / 目标: {abs(self.target_altitude):.2f}m", end="",
|
||||
flush=True)
|
||||
# NED 坐标系下 Z 为负数，判断是否接近目标高度
|
||||
if abs(current_z - self.target_altitude) < self.arrival_radius:
|
||||
print("\n[起飞完成] 达到预定高度。开始前往第 1 个巡航航点...")
|
||||
self.state = MissionState.NAVIGATING
|
||||
self.current_wp_index = 0
|
||||
|
||||
# 4. NAVIGATING: 航点导航巡航
|
||||
elif self.state == MissionState.NAVIGATING:
|
||||
if not self.waypoints or self.current_wp_index >= len(self.waypoints):
|
||||
# 航点全部飞完
|
||||
if self.return_to_home:
|
||||
print("\n[巡航完成] 所有航点已巡视完毕。正在返回起始点上方...")
|
||||
self.state = MissionState.RETURNING
|
||||
else:
|
||||
print("\n[巡航完成] 所有航点已巡视完毕。根据配置切换至末端就地悬停...")
|
||||
self.state = MissionState.HOLD_FINAL
|
||||
return
|
||||
|
||||
# 获取当前目标航点
|
||||
wp = self.waypoints[self.current_wp_index]
|
||||
# “零期望命令”防失控刹车：流式高频持续推流当前目标点
|
||||
self.drone.send_target_position(wp['x'], wp['y'], wp['z'], wp['yaw'])
|
||||
|
||||
dist = self.get_distance(wp)
|
||||
print(f"\r[任务中] 正在飞往航点 [{self.current_wp_index + 1}] | 剩余距离: {dist:.2f}m", end="", flush=True)
|
||||
# 计算水平及高度距离
|
||||
dx = wp['x'] - self.drone.local_position[0]
|
||||
dy = wp['y'] - self.drone.local_position[1]
|
||||
dz = wp['z'] - self.drone.local_position[2]
|
||||
dist_xyz = math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
|
||||
|
||||
if dist < self.arrival_radius:
|
||||
print(f"\n[到达] 已精准触达航点 [{self.current_wp_index + 1}]，开启原地任务阶段。")
|
||||
self.hold_start_time = time.time()
|
||||
self.state = MissionState.HOLD_TASK
|
||||
|
||||
elif self.state == MissionState.HOLD_TASK:
|
||||
wp = self.waypoints[self.current_wp_index]
|
||||
# 悬停期间同样高频重发当前坐标，让飞控稳定在原地
|
||||
self.drone.send_target_position(wp['x'], wp['y'], wp['z'], wp['yaw'])
|
||||
|
||||
elapsed = time.time() - self.hold_start_time
|
||||
print(
|
||||
f"\r[任务中] 航点 [{self.current_wp_index + 1}] 原地悬停检测中... 已耗时: {elapsed:.1f}s / {self.hold_duration}s",
|
||||
f"\n\r[巡航中] 前往航点 [{self.current_wp_index + 1}/{len(self.waypoints)}] | 距离目标: {dist_xyz:.2f}m",
|
||||
end="", flush=True)
|
||||
|
||||
if elapsed >= self.hold_duration:
|
||||
print(f"\n[完成] 航点 [{self.current_wp_index + 1}] 任务执行完毕。")
|
||||
# 切换下一个点
|
||||
self.current_wp_index += 1
|
||||
if self.current_wp_index < len(self.waypoints):
|
||||
self.state = MissionState.NAVIGATING
|
||||
else:
|
||||
# 【核心修改点】所有航点巡视完毕，根据参数决定去向
|
||||
if self.return_to_home:
|
||||
print("\n[完成] 所有航点均已巡视完毕！准备返航起始点。")
|
||||
self.state = MissionState.RETURNING
|
||||
else:
|
||||
print("\n[完成] 所有航点均已巡视完毕！[设置] 不执行返航，就地保持悬停。")
|
||||
self.state = MissionState.HOLD_FINAL
|
||||
if dist_xyz < self.arrival_radius:
|
||||
print(f"\n[到达航点 {self.current_wp_index + 1}] 进入原地悬停与视觉检索阶段...")
|
||||
self.state = MissionState.HOLD_TASK
|
||||
self.hold_start_time = time.time()
|
||||
|
||||
# 5. HOLD_TASK: 抵达巡点后的搜索悬停阶段
|
||||
elif self.state == MissionState.HOLD_TASK:
|
||||
# 优先判定视觉：如果在巡点悬停搜寻期间，视觉发现了目标
|
||||
if target_detected:
|
||||
print("\n🎯 [目标锁定] 视觉检测到目标！立刻中断悬停，切入 [视觉追踪] 状态！")
|
||||
self.state = MissionState.VISUAL_TRACKING
|
||||
self.tracking_start_time = time.time()
|
||||
return
|
||||
|
||||
# 如果没有发现目标，则保持航点位置，并检查悬停是否超时
|
||||
wp = self.waypoints[self.current_wp_index]
|
||||
self.drone.send_target_position(wp['x'], wp['y'], wp['z'], wp['yaw'])
|
||||
|
||||
elapsed_time = time.time() - self.hold_start_time
|
||||
print(f"\r[检索中] 保持航点中... 剩余搜寻时间: {max(0.0, self.hold_duration - elapsed_time):.1f}s", end="",
|
||||
flush=True)
|
||||
|
||||
if elapsed_time >= self.hold_duration:
|
||||
print(f"\n[未发现目标] 搜寻超时。放弃当前点，前往下一个航点...")
|
||||
self.current_wp_index += 1
|
||||
self.state = MissionState.NAVIGATING
|
||||
|
||||
# 6. VISUAL_TRACKING: 🎯 视觉闭环追踪状态（核心新增）
|
||||
elif self.state == MissionState.VISUAL_TRACKING:
|
||||
# 在此状态下，高层状态机【不主动发送】绝对位置坐标，把控制权完全让渡给主循环的 PID 速度控制
|
||||
# 我们只需要在这里进行任务结束判定
|
||||
print(f"\r[追踪中] 正在执行AI视觉闭环精准追踪... 计时: {time.time() - self.tracking_start_time:.1f}s",
|
||||
end="", flush=True)
|
||||
|
||||
if self._is_tracking_task_complete():
|
||||
print("\n✅ [任务完成] 当前目标追踪任务圆满完成！重置追踪器，继续执行航点巡航...")
|
||||
self.current_wp_index += 1 # 切换到下一个航点
|
||||
self.state = MissionState.NAVIGATING
|
||||
|
||||
# 7. RETURNING: 自动返航至起始点上方
|
||||
elif self.state == MissionState.RETURNING:
|
||||
# 飞回记录的 Home 起始点高度位置
|
||||
self.drone.send_target_position(self.home_x, self.home_y, self.home_z, 0)
|
||||
self.drone.send_target_position(self.home_x, self.home_y, self.target_altitude, 0)
|
||||
|
||||
dist_to_home = math.sqrt((self.home_x - self.drone.local_position[0]) ** 2 +
|
||||
(self.home_y - self.drone.local_position[1]) ** 2)
|
||||
@@ -163,25 +190,19 @@ class MissionManager:
|
||||
else:
|
||||
print("⚠️ 降落模式切换失败，正在重新下发...")
|
||||
|
||||
# 8. LANDING: 自动降落中
|
||||
elif self.state == MissionState.LANDING:
|
||||
print(f"\r[降落中] 当前高度: {self.drone.local_position[2]:+.2f}m | 解锁状态: {self.drone.is_armed}",
|
||||
end="", flush=True)
|
||||
if not self.drone.is_armed:
|
||||
print("\n[完结] 无人机已安全着陆并自动上锁。整个任务流结束。")
|
||||
self.state = MissionState.FINISHED
|
||||
|
||||
# 9. HOLD_FINAL: 末端就地悬停
|
||||
elif self.state == MissionState.HOLD_FINAL:
|
||||
# 【新增状态】不返航时的原地最终悬停状态
|
||||
# 锁定在最后一个航点，持续高频推流，维持控制链路，防止 Failsafe 刹车
|
||||
final_wp = self.waypoints[-1] if self.waypoints else {'x': self.home_x, 'y': self.home_y,
|
||||
'z': self.target_altitude, 'yaw': 0}
|
||||
self.drone.send_target_position(final_wp['x'], final_wp['y'], final_wp['z'], final_wp['yaw'])
|
||||
print(
|
||||
f"\r[挂起] 无人机保持在最终航点悬停。坐标:({final_wp['x']}, {final_wp['y']}, {final_wp['z']}) | 等待人工接管中...",
|
||||
end="", flush=True)
|
||||
|
||||
elif self.state == MissionState.LANDING:
|
||||
# 判断是否已经安全着陆并自动上锁
|
||||
if not self.drone.is_armed:
|
||||
print("\n[完成] 无人机已安全着陆并自动上锁，任务圆满结束！")
|
||||
self.state = MissionState.FINISHED
|
||||
else:
|
||||
print("\r[降落中] 正在下降着陆中...", end="", flush=True)
|
||||
|
||||
elif self.state == MissionState.FINISHED:
|
||||
return False
|
||||
|
||||
return True
|
||||
end="", flush=True)
|
||||
Reference in New Issue
Block a user