2026.8.20 视觉追踪器优化

Optoelectronics/transform_app.py

@@ -239,7 +239,7 @@ def calculate_target_gps(ptz_lat, ptz_lon, ptz_alt, pan, tilt, distance):
 def main():
     sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
     # 实际测试时请修改为光电外设的正确 IP
-    sock.connect(('192.168.1.64', 39020))
+    sock.connect(('10.10.61.64', 39020))
 
     # 【修复 Bug 2】：传入文档推测的默认密码 'Abc.12345'
     token = login_to_ipc(sock, 'admin', 'Abc.12345')

config.json

@@ -17,6 +17,7 @@
         "sub_endpoint": "tcp://127.0.0.1:5556",
         "takeoff_alt": 5.0,
         "control_hz": 20,
+        "cruise_speed":1.0,
         "target_tracker": {
             "max_lost_frames": 8,
             "max_asscociation_dist": 200.0,
@@ -34,16 +35,32 @@
     },
     "missions": {
         "target_altitude": 3.0,
-        "arrival_radius": 0.3,
+        "arrival_radius": 0.8,
         "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,
+        "reset_threshold":30
     },
     "log": {
     "enable_flight_log": true,
     "flight_log_interval": 20,
     "log_dir": "./log"
     },
+    "pid_x":{
+        "kp": 1.2,
+        "ki": 0.1,
+        "kd": 0.05,
+        "max_out": 0.8,
+        "min_out": -0.8
+    },
     "pid_y": {
         "kp": 1.0,
         "ki": 0.02,
@@ -57,5 +74,12 @@
         "kd": 0.3,
         "max_out": 0.4,
         "min_out": -0.4
+    },
+    "pid_yaw":{
+        "kp":0.8,
+        "ki":0.05,
+        "kd":0.0,
+        "max_out":1.0,
+        "min_out":0.0
     }
 }

drone_controller/mission_manager.py

@@ -2,9 +2,17 @@
 mission_manager.py — 基于 ZeroMQ RouterProxy 的航点巡航状态机
 
 【协议变更 v2 — 2026-07】
-  SETPOINT 指令的 x/y 字段改为接收经纬度（lat/lon），z 字段接收
-  相对高度（正值向上）。航点存储格式从 NED (x, y, z) 切换为
-  经纬度 (lat, lon, alt)。
+  航点存储格式使用经纬度 (lat, lon, alt)。导航/返航/悬停均发送
+  VELOCITY SETPOINT：把目标经纬度换算成相对当前位置的 NED 方向，
+  再按 max_speed 限速成速度命令（差分量，与参考原点无关）。
+
+【坐标系说明 — 2026-08 修复】
+  此前 POSITION LOCAL_OFFSET_NED 路径用 GPS 经纬度反算相对偏移，
+  交给 PX4 在自身 LOCAL_NED 帧解析。GPS 与本地估计一旦不一致
+  （跳变/漂移），目标跟着跳、飞控满速追（实测 HOLD_FINAL 7.77m/s）。
+  故 HOLD_TASK / HOLD_FINAL 悬停改为速度闭环，从根上消除
+  GPS↔本地帧混用。POSITION setpoint 仅保留给 TAKEOFF 预热/降级爬升
+  （零水平偏移，且已加单帧钳制）。
 
 【核心改动】
   1. self.drone → self.proxy（注入的 RouterProxy 实例）
@@ -25,11 +33,12 @@ mission_manager.py — 基于 ZeroMQ RouterProxy 的航点巡航状态机
   SETPOINT 流同时激活产生冲突。
 """
 
+import math
 import time
 import logging
 from enum import Enum
 
-from utils.coord import haversine, dist_3d_latlon
+from utils.coord import haversine, dist_3d_latlon, latlon_alt_to_local_offset
 
 logger = logging.getLogger(__name__)
 
@@ -47,6 +56,7 @@ class MissionState(Enum):
     LANDING = 7             # 自动降落中
     FINISHED = 8            # 任务结束 / 已上锁
     HOLD_FINAL = 9          # 结束巡点后原地悬停（return_to_home=False）
+    PRE_LAND_HOVER = 10     # 🛬 降落前悬停（到达降落点后等待片刻再下降）
 
 
 class FailsafeTriggered(Exception):
@@ -63,7 +73,7 @@ class FailsafeTriggered(Exception):
 
 class MissionManager:
     """
-    🛸 ZMQ 航点巡航状态机管理器（经纬度协议 v2）
+    🛸 ZMQ 航点巡航状态机管理器（经纬度航点 + 相对位置 SETPOINT）
 
     职责：
       - 封装完整的无人机任务生命周期（初始化 → 解锁 → 起飞 →
@@ -97,6 +107,17 @@ class MissionManager:
         "OFFBOARD", "GUIDED", "AUTO", "HOLD", "LAND", "RTL",
     })
 
+    # ---- 解锁/起飞阶段允许的过渡模式 ----
+    TRANSITION_MODES = frozenset({
+        "MANUAL", "ALTCTL", "POSCTL", "AUTO", "HOLD",
+        "AUTO.READY", "AUTO.TAKEOFF", "AUTO.LOITER",
+    })
+
+    # ---- 起飞完成后可接受的悬停/保持模式 ----
+    TAKEOFF_HOLD_MODES = frozenset({
+        "HOLD", "AUTO", "AUTO.LOITER", "AUTO.TAKEOFF",
+    })
+
     # ---- 目标丢失超时（秒） ----
     DEFAULT_TARGET_LOST_TIMEOUT = 3.0
 
@@ -105,22 +126,49 @@ class MissionManager:
     OFFBOARD_TIMEOUT = 5.0
     ARM_TIMEOUT = 5.0
 
+    # ================================================================
+    #  状态切换日志（property 拦截所有 self.state = X 赋值）
+    # ================================================================
+
+    @property
+    def state(self):
+        return self._state
+
+    @state.setter
+    def state(self, new_state):
+        old_state = getattr(self, '_state', None)
+        if old_state is not new_state:
+            logger.info("🔄 [状态切换] %s → %s",
+                         old_state.name if old_state is not None else "∅",
+                         new_state.name)
+        self._state = new_state
+
+    # ================================================================
+
     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
 
+        # ---- 状态初始值（property 用 _state，须在任何 self.state = 前初始化） ----
+        self._state = None
+
         # ---- 任务参数 ----
         # 航点列表（经纬度协议 v2：[{lat, lon, alt, yaw, command}, ...]）
         self.waypoints = list(waypoints) if waypoints else []
@@ -142,6 +190,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
@@ -165,10 +216,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 = {}
@@ -197,10 +250,7 @@ class MissionManager:
         if self.state not in (MissionState.INIT, MissionState.FINISHED):
             if state is not None:
                 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)
+                    # 过期警告已由 is_state_fresh 内部输出，此处不重复打印
                     return
             else:
                 logger.warning("⚠️ 尚未收到 STATE，跳过本轮处理")
@@ -219,7 +269,24 @@ class MissionManager:
         if self.state not in (MissionState.INIT, MissionState.FINISHED):
             if state is not None:
                 mode = state["drone"].get("mode", "")
-                if mode not in self.AUTONOMOUS_MODES:
+                # 飞控切到 AUTO.LAND 或 AUTO.RTL → 自动让渡控制权
+                if mode in ("AUTO.LAND", "AUTO.RTL"):
+                    # 去重：同一 mode 只在首次出现时打印，避免每帧刷屏
+                    self._throttle_log_on_change(
+                        "yield_mode", mode,
+                        "⚠️ 飞控切换到 %s，Python 端主动让渡控制权", mode,
+                        level=logging.WARNING)
+                    # 根据模式决定切换到的状态
+                    if mode == "AUTO.LAND":
+                        self.state = MissionState.LANDING
+                    elif mode == "AUTO.RTL":
+                        self.state = MissionState.RETURNING
+                    # 重置状态开始时间，让降落/返航处理从当前帧开始
+                    self._state_start_time = time.time()
+                    # 停止控制线程（通过 run_mission.py 的协调逻辑）
+                    # 注意：state 变化后，下一帧 _coordinate_control 会自动停止线程
+                    return
+                if not self._is_mode_allowed_for_state(mode, self.state):
                     logger.warning(
                         "\n⚠️ [Failsafe] 飞控模式被切为: %s！"
                         "人工接管，熔断任务！", mode
@@ -257,6 +324,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:
@@ -267,6 +336,28 @@ class MissionManager:
     #  辅助方法
     # ================================================================
 
+    def _is_autonomous_mode(self, mode):
+        """判断是否属于自主飞行模式，兼容 PX4 AUTO 子模式。"""
+        if mode in self.AUTONOMOUS_MODES:
+            return True
+        return isinstance(mode, str) and mode.startswith("AUTO.")
+
+    def _is_transition_mode(self, mode):
+        """解锁/起飞阶段允许的 PX4 过渡模式。"""
+        if mode in self.TRANSITION_MODES:
+            return True
+        return isinstance(mode, str) and mode.startswith("AUTO.")
+
+    def _is_mode_allowed_for_state(self, mode, state):
+        """按任务阶段判断当前飞控模式是否允许。"""
+        if state in (MissionState.ARMING, MissionState.TAKEOFF):
+            return self._is_transition_mode(mode)
+        return self._is_autonomous_mode(mode)
+
+    def _is_takeoff_hold_mode(self, mode):
+        """起飞高度到位后可接受的保持/悬停状态。"""
+        return mode in self.TAKEOFF_HOLD_MODES
+
     def _get_current_alt(self, state):
         """从 STATE 获取当前相对高度（正值向上）"""
         try:
@@ -328,6 +419,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) 未解锁
@@ -395,10 +487,10 @@ class MissionManager:
             else:
                 logger.warning("[TAKEOFF] WAYPOINT 失败: %s，尝试 SETPOINT 爬升", ack)
                 drone = state.get("drone", {})
