diff --git a/config.json b/config.json index 3b19808..6b04e5b 100644 --- a/config.json +++ b/config.json @@ -38,7 +38,12 @@ "arrival_radius": 0.3, "hold_duration": 8.0, "return_to_home": false, - "target_lost_timeout": null + "target_lost_timeout": null, + "max_speed": 1.2, + "landing": { + "pre_hover_duration": 3.0, + "descent_speed": 0.3 + } }, "optical_flow":{ "grid_step":25, diff --git a/drone_controller/mission_manager.py b/drone_controller/mission_manager.py index ce182e0..7150324 100644 --- a/drone_controller/mission_manager.py +++ b/drone_controller/mission_manager.py @@ -25,6 +25,7 @@ mission_manager.py — 基于 ZeroMQ RouterProxy 的航点巡航状态机 SETPOINT 流同时激活产生冲突。 """ +import math import time import logging from enum import Enum @@ -47,6 +48,7 @@ class MissionState(Enum): LANDING = 7 # 自动降落中 FINISHED = 8 # 任务结束 / 已上锁 HOLD_FINAL = 9 # 结束巡点后原地悬停（return_to_home=False） + PRE_LAND_HOVER = 10 # 🛬 降落前悬停（到达降落点后等待片刻再下降） class FailsafeTriggered(Exception): @@ -118,16 +120,21 @@ class MissionManager: def __init__(self, proxy, waypoints, target_altitude=1.5, arrival_radius=0.3, hold_duration=5.0, - return_to_home=True, target_lost_timeout=None): + return_to_home=True, target_lost_timeout=None, + max_speed=1.2, pre_land_hover_duration=3.0, + landing_descent_speed=0.3): """ Args: - proxy: RouterProxy 实例（调用方负责 start/close） - waypoints: 航点列表（经纬度格式 v2） - target_altitude: 默认起飞高度（米，正值向上） - arrival_radius: 航点到达判定半径（米） - hold_duration: 巡点无目标的最大悬停搜索时间（秒） - return_to_home: 航点巡完后是否返航，False 则终点悬停 - target_lost_timeout: VISUAL_TRACKING 目标丢失超时（秒） + proxy: RouterProxy 实例（调用方负责 start/close） + waypoints: 航点列表（经纬度格式 v2） + target_altitude: 默认起飞高度（米，正值向上） + arrival_radius: 航点到达判定半径（米） + hold_duration: 巡点无目标的最大悬停搜索时间（秒） + return_to_home: 航点巡完后是否返航，False 则终点悬停 + target_lost_timeout: VISUAL_TRACKING 目标丢失超时（秒） + max_speed: 航点巡航最大速度（m/s，默认 1.2） + pre_land_hover_duration: 降落前悬停时长（秒，默认 3.0） + landing_descent_speed: 降落下降速度（m/s，正值向下，默认 0.3） """ # ---- 依赖注入 ---- self.proxy = proxy @@ -153,6 +160,9 @@ class MissionManager: target_lost_timeout if target_lost_timeout is not None else self.DEFAULT_TARGET_LOST_TIMEOUT ) + self.max_speed = max(max_speed, 0.1) # 最大巡航速度（m/s），最低 0.1 + self.pre_land_hover_duration = max(pre_land_hover_duration, 0.0) # 降落前悬停时长 + self.landing_descent_speed = max(0.1, min(landing_descent_speed, 1.0)) # 下降速度 [0.1, 1.0] # ---- 状态机 ---- self.current_wp_index = 0 @@ -176,10 +186,12 @@ class MissionManager: self.tracking_max_duration = 10.0 logger.info("MissionManager 初始化: %d 个航点, 目标高度=%.1fm, " - "到达半径=%.1fm, 搜寻时长=%.1fs, 返航=%s", + "到达半径=%.1fm, 搜寻时长=%.1fs, 最大速度=%.1fm/s, " + "降落悬停=%.1fs, 下降速度=%.1fm/s, 返航=%s", len(self.waypoints), self.target_altitude, self.arrival_radius, self.hold_duration, - self.return_to_home) + self.max_speed, self.pre_land_hover_duration, + self.landing_descent_speed, self.return_to_home) # ---- 日志节流控制 ---- self._last_throttle_time = {} @@ -210,8 +222,9 @@ class MissionManager: if not self.proxy.is_state_fresh(state): age_us, _ = self.proxy.get_state_freshness(state) age_s = (age_us or 0) / 1_000_000 - logger.warning("⚠️ STATE 严重过期 (age=%.2fs)，" - "本轮保守跳过", age_s) + self._throttle_log("stale_state", 5.0, + "⚠️ STATE 严重过期 (age=%.2fs)，" + "本轮保守跳过", age_s, level=logging.WARNING) return else: logger.warning("⚠️ 尚未收到 STATE，跳过本轮处理") @@ -281,6 +294,8 @@ class MissionManager: self._handle_landing(state) elif self.state == MissionState.HOLD_FINAL: self._handle_hold_final(state) + elif self.state == MissionState.PRE_LAND_HOVER: + self._handle_pre_land_hover(state) except FailsafeTriggered: raise except Exception as e: @@ -374,6 +389,7 @@ class MissionManager: self.state = MissionState.NAVIGATING self.current_wp_index = 0 self._set_current_target_from_wp(0) + self.proxy.reset_setpoint_log() return # B) 未解锁 @@ -507,6 +523,7 @@ class MissionManager: self.state = MissionState.NAVIGATING self.current_wp_index = 0 self._set_current_target_from_wp(0) + self.proxy.reset_setpoint_log() return # 等待模式切换 @@ -517,6 +534,7 @@ class MissionManager: self.state = MissionState.NAVIGATING self.current_wp_index = 0 self._set_current_target_from_wp(0) + self.proxy.reset_setpoint_log() return time.sleep(0.1) @@ -524,6 +542,7 @@ class MissionManager: self.state = MissionState.NAVIGATING self.current_wp_index = 0 self._set_current_target_from_wp(0) + self.proxy.reset_setpoint_log() except (KeyError, TypeError) as e: logger.warning("[TAKEOFF] 异常: %s", e) @@ -532,10 +551,12 @@ class MissionManager: def _handle_navigating(self, state): """ - [NAVIGATING] 每帧发送 POSITION SETPOINT 前往当前航点 + [NAVIGATING] 每帧发送 VELOCITY SETPOINT 前往当前航点 - POSITION SETPOINT 发送 LOCAL_OFFSET_NED 相对偏移；到达判定使用 Haversine 距离。 - 到达判定后推进索引；所有航点飞完后根据 return_to_home 决定返航或悬停。 + 速度受 max_speed 限制，含自动减速区。 + 到达判定使用 Haversine 距离。 + - 普通航点到达 → HOLD_TASK + - 降落航点到达 → PRE_LAND_HOVER（先悬停再降落） """ if not self.waypoints or self.current_wp_index >= len(self.waypoints): self._finish_navigation(state) @@ -544,29 +565,19 @@ class MissionManager: wp = self.waypoints[self.current_wp_index] cmd = wp.get("command", "WAYPOINT") - # ---- 起飞航点 ---- + # ---- 起飞航点 直接跳过 ---- if cmd == "TAKEOFF": - # 起飞已完成，直接推进到下一个航点 self.current_wp_index += 1 return - # ---- 降落航点 ---- - if cmd == "LAND": - logger.info("🛬 执行降落") - self.proxy.send_command("LAND") - self.state = MissionState.LANDING - return - - # ---- 普通航点 ---- + # ---- 普通航点 / 降落航点 — 统一导航到目标坐标 ---- wp_lat = wp["lat"] wp_lon = wp["lon"] wp_alt = wp.get("alt", self.target_altitude) wp_yaw = wp.get("yaw", 0.0) - # 发送 POSITION SETPOINT（相对当前位置 LOCAL_OFFSET_NED） - self._send_relative_position_setpoint( - state, wp_lat, wp_lon, wp_alt, yaw=wp_yaw, - ) + # 发送 VELOCITY SETPOINT，速度受 max_speed 限制 + self._send_velocity_to_target(state, wp_lat, wp_lon, wp_alt) # ---- 共享目标 ---- self.current_target = {"lat": wp_lat, "lon": wp_lon, @@ -581,15 +592,22 @@ class MissionManager: wp_lat, wp_lon, wp_alt) if dist < self.arrival_radius: - logger.info("\n✅ [到达航点 %d/%d] dist=%.2fm", - self.current_wp_index + 1, len(self.waypoints), dist) - self.state = MissionState.HOLD_TASK - self.hold_start_time = time.time() + if cmd == "LAND": + logger.info("\n🛬 [到达降落点] 悬停 %.1fs 后降落...", + self.pre_land_hover_duration) + self.state = MissionState.PRE_LAND_HOVER + self._state_start_time = time.time() + else: + logger.info("\n✅ [到达航点 %d/%d] dist=%.2fm", + self.current_wp_index + 1, len(self.waypoints), dist) + self.state = MissionState.HOLD_TASK + self.hold_start_time = time.time() else: self._throttle_log("navigating", 5.0, - "[巡航中] 航点 [%d/%d] 距离=%.2fm", + "[巡航中] 航点 [%d/%d] 距离=%.2fm 速度≤%.1fm/s", self.current_wp_index + 1, - len(self.waypoints), dist) + len(self.waypoints), dist, + self.max_speed) def _finish_navigation(self, state): """所有航点飞完后的跳转""" @@ -650,6 +668,7 @@ class MissionManager: self.hold_duration) self.current_wp_index += 1 self.state = MissionState.NAVIGATING + self.proxy.reset_setpoint_log() else: self._throttle_log("hold_task", 3.0, "[检索中] 航点 %d 剩余 %.1fs", @@ -718,6 +737,7 @@ class MissionManager: self.current_wp_index += 1 self.state = MissionState.NAVIGATING self._target_lost_start = None + self.proxy.reset_setpoint_log() logger.info("[状态切换] VISUAL_TRACKING → NAVIGATING，" "前往航点 %d", self.current_wp_index + 1) @@ -725,19 +745,18 @@ class MissionManager: def _handle_returning(self, state): """ - [RETURNING] 发送 POSITION SETPOINT 到 HOME 点 + [RETURNING] 每帧发送 VELOCITY SETPOINT 返回 HOME 点 - POSITION SETPOINT 发送到 HOME 的 LOCAL_OFFSET_NED 相对偏移； - 到达判定使用 Haversine 球面距离。 + 速度受 max_speed 限制，到达判定使用 Haversine 球面距离。 """ home = state.get("home", {}) drone = state.get("drone", {}) home_lat = home.get("lat", 0.0) home_lon = home.get("lon", 0.0) - # 发送 POSITION SETPOINT（相对当前位置 LOCAL_OFFSET_NED） - self._send_relative_position_setpoint( - state, home_lat, home_lon, self.target_altitude, yaw=0, + # 发送 VELOCITY SETPOINT，速度受 max_speed 限制 + self._send_velocity_to_target( + state, home_lat, home_lon, self.target_altitude, ) self.current_target = { "lat": home_lat, "lon": home_lon, @@ -762,8 +781,39 @@ class MissionManager: else: logger.warning("[返航] LAND 失败: %s", ack) - # ---- 降落 ---- + # ---- 降落前悬停 ---- + def _handle_pre_land_hover(self, state): + """ + [PRE_LAND_HOVER] 到达降落点后的悬停等待 + + 在降落点上方悬停 pre_land_hover_duration 秒后， + 发送 LAND 指令进入 LANDING 状态（由 PX4 接管后续自动降落）。 + """ + elapsed = time.time() - self._state_start_time + + # 保持位置悬停（目标点速度控制 → 接近时自动归零） + wp = self.waypoints[self.current_wp_index] + self._send_velocity_to_target( + state, + wp["lat"], wp["lon"], + wp.get("alt", self.target_altitude), + ) + + if elapsed >= self.pre_land_hover_duration: + logger.info("\n🛬 [悬停完成] 执行降落指令") + ok, ack = self.proxy.send_command("LAND") + if ok: + self.state = MissionState.LANDING + else: + logger.warning("[降落] LAND 指令失败: %s，仍进入降落状态", ack) + self.state = MissionState.LANDING + else: + self._throttle_log("pre_land", 1.0, + "[降落悬停] 剩余 %.1fs", + self.pre_land_hover_duration - elapsed) + + # ---- 降落 ---- def _handle_landing(self, state): """[LANDING] 监控 disarm 状态判断着陆""" try: @@ -832,6 +882,65 @@ class MissionManager: control_mode="POSITION", ) + def _send_velocity_to_target(self, state, target_lat, target_lon, + target_alt, max_speed=None): + """ + 按相对目标的 NED 方向发送 VELOCITY SETPOINT，速度受 max_speed 限制。 + + 当距离目标较近时自动减速（线性减速区 = 2 × max_speed 米）， + 到达 arrival_radius 范围后由调用方（arrival 判定）负责切换状态。 + + Args: + state: 最新飞机 STATE + target_lat: 目标纬度（度） + target_lon: 目标经度（度） + target_alt: 目标相对高度（米，正值向上） + max_speed: 最大速度（m/s），默认 self.max_speed + + Returns: + (成功: bool, ACK 字典: dict) + """ + if max_speed is None: + max_speed = self.max_speed + + drone = state.get("drone", {}) if state else {} + current_lat = drone.get("lat", 0.0) + current_lon = drone.get("lon", 0.0) + current_alt = self._get_current_alt(state) + + # 计算 NED 偏移 (x=北, y=东, z=下) + x, y, z = latlon_alt_to_local_offset( + target_lat, target_lon, target_alt, + current_lat, current_lon, current_alt, + ) + + dist_h = math.sqrt(x * x + y * y) + + # ---- 水平速度：方向归一化 × 限速（含减速区） ---- + DECEL_FACTOR = 2.0 # 减速区 = max_speed × 2 米 + if dist_h < 0.15: + vx, vy = 0.0, 0.0 + else: + desired_speed = max_speed + decel_dist = max_speed * DECEL_FACTOR + if dist_h < decel_dist: + # 线性减速：距离越近速度越慢 + desired_speed = max_speed * (dist_h / decel_dist) + desired_speed = max(desired_speed, 0.15) # 最低速度 + vx = (x / dist_h) * desired_speed + vy = (y / dist_h) * desired_speed + + # ---- 垂直速度：简单 P 控制 ---- + # z > 0 表示目标在下 → 需要正 vz（NED 向下为正） + vz = z * 2.0 # 增益 2.0 + vz = max(min(vz, max_speed), -max_speed) + + return self.proxy.send_setpoint( + vx=vx, vy=vy, vz=vz, + yaw_rate=0.0, + control_mode="VELOCITY", + ) + def _set_current_target_from_wp(self, index): """从航点列表更新 current_target""" if self.waypoints and index < len(self.waypoints): diff --git a/drone_controller/router_proxy.py b/drone_controller/router_proxy.py index 06a5f12..d44685e 100644 --- a/drone_controller/router_proxy.py +++ b/drone_controller/router_proxy.py @@ -106,7 +106,12 @@ class RouterProxy: self._waypoints_callback = None self._last_logged_msg_type = None # 记录上次打印的消息类型 - self._last_logged_ack_type = None + self._last_logged_ack_type = None + self._setpoint_log_allowed = True # SETPOINT 日志是否允许输出（每个航点重置一次） + + # ---- 日志节流：STATE 过期警告 ---- + self._last_stale_warn_time = 0.0 # 上次 "STATE 过期" 日志时间 + self._last_severe_warn_time = 0.0 # 上次 "STATE 严重过期" 日志时间 # ================================================================ # 内部工具 @@ -126,6 +131,12 @@ class RouterProxy: return current_seq + def reset_setpoint_log(self): + """重置 SETPOINT 日志输出标记（新航点开始时调用，使下一个 SETPOINT 可被打印）""" + self._setpoint_log_allowed = True + self._last_logged_msg_type = None + self._last_logged_ack_type = None + def _build_send_summary(self, msg: dict) -> str: """构建发送消息的内容概要，用于日志""" msg_type = msg.get("type", "?") @@ -262,26 +273,32 @@ class RouterProxy: msg_id = msg.get("id", "?") summary = self._build_send_summary(msg) - # ---- 发送日志：类型变化时才打印 ---- - if msg_type != self._last_logged_msg_type: - self._last_logged_msg_type = msg_type - logger.debug("Sending %s id=%s: %s", msg_type, msg_id, summary) - logger.info("🚀 发送 %s (类型变化)", msg_type) - # 否则静默，不打印任何东西 + # ---- 发送日志 ---- + should_log = False + if msg_type == "SETPOINT": + # SETPOINT：每个航点只打印第一次 + if self._setpoint_log_allowed: + self._setpoint_log_allowed = False + should_log = True + else: + # 非 SETPOINT（COMMAND / WAYPOINT / QUERY）：类型变化时打印 + if msg_type != self._last_logged_msg_type: + self._last_logged_msg_type = msg_type + should_log = True + + if should_log: + logger.info("🚀 发送 %s id=%s: %s", msg_type, msg_id, summary) try: self.req.send_string(json.dumps(msg)) ack_str = self.req.recv_string() ack = json.loads(ack_str) ok = ack.get("status") == "OK" - ack_type = ack.get("type", "ACK") # 通常是 "ACK" - # ---- ACK 日志：类型变化时才打印 ---- - if ack_type != self._last_logged_ack_type: - self._last_logged_ack_type = ack_type + # ---- ACK 日志：与发送日志同步，只在对应发送被打印时输出 ---- + if should_log: logger.info("ACK received for id=%s, status=%s", - ack.get("ref_id"), ack.get("status")) - # 否则 ACK 也静默 + ack.get("ref_id"), ack.get("status")) if not ok: # 非 OK 状态无论如何都打印（这是异常） @@ -334,12 +351,18 @@ class RouterProxy: except (KeyError, TypeError): return False if age_us >= stale_threshold_us: + # 日志节流：间隔 ≥ THROTTLE_INTERVAL 秒才打印一次 + now = time.time() if age_us >= self.STALE_THRESHOLD_WARN_US: - logger.warning("STATE 严重过期: age=%.2fs（阈值=%.0fms）", - age_us / 1e6, stale_threshold_us / 1000) + if now - self._last_severe_warn_time >= 5.0: + self._last_severe_warn_time = now + logger.warning("STATE 严重过期: age=%.2fs（阈值=%.0fms）", + age_us / 1e6, stale_threshold_us / 1000) else: - logger.warning("STATE 过期: age=%.1fms（阈值=%.0fms）", - age_us / 1000, stale_threshold_us / 1000) + if now - self._last_stale_warn_time >= 5.0: + self._last_stale_warn_time = now + logger.warning("STATE 过期: age=%.1fms（阈值=%.0fms）", + age_us / 1000, stale_threshold_us / 1000) return age_us < stale_threshold_us def get_state_freshness(self, state): diff --git a/infer_camera_modular.py b/infer_camera_modular.py index b3b9925..27cab13 100644 --- a/infer_camera_modular.py +++ b/infer_camera_modular.py @@ -253,7 +253,8 @@ class VideoStreaming: send_count += 1 if send_count % 100 == 0: - self.logger.info("图传统计: 已发送 %d 帧 UDP 数据包", send_count) + # self.logger.info("图传统计: 已发送 %d 帧 UDP 数据包", send_count) + pass except queue.Empty: continue diff --git a/run_mission.py b/run_mission.py index 71d2a54..a495b2d 100644 --- a/run_mission.py +++ b/run_mission.py @@ -122,6 +122,7 @@ class MissionOrchestrator: self.proxy.set_waypoints_callback(self._on_qgc_waypoints) missions = cfg["missions"] + landing_cfg = missions.get("landing", {}) self.mission = MissionManager( proxy=self.proxy, waypoints=self.waypoints, @@ -129,6 +130,9 @@ class MissionOrchestrator: arrival_radius=missions.get("arrival_radius", 0.3), hold_duration=missions.get("hold_duration", 5.0), return_to_home=missions.get("return_to_home", True), + max_speed=missions.get("max_speed", 1.2), + pre_land_hover_duration=landing_cfg.get("pre_hover_duration", 3.0), + landing_descent_speed=landing_cfg.get("descent_speed", 0.3), ) # ================================================================