songhaoyu/opi-yolo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

2026.7.1局部坐标模式被舍弃，现全部统一为经纬度

Browse Source
This commit is contained in:
songhaoyu
2026-07-01 11:47:32 +08:00
parent d98cfec930
commit dd4ce77e33
4 changed files with 219 additions and 247 deletions
Download Patch File Download Diff File Expand all files Collapse all files
drone_controller/mission_manager.py
+126 -183
View File
@@ -1,9 +1,10 @@
""" """
mission_manager.py — 基于 ZeroMQ RouterProxy 的航点巡航状态机 mission_manager.py — 基于 ZeroMQ RouterProxy 的航点巡航状态机
【重构背景】 【协议变更 v2 — 2026-07】
系统底层架构已升级为 ZMQ Router 代理通信协议。本模块彻底摒弃 SETPOINT 指令的 x/y 字段改为接收经纬度（lat/lon），z 字段接收
旧版 DroneController 直连方式，全面迁移至 RouterProxy。 相对高度（正值向上）。航点存储格式从 NED (x, y, z) 切换为
经纬度 (lat, lon, alt)。
【核心改动】 【核心改动】
1. self.drone → self.proxy（注入的 RouterProxy 实例） 1. self.drone → self.proxy（注入的 RouterProxy 实例）
@@ -13,10 +14,9 @@ mission_manager.py — 基于 ZeroMQ RouterProxy 的航点巡航状态机
5. 新增 Failsafe 模式断流保护，退出 OFFBOARD 即熔断 5. 新增 Failsafe 模式断流保护，退出 OFFBOARD 即熔断
6. VISUAL_TRACKING 状态下完全让渡控制权，不发送任何 SETPOINT 6. VISUAL_TRACKING 状态下完全让渡控制权，不发送任何 SETPOINT
【位置估计】 【位置判定】
Router 协议未暴露局部 NED 坐标，本模块使用速度积分 航点存储为经纬高 (lat, lon, alt)，使用 Haversine 公式计算
（vx × dt 累加）推算相对起飞点的水平位置，用于航点到 球面距离进行到达判定，不再依赖速度积分推算 NED 位置。
达判定。垂直轴直接使用 state["drone"]["alt_rel"]。
【控制权协调】 【控制权协调】
MissionManager 通过自有 proxy 发送指令。连续 20Hz SETPOINT MissionManager 通过自有 proxy 发送指令。连续 20Hz SETPOINT
@@ -26,10 +26,11 @@ mission_manager.py — 基于 ZeroMQ RouterProxy 的航点巡航状态机
""" """
import time import time
import math
import logging import logging
from enum import Enum from enum import Enum
from utils.coord import haversine, dist_3d_latlon
logger = logging.getLogger(__name__) logger = logging.getLogger(__name__)
@@ -56,22 +57,13 @@ class FailsafeTriggered(Exception):
pass pass
# ================================================================
# 内部工具
# ================================================================
def _dist_3d(x1, y1, z1, x2, y2, z2):
"""三维欧氏距离"""
return math.sqrt((x1 - x2) ** 2 + (y1 - y2) ** 2 + (z1 - z2) ** 2)
# ================================================================ # ================================================================
# 航点巡航管理器 # 航点巡航管理器
# ================================================================ # ================================================================
class MissionManager: class MissionManager:
""" """
🛸 ZMQ 航点巡航状态机管理器 🛸 ZMQ 航点巡航状态机管理器（经纬度协议 v2）
职责： 职责：
- 封装完整的无人机任务生命周期（初始化 → 解锁 → 起飞 → - 封装完整的无人机任务生命周期（初始化 → 解锁 → 起飞 →
@@ -80,6 +72,16 @@ class MissionManager:
- 在 VISUAL_TRACKING 状态下让渡控制权给 UAVControlLoop - 在 VISUAL_TRACKING 状态下让渡控制权给 UAVControlLoop
- Failsafe 熔断保护 - Failsafe 熔断保护
航点格式（经纬度协议 v2）：
{
"lat": 37.7749, # 纬度（度）
"lon": -122.4194, # 经度（度）
"alt": 1.5, # 相对起飞点高度（米，正值向上）
"yaw": 0.0, # 偏航角（度）
"command": "WAYPOINT", # TAKEOFF / LAND / WAYPOINT
"hold_duration": 8.0, # 悬停搜索等待时长（秒）
}
使用示例： 使用示例：
proxy = RouterProxy() proxy = RouterProxy()
proxy.start() proxy.start()
@@ -95,9 +97,6 @@ class MissionManager:
"OFFBOARD", "GUIDED", "AUTO", "HOLD", "LAND", "RTL", "OFFBOARD", "GUIDED", "AUTO", "HOLD", "LAND", "RTL",
}) })
# ---- 位置估计：速度积分有效时间窗口 ----
MAX_POSITION_DT = 1.0
# ---- 目标丢失超时（秒） ---- # ---- 目标丢失超时（秒） ----
DEFAULT_TARGET_LOST_TIMEOUT = 3.0 DEFAULT_TARGET_LOST_TIMEOUT = 3.0
@@ -112,8 +111,8 @@ class MissionManager:
""" """
Args: Args:
proxy: RouterProxy 实例（调用方负责 start/close） proxy: RouterProxy 实例（调用方负责 start/close）
waypoints: 航点列表 [{'x':1,'y':2,'z':-1.5,'yaw':0}, ...] waypoints: 航点列表（经纬度格式 v2）
target_altitude: 默认起飞高度（米，正值=向上） target_altitude: 默认起飞高度（米，正值向上）
arrival_radius: 航点到达判定半径（米） arrival_radius: 航点到达判定半径（米）
hold_duration: 巡点无目标的最大悬停搜索时间（秒） hold_duration: 巡点无目标的最大悬停搜索时间（秒）
return_to_home: 航点巡完后是否返航，False 则终点悬停 return_to_home: 航点巡完后是否返航，False 则终点悬停
@@ -123,20 +122,19 @@ class MissionManager:
self.proxy = proxy self.proxy = proxy
# ---- 任务参数 ---- # ---- 任务参数 ----
# 航点列表（外部队列已经使用 NED 坐标系，Z 负值向上） # 航点列表（经纬度协议 v2：[{lat, lon, alt, yaw, command}, ...]）
self.waypoints = list(waypoints) if waypoints else [] self.waypoints = list(waypoints) if waypoints else []
# 根据是否有航点决定初始状态 # 根据是否有航点决定初始状态
if self.waypoints: if self.waypoints:
# 有预置航点（如测试用硬编码），直接进入初始化
self.state = MissionState.INIT self.state = MissionState.INIT
self._waypoints_ready = True self._waypoints_ready = True
logger.info("MissionManager 初始化完成，已有 %d 个航点", len(self.waypoints)) logger.info("MissionManager 初始化完成，已有 %d 个航点", len(self.waypoints))
else: else:
# 无航点，进入等待状态（等待 QGC 下发）
self.state = MissionState.WAITING_WAYPOINTS self.state = MissionState.WAITING_WAYPOINTS
self._waypoints_ready = False self._waypoints_ready = False
logger.info("MissionManager 初始化完成，等待 QGC 下发航点...") logger.info("MissionManager 初始化完成，等待 QGC 下发航点...")
self.target_altitude = -abs(target_altitude) # NED Z（负值向上） # v2: 目标高度正值向上（原 NED 负值协议已废除）
self.target_altitude = abs(target_altitude)
self.arrival_radius = arrival_radius self.arrival_radius = arrival_radius
self.hold_duration = hold_duration self.hold_duration = hold_duration
self.return_to_home = return_to_home self.return_to_home = return_to_home
@@ -150,18 +148,10 @@ class MissionManager:
# ---- 当前目标航点（共享给外部主循环 / UAVControlLoop） ---- # ---- 当前目标航点（共享给外部主循环 / UAVControlLoop） ----
self.current_target = { self.current_target = {
"x": 0.0, "y": 0.0, "lat": 0.0, "lon": 0.0,
"z": self.target_altitude, "yaw": 0.0, "alt": self.target_altitude, "yaw": 0.0,
} }
# ================================================================
# 本地位置估计（速度积分法）
# ================================================================
self._est_x = 0.0 # NED 北向（相对起飞点）
self._est_y = 0.0 # NED 东向
self._pos_time = 0.0 # 上次位置更新时间戳
self._home_set = False # 首次 STATE 标记
# ---- 计时器 ---- # ---- 计时器 ----
self.hold_start_time = 0.0 self.hold_start_time = 0.0
self.tracking_start_time = 0.0 self.tracking_start_time = 0.0
@@ -171,20 +161,18 @@ class MissionManager:
# ================================================================ # ================================================================
# 追踪完成信号（外部设置） # 追踪完成信号（外部设置）
# ================================================================ # ================================================================
# UAVControlLoop 或 run_mission.py 在判定视觉追踪完成时置 True
self.tracking_completed = False self.tracking_completed = False
# 追踪最大时长兜底
self.tracking_max_duration = 10.0 self.tracking_max_duration = 10.0
logger.info("MissionManager 初始化: %d 个航点, 目标高度=%.1fm, " logger.info("MissionManager 初始化: %d 个航点, 目标高度=%.1fm, "
"到达半径=%.1fm, 搜寻时长=%.1fs, 返航=%s", "到达半径=%.1fm, 搜寻时长=%.1fs, 返航=%s",
len(self.waypoints), -self.target_altitude, len(self.waypoints), self.target_altitude,
self.arrival_radius, self.hold_duration, self.arrival_radius, self.hold_duration,
self.return_to_home) self.return_to_home)
# ---- 日志节流控制 ---- # ---- 日志节流控制 ----
self._last_throttle_time = {} # 节流日志: key → 上次打印时间戳 self._last_throttle_time = {}
self._last_throttle_state = {} # 状态缓存对比: key → 上次打印的值 self._last_throttle_state = {}
# ================================================================ # ================================================================
# 主入口 # 主入口
@@ -248,12 +236,7 @@ class MissionManager:
return return
# --------------------------------------------------------------- # ---------------------------------------------------------------
# 步骤 1：更新本地位置估计 # 状态机分支（位置估计已移除 v2 — 使用经纬度距离计算）
# ---------------------------------------------------------------
self._update_local_position(state)
# ---------------------------------------------------------------
# 步骤 2：状态机分支
# --------------------------------------------------------------- # ---------------------------------------------------------------
try: try:
if self.state == MissionState.INIT: if self.state == MissionState.INIT:
@@ -281,38 +264,13 @@ class MissionManager:
exc_info=True) exc_info=True)
# ================================================================ # ================================================================
# 位置估计 # 辅助方法
# ================================================================ # ================================================================
def _update_local_position(self, state): def _get_current_alt(self, state):
"""通过速度积分更新本地 NED 位置估计""" """从 STATE 获取当前相对高度（正值向上）"""
try: try:
drone = state["drone"] return state["drone"].get("alt_rel", 0.0)
now = time.time()
if not self._home_set:
self._est_x = 0.0
self._est_y = 0.0
self._pos_time = now
self._home_set = True
logger.info("[位置] 初始位置记录完成")
dt = now - self._pos_time
if 0 < dt < self.MAX_POSITION_DT:
vx = drone.get("vx", 0.0)
vy = drone.get("vy", 0.0)
self._est_x += vx * dt
self._est_y += vy * dt
# dt ≥ MAX_POSITION_DT 时跳过积分防跳变
self._pos_time = now
except (KeyError, TypeError) as e:
logger.warning("[位置] 更新异常: %s", e)
def _get_current_z(self, state):
"""从 STATE 获取当前 NED Z（负值向上）"""
try:
return -state["drone"].get("alt_rel", 0.0)
except (KeyError, TypeError): except (KeyError, TypeError):
return self.target_altitude return self.target_altitude
@@ -356,7 +314,8 @@ class MissionManager:
alt_rel = drone.get("alt_rel", 0.0) alt_rel = drone.get("alt_rel", 0.0)
home = state.get("home", {}) home = state.get("home", {})
self._throttle_log("init_state",5.0,"[INIT] mode=%s armed=%s alt_rel=%.1fm",mode, armed, alt_rel) self._throttle_log("init_state", 5.0, "[INIT] mode=%s armed=%s alt_rel=%.1fm",
mode, armed, alt_rel)
if not home.get("valid", False): if not home.get("valid", False):
self._throttle_log("home_valid_init", 5.0, self._throttle_log("home_valid_init", 5.0,
@@ -418,13 +377,12 @@ class MissionManager:
return return
# ---- 解锁成功 → 起飞 ---- # ---- 解锁成功 → 起飞 ----
takeoff_alt = -self.target_altitude # 正值（协议 altitude 为正） logger.info("\n[解锁成功] 发送起飞指令 (alt=%.1fm)", self.target_altitude)
logger.info("\n[解锁成功] 发送起飞指令 (alt=%.1fm)", takeoff_alt)
ok, ack = self.proxy.send_waypoint( ok, ack = self.proxy.send_waypoint(
lat=state["home"]["lat"], lat=state["home"]["lat"],
lon=state["home"]["lon"], lon=state["home"]["lon"],
alt=takeoff_alt, alt=self.target_altitude,
alt_frame="RELATIVE", alt_frame="RELATIVE",
speed=3.0, action="TAKEOFF", speed=3.0, action="TAKEOFF",
yaw=state["drone"]["yaw"], yaw=state["drone"]["yaw"],
@@ -436,8 +394,10 @@ class MissionManager:
self._state_start_time = time.time() self._state_start_time = time.time()
else: else:
logger.warning("[TAKEOFF] WAYPOINT 失败: %s，尝试 SETPOINT 爬升", ack) logger.warning("[TAKEOFF] WAYPOINT 失败: %s，尝试 SETPOINT 爬升", ack)
drone = state.get("drone", {})
ok2, _ = self.proxy.send_setpoint( ok2, _ = self.proxy.send_setpoint(
x=0, y=0, z=self.target_altitude, yaw=0.0, lat=drone.get("lat", 0.0), lon=drone.get("lon", 0.0),
z=self.target_altitude, yaw=0.0,
control_mode="POSITION", control_mode="POSITION",
) )
if ok2: if ok2:
@@ -457,7 +417,7 @@ class MissionManager:
流程（协议 §3.3.1）： 流程（协议 §3.3.1）：
① 等待高度到达目标 90% 且 mode == "HOLD" ① 等待高度到达目标 90% 且 mode == "HOLD"
② 发送预热 SETPOINT（当前位置悬停） ② 发送预热 SETPOINT（当前位置经纬度悬停）
③ time.sleep(0.2) 等待流建立 ③ time.sleep(0.2) 等待流建立
④ 发送 COMMAND("OFFBOARD") ④ 发送 COMMAND("OFFBOARD")
⑤ 轮询等待 mode == "OFFBOARD" ⑤ 轮询等待 mode == "OFFBOARD"
@@ -465,20 +425,18 @@ class MissionManager:
""" """
try: try:
drone = state["drone"] drone = state["drone"]
home = state.get("home", {})
alt_rel = drone.get("alt_rel", 0.0) alt_rel = drone.get("alt_rel", 0.0)
mode = drone.get("mode", "") mode = drone.get("mode", "")
target_up = -self.target_altitude # 正值
# ---- ① 等待 TAKEOFF 完成 ---- # ---- ① 等待 TAKEOFF 完成 ----
# 协议要求：高度到达目标 90% 且模式为 HOLD reached_alt = alt_rel >= self.target_altitude * 0.9
reached_alt = alt_rel >= target_up * 0.9
in_hold = mode == "HOLD" in_hold = mode == "HOLD"
if not (reached_alt and in_hold): if not (reached_alt and in_hold):
# 节流输出等待状态
self._throttle_log("takeoff_wait", 3.0, self._throttle_log("takeoff_wait", 3.0,
"[起飞中] alt=%.1f/%.1f mode=%s 耗时=%.1fs", "[起飞中] alt=%.1f/%.1f mode=%s 耗时=%.1fs",
alt_rel, target_up, mode, alt_rel, self.target_altitude, mode,
time.time() - self._state_start_time) time.time() - self._state_start_time)
return return
@@ -486,9 +444,10 @@ class MissionManager:
logger.info("[TAKEOFF] 高度 %.1fm，模式 %s，准备切换 OFFBOARD...", logger.info("[TAKEOFF] 高度 %.1fm，模式 %s，准备切换 OFFBOARD...",
alt_rel, mode) alt_rel, mode)
# 先预热悬停（协议 §3.3.1 ③→④） # 发送预热 SETPOINT（v2: x=经度, y=纬度, z=相对高度）
self.proxy.send_setpoint( self.proxy.send_setpoint(
x=0, y=0, z=-alt_rel, yaw=0.0, lat=drone.get("lat", 0.0), lon=drone.get("lon", 0.0),
z=alt_rel, yaw=0.0,
control_mode="POSITION", control_mode="POSITION",
) )
time.sleep(0.2) time.sleep(0.2)
@@ -497,7 +456,6 @@ class MissionManager:
ok, ack = self.proxy.send_command("OFFBOARD") ok, ack = self.proxy.send_command("OFFBOARD")
if not ok: if not ok:
logger.error("[TAKEOFF] OFFBOARD 指令失败: %s", ack) logger.error("[TAKEOFF] OFFBOARD 指令失败: %s", ack)
# 超时后强行进入巡航（降级）
if time.time() - self._state_start_time > self.TAKEOFF_TIMEOUT: if time.time() - self._state_start_time > self.TAKEOFF_TIMEOUT:
logger.warning("[TAKEOFF] 超时，强行进入巡航") logger.warning("[TAKEOFF] 超时，强行进入巡航")
self.state = MissionState.NAVIGATING self.state = MissionState.NAVIGATING
@@ -516,7 +474,6 @@ class MissionManager:
return return
time.sleep(0.1) time.sleep(0.1)
# 超时
logger.warning("[TAKEOFF] OFFBOARD 切换超时，强行进入巡航") logger.warning("[TAKEOFF] OFFBOARD 切换超时，强行进入巡航")
self.state = MissionState.NAVIGATING self.state = MissionState.NAVIGATING
self.current_wp_index = 0 self.current_wp_index = 0
@@ -531,57 +488,52 @@ class MissionManager:
""" """
[NAVIGATING] 每帧发送 POSITION SETPOINT 前往当前航点 [NAVIGATING] 每帧发送 POSITION SETPOINT 前往当前航点
到达判定后推进索引；所有航点飞完后根据 return_to_home v2: send_setpoint(x=lat, y=lon, z=alt)，到达判定使用 Haversine 距离。
决定返航或悬停。 到达判定后推进索引；所有航点飞完后根据 return_to_home 决定返航或悬停。
""" """
if not self.waypoints or self.current_wp_index >= len(self.waypoints): if not self.waypoints or self.current_wp_index >= len(self.waypoints):
self._finish_navigation() self._finish_navigation(state)
return return
wp = self.waypoints[self.current_wp_index] wp = self.waypoints[self.current_wp_index]
cmd = wp.get("command", "WAYPOINT") cmd = wp.get("command", "WAYPOINT")
# 特殊航点分支选择 # ---- 起飞航点 ----
# 起飞航点
if cmd == "TAKEOFF": if cmd == "TAKEOFF":
# 飞到原点 + 目标高度 # 起飞已完成，直接推进到下一个航点
self.proxy.send_setpoint(
x=0, y=0, z=wp.get("z", self.target_altitude),
yaw=wp.get("yaw", 0.0),control_mode="POSITION",
)
current_z = self._get_current_z(state)
target_z = wp.get("z", self.target_altitude)
if abs(current_z - target_z) < 0.5:
self.current_wp_index += 1 self.current_wp_index += 1
return return
# 降落航点 # ---- 降落航点 ----
if cmd == "LAND": if cmd == "LAND":
logger.info("🛬 执行降落") logger.info("🛬 执行降落")
self.proxy.send_command("LAND") self.proxy.send_command("LAND")
self.state = MissionState.LANDING self.state = MissionState.LANDING
return return
# ---- 普通航点 ----
wp_x = wp["x"] wp_lat = wp["lat"]
wp_y = wp["y"] wp_lon = wp["lon"]
wp_z = wp.get("z", self.target_altitude) wp_alt = wp.get("alt", self.target_altitude)
wp_yaw = wp.get("yaw", 0.0) wp_yaw = wp.get("yaw", 0.0)
# ---- 发送 POSITION SETPOINT（通过自有 proxy） ---- # v2: 发送 POSITION SETPOINT（经纬度 + 高度）
# 此 SETPOINT 让 PX4 飞向目标航点
self.proxy.send_setpoint( self.proxy.send_setpoint(
x=wp_x, y=wp_y, z=wp_z, yaw=wp_yaw, lat=wp_lat, lon=wp_lon, z=wp_alt, yaw=wp_yaw,
control_mode="POSITION", control_mode="POSITION",
) )
# ---- 共享目标 ---- # ---- 共享目标 ----
self.current_target = {"x": wp_x, "y": wp_y, "z": wp_z, "yaw": wp_yaw} self.current_target = {"lat": wp_lat, "lon": wp_lon,
"alt": wp_alt, "yaw": wp_yaw}
# ---- 到达判定 ---- # ---- 到达判定（Haversine 球面距离） ----
current_z = self._get_current_z(state) drone = state["drone"]
dist = _dist_3d(self._est_x, self._est_y, current_z, drone_lat = drone.get("lat", 0.0)
wp_x, wp_y, wp_z) drone_lon = drone.get("lon", 0.0)
current_alt = self._get_current_alt(state)
dist = dist_3d_latlon(drone_lat, drone_lon, current_alt,
wp_lat, wp_lon, wp_alt)
if dist < self.arrival_radius: if dist < self.arrival_radius:
logger.info("\n✅ [到达航点 %d/%d] dist=%.2fm", logger.info("\n✅ [到达航点 %d/%d] dist=%.2fm",
@@ -589,19 +541,25 @@ class MissionManager:
self.state = MissionState.HOLD_TASK self.state = MissionState.HOLD_TASK
self.hold_start_time = time.time() self.hold_start_time = time.time()
else: else:
# 节流：每 5 秒输出一次巡航进度，不刷屏
self._throttle_log("navigating", 5.0, self._throttle_log("navigating", 5.0,
"[巡航中] 航点 [%d/%d] 距离=%.2fm", "[巡航中] 航点 [%d/%d] 距离=%.2fm",
self.current_wp_index + 1, self.current_wp_index + 1,
len(self.waypoints), dist) len(self.waypoints), dist)
def _finish_navigation(self): def _finish_navigation(self, state):
"""所有航点飞完后的跳转""" """所有航点飞完后的跳转"""
home = state.get("home", {}) if state else {}
home_lat = home.get("lat", 0.0)
home_lon = home.get("lon", 0.0)
if self.return_to_home: if self.return_to_home:
logger.info("\n[巡航完成] 返回起始点...") logger.info("\n[巡航完成] 返回起始点 (lat=%.6f lon=%.6f)...",
home_lat, home_lon)
self.state = MissionState.RETURNING self.state = MissionState.RETURNING
self.current_target = {"x": 0, "y": 0, self.current_target = {
"z": self.target_altitude, "yaw": 0} "lat": home_lat, "lon": home_lon,
"alt": self.target_altitude, "yaw": 0,
}
else: else:
logger.info("\n[巡航完成] 末端悬停...") logger.info("\n[巡航完成] 末端悬停...")
self.state = MissionState.HOLD_FINAL self.state = MissionState.HOLD_FINAL
@@ -627,17 +585,17 @@ class MissionManager:
self.tracking_completed = False self.tracking_completed = False
return return
# ---- 保持当前位置悬停 ---- # ---- 保持当前位置悬停（v2: x=lat, y=lon, z=alt） ----
wp = self.waypoints[self.current_wp_index] wp = self.waypoints[self.current_wp_index]
self.proxy.send_setpoint( self.proxy.send_setpoint(
x=wp["x"], y=wp["y"], lat=wp["lat"], lon=wp["lon"],
z=wp.get("z", self.target_altitude), z=wp.get("alt", self.target_altitude),
yaw=wp.get("yaw", 0), yaw=wp.get("yaw", 0),
control_mode="POSITION", control_mode="POSITION",
) )
self.current_target = { self.current_target = {
"x": wp["x"], "y": wp["y"], "lat": wp["lat"], "lon": wp["lon"],
"z": wp.get("z", self.target_altitude), "alt": wp.get("alt", self.target_altitude),
"yaw": wp.get("yaw", 0), "yaw": wp.get("yaw", 0),
} }
@@ -649,7 +607,6 @@ class MissionManager:
self.current_wp_index += 1 self.current_wp_index += 1
self.state = MissionState.NAVIGATING self.state = MissionState.NAVIGATING
else: else:
# 静默悬停检索，不刷屏；仅每 3 秒轻量心跳
self._throttle_log("hold_task", 3.0, self._throttle_log("hold_task", 3.0,
"[检索中] 航点 %d 剩余 %.1fs", "[检索中] 航点 %d 剩余 %.1fs",
self.current_wp_index + 1, self.current_wp_index + 1,
@@ -661,20 +618,15 @@ class MissionManager:
""" """
[VISUAL_TRACKING] 🎯 完全让渡控制权给 UAVControlLoop [VISUAL_TRACKING] 🎯 完全让渡控制权给 UAVControlLoop
【核心设计】
MissionManager 在此状态下 **不发送任何 SETPOINT**。 MissionManager 在此状态下 **不发送任何 SETPOINT**。
UAVControlLoop (20Hz 独立线程) 通过其自有 proxy 发送 UAVControlLoop (20Hz 独立线程) 通过其自有 proxy 发送
VELOCITY SETPOINT 进行视觉闭环追踪。 VELOCITY SETPOINT 进行视觉闭环追踪。
本模块仅负责追踪退出条件的判定和状态切换。 退出条件：
【退出条件】
1. self.tracking_completed == True（外部模块设置） 1. self.tracking_completed == True（外部模块设置）
2. 目标丢失超过 target_lost_timeout 秒 2. 目标丢失超过 target_lost_timeout 秒
3. 追踪总时长超过 tracking_max_duration（兜底） 3. 追踪总时长超过 tracking_max_duration（兜底）
""" """
# ---- 不发送任何 SETPOINT — 控制权已让渡 ----
tracking_elapsed = time.time() - self.tracking_start_time tracking_elapsed = time.time() - self.tracking_start_time
# ---- 兜底超时 ---- # ---- 兜底超时 ----
@@ -703,20 +655,17 @@ class MissionManager:
lost_elapsed) lost_elapsed)
self._exit_tracking() self._exit_tracking()
return return
# 节流：每 2 秒输出一次丢失状态（不刷屏）
self._throttle_log("track_lost", 2.0, self._throttle_log("track_lost", 2.0,
"[追踪中] 丢失 %.1fs/%.1fs", "[追踪中] 丢失 %.1fs/%.1fs",
lost_elapsed if self._target_lost_start lost_elapsed if self._target_lost_start
else 0.0, else 0.0,
self.target_lost_timeout) self.target_lost_timeout)
else: else:
# 目标重新出现时，单次日志（事件驱动）
self._throttle_log_on_change("track_lost_clear", self._throttle_log_on_change("track_lost_clear",
self._target_lost_start is not None, self._target_lost_start is not None,
"[追踪中] 目标恢复") "[追踪中] 目标恢复")
self._target_lost_start = None self._target_lost_start = None
# 节流：每 5 秒输出一次追踪心跳，20Hz 静默
self._throttle_log("track_heartbeat", 5.0, self._throttle_log("track_heartbeat", 5.0,
"[追踪中] 进行中... %.1fs", tracking_elapsed) "[追踪中] 进行中... %.1fs", tracking_elapsed)
@@ -732,19 +681,33 @@ class MissionManager:
def _handle_returning(self, state): def _handle_returning(self, state):
""" """
[RETURNING] 发送 POSITION SETPOINT 到起始点 (0,0,z) [RETURNING] 发送 POSITION SETPOINT 到 HOME 点
v2: send_setpoint(x=home_lat, y=home_lon, z=target_alt)
到达判定使用 Haversine 球面距离。
""" """
home = state.get("home", {})
drone = state.get("drone", {})
home_lat = home.get("lat", 0.0)
home_lon = home.get("lon", 0.0)
# v2: x=纬度, y=经度, z=相对高度
self.proxy.send_setpoint( self.proxy.send_setpoint(
x=0, y=0, z=self.target_altitude, yaw=0, lat=home_lat, lon=home_lon, z=self.target_altitude, yaw=0,
control_mode="POSITION", control_mode="POSITION",
) )
self.current_target = {"x": 0, "y": 0, self.current_target = {
"z": self.target_altitude, "yaw": 0} "lat": home_lat, "lon": home_lon,
"alt": self.target_altitude, "yaw": 0,
}
# ---- Haversine 距离到达判定 ----
current_alt = self._get_current_alt(state)
drone_lat = drone.get("lat", 0.0)
drone_lon = drone.get("lon", 0.0)
dist = dist_3d_latlon(drone_lat, drone_lon, current_alt,
home_lat, home_lon, self.target_altitude)
current_z = self._get_current_z(state)
dist = _dist_3d(self._est_x, self._est_y, current_z,
0.0, 0.0, self.target_altitude)
# 节流：每 3 秒输出一次返航距离，不刷屏
self._throttle_log("returning", 3.0, self._throttle_log("returning", 3.0,
"[返航中] 距离=%.2fm", dist) "[返航中] 距离=%.2fm", dist)
@@ -759,13 +722,10 @@ class MissionManager:
# ---- 降落 ---- # ---- 降落 ----
def _handle_landing(self, state): def _handle_landing(self, state):
""" """[LANDING] 监控 disarm 状态判断着陆"""
[LANDING] 监控 disarm 状态判断着陆
"""
try: try:
armed = state["drone"].get("armed", False) armed = state["drone"].get("armed", False)
alt_rel = state["drone"].get("alt_rel", 0.0) alt_rel = state["drone"].get("alt_rel", 0.0)
# 节流：每 3 秒输出一次高度，不刷屏
self._throttle_log("landing", 3.0, self._throttle_log("landing", 3.0,
"[降落中] 高度=%.1fm 已解锁=%s", "[降落中] 高度=%.1fm 已解锁=%s",
alt_rel, not armed) alt_rel, not armed)
@@ -781,29 +741,30 @@ class MissionManager:
def _handle_hold_final(self, state): def _handle_hold_final(self, state):
""" """
[HOLD_FINAL] 末端悬停，保持最终航点位置 [HOLD_FINAL] 末端悬停，保持最终航点位置
v2: x=lat, y=lon, z=alt
""" """
if self.waypoints: if self.waypoints:
final_wp = self.waypoints[-1] final_wp = self.waypoints[-1]
else: else:
final_wp = {"x": 0, "y": 0, final_wp = {"lat": 0.0, "lon": 0.0,
"z": self.target_altitude, "yaw": 0} "alt": self.target_altitude, "yaw": 0}
self.proxy.send_setpoint( self.proxy.send_setpoint(
x=final_wp["x"], y=final_wp["y"], lat=final_wp["lat"], lon=final_wp["lon"],
z=final_wp.get("z", self.target_altitude), z=final_wp.get("alt", self.target_altitude),
yaw=final_wp.get("yaw", 0), yaw=final_wp.get("yaw", 0),
control_mode="POSITION", control_mode="POSITION",
) )
self.current_target = { self.current_target = {
"x": final_wp["x"], "y": final_wp["y"], "lat": final_wp["lat"], "lon": final_wp["lon"],
"z": final_wp.get("z", self.target_altitude), "alt": final_wp.get("alt", self.target_altitude),
"yaw": final_wp.get("yaw", 0), "yaw": final_wp.get("yaw", 0),
} }
# 节流：每 10 秒输出一次末端悬停状态，不刷屏
self._throttle_log("hold_final", 10.0, self._throttle_log("hold_final", 10.0,
"[挂起] 保持 (%.1f, %.1f, %.1f) 等待接管", "[挂起] 保持 (%.6f, %.6f, %.1f) 等待接管",
final_wp["x"], final_wp["y"], final_wp["lat"], final_wp["lon"],
final_wp.get("z", self.target_altitude)) final_wp.get("alt", self.target_altitude))
# ================================================================ # ================================================================
# 辅助 # 辅助
@@ -814,49 +775,31 @@ class MissionManager:
if self.waypoints and index < len(self.waypoints): if self.waypoints and index < len(self.waypoints):
wp = self.waypoints[index] wp = self.waypoints[index]
self.current_target = { self.current_target = {
"x": wp["x"], "y": wp["y"], "lat": wp["lat"], "lon": wp["lon"],
"z": wp.get("z", self.target_altitude), "alt": wp.get("alt", self.target_altitude),
"yaw": wp.get("yaw", 0), "yaw": wp.get("yaw", 0),
} }
def set_waypoints(self, waypoints): def set_waypoints(self, waypoints):
"""由外部调用，动态注入/更新航点列表""" """由外部调用，动态注入/更新航点列表（经纬度格式 v2）"""
self.waypoints = list(waypoints) self.waypoints = list(waypoints)
self.current_wp_index = 0 self.current_wp_index = 0
self._waypoints_ready = True # 这个标志位结合 WAITING_WAYPOINTS 状态使用 self._waypoints_ready = True
logger.info("MissionManager 航点已更新，共 %d 个", len(waypoints)) logger.info("MissionManager 航点已更新，共 %d 个", len(waypoints))
# ================================================================ # ================================================================
# 🚨 紧急停靠（外部调用） # 🚨 紧急停靠（外部调用）
# ================================================================ # ================================================================
def emergency_stop(self): def emergency_stop(self):
""" """外部触发的紧急降落入口（可在任何状态下调用）"""
外部触发的紧急降落入口（可在任何状态下调用）
行为：
1. 发送 LAND 指令到飞控
2. 将状态机切换到 LANDING 状态
3. 记录警告日志（供事后分析）
注意：
- 此方法可在任何线程中调用（内部使用 proxy，proxy 本身是线程安全的）
- 调用后，主循环会检测到 state == LANDING 并执行降落逻辑
- 如果已经在 LANDING 状态，重复调用只会重新发送 LAND 指令
"""
logger.warning("🚨 [emergency_stop] 紧急降落触发！") logger.warning("🚨 [emergency_stop] 紧急降落触发！")
# 1. 发送 LAND 指令（不管当前状态如何，直接发）
ok, ack = self.proxy.send_command("LAND") ok, ack = self.proxy.send_command("LAND")
if ok: if ok:
logger.info("✅ LAND 指令已发送") logger.info("✅ LAND 指令已发送")
else: else:
logger.error(f"❌ LAND 指令发送失败: {ack}") logger.error(f"❌ LAND 指令发送失败: {ack}")
# 2. 状态机强制切换到 LANDING（让主循环接管后续降落逻辑）
self.state = MissionState.LANDING self.state = MissionState.LANDING
# 3. 重置一些计时器，让降落逻辑从当前状态开始
self._state_start_time = time.time() self._state_start_time = time.time()
drone_controller/router_proxy.py
+1 -1
View File
@@ -173,7 +173,7 @@ class RouterProxy:
位置控制: 位置控制:
proxy.send_setpoint(control_mode="POSITION", proxy.send_setpoint(control_mode="POSITION",
x=10.0, y=0.0, z=-8.0, yaw=0.0) lat=22.543210, lon=113.887650, z=-8.0, yaw=0.0)
速度控制: 速度控制:
proxy.send_setpoint(control_mode="VELOCITY", proxy.send_setpoint(control_mode="VELOCITY",
vx=1.0, vy=0.0, vz=0.0, yaw_rate=0.0) vx=1.0, vy=0.0, vz=0.0, yaw_rate=0.0)
run_mission.py
+28 -41
View File
@@ -60,7 +60,7 @@ from drone_controller.router_proxy import RouterProxy
# 注意：此处不修改 UAVControlLoop 任何内部逻辑。 # 注意：此处不修改 UAVControlLoop 任何内部逻辑。
# run_mission.py 在外部协调其控制线程的启停。 # run_mission.py 在外部协调其控制线程的启停。
from infer_camera_modular import UAVControlLoop, VideoStreaming, YOLO26UAVInfer from infer_camera_modular import UAVControlLoop, VideoStreaming, YOLO26UAVInfer
from utils.coord import latlon_to_ned # 注：coord.latlon_to_ned 不再需要 — 航点直接存储原始经纬度
logger = logging.getLogger("RunMission") logger = logging.getLogger("RunMission")
@@ -88,7 +88,7 @@ class MissionOrchestrator:
self.streamer = VideoStreaming(cfg["video"], cfg["network"]) self.streamer = VideoStreaming(cfg["video"], cfg["network"])
self.detector = YOLO26UAVInfer(cfg["model"]) self.detector = YOLO26UAVInfer(cfg["model"])
# ---- 航线定义（NED 坐标系，Z 向上为负） ---- # ---- 航线定义（经纬度坐标系，Z 向上为负） ----
self.waypoints = waypoints self.waypoints = waypoints
# ================================================================ # ================================================================
@@ -292,7 +292,12 @@ class MissionOrchestrator:
def _on_qgc_waypoints(self, data: dict): def _on_qgc_waypoints(self, data: dict):
""" """
收到 QGC 下发的航点列表 -> 转换坐标系 -> 注入 MissionManager 收到 QGC 下发的航点列表 → 直接存储原始经纬度 → 注入 MissionManager
【协议变更 v2】
航点格式已从 NED (x, y, z) 切换为经纬度 (lat, lon, alt)。
send_setpoint 的 x=纬度, y=经度, z=相对高度（正值向上）。
因此不再调用 latlon_to_ned 转换，直接存储原始数据。
""" """
try: try:
raw_wps = data.get("waypoints", []) raw_wps = data.get("waypoints", [])
@@ -300,81 +305,63 @@ class MissionOrchestrator:
self.logger.warning("QGC 航点列表为空，请检查是否提交或路由问题") self.logger.warning("QGC 航点列表为空，请检查是否提交或路由问题")
return return
self.logger.info("📥 收到 %d 个航点，开始坐标转换...", len(raw_wps)) self.logger.info("📥 收到 %d 个航点，直接存储经纬度", len(raw_wps))
# ---- ① 获取参考点（起飞点） ---- # ---- ① 获取 STATE 用于后续校验（不再需要参考原点） ----
state = self.proxy.get_latest_state() state = self.proxy.get_latest_state()
if state is None: if state is None:
self.logger.error("无法获取 STATE，缺少参考经纬度，航点转换失败") self.logger.warning("无法获取 STATE，航点仍将加载但 HOME 校验跳过")
return return
home = state.get("home", {}) # ---- ② 逐点存储（原始经纬度，不做 NED 转换） ----
ref_lat = home.get("lat", 0.0)
ref_lon = home.get("lon", 0.0)
ref_alt = home.get("alt", 0.0)
# ---- 容错：如果 HOME 未初始化，用第一个航点作为参考（纯相对飞行） ----
if ref_lat == 0.0 and ref_lon == 0.0:
self.logger.warning("STATE.home 未初始化，使用第一个航点作为参考原点")
ref_lat = raw_wps[0].get("lat", 0.0)
ref_lon = raw_wps[0].get("lon", 0.0)
ref_alt = raw_wps[0].get("z", 0.0)
self.logger.info("参考原点 (起飞点): lat=%.6f, lon=%.6f, alt=%.1fm",
ref_lat, ref_lon, ref_alt)
# ---- ② 逐点转换 ----
converted = [] converted = []
for idx, wp in enumerate(raw_wps): for idx, wp in enumerate(raw_wps):
lat = wp.get("lat", 0.0) lat = wp.get("lat", 0.0)
lon = wp.get("lon", 0.0) lon = wp.get("lon", 0.0)
alt = wp.get("z", 0.0) # QGC 中 z 为海拔 alt = wp.get("z", 0.0) # QGC 中 z 为相对高度（正值向上）
cmd = wp.get("command", 16) # 16=WAYPOINT, 22=TAKEOFF, 21=LAND cmd = wp.get("command", 16) # 16=WAYPOINT, 22=TAKEOFF, 21=LAND
yaw = wp.get("param4", 0.0) # param4 通常存偏航角 yaw = wp.get("param4", 0.0) # param4 通常存偏航角
hold_time = wp.get("param1", 8.0) # 悬停时间（如有） hold_time = wp.get("param1", 8.0) # 悬停时间（如有）
x, y, z = latlon_to_ned(lat, lon, alt, ref_lat, ref_lon, ref_alt)
# print(f"航点信息；{x}{y}{z}")
# 根据 MAV_CMD 类型构建内部航点 # 根据 MAV_CMD 类型构建内部航点
if cmd == 22: # TAKEOFF if cmd == 22: # TAKEOFF
# 起飞点强制归零水平位置，高度取绝对海拔差
converted.append({ converted.append({
"x": 0.0, "lat": lat,
"y": 0.0, "lon": lon,
"z": z, # 已按 NED 转换（向上为负） "alt": alt,
"yaw": yaw, "yaw": yaw,
"command": "TAKEOFF", "command": "TAKEOFF",
"hold_duration": hold_time, "hold_duration": hold_time,
}) })
self.logger.info("航点 %d [TAKEOFF]: 高度 %.1fm", idx, -z) self.logger.info("航点 %d [TAKEOFF]: lat=%.6f lon=%.6f alt=%.1fm",
idx, lat, lon, alt)
elif cmd == 21: # LAND elif cmd == 21: # LAND
converted.append({ converted.append({
"x": x, "lat": lat,
"y": y, "lon": lon,
"z": 0.0, # 着陆点 z=0（地面） "alt": alt,
"yaw": yaw, "yaw": yaw,
"command": "LAND", "command": "LAND",
}) })
self.logger.info("航点 %d [LAND]: NED(%.1f, %.1f)", idx, x, y) self.logger.info("航点 %d [LAND]: lat=%.6f lon=%.6f", idx, lat, lon)
else: # 普通航点 (command=16 或未识别) else: # 普通航点 (command=16 或未识别)
converted.append({ converted.append({
"x": x, "lat": lat,
"y": y, "lon": lon,
"z": z, # NED 高度 "alt": alt,
"yaw": yaw, "yaw": yaw,
"command": "WAYPOINT", "command": "WAYPOINT",
"hold_duration": hold_time, "hold_duration": hold_time,
}) })
self.logger.debug("航点 %d [WAYPOINT]: NED(%.1f, %.1f, %.1f)", self.logger.debug("航点 %d [WAYPOINT]: lat=%.6f lon=%.6f alt=%.1f",
idx, x, y, z) idx, lat, lon, alt)
self.mission.set_waypoints(converted) self.mission.set_waypoints(converted)
# 如果当前处于等待航点状态，状态机会在下一帧自动响应 # 如果当前处于等待航点状态，状态机会在下一帧自动响应
self.logger.info("✅ 航点转换完成，共 %d 个航点已加载", len(converted)) self.logger.info("✅ 航点加载完成，共 %d 个航点（原始经纬度格式）", len(converted))
except Exception as e: except Exception as e:
self.logger.error("航点处理异常: %s", e) self.logger.error("航点处理异常: %s", e)
utils/coord.py
+44 -2
View File
@@ -1,5 +1,9 @@
import math import math
# WGS-84 赤道半径（米）
EARTH_RADIUS = 6378137.0
def latlon_to_ned(lat, lon, alt, ref_lat, ref_lon, ref_alt): def latlon_to_ned(lat, lon, alt, ref_lat, ref_lon, ref_alt):
""" """
将 WGS-84 经纬高 (LLA) 转换为局部 NED 坐标 (x, y, z) 将 WGS-84 经纬高 (LLA) 转换为局部 NED 坐标 (x, y, z)
@@ -14,8 +18,6 @@ def latlon_to_ned(lat, lon, alt, ref_lat, ref_lon, ref_alt):
y: 东向距离 (正东为正) y: 东向距离 (正东为正)
z: 向下距离 (向下为正，即向上为负) z: 向下距离 (向下为正，即向上为负)
""" """
# WGS-84 椭球参数（赤道半径，单位：米）
EARTH_RADIUS = 6378137.0
# 1. 纬度差 -> 北向距离 (x) # 1. 纬度差 -> 北向距离 (x)
dlat = math.radians(lat - ref_lat) dlat = math.radians(lat - ref_lat)
x = dlat * EARTH_RADIUS x = dlat * EARTH_RADIUS
@@ -29,3 +31,43 @@ def latlon_to_ned(lat, lon, alt, ref_lat, ref_lon, ref_alt):
z = -(alt - ref_alt) z = -(alt - ref_alt)
return x, y, z return x, y, z
def haversine(lat1, lon1, lat2, lon2):
"""
使用 Haversine 公式计算两个经纬度点之间的球面距离（米）
Args:
lat1, lon1: 起点纬度、经度（度）
lat2, lon2: 终点纬度、经度（度）
Returns:
水平距离（米）
"""
dlat = math.radians(lat2 - lat1)
dlon = math.radians(lon2 - lon1)
a = (math.sin(dlat / 2) ** 2 +
math.cos(math.radians(lat1)) *
math.cos(math.radians(lat2)) *
math.sin(dlon / 2) ** 2)
c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
return EARTH_RADIUS * c
def dist_3d_latlon(lat1, lon1, alt1, lat2, lon2, alt2):
"""
计算两个经纬高坐标之间的 3D 距离（米）
水平距离使用 Haversine 公式，垂直距离使用高度差。
此处的 alt 为相对高度（正值向上），与 SETPOINT 协议一致。
Args:
lat1, lon1, alt1: 起点纬度(度)、经度(度)、相对高度(米)
lat2, lon2, alt2: 终点纬度(度)、经度(度)、相对高度(米)
Returns:
3D 距离（米）
"""
horizontal = haversine(lat1, lon1, lat2, lon2)
vertical = alt1 - alt2
return math.sqrt(horizontal ** 2 + vertical ** 2)