-                ok2, _ = self.proxy.send_setpoint(
-                    lat=drone.get("lat", 0.0), lon=drone.get("lon", 0.0),
-                    z=self.target_altitude, yaw=0.0,
-                    control_mode="POSITION",
+                ok2, _ = self._send_relative_position_setpoint(
+                    state,
+                    drone.get("lat", 0.0), drone.get("lon", 0.0),
+                    self.target_altitude, yaw=0.0,
                 )
                 if ok2:
                     self.state = MissionState.TAKEOFF
@@ -417,7 +509,7 @@ class MissionManager:
 
         流程（协议 §3.3.1）：
           ① 等待高度到达目标 90% 且 mode == "HOLD"
-          ② 发送预热 SETPOINT（当前位置经纬度悬停）
+          ② 发送预热 SETPOINT（相对当前位置悬停）
           ③ time.sleep(0.2) 等待流建立
           ④ 发送 COMMAND("OFFBOARD")
           ⑤ 轮询等待 mode == "OFFBOARD"
@@ -431,7 +523,7 @@ class MissionManager:
 
             # ---- ① 等待 TAKEOFF 完成 ----
             reached_alt = alt_rel >= self.target_altitude * 0.9
-            in_hold = mode == "HOLD"
+            in_hold = self._is_takeoff_hold_mode(mode)
 
             if not (reached_alt and in_hold):
                 self._throttle_log("takeoff_wait", 3.0,
@@ -444,11 +536,11 @@ class MissionManager:
             logger.info("[TAKEOFF] 高度 %.1fm，模式 %s，准备切换 OFFBOARD...",
                         alt_rel, mode)
 
-            # 发送预热 SETPOINT（v2: x=经度, y=纬度, z=相对高度）
-            self.proxy.send_setpoint(
-                lat=drone.get("lat", 0.0), lon=drone.get("lon", 0.0),
-                z=alt_rel, yaw=0.0,
-                control_mode="POSITION",
+            # 发送预热 SETPOINT（相对当前位置悬停）
+            self._send_relative_position_setpoint(
+                state,
+                drone.get("lat", 0.0), drone.get("lon", 0.0),
+                alt_rel, yaw=0.0,
             )
             time.sleep(0.2)
 
@@ -461,6 +553,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
 
             # 等待模式切换
@@ -471,6 +564,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)
 
@@ -478,6 +572,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)
@@ -486,10 +581,12 @@ class MissionManager:
 
     def _handle_navigating(self, state):
         """
-        [NAVIGATING] 每帧发送 POSITION SETPOINT 前往当前航点
+        [NAVIGATING] 每帧发送 VELOCITY SETPOINT 前往当前航点
 
-        v2: send_setpoint(x=lat, y=lon, z=alt)，到达判定使用 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)
@@ -498,30 +595,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)
 
-        # v2: 发送 POSITION SETPOINT（经纬度 + 高度）
-        self.proxy.send_setpoint(
-            lat=wp_lat, lon=wp_lon, z=wp_alt, yaw=wp_yaw,
-            control_mode="POSITION",
-        )
+        # 发送 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,
@@ -536,15 +622,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):
         """所有航点飞完后的跳转"""
@@ -585,13 +678,14 @@ class MissionManager:
             self.tracking_completed = False
             return
 
-        # ---- 保持当前位置悬停（v2: x=lat, y=lon, z=alt） ----
+        # ---- 保持当前位置悬停（速度闭环，坐标系一致） ----
+        # 说明：悬停不用 POSITION LOCAL_OFFSET_NED。位置模式把"由 GPS 经纬度
+        # 反算的相对偏移"交给 PX4 在自身 LOCAL_NED 帧里解析，GPS 与本地估计
+        # 一旦不一致（跳变/漂移），目标跟着跳、飞控满速追（曾实测 7.77m/s）。
+        # 速度模式是差分量，与参考原点无关，命令值即速度上界，天然安全。
         wp = self.waypoints[self.current_wp_index]
-        self.proxy.send_setpoint(
-            lat=wp["lat"], lon=wp["lon"],
-            z=wp.get("alt", self.target_altitude),
-            yaw=wp.get("yaw", 0),
-            control_mode="POSITION",
+        self._send_velocity_to_target(
+            state, wp["lat"], wp["lon"], wp.get("alt", self.target_altitude),
         )
         self.current_target = {
             "lat": wp["lat"], "lon": wp["lon"],
@@ -606,6 +700,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",
@@ -674,6 +769,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)
 
@@ -681,20 +777,18 @@ class MissionManager:
 
     def _handle_returning(self, state):
         """
-        [RETURNING] 发送 POSITION SETPOINT 到 HOME 点
+        [RETURNING] 每帧发送 VELOCITY SETPOINT 返回 HOME 点
 
-        v2: send_setpoint(x=home_lat, y=home_lon, z=target_alt)
-        到达判定使用 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)
 
-        # v2: x=纬度, y=经度, z=相对高度
-        self.proxy.send_setpoint(
-            lat=home_lat, lon=home_lon, z=self.target_altitude, yaw=0,
-            control_mode="POSITION",
+        # 发送 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,
@@ -719,8 +813,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:
@@ -742,7 +867,8 @@ class MissionManager:
         """
         [HOLD_FINAL] 末端悬停，保持最终航点位置
 
-        v2: x=lat, y=lon, z=alt
+        使用速度闭环（_send_velocity_to_target）而非 POSITION LOCAL_OFFSET_NED，
+        避免 GPS 偏移与 PX4 本地坐标系不一致导致飞控满速追目标（曾实测 7.77m/s）。
         """
         if self.waypoints:
             final_wp = self.waypoints[-1]
@@ -750,11 +876,10 @@ class MissionManager:
             final_wp = {"lat": 0.0, "lon": 0.0,
                         "alt": self.target_altitude, "yaw": 0}
 
-        self.proxy.send_setpoint(
-            lat=final_wp["lat"], lon=final_wp["lon"],
-            z=final_wp.get("alt", self.target_altitude),
-            yaw=final_wp.get("yaw", 0),
-            control_mode="POSITION",
+        self._send_velocity_to_target(
+            state,
+            final_wp["lat"], final_wp["lon"],
+            final_wp.get("alt", self.target_altitude),
         )
         self.current_target = {
             "lat": final_wp["lat"], "lon": final_wp["lon"],
@@ -770,6 +895,112 @@ class MissionManager:
     #  辅助
     # ================================================================
 
+    def _send_relative_position_setpoint(self, state, target_lat, target_lon,
+                                         target_alt, yaw=0.0, max_speed=None):
+        """
+        按相对当前位置的 LOCAL_OFFSET_NED 发送 POSITION SETPOINT。
+
+        统一限速：位置设定点按单帧最大推进距离 max_step 钳制，
+        即大偏移被拆成多帧逐步逼近，从源头避免飞控满速冲撞。
+        （此前 HOLD_FINAL 阶段曾因偏移异常出现 7.77m/s 失控）
+
+        max_step = max(max_speed * 0.5, 0.3)。正常悬停（偏移 < 0.3m）
+        不受影响；异常大偏移被钳制的同时打印 WARNING 便于诊断。
+
+        Args:
+            max_speed: 限速（m/s），默认 self.max_speed
+        """
+        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)
+
+        x, y, z = latlon_alt_to_local_offset(
+            target_lat, target_lon, target_alt,
+            current_lat, current_lon, current_alt,
+        )
+
+        # ---- 统一限速：单帧最大目标偏移（约半个限速秒距离） ----
+        max_step = max(max_speed * 0.5, 0.3)
+        dist_h = math.sqrt(x * x + y * y)
+        if dist_h > max_step * 1.5:
+            logger.warning(
+                "[限速] POSITION 偏移过大 %.1fm，钳制为 %.1fm（max_speed=%.1f）",
+                dist_h, max_step, max_speed)
+        if dist_h > max_step:
+            x = x / dist_h * max_step
+            y = y / dist_h * max_step
+        # 垂直方向同样钳制（z>0 表示目标在下）
+        z = max(min(z, max_step), -max_step)
+
+        return self.proxy.send_setpoint(
+            x=x, y=y, z=z, yaw=yaw,
+            frame="LOCAL_OFFSET_NED",
+            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):

drone_controller/optical_flow_controller.py

@@ -0,0 +1,482 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+光流法障碍检测 - 网格点追踪版（复位点不参与统计）
+特性：
+1. 全图均匀网格点，覆盖无盲区
+2. 点漂移后硬复位（当前帧和上一帧均置为初始位置）
+3. 复位点在该帧不参与任何统计（散度、平均光流等）
+4. 分区（左/中/右）风险统计，仅基于有效运动点
+5. 与 PID 和 MAVLink 无缝集成
+"""
+
+import cv2
+import numpy as np
+import time
+from collections import deque
+from typing import Optional, Dict, Any
+
+# ==================== PID 部分 =====================
+from dataclasses import dataclass
+
+
+@dataclass
+class PIDConfig:
+    """PID 参数配置"""
+    kp: float
+    ki: float
+    kd: float
+    max_out: float
+    min_out: float
+    dt_default: float = 0.033
+    alpha: float = 0.2  # 微分滤波系数 (0 < alpha <= 1)
+
+
+class ImprovedPID:
+    def __init__(self, config: PIDConfig):
+        # 参数校验
+        if config.max_out <= config.min_out:
+            raise ValueError("max_out must be greater than min_out")
+        if not (0 < config.alpha <= 1):
+            raise ValueError("alpha must be in (0, 1]")
+
+        self.cfg = config
+
+        self.last_err: float = 0.0
+        self.last_fb: float = 0.0
+        self.integral: float = 0.0
+        self.last_d: float = 0.0
+        self.last_time: float = time.perf_counter()
+        self.first_run: bool = True   # 标记首次调用
+
+    def update(self, error: float, feedback: Optional[float] = None, dt: Optional[float] = None) -> float:
+        current_time = time.perf_counter()
+
+        # --- 首次调用处理：初始化状态，避免微分冲击 ---
+        if self.first_run:
+            self.first_run = False
+            self.last_err = error
+            if feedback is not None:
+                self.last_fb = feedback
+            self.last_time = current_time
+            # 首次仅返回比例项，微分和积分暂不参与（或也可返回0，视应用而定）
+            # 通常直接返回 P 项能让系统平滑起步
+            return self.cfg.kp * error
+
+        # 1. 计算或使用传入的 dt
+        if dt is None:
+            dt = current_time - self.last_time
+            if dt <= 0:
+                dt = self.cfg.dt_default
+
+        # 2. 比例项 (P)
+        p_out = self.cfg.kp * error
+
+        # 3. 微分项 (D) —— 微分先行 + 低通滤波
+        if feedback is not None:
+            # 对反馈微分，设定值变化不引入冲击
+            d_raw = (self.last_fb - feedback) / dt
+            self.last_fb = feedback
+        else:
+            d_raw = (error - self.last_err) / dt
+
+        self.last_d = self.cfg.alpha * d_raw + (1.0 - self.cfg.alpha) * self.last_d
+        d_out = self.cfg.kd * self.last_d
+
+        # 4. 积分项 (I) —— 先计算潜在值
+        potential_integral = self.integral + error * dt
+        i_out = self.cfg.ki * potential_integral
+
+        # 5. 总输出与抗积分饱和（基于积分方向判断）
+        output = p_out + i_out + d_out
+
+        if output > self.cfg.max_out:
+            output = self.cfg.max_out
+            # 如果积分项仍在推高输出（i_out > 0），则冻结积分
+            if i_out > 0:
+                pass   # 不更新 self.integral
+            else:
+                self.integral = potential_integral
+        elif output < self.cfg.min_out:
+            output = self.cfg.min_out
+            # 如果积分项仍在拉低输出（i_out < 0），则冻结积分
+            if i_out < 0:
+                pass
+            else:
+                self.integral = potential_integral
+        else:
+            # 未饱和，正常更新积分
+            self.integral = potential_integral
+
+        # 6. 更新状态变量
+        self.last_err = error
+        self.last_time = current_time
+
+        return output
+
+    def reset(self) -> None:
+        """重置所有状态，恢复到首次运行前的状态"""
+        self.last_err = 0.0
+        self.last_fb = 0.0
+        self.integral = 0.0
+        self.last_d = 0.0
+        self.last_time = time.perf_counter()
+        self.first_run = True
+
+# =================================================
+
+
+class OpticalFlowObstacleDetector:
+    """网格点光流检测器，复位点不参与统计"""
+    def __init__(self, grid_step: int = 25, reset_threshold: float = 30.0):
+        """
+        Args:
+            grid_step: 网格点间距（像素），值越小点越多
+            reset_threshold: 点偏离初始位置超过此值时复位（像素）
+        """
+        self.grid_step = grid_step
+        self.reset_threshold = reset_threshold
+
+        # 光流跟踪参数
+        self.lk_params = dict(
+            winSize=(21, 21),
+            maxLevel=3,
+            criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01)
+        )
+
+        # 状态变量
+        self.old_gray = None
+        self.p0 = None               # 当前点坐标 (N,1,2)
+        self.grid_initial = None     # 初始网格坐标 (N,2)
+        self.frame_count = 0
+
+        # 分区统计历史（用于兜底）
+        self.last_zone_stats = None
+        self.flow_history = deque(maxlen=10)
+
+    def _generate_grid(self, img_w: int, img_h: int) -> np.ndarray:
+        """生成网格点，返回 (N,2) 坐标数组"""
+        x = np.arange(self.grid_step // 2, img_w, self.grid_step, dtype=np.float32)
+        y = np.arange(self.grid_step // 2, img_h, self.grid_step, dtype=np.float32)
+        xv, yv = np.meshgrid(x, y)
+        points = np.stack((xv.ravel(), yv.ravel()), axis=1)
+        return points
+
+    def process_frame(self, frame):
+        """
+        处理单帧，返回 (is_collision, flow_data)
+        更新 self.last_zone_stats（仅包含有效点）
+        """
+        self.frame_count += 1
+        if frame is None:
+            return False, None
+
+        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+        h, w = gray.shape
+
+        # --- 1. 初始化第一帧 ---
+        if self.old_gray is None:
+            self.old_gray = gray
+            grid_pts = self._generate_grid(w, h)
+            self.grid_initial = grid_pts.copy()
+            self.p0 = grid_pts.reshape(-1, 1, 2).astype(np.float32)
+            self.last_zone_stats = None
+            return False, None
+
+        # --- 2. 光流跟踪 ---
+        p1, st, err = cv2.calcOpticalFlowPyrLK(
+            self.old_gray, gray, self.p0, None, **self.lk_params
+        )
+
+        if p1 is None:
+            p1 = self.p0.copy()
+            st = np.ones(len(self.p0), dtype=np.uint8)
+
+        # 将跟踪结果展开为 (N,2)
+        p1_flat = p1.reshape(-1, 2)
+        p0_flat = self.p0.reshape(-1, 2)
+        st_flat = st.flatten()
+
+        # 初始化新的点数组（默认保持旧位置）
+        new_points = p0_flat.copy()
+        old_points = p0_flat.copy()   # 上一帧位置，用于计算光流
+
+        # 成功跟踪的点，先更新位置
+        success_indices = np.where(st_flat == 1)[0]
+        if len(success_indices) > 0:
+            new_points[success_indices] = p1_flat[success_indices]
+            old_points[success_indices] = p0_flat[success_indices]
+
+        # --- 3. 复位逻辑与有效点掩码生成 ---
+        # 复位掩码：True 表示该点需要复位（跟踪失败或漂移过大）
+        reset_mask = np.zeros(len(new_points), dtype=bool)
+
+        # 3.1 跟踪失败的点直接复位
+        failed_indices = np.where(st_flat == 0)[0]
+        reset_mask[failed_indices] = True
+
+        # 3.2 检查成功点是否漂移过大
+        for idx in success_indices:
+            if np.linalg.norm(new_points[idx] - self.grid_initial[idx]) > self.reset_threshold:
+                reset_mask[idx] = True
+
+        # 执行复位（将当前和上一帧位置均设为初始位置）
+        if np.any(reset_mask):
+            new_points[reset_mask] = self.grid_initial[reset_mask]
+            old_points[reset_mask] = self.grid_initial[reset_mask]
+
+        # --- 4. 生成有效点掩码（未复位且跟踪成功） ---
+        # 有效点：跟踪成功且未复位
+        valid_mask = np.ones(len(new_points), dtype=bool)
+        valid_mask[reset_mask] = False
+
+        # --- 5. 更新状态 ---
+        self.p0 = new_points.reshape(-1, 1, 2).astype(np.float32)
+        self.old_gray = gray
+
+        # 如果有效点太少，不进行统计（但保留状态）
+        if np.sum(valid_mask) < 5:
+            # 保留上一帧统计值，或返回空
+            return False, None
+
+        # --- 6. 计算光流向量（仅针对有效点） ---
+        valid_old = old_points[valid_mask]
+        valid_new = new_points[valid_mask]
+        flow_vectors = valid_new - valid_old   # (M,2)
+        mean_mag = np.mean(np.linalg.norm(flow_vectors, axis=1))
+
+        # --- 7. 分区统计（仅使用有效点） ---
+        zone_stats = self._compute_zone_stats(valid_old, flow_vectors, w, h)
+        self.last_zone_stats = zone_stats
+
+        # 整体散度（用于碰撞警告）
+        divergence = self._compute_divergence(valid_old, flow_vectors, w, h)
+
+        # 碰撞判断（保留阈值）
+        is_collision = False
+        if divergence > 0.8 and mean_mag > 1.0:
+            is_collision = True
+        elif mean_mag > 3.0 and len(valid_old) > 50:
+            if self._check_roi_expansion(valid_old, flow_vectors):
+                is_collision = True
+
+        # 历史记录
+        self.flow_history.append({
+            'mean_flow': np.mean(flow_vectors, axis=0),
+            'mean_mag': mean_mag,
+            'divergence': divergence,
+            'points': np.sum(valid_mask)
+        })
+
+        return is_collision, {
+            'points': np.sum(valid_mask),
+            'mean_mag': mean_mag,
+            'divergence': divergence,
+        }
+
+    # ---------- 辅助方法（与之前一致，但传入的点已过滤） ----------
+    def _compute_divergence(self, old_points, flow_vectors, img_w, img_h):
+        if len(old_points) < 5:
+            return 0.0
+        center = np.array([img_w / 2, img_h / 2])
+        dir_to_center = center - old_points
+        dir_to_center /= (np.linalg.norm(dir_to_center, axis=1, keepdims=True) + 1e-6)
+        radial_flow = np.sum(flow_vectors * dir_to_center, axis=1)
+        return np.mean(radial_flow)
+
+    def _compute_zone_stats(self, old_pts, flow_vecs, img_w, img_h):
+        left_bound = img_w / 3
+        right_bound = 2 * img_w / 3
+        zones = {'left': [], 'center': [], 'right': []}
+        for pt, vec in zip(old_pts, flow_vecs):
+            x = pt[0]
+            if x < left_bound:
+                zones['left'].append((pt, vec))
+            elif x < right_bound:
+                zones['center'].append((pt, vec))
+            else:
+                zones['right'].append((pt, vec))
+
+        stats = {}
+        for name, pts_vecs in zones.items():
+            if len(pts_vecs) < 3:  # 阈值降低，因为点可能较少
+                stats[name] = {'mean_vec': np.zeros(2), 'divergence': 0.0, 'count': 0, 'risk': 0.0}
+                continue
+            old_pts_zone = np.array([p for p, v in pts_vecs])
+            flow_zone = np.array([v for p, v in pts_vecs])
+            div = self._compute_divergence(old_pts_zone, flow_zone, img_w, img_h)
+            stats[name] = {
+                'mean_vec': np.mean(flow_zone, axis=0),
+                'divergence': div,
+                'count': len(pts_vecs),
+                'risk': div if div > 0 else 0.0
+            }
+        return stats
+
+    def _check_roi_expansion(self, old_points, flow_vectors):
+        h, w = 480, 640
+        roi_center = np.array([w / 2, h / 2])
+        roi_mask = (np.abs(old_points[:, 0] - w / 2) < w / 4) & (np.abs(old_points[:, 1] - h / 2) < h / 4)
+        roi_points = old_points[roi_mask]
+        roi_flow = flow_vectors[roi_mask]
+        if len(roi_points) < 5:
+            return False
+        dir_vec = roi_points - roi_center
+        dir_unit = dir_vec / (np.linalg.norm(dir_vec, axis=1, keepdims=True) + 1e-6)
+        radial = np.sum(roi_flow * dir_unit, axis=1)
+        return np.mean(radial) > 0.5
+
+    def get_zone_decision(self, zone_stats=None, threshold_div=0.8):
+        """根据分区风险生成控制建议（返回 yaw_rate, speed_scale）"""
+        if zone_stats is None:
+            zone_stats = self.last_zone_stats
+        if zone_stats is None:
+            return 0.0, 1.0
+
+        left = zone_stats['left']['risk']
+        center = zone_stats['center']['risk']
+        right = zone_stats['right']['risk']
+        max_r = max(left, center, right)
+
+        if max_r < 0.3:
+            return 0.0, 1.0
+
+        if center >= left and center >= right and center > threshold_div:
+            if max(left, right) > threshold_div * 0.6:
+                return 0.0, 0.0
+            else:
+                return 0.0, 0.3
+
+        if left > right + 0.2:
+            yaw = min(0.8, 0.3 + 0.5 * (left - right))
+            return yaw, 0.7
+        elif right > left + 0.2:
+            yaw = -min(0.8, 0.3 + 0.5 * (right - left))
+            return yaw, 0.7
+        else:
+            return 0.0, 0.5
+
+
+# ==================== 控制器（与之前相同） ====================
+class ObstacleAvoidanceController:
+    """避障控制器，集成网格点光流检测器、PID 和 MAVLink 发送"""
+    def __init__(self,
+                 camera_id: int = 0,
+                 frame_width: int = 640,
+                 frame_height: int = 480,
+                 cruise_speed: float = 2.0,
+                 grid_step: int = 25,
+                 reset_threshold: float = 30.0,
+                 yaw_pid_cfg: Optional[PIDConfig] = None,
+                 speed_pid_cfg: Optional[PIDConfig] = None):
+        self.cap = cv2.VideoCapture(camera_id)
+        self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, frame_width)
+        self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, frame_height)
+        self.cap.set(cv2.CAP_PROP_FPS, 30)
+        if not self.cap.isOpened():
+            raise RuntimeError("无法打开摄像头")
+
+        self.detector = OpticalFlowObstacleDetector(grid_step=grid_step, reset_threshold=reset_threshold)
+        self.cruise_speed = cruise_speed
+
+        if yaw_pid_cfg is None:
+            yaw_pid_cfg = PIDConfig(kp=1.2, ki=0.1, kd=0.05, max_out=0.8, min_out=-0.8)
+        if speed_pid_cfg is None:
+            speed_pid_cfg = PIDConfig(kp=0.8, ki=0.05, kd=0.0, max_out=1.0, min_out=0.0)
+
+        self.yaw_pid = ImprovedPID(yaw_pid_cfg)
+        self.speed_pid = ImprovedPID(speed_pid_cfg)
+
+        self.current_yaw_rate = 0.0
+        self.current_speed = 0.0
+        self.running = False
+
+    def update_feedback(self, yaw_rate: float, speed: float):
+        self.current_yaw_rate = yaw_rate
+        self.current_speed = speed
+
+    def run_iteration(self, mav_proxy, enable_pid: bool = True) -> Dict[str, Any]:
+        ret, frame = self.cap.read()
+        if not ret:
+            return {'success': False, 'error': '摄像头读取失败'}
+
+        is_collision, flow_data = self.detector.process_frame(frame)
+        yaw_setpoint, speed_scale = self.detector.get_zone_decision()
+        speed_setpoint = speed_scale * self.cruise_speed
+
+        if enable_pid:
+            yaw_error = yaw_setpoint - self.current_yaw_rate
+            speed_error = speed_setpoint - self.current_speed
+            yaw_output = self.yaw_pid.update(yaw_error, feedback=self.current_yaw_rate)
+            speed_output = self.speed_pid.update(speed_error, feedback=self.current_speed)
+        else:
+            yaw_output = yaw_setpoint
+            speed_output = speed_setpoint
+
+        # 调用您的 MAVLink 代理的 send_setpoint 方法
+        success, ack = mav_proxy.send_setpoint(
+            control_mode="VELOCITY",
+            vx=speed_output,
+            vy=0.0,
+            vz=0.0,
+            yaw_rate=yaw_output
+        )
+
+        return {
+            'success': success,
+            'ack': ack,
+            'is_collision': is_collision,
+            'yaw_setpoint': yaw_setpoint,
+            'speed_setpoint': speed_setpoint,
+            'yaw_output': yaw_output,
+            'speed_output': speed_output,
+            'points': flow_data['points'] if flow_data else 0,
+        }
+
+    def run_forever(self, mav_proxy, enable_pid: bool = True):
+        self.running = True
+        print("🔄 网格点光流避障（复位点不统计）已启动，按 'q' 退出")
+        while self.running:
+            result = self.run_iteration(mav_proxy, enable_pid)
+            if not result.get('success', False):
+                print("⚠️ 迭代失败:", result.get('error', ''))
+                continue
+            if result['is_collision']:
+                print("🚨 碰撞警告!")
+
+            # 显示画面（方便调试）
+            ret, frame = self.cap.read()
+            if ret:
+                cv2.imshow('Grid Optical Flow', frame)
+            if cv2.waitKey(1) & 0xFF == ord('q'):
+                self.stop()
+                break
+
+    def stop(self):
+        self.running = False
+        self.cap.release()
+        cv2.destroyAllWindows()
+
+
+# ==================== 测试入口 ====================
+if __name__ == "__main__":
+    # 模拟 MAVLink 代理（实际应替换为真实对象）
+    class MockMavProxy:
+        def send_setpoint(self, control_mode="VELOCITY", **kwargs):
+            print(f"📤 发送指令: {control_mode} {kwargs}")
+            return True, {"result": "ok"}
+
+    mav = MockMavProxy()
+    controller = ObstacleAvoidanceController(
+        camera_id=0,
+        grid_step=25,
+        reset_threshold=30.0,
+        cruise_speed=2.0
+    )
+    controller.update_feedback(0.0, 0.0)  # 模拟初始反馈
+
+    try:
+        controller.run_forever(mav, enable_pid=True)
+    except KeyboardInterrupt:
+        controller.stop()
+        print("🛑 已退出")

drone_controller/pid_counter.py

@@ -15,6 +15,8 @@ class PID:
         self.last_err = 0.0
         self.integral = 0.0
         self.last_time = time.time()
+        # 积分冻结标志位，默认为不冻结（False）
+        self.integral_frozen = False
 
     def update(self, error):
         """
@@ -28,8 +30,11 @@ class PID:
         # 1. 比例项
         p_out = self.kp * error
 
-        # 2. 积分项（增加累加计算，并对积分做出局部限幅防止初段积分饱和）
-        self.integral += error * dt
+        # 2. 积分项
+        # 增加策略：只有在未冻结时，才进行积分累加
+        if not self.integral_frozen:
+            self.integral += error * dt
+            
         i_out = self.ki * self.integral
         # 将积分上限限制在总输出幅值的 20%
         i_out = max(min(i_out, self.max_out * 0.2), self.min_out * 0.2)
@@ -56,12 +61,41 @@ class PID:
         self.last_err = 0.0
         self.integral = 0.0
         self.last_time = time.time()
+        self.integral_frozen = False  # 重置时默认恢复积分功能
 
     def reset_integral(self):
         """
         仅清空积分项（I term），保留上一帧误差以维持微分（D term）连续性
-
-        用于卡尔曼 coast（滑行预测）期间：
-          只靠比例 (P) + 微分 (D) 稳住，积分项清零防止过冲。
+            用于卡尔曼 coast（滑行预测）期间：
+            只靠比例 (P) + 微分 (D) 稳住，积分项清零防止过冲。
         """
         self.integral = 0.0
+
+    # ================== 新增方法 ==================
+
+    def freeze_integral(self):
+        """
+        冻结积分项：
+        保持当前的积分值（保持之前的成果），但不再随时间累加新的误差。
+        
+        适用场景：执行机构饱和（如马达已满载）、或者进入特定暂态过程。
+        """
+        self.integral_frozen = True
+
+    def unfreeze_integral(self, mode="maintain"):
+        """
+        配套解冻策略：
+        
+        :param mode: 解冻模式选择
+            - "maintain": 保持冻结期间的值，直接从该点继续累加（最常用，平滑过渡）。
+            - "clear": 解冻的同时清空历史积分，完全重新开始。
+            - "fade": 仅保持原有积分的 50%（衰减），防止突变过冲。
+        """
+        self.integral_frozen = False
+        
+        if mode == "clear":
+            self.integral = 0.0
+        elif mode == "fade":
+            self.integral *= 0.5
+        elif mode == "maintain":
+            pass # 保持原样，直接继续累加

drone_controller/router_proxy.py

@@ -105,6 +105,17 @@ class RouterProxy:
         self._alert_callback = None
         self._waypoints_callback = None
 
+        self._last_logged_msg_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 严重过期" 日志时间
+
+        # ---- 日志节流：PX4_ACK 按 (cmd, result) 去重 ----
+        self._last_ack_log_time = {}       # {(cmd, result): 上次打印时间}
+
     # ================================================================
     #  内部工具
     # ================================================================
@@ -123,6 +134,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", "?")
@@ -173,7 +190,8 @@ class RouterProxy:
 
         位置控制:
             proxy.send_setpoint(control_mode="POSITION",
-                                lat=22.543210, lon=113.887650, z=-8.0, yaw=0.0)
+                                x=1.0, y=0.0, z=-0.5, yaw=0.0,
+                                frame="LOCAL_OFFSET_NED")
         速度控制:
             proxy.send_setpoint(control_mode="VELOCITY",
                                 vx=1.0, vy=0.0, vz=0.0, yaw_rate=0.0)
@@ -254,31 +272,41 @@ class RouterProxy:
     # ================================================================
 
     def _send_req(self, msg):
-        """
-        发送 REQ 并等待 ACK 回复
-
-        ZeroMQ REQ-REP 是严格的交替模式：send → recv → send → recv
-        本方法保证每次调用完成一次完整的 send-recv 周期。
-
-        Returns:
-            (ok: bool, ack_dict: dict)
-            ack_dict 示例：{"type": "ACK", "ref_id": 1, "status": "OK", "message": ""}
-        """
         msg_type = msg.get("type", "?")
         msg_id = msg.get("id", "?")
         summary = self._build_send_summary(msg)
-        logger.debug("Sending %s id=%s: %s", msg_type, msg_id, summary)
-        logger.info("🚀 发送原始 JSON 指令: %s", json.dumps(msg))
+        
+        # ---- 发送日志 ----
+        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"
-            logger.info("ACK received for id=%s, status=%s",
-                        ack.get("ref_id"), ack.get("status"))
+            
+            # ---- ACK 日志：与发送日志同步，只在对应发送被打印时输出 ----
+            if should_log:
+                logger.info("ACK received for id=%s, status=%s", 
+                            ack.get("ref_id"), ack.get("status"))
+            
             if not ok:
+                # 非 OK 状态无论如何都打印（这是异常）
                 logger.warning("ACK 非 OK: type=%s id=%s ack=%s", 
-                               msg_type, msg_id, ack)
+                            msg_type, msg_id, ack)
             return ok, ack
         except zmq.Again:
             logger.error("REQ recv 超时 (type=%s id=%s)", msg_type, msg_id)
@@ -326,12 +354,19 @@ 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)
+                # 严重过期（长时断流）30 秒提示一次即可，避免持续刷屏
+                if now - self._last_severe_warn_time >= 30.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):
@@ -438,8 +473,14 @@ class RouterProxy:
                             logger.error("ALERT 回调异常: %s", e)
 
                 elif msg_type == "PX4_ACK":
-                    logger.info("PX4_ACK: cmd=%s result=%s",
-                                data.get("ref_cmd"), data.get("result"))
+                    # 节流：相同 (cmd, result) 30 秒内只打印一次，新组合立即可见
+                    ack_key = (data.get("ref_cmd"), data.get("result"))
+                    now = time.time()
+                    last = self._last_ack_log_time.get(ack_key, 0)
+                    if now - last >= 30.0:
+                        self._last_ack_log_time[ack_key] = now
+                        logger.info("PX4_ACK: cmd=%s result=%s",
+                                    data.get("ref_cmd"), data.get("result"))
 
                 elif msg_type == "QGC_WAYPOINTS":
                     logger.info("📥 收到 QGC_WAYPOINTS: count=%d", data.get("count", 0))

drone_controller/target_tracker.py

@@ -3,11 +3,12 @@ target_tracker.py — 轻量级多目标追踪过滤器
 
 【解决的问题】
   1. 目标短暂丢帧（2-5帧）导致无人机抽搐 → 卡尔曼滤波 coast/predict 平滑过渡
-  2. 多目标 ID 飘移导致甩头 → 最邻近欧氏距离关联锁定 ID 不变
+  2. 多目标 ID 飘移导致甩头 → 中心距离 + IoU 加权关联 + 主目标锁定
 
 【核心设计】
   - 恒定速度卡尔曼滤波器（Kalman Filter × 8状态：cx, cy, w, h, vx, vy, vw, vh）
-  - 最邻近贪婪关联，阈值 200px 内视为同一目标
+  - 最邻近贪婪关联（中心距离 + IoU 加权代价），提高交叉/靠近时的 ID 稳定性
+  - 主目标锁定：一旦选定 primary_id，只要该轨道还活着就持续锁定，避免被新高置信度目标抢走
   - 死区预测（Coast/Predict）：连续丢失 ≤ max_lost_frames 帧时用滤波器预测值
   - 超过 max_lost_frames → 释放轨道，对外判丢失
 
@@ -289,7 +290,8 @@ class TargetTracker:
 
     职责：
       - 接收每帧原始 YOLO 检测列表
-      - 通过卡尔曼滤波 + 最邻近关联维持稳定目标ID
+      - 通过卡尔曼滤波 + 中心距离/IoU 关联维持稳定目标ID
+      - 主目标一旦锁定就持续跟随，直到该轨道真正死亡才重新选择
       - 提供平滑后的目标框 / 中心点供 PID 控制使用
       - 自动回收超过丢失上限的旧轨道
 
@@ -297,8 +299,10 @@ class TargetTracker:
       | 参数 | 默认值 | 说明 |
       |------|--------|------|
       | max_lost_frames | 8 | 目标连续消失帧数上限（~250ms @ 30fps） |
-      | max_association_dist | 200 | 关联距离阈值（像素），超过视为新目标 |
+      | max_association_dist | 200 | 中心距离软阈值（像素），用于归一化代价 |
       | min_hits | 3 | 新轨道确认前需要连续匹配帧数 |
+      | dist_weight | 0.5 | 关联代价中中心距离的权重 |
+      | iou_weight | 0.5 | 关联代价中 (1-IoU) 的权重 |
 
     update() 返回值:
       {
@@ -322,21 +326,28 @@ class TargetTracker:
     """
 
     def __init__(self, max_lost_frames=8, max_association_dist=200.0,
-                 min_hits=3):
+                 min_hits=3, dist_weight=0.5, iou_weight=0.5):
         """
         Args:
-            max_lost_frames:     最大连续丢失帧数（默认 8 ≈ 250-300ms）
-            max_association_dist:关联距离阈值（像素，默认 200px）
-            min_hits:            新轨道确认前最少连续匹配帧数（默认 3）
+            max_lost_frames:      最大连续丢失帧数（默认 8 ≈ 250-300ms）
+            max_association_dist: 中心距离软阈值（像素，默认 200px），用于归一化
+            min_hits:             新轨道确认前最少连续匹配帧数（默认 3）
+            dist_weight:          关联代价中中心距离项的权重（默认 0.5）
+            iou_weight:           关联代价中 (1 - IoU) 项的权重（默认 0.5）
         """
         self.max_lost_frames = max_lost_frames
         self.max_association_dist = max_association_dist
         self.min_hits = min_hits
+        self.dist_weight = dist_weight
+        self.iou_weight = iou_weight
 
         self._next_id = 0
         self._tracks = {}         # {track_id: _Tracklet}
         self._frame_count = 0
 
+        # 主目标锁定：一旦选定就持续锁定，直到该轨道死亡
+        self._locked_primary_id = None
+
         # 缓存上一帧的 primary 结果（锁外创建副本供控制线程读取）
         self._last_result = {
             "tracked": False,
@@ -392,6 +403,7 @@ class TargetTracker:
             matches = []
             unmatched_tids = list(self._tracks.keys())
             unmatched_dids = []
+            det_centers = []
 
         # ---- 步骤 3：更新匹配的轨道 ----
         for tid, did in matches:
@@ -406,6 +418,9 @@ class TargetTracker:
                 if self._tracks[tid].is_dead:
                     logger.debug("轨道 %d 已死亡 (lost=%d), 回收",
                                  tid, self._tracks[tid].lost_count)
+                    # 如果死的是锁定主目标，清除锁定
+                    if tid == self._locked_primary_id:
+                        self._locked_primary_id = None
                     del self._tracks[tid]
 
         # ---- 步骤 5：未匹配的检测 → 创建新轨道 ----
@@ -424,7 +439,7 @@ class TargetTracker:
             self._tracks[self._next_id] = new_track
             self._next_id += 1
 
-        # ---- 步骤 6：选取主目标（优先级：置信度 > 命中数 > 贴近画面中心） ----
+        # ---- 步骤 6：选取主目标（带锁定逻辑） ----
         result = self._select_primary(frame_shape)
         self._last_result = result
         return result
@@ -438,6 +453,7 @@ class TargetTracker:
         self._tracks.clear()
         self._next_id = 0
         self._frame_count = 0
+        self._locked_primary_id = None
         self._last_result = {
             "tracked": False,
             "primary_id": None,
@@ -469,9 +485,52 @@ class TargetTracker:
     #  内部方法
     # ------------------------------------------------------------------
 
+    @staticmethod
+    def _iou(box_a, box_b):
+        """
+        计算两个轴对齐框的 IoU
+
+        Args:
+            box_a, box_b: [x1, y1, x2, y2]
+
+        Returns:
+            float — IoU ∈ [0, 1]
+        """
+        if box_a is None or box_b is None:
+            return 0.0
+
+        ax1, ay1, ax2, ay2 = box_a
+        bx1, by1, bx2, by2 = box_b
+
+        inter_x1 = max(ax1, bx1)
+        inter_y1 = max(ay1, by1)
+        inter_x2 = min(ax2, bx2)
+        inter_y2 = min(ay2, by2)
+
+        inter_w = max(0.0, inter_x2 - inter_x1)
+        inter_h = max(0.0, inter_y2 - inter_y1)
+        inter_area = inter_w * inter_h
+
+        if inter_area <= 0.0:
+            return 0.0
+
+        area_a = max(0.0, (ax2 - ax1) * (ay2 - ay1))
+        area_b = max(0.0, (bx2 - bx1) * (by2 - by1))
+        union = area_a + area_b - inter_area
+
+        if union <= 0.0:
+            return 0.0
+        return inter_area / union
+
     def _associate(self, det_centers):
         """
-        最邻近贪婪关联
+        中心距离 + IoU 加权的贪婪关联
+
+        代价公式：
+            cost = dist_weight * (center_dist / max_association_dist)
+                 + iou_weight  * (1 - IoU)
+
+        仅当 cost < 1.0 时才视为有效匹配。
 
         Args:
             det_centers: [(idx, cx, cy, w, h, conf, cls_id), ...]
@@ -482,37 +541,48 @@ class TargetTracker:
         if not self._tracks or not det_centers:
             return [], list(self._tracks.keys()), list(range(len(det_centers)))
 
-        # 提取所有轨道的预测位置
-        track_preds = {}  # {tid: (cx, cy)}
-        for tid, track in self._tracks.items():
-            cx, cy = track.last_center
-            track_preds[tid] = (cx, cy)
+        # 预先计算每个检测的框
+        det_boxes = []
+        for _, cx, cy, w, h, _, _ in det_centers:
+            det_boxes.append([cx - w / 2.0, cy - h / 2.0,
+                              cx + w / 2.0, cy + h / 2.0])
 
-        # 贪婪匹配：每条轨道找最近的未分配检测
         matches = []
         assigned_dets = set()
         assigned_tracks = set()
 
-        # 先将轨道按置信度排序（更可靠的轨道优先匹配）
+        # 置信度高的轨道优先匹配
         sorted_tracks = sorted(
             self._tracks.items(),
             key=lambda kv: kv[1].confidence,
             reverse=True
         )
 
+        max_dist = max(self.max_association_dist, 1e-3)
+
         for tid, track in sorted_tracks:
             if tid in assigned_tracks:
                 continue
-            pred_cx, pred_cy = track_preds[tid]
 
-            best_dist = self.max_association_dist
+            pred_cx, pred_cy = track.last_center
+            pred_box = track.last_box
+
+            best_cost = 1.0
             best_did = None
+
             for did, _, d_cx, d_cy, _, _, _ in det_centers:
                 if did in assigned_dets:
                     continue
+
                 dist = np.hypot(pred_cx - d_cx, pred_cy - d_cy)
-                if dist < best_dist:
-                    best_dist = dist
+                norm_dist = dist / max_dist
+                iou_val = self._iou(pred_box, det_boxes[did])
+
+                cost = (self.dist_weight * norm_dist +
+                        self.iou_weight * (1.0 - iou_val))
+
+                if cost < best_cost:
+                    best_cost = cost
                     best_did = did
 
             if best_did is not None:
@@ -527,9 +597,13 @@ class TargetTracker:
 
     def _select_primary(self, frame_shape=None):
         """
-        从活跃轨道中选出主目标
+        从活跃轨道中选出主目标（带锁定）
 
-        优先级：
+        锁定逻辑：
+          1. 如果已有 _locked_primary_id，且该轨道仍存在 → 直接继续用它
+          2. 只有锁定目标真正消失（被回收）后，才重新按优先级选新主目标并锁定
+
+        新选主目标时的优先级：
           1. 含有实测（非 coasting）且确认（hits ≥ min_hits）
           2. 实测轨道中置信度最高
           3. 若无实测轨道，取 coast 中最接近画面中心的
@@ -538,6 +612,7 @@ class TargetTracker:
             dict — 与 update() 返回值格式一致
         """
         if not self._tracks:
+            self._locked_primary_id = None
             return {
                 "tracked": False,
                 "primary_id": None,
@@ -550,28 +625,47 @@ class TargetTracker:
                 "raw": None,
             }
 
-        # 分组：实测轨道 vs coasting 轨道
+        # ---------- 优先使用已锁定的主目标 ----------
+        if self._locked_primary_id is not None and self._locked_primary_id in self._tracks:
+            track = self._tracks[self._locked_primary_id]
+            cx, cy = track.last_center
+            box = track.last_box
+            conf = track.confidence
+            lost = track.lost_count
+            is_predicted = track.lost_count > 0
+
+            return {
+                "tracked": True,
+                "primary_id": self._locked_primary_id,
+                "center": (float(cx), float(cy)),
+                "box": box,
+                "confidence": float(conf),
+                "is_predicted": is_predicted,
+                "lost_frames": lost,
+                "n_active": len(self._tracks),
+                "raw": (None if is_predicted else box),
+            }
+
+        # ---------- 没有锁定目标，或锁定目标已死 → 重新选择 ----------
         measured = []
         coasting = []
 
         for tid, track in self._tracks.items():
             entry = (tid, track)
-            if track.lost_count == 0 and track.hits >= self.min_hits:
-                measured.append(entry)
-            elif track.lost_count == 0:
-                # hits < min_hits 尚未确认，视为暂态
+            if track.lost_count == 0:
                 measured.append(entry)
             else:
                 coasting.append(entry)
 
-        # 优先从实测轨道选
         chosen = None
         is_predicted = False
 
         if measured:
-            # 置信度最高的实测轨道
-            measured.sort(key=lambda kv: kv[1].confidence, reverse=True)
-            chosen = measured[0]
+            # 优先选已确认（hits 足够）且置信度最高的
+            confirmed = [e for e in measured if e[1].hits >= self.min_hits]
+            pool = confirmed if confirmed else measured
+            pool.sort(key=lambda kv: kv[1].confidence, reverse=True)
+            chosen = pool[0]
         elif coasting:
             # 无实测时选最靠近画面中心的
             cx_frame = frame_shape[1] / 2.0 if frame_shape else 320.0
@@ -584,6 +678,7 @@ class TargetTracker:
             is_predicted = True
 
         if chosen is None:
+            self._locked_primary_id = None
             return {
                 "tracked": False,
                 "primary_id": None,
@@ -597,6 +692,9 @@ class TargetTracker:
             }
 
         tid, track = chosen
+        # 锁定新主目标
+        self._locked_primary_id = tid
+
         cx, cy = track.last_center
         box = track.last_box
         conf = track.confidence
@@ -618,4 +716,5 @@ class TargetTracker:
         tracks_info = ", ".join(str(t) for t in self._tracks.values())
         return (f"TargetTracker(frame={self._frame_count}, "
                 f"active={len(self._tracks)}, "
+                f"locked={self._locked_primary_id}, "
                 f"tracks=[{tracks_info}])")

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
@@ -440,6 +441,14 @@ class UAVControlLoop:
         self._log_throttle = {}  # key → 上次打印时间戳
         self._brake_last_logged = False  # 刹车是否已输出过日志
 
+        # ---- 光流避障决策（外部注入） ----
+        self.obstacle_yaw_rate = 0.0
+        self.obstacle_speed_scale = 1.0
+        self.obstacle_active = False
+        self.cruise_speed = fc_cfg.get("cruise_speed", 1.0)   # 默认 1 m/s
+        # ---- 光流线程锁 ----
+        self._obstacle_lock = threading.Lock()
+
     def start_uav(self):
         """连接 Router，执行标准起飞序列（协议 §3.3.1）
 
@@ -648,6 +657,21 @@ class UAVControlLoop:
 
         return tracked, result
     
+    # ---- 光流避障决策 ----
+    def set_obstacle_decision(self, yaw_rate: float, speed_scale: float):
+        """
+        由外部注入光流避障决策。
+        当 yaw_rate != 0 或 speed_scale < 0.95 时视为激活。
+        """
+        with self._obstacle_lock:
+            self.obstacle_yaw_rate = yaw_rate
+            self.obstacle_speed_scale = speed_scale
+            self.obstacle_active = (abs(yaw_rate) > 0.02 or speed_scale < 0.95)
+            if self.obstacle_active:
+                self.logger.debug("光流避障激活: yaw=%.2f, scale=%.2f", yaw_rate, speed_scale)
+            else:
+                self.logger.debug("光流避障已清除")
+
     # ---- 内部控制线程 ----
 
     def _control_loop(self):
@@ -703,15 +727,41 @@ class UAVControlLoop:
                 else:
                     self._brake_last_logged = False
 
-            # ---- 记录当前速度指令（供外部日志读取） ----
-            self._last_vx, self._last_vy, self._last_vz = vx, vy, vz
             
             # ---- ④ 通过 ZMQ 发送 VELOCITY SETPOINT（协议 §3.1 速度控制） ----
+
+            # ---- 防御性检查：飞控已切到 AUTO.LAND/AUTO.RTL 则不再发送 ----
+            state = self.proxy.get_latest_state()
+            if state:
+                mode = state.get("drone", {}).get("mode", "")
+                if mode in ("AUTO.LAND", "AUTO.RTL"):
+                    # 静默跳过，不发送 SETPOINT
+                    continue
+            # ---- 决策融合：光流避障是否覆盖 ----
+            with self._obstacle_lock:
+                if self.obstacle_active:
+                    # 避障模式：前向速度 × 缩放，偏航角速度由光流决策给出
+                    cmd_vx = self.cruise_speed * self.obstacle_speed_scale
+                    cmd_vy = 0.0
+                    cmd_vz = 0.0
+                    cmd_yaw_rate = self.obstacle_yaw_rate
+                else:
+                    # 正常追踪模式
+                    cmd_vx = vx       # 原逻辑 vx 始终为 0（可保留）
+                    cmd_vy = vy
+                    cmd_vz = vz
+                    cmd_yaw_rate = 0.0
+
+            # 记录当前速度指令（供外部日志读取）
+            self._last_vx, self._last_vy, self._last_vz = cmd_vx, cmd_vy, cmd_vz
+
+            # ---- ⑤ 通过 ZMQ 发送 VELOCITY SETPOINT ----
             self.logger.debug("SETPOINT: control_mode=VELOCITY "
-                              "vx=%.2f vy=%.2f vz=%.2f", vx, vy, vz)
+                              "vx=%.2f vy=%.2f vz=%.2f yaw_rate=%.2f",
+                              cmd_vx, cmd_vy, cmd_vz, cmd_yaw_rate)
             ok, ack = self.proxy.send_setpoint(
-                vx=vx, vy=vy, vz=vz,
-                yaw_rate=0.0,
+                vx=cmd_vx, vy=cmd_vy, vz=cmd_vz,
+                yaw_rate=cmd_yaw_rate,
                 control_mode="VELOCITY"
             )
             if not ok and self.running:
@@ -719,7 +769,7 @@ class UAVControlLoop:
                 # recv 超时或格式错误由 _send_req 内部恢复 REQ socket
                 self.logger.warning("SETPOINT ACK 失败: %s", ack)
 
-            # ---- ⑤ 精确控制循环频率（20Hz → 每 50ms） ----
+            # ---- ⑥ 精确控制循环频率（20Hz → 每 50ms） ----
             elapsed = time.time() - start
             sleep_time = self.control_interval - elapsed
             if sleep_time > 0:

run_mission.py

@@ -13,7 +13,7 @@ run_mission.py — 基于 ZMQ Router 协议的主任务入口
          ▼              ▼
     ┌─ MissionManager   ┌─ UAVControlLoop ──┐
     │  状态机 + 航点巡航  │  20Hz VELOCITY    │
-    │  POSITION SETPOINT │  闭环追踪          │
+    │  VELOCITY SETPOINT │  闭环追踪          │
     │  (非追踪期间)      │  (VISUAL_TRACKING) │
     └───────────────────┴────────────────────┘
     ┌─ 感知模块 ─────────┐
@@ -23,7 +23,8 @@ run_mission.py — 基于 ZMQ Router 协议的主任务入口
 
 【控制权协调规则】
   同一时刻只有一个人持有「SETPOINT 发令权」：
-    - VISUAL_TRACKING 之外 → MissionManager 发 POSITION SETPOINT
+    - VISUAL_TRACKING 之外 → MissionManager 发 VELOCITY SETPOINT（航点/
+      悬停均用速度闭环，与坐标系无关；仅 TAKEOFF 预热用零偏移 POSITION）
     - VISUAL_TRACKING 之中 → UAVControlLoop 线程发 VELOCITY SETPOINT
     - 切换由主循环检测 MissionManager.state 变化时触发
 
@@ -46,8 +47,6 @@ import threading
 import time
 import socket
 import cv2
-import queue
-import csv
 
 from drone_controller.mission_manager import (
     MissionManager,
@@ -61,6 +60,10 @@ from drone_controller.router_proxy import RouterProxy
 # run_mission.py 在外部协调其控制线程的启停。
 from infer_camera_modular import UAVControlLoop, VideoStreaming, YOLO26UAVInfer
 # 注：coord.latlon_to_ned 不再需要 — 航点直接存储原始经纬度
+# ---- 飞行日志 ----
+from utils.flight_logger import FlightLogger
+# ---- 光流模块 ----
+from drone_controller.optical_flow_controller import OpticalFlowObstacleDetector
 
 logger = logging.getLogger("RunMission")
 
@@ -95,16 +98,7 @@ class MissionOrchestrator:
         # csv飞行日志参数
         # ================================================================
         log_cfg = cfg.get("log", {})
-        self.flight_log_enabled = log_cfg.get("enable_flight_log", True)
-        self.flight_log_interval = log_cfg.get("flight_log_interval", 20)  # 20帧 ≈ 1秒
-        self.log_dir = log_cfg.get("log_dir", "./log")
-        self._log_queue = queue.Queue(maxsize=500)  # 最多缓存500条，防止内存泄漏
-        self._log_thread = None
-        self._log_running = False
-        self._log_frame_counter = 0
-        self._log_file_path = None
-        self._log_worker_started = False
-
+        self.flight_logger = FlightLogger(log_cfg)
         self._latest_detections = None
 
         # ================================================================
@@ -120,6 +114,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,
@@ -127,6 +122,18 @@ 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),
+        )
+
+        # ================================================================
+        # 💡 OpticalFlowObstacleDetector （光流计算）
+        # ================================================================
+        flow_cfg = cfg.get("optical_flow", {})
+        self.optical_flow = OpticalFlowObstacleDetector(
+            grid_step=flow_cfg.get("grid_step", 25),
+            reset_threshold=flow_cfg.get("reset_threshold", 30.0)
         )
 
         # ================================================================
@@ -153,141 +160,6 @@ class MissionOrchestrator:
         self.logger.info("MissionOrchestrator 初始化完成")
 
     # ================================================================
-    # 📝 飞行日志相关
-    # ================================================================
-    def _start_flight_logger(self):
-        """启动异步日志后台线程"""
-        if not self.flight_log_enabled:
-            self.logger.info("📝 飞行日志已禁用（config 中 enable_flight_log=false）")
-            return
-
-        if self._log_worker_started:
-            return
-
-        # 确保日志目录存在
-        os.makedirs(self.log_dir, exist_ok=True)
-
-        # 生成日志文件名（按时间命名）
-        timestamp = time.strftime("%Y%m%d_%H%M%S")
-        self._log_file_path = os.path.join(self.log_dir, f"flight_{timestamp}.csv")
-
-        # 写入 CSV 表头（如果文件不存在）
-        with open(self._log_file_path, 'w', newline='') as f:
-            writer = csv.writer(f)
-            writer.writerow([
-                'timestamp_unix',
-                'alt_rel_m',
-                'ground_speed_mps',
-                'vx', 'vy', 'vz',
-                'battery_percent',
-                'state',
-                'wp_index',
-                'detect_count',
-                'tracking_active'
-            ])
-
-        self._log_running = True
-        self._log_thread = threading.Thread(
-            target=self._flight_log_worker,
-            daemon=True,
-            name="flight-log-writer"
-        )
-        self._log_thread.start()
-        self._log_worker_started = True
-        self.logger.info(f"📝 异步飞行日志已启动 -> {self._log_file_path} (降采样: 每{self.flight_log_interval}帧写1次)")
-
-    def _flight_log_worker(self):
-        """后台线程：从队列取数据，批量写入 CSV"""
-        buffer = []
-        BATCH_SIZE = 10  # 攒够 10 条一次性写入，减少 I/O
-
-        while self._log_running:
-            try:
-                # 阻塞等待 1 秒，有数据则取，超时则检查是否退出
-                item = self._log_queue.get(timeout=1.0)
-                if item is None:  # 停止信号
-                    break
-                buffer.append(item)
-
-                # 攒够一批或队列为空且需要退出时写入
-                if len(buffer) >= BATCH_SIZE:
-                    self._flush_log_buffer(buffer)
-                    buffer = []
-
-            except queue.Empty:
-                # 超时无数据，检查是否还有残留
-                if not self._log_running and not self._log_queue.empty():
-                    # 收尾：把剩余的数据写完
-                    while not self._log_queue.empty():
-                        try:
-                            buffer.append(self._log_queue.get_nowait())
-                        except queue.Empty:
-                            break
-                    if buffer:
-                        self._flush_log_buffer(buffer)
-                continue
-            except Exception as e:
-                self.logger.error(f"飞行日志线程异常: {e}")
-                time.sleep(0.1)
-
-        # 最终收尾：清空缓冲区
-        if buffer:
-            self._flush_log_buffer(buffer)
-        self.logger.info("📝 飞行日志线程已停止")
-
-    def _flush_log_buffer(self, buffer):
-        """将缓冲区数据写入 CSV 文件（由后台线程调用）"""
-        if not buffer or not self._log_file_path:
-            return
-        try:
-            with open(self._log_file_path, 'a', newline='') as f:
-                writer = csv.writer(f)
-                writer.writerows(buffer)
-        except Exception as e:
-            self.logger.error(f"写入飞行日志失败: {e}")
-
-    # run_mission.py — MissionOrchestrator 类中新增
-
-    def _collect_and_enqueue_log(self):
-        """采集当前飞行状态，推入日志队列（主线程执行，极轻量）"""
-        try:
-            state = self.proxy.get_latest_state()
-            if state is None:
-                return
-
-            drone = state.get("drone", {})
-            # 当前状态机的状态
-            current_state = self.mission.state.name if self.mission else "UNKNOWN"
-            wp_idx = self.mission.current_wp_index if self.mission else 0
-            # 追踪线程是否激活（来自控制权协调标志）
-            tracking_active = 1 if self._control_active else 0
-
-            # 组装日志行（顺序要与 CSV 表头一致）
-            row = (
-                time.time(),  # timestamp_unix
-                drone.get("alt_rel", 0.0),  # alt_rel_m
-                drone.get("ground_speed", 0.0),  # ground_speed_mps
-                drone.get("vx", 0.0),  # vx
-                drone.get("vy", 0.0),  # vy
-                drone.get("vz", 0.0),  # vz
-                drone.get("battery", 0.0),  # battery_percent
-                current_state,  # state (string)
-                wp_idx,  # wp_index
-                len(self._latest_detections) if self._latest_detections is not None else 0,  # detect_count
-                tracking_active  # tracking_active
-            )
-
-            # 非阻塞推入队列（如果队列满了，丢弃旧数据，防止内存爆炸）
-            try:
-                self._log_queue.put_nowait(row)
-            except queue.Full:
-                # 队列满了说明写入线程跟不上，丢掉这条数据，记录警告（但不要经常触发）
-                pass
-
-        except Exception as e:
-            # 日志采集失败不影响主循环
-            pass
-    # ================================================================
     # 📥 QGC 航点回调（由 RouterProxy 的 SUB 线程触发）
     # ================================================================
 
@@ -356,8 +228,8 @@ class MissionOrchestrator:
                         "command": "WAYPOINT",
                         "hold_duration": hold_time,
                     })
-                    self.logger.debug("航点 %d [WAYPOINT]: lat=%.6f lon=%.6f alt=%.1f",
-                                      idx, lat, lon, alt)
+                    self.logger.info("航点 %d [WAYPOINT]: lat=%.6f lon=%.6f alt=%.1f",
+                                     idx, lat, lon, alt)
 
             self.mission.set_waypoints(converted)
 
@@ -491,8 +363,7 @@ class MissionOrchestrator:
         if cmd_cfg.get("enabled", True):
             self._start_tcp_command_server()
 
-        # ---- 启动异步飞行日志 ----
-        self._start_flight_logger()
+        # ---- 飞行日志由 FlightLogger 在构造时自动启动 ----
 
         self.running = True
         self.logger.info("🚀 所有子系统就绪，主循环启动")
@@ -555,6 +426,15 @@ class MissionOrchestrator:
                                          detections, orig_shape)
                 prev_state = current_state
 
+                # ---- 光流避障决策 ----
+                if self.mission.state == MissionState.VISUAL_TRACKING:
+                    _, _ = self.optical_flow.process_frame(frame)
+                    yaw_setpoint, speed_scale = self.optical_flow.get_zone_decision()
+                    self.controller.set_obstacle_decision(yaw_setpoint, speed_scale)
+                else:
+                    # 非追踪状态清除避障
+                    self.controller.set_obstacle_decision(0.0, 1.0)
+
                 # ---- ④ 状态机更新 ----
                 self.mission.update(target_detected=target_detected)
 
@@ -563,10 +443,14 @@ class MissionOrchestrator:
                 self.streamer.push_to_stream(frame)
 
                 # ---- 采集飞行日志 ----
-                if self.flight_log_enabled and self._log_worker_started:
-                    self._log_frame_counter += 1
-                    if self._log_frame_counter % self.flight_log_interval == 0:
-                        self._collect_and_enqueue_log()
+                if self.flight_logger.enabled and self.flight_logger.should_collect():
+                    self.flight_logger.collect(
+                        state=self.proxy.get_latest_state(),
+                        mission_state_name=self.mission.state.name,
+                        wp_index=self.mission.current_wp_index,
+                        detect_count=len(self._latest_detections) if self._latest_detections is not None else 0,
+                        tracking_active=1 if self._control_active else 0,
+                    )
 
                 # ---- 频率控制 ----
                 elapsed = time.time() - loop_start
@@ -596,7 +480,7 @@ class MissionOrchestrator:
         ┌──────────────┬─────────────────────┬──────────────────────┐
         │ 状态机状态    │ MissionManager      │ UAVControlLoop       │
         ├──────────────┼─────────────────────┼──────────────────────┤
-        │ 非 TRACKING  │ proxy → POSITION    │ 控制线程已停止        │
+        │ 非 TRACKING  │ proxy → VELOCITY    │ 控制线程已停止        │
         │              │   SETPOINT          │                      │
         ├──────────────┼─────────────────────┼──────────────────────┤
         │ TRACKING     │ 不发 SETPOINT       │ 20Hz VELOCITY 线程   │
@@ -727,17 +611,7 @@ class MissionOrchestrator:
         self._stop_control_thread()
 
         # ---- 停止飞行日志线程 ----
-        if self._log_running:
-            self._log_running = False
-            # 给队列发送停止信号
-            try:
-                self._log_queue.put_nowait(None)
-            except queue.Full:
-                pass
-
-            if self._log_thread and self._log_thread.is_alive():
-                self._log_thread.join(timeout=2.0)
-                self.logger.info("✔ 飞行日志线程已停止")
+        self.flight_logger.stop()
 
         # 2. 关闭共享 ZMQ 代理（MissionManager + UAVControlLoop 共用）
         #    注意：UAVControlLoop 不拥有 proxy 生命周期（_owns_proxy=False），

utils/coord.py

@@ -33,6 +33,29 @@ def latlon_to_ned(lat, lon, alt, ref_lat, ref_lon, ref_alt):
     return x, y, z
 
 
+def latlon_alt_to_local_offset(target_lat, target_lon, target_alt,
+                               current_lat, current_lon, current_alt):
+    """
+    将目标经纬高转换为相对当前位置的 LOCAL_OFFSET_NED 偏移量。
+
+    Args:
+        target_lat, target_lon: 目标纬度/经度（度）
+        target_alt:             目标相对高度（米，正值向上）
+        current_lat, current_lon: 当前纬度/经度（度）
+        current_alt:              当前相对高度（米，正值向上）
+
+    Returns:
+        (x, y, z): 相对当前位置的 NED 偏移（米）
+            x: 北向偏移，正值向北
+            y: 东向偏移，正值向东
+            z: 下向偏移，正值下降，负值爬升
+    """
+    return latlon_to_ned(
+        target_lat, target_lon, target_alt,
+        current_lat, current_lon, current_alt,
+    )
+
+
 def haversine(lat1, lon1, lat2, lon2):
     """
     使用 Haversine 公式计算两个经纬度点之间的球面距离（米）

utils/flight_logger.py

@@ -0,0 +1,220 @@
+"""
+flight_logger.py — 异步飞行日志记录器
+
+将飞行状态数据异步写入 CSV 文件，不影响主循环性能。
+
+用法：
+    logger = FlightLogger(log_config)
+
+    # 在主循环中（按间隔调用）
+    logger.collect(state, mission_state_name, wp_index, detect_count, tracking_active)
+
+    # 退出时
+    logger.stop()
+"""
+
+import os
+import time
+import csv
+import queue
+import threading
+import logging
+
+logger = logging.getLogger(__name__)
+
+
+class FlightLogger:
+    """
+    异步飞行日志记录器
+
+    后台线程负责批量写入 CSV，主线程仅做轻量级入队操作。
+    """
+
+    def __init__(self, log_config=None):
+        """
+        Args:
+            log_config: 日志配置字典，支持字段：
+                enable_flight_log (bool):   是否启用（默认 True）
+                flight_log_interval (int):  写入间隔（帧数，默认 20）
+                log_dir (str):              日志目录（默认 "./log"）
+        """
+        if log_config is None:
+            log_config = {}
+
+        self.enabled = log_config.get("enable_flight_log", True)
+        self.log_interval = log_config.get("flight_log_interval", 20)
+        self.log_dir = log_config.get("log_dir", "./log")
+
+        self._queue = queue.Queue(maxsize=500)
+        self._thread = None
+        self._running = False
+        self._file_path = None
+        self._started = False
+        self._frame_counter = 0
+
+        if self.enabled:
+            self._start()
+
+    # ================================================================
+    #  内部方法
+    # ================================================================
+
+    def _start(self):
+        """启动后台日志线程"""
+        os.makedirs(self.log_dir, exist_ok=True)
+
+        timestamp = time.strftime("%Y%m%d_%H%M%S")
+        self._file_path = os.path.join(self.log_dir, f"flight_{timestamp}.csv")
+
+        # 写入 CSV 表头
+        with open(self._file_path, 'w', newline='') as f:
+            writer = csv.writer(f)
+            writer.writerow([
+                'timestamp_unix',
+                'alt_rel_m',
+                'ground_speed_mps',
+                'vx', 'vy', 'vz',
+                'battery_percent',
+                'state',
+                'wp_index',
+                'detect_count',
+                'tracking_active',
+                'yaw',          # 姿态偏航角 (rad)
+                'heading'       # GPS 航向角 (deg)
+            ])
+
+        self._running = True
+        self._thread = threading.Thread(
+            target=self._worker,
+            daemon=True,
+            name="flight-log-writer"
+        )
+        self._thread.start()
+        self._started = True
+        logger.info("📝 异步飞行日志已启动 -> %s (降采样: 每%d帧写1次)",
+                     self._file_path, self.log_interval)
+
+    def _worker(self):
+        """后台线程：从队列取数据，批量写入 CSV"""
+        buffer = []
+        BATCH_SIZE = 10  # 攒够 10 条一次性写入，减少 I/O
+
+        while self._running:
+            try:
+                item = self._queue.get(timeout=1.0)
+                if item is None:  # 停止信号
+                    break
+                buffer.append(item)
+
+                if len(buffer) >= BATCH_SIZE:
+                    self._flush(buffer)
+                    buffer = []
+
+            except queue.Empty:
+                # 超时无数据，检查是否还有残留
+                if not self._running and not self._queue.empty():
+                    while not self._queue.empty():
+                        try:
+                            buffer.append(self._queue.get_nowait())
+                        except queue.Empty:
+                            break
+                    if buffer:
+                        self._flush(buffer)
+                continue
+            except Exception as e:
+                logger.error("飞行日志线程异常: %s", e)
+                time.sleep(0.1)
+
+        # 最终收尾：清空缓冲区
+        if buffer:
+            self._flush(buffer)
+        logger.info("📝 飞行日志线程已停止")
+
+    def _flush(self, buffer):
+        """将缓冲区数据写入 CSV 文件"""
+        if not buffer or not self._file_path:
+            return
+        try:
+            with open(self._file_path, 'a', newline='') as f:
+                writer = csv.writer(f)
+                writer.writerows(buffer)
+        except Exception as e:
+            logger.error("写入飞行日志失败: %s", e)
+
+    # ================================================================
+    #  公开接口
+    # ================================================================
+
+    def should_collect(self):
+        """
+        判断当前帧是否需要采集（基于帧数间隔降采样）
+
+        Returns:
+            bool: 当前帧是否应采集
+
+        用法：
+            if flight_logger.should_collect():
+                flight_logger.collect(...)
+        """
+        if not self._started:
+            return False
+        self._frame_counter += 1
+        return self._frame_counter % self.log_interval == 0
+
+    def collect(self, state, mission_state_name, wp_index,
+                detect_count, tracking_active):
+        """
+        采集当前飞行状态，推入写入队列（主线程调用，极轻量）
+
+        Args:
+            state:               proxy.get_latest_state() 的返回值
+            mission_state_name:  状态机当前状态名 (str)，如 "NAVIGATING"
+            wp_index:            当前航点索引
+            detect_count:        当前帧检测到的目标数
+            tracking_active:     追踪线程是否激活（1 激活 / 0 未激活）
+        """
+        if not self._started or state is None:
+            return
+
+        try:
+            drone = state.get("drone", {})
+            row = (
+                time.time(),                       # timestamp_unix
+                drone.get("alt_rel", 0.0),         # alt_rel_m
+                drone.get("ground_speed", 0.0),    # ground_speed_mps
+                drone.get("vx", 0.0),              # vx
+                drone.get("vy", 0.0),              # vy
+                drone.get("vz", 0.0),              # vz
+                drone.get("battery", 0.0),         # battery_percent
+                mission_state_name,                # state
+                wp_index,                          # wp_index
+                detect_count,                      # detect_count
+                tracking_active,                   # tracking_active
+                drone.get("yaw", 0.0),             # yaw: 姿态偏航角 (rad)
+                drone.get("heading", 0.0),         # heading: GPS 航向角 (deg)
+            )
+
+            # 非阻塞入队，满则丢弃
+            try:
+                self._queue.put_nowait(row)
+            except queue.Full:
+                pass
+
+        except Exception:
+            pass  # 日志采集失败不影响主循环
+
+    def stop(self):
+        """停止后台日志线程"""
+        if not self._started:
+            return
+
+        self._running = False
+        # 发送停止信号
+        try:
+            self._queue.put_nowait(None)
+        except queue.Full:
+            pass
+
+        if self._thread and self._thread.is_alive():
+            self._thread.join(timeout=2.0)
+            logger.info("✔ 飞行日志线程已停止")