Send position setpoints as local offsets

drone_controller/mission_manager.py

@@ -2,9 +2,9 @@
 mission_manager.py — 基于 ZeroMQ RouterProxy 的航点巡航状态机
 
 【协议变更 v2 — 2026-07】
-  SETPOINT 指令的 x/y 字段改为接收经纬度（lat/lon），z 字段接收
+  航点存储格式使用经纬度 (lat, lon, alt)。发送 POSITION SETPOINT
-  相对高度（正值向上）。航点存储格式从 NED (x, y, z) 切换为
+  时在本模块内转换为相对当前位置的 LOCAL_OFFSET_NED 偏移
-  经纬度 (lat, lon, alt)。
+  (x=北向, y=东向, z=向下)。
 
 【核心改动】
   1. self.drone → self.proxy（注入的 RouterProxy 实例）
@@ -29,7 +29,7 @@ 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__)
 
@@ -63,7 +63,7 @@ class FailsafeTriggered(Exception):
 
 class MissionManager:
     """
-    🛸 ZMQ 航点巡航状态机管理器（经纬度协议 v2）
+    🛸 ZMQ 航点巡航状态机管理器（经纬度航点 + 相对位置 SETPOINT）
 
     职责：
       - 封装完整的无人机任务生命周期（初始化 → 解锁 → 起飞 →
@@ -395,10 +395,10 @@ class MissionManager:
             else:
                 logger.warning("[TAKEOFF] WAYPOINT 失败: %s，尝试 SETPOINT 爬升", ack)
                 drone = state.get("drone", {})
-                ok2, _ = self.proxy.send_setpoint(
+                ok2, _ = self._send_relative_position_setpoint(
-                    lat=drone.get("lat", 0.0), lon=drone.get("lon", 0.0),
+                    state,
-                    z=self.target_altitude, yaw=0.0,
+                    drone.get("lat", 0.0), drone.get("lon", 0.0),
-                    control_mode="POSITION",
+                    self.target_altitude, yaw=0.0,
                 )
                 if ok2:
                     self.state = MissionState.TAKEOFF
@@ -417,7 +417,7 @@ class MissionManager:
 
         流程（协议 §3.3.1）：
           ① 等待高度到达目标 90% 且 mode == "HOLD"
-          ② 发送预热 SETPOINT（当前位置经纬度悬停）
+          ② 发送预热 SETPOINT（相对当前位置悬停）
           ③ time.sleep(0.2) 等待流建立
           ④ 发送 COMMAND("OFFBOARD")
           ⑤ 轮询等待 mode == "OFFBOARD"
@@ -444,11 +444,11 @@ class MissionManager:
             logger.info("[TAKEOFF] 高度 %.1fm，模式 %s，准备切换 OFFBOARD...",
                         alt_rel, mode)
 
-            # 发送预热 SETPOINT（v2: x=经度, y=纬度, z=相对高度）
+            # 发送预热 SETPOINT（相对当前位置悬停）
-            self.proxy.send_setpoint(
+            self._send_relative_position_setpoint(
-                lat=drone.get("lat", 0.0), lon=drone.get("lon", 0.0),
+                state,
-                z=alt_rel, yaw=0.0,
+                drone.get("lat", 0.0), drone.get("lon", 0.0),
-                control_mode="POSITION",
+                alt_rel, yaw=0.0,
             )
             time.sleep(0.2)
 
@@ -488,7 +488,7 @@ class MissionManager:
         """
         [NAVIGATING] 每帧发送 POSITION SETPOINT 前往当前航点
 
-        v2: send_setpoint(x=lat, y=lon, z=alt)，到达判定使用 Haversine 距离。
+        POSITION SETPOINT 发送 LOCAL_OFFSET_NED 相对偏移；到达判定使用 Haversine 距离。
         到达判定后推进索引；所有航点飞完后根据 return_to_home 决定返航或悬停。
         """
         if not self.waypoints or self.current_wp_index >= len(self.waypoints):
@@ -517,10 +517,9 @@ class MissionManager:
         wp_alt = wp.get("alt", self.target_altitude)
         wp_yaw = wp.get("yaw", 0.0)
 
-        # v2: 发送 POSITION SETPOINT（经纬度 + 高度）
+        # 发送 POSITION SETPOINT（相对当前位置 LOCAL_OFFSET_NED）
-        self.proxy.send_setpoint(
+        self._send_relative_position_setpoint(
-            lat=wp_lat, lon=wp_lon, z=wp_alt, yaw=wp_yaw,
+            state, wp_lat, wp_lon, wp_alt, yaw=wp_yaw,
-            control_mode="POSITION",
         )
 
         # ---- 共享目标 ----
@@ -585,13 +584,12 @@ class MissionManager:
             self.tracking_completed = False
             return
 
-        # ---- 保持当前位置悬停（v2: x=lat, y=lon, z=alt） ----
+        # ---- 保持当前位置悬停（相对当前位置 LOCAL_OFFSET_NED） ----
         wp = self.waypoints[self.current_wp_index]
-        self.proxy.send_setpoint(
+        self._send_relative_position_setpoint(
-            lat=wp["lat"], lon=wp["lon"],
+            state,
-            z=wp.get("alt", self.target_altitude),
+            wp["lat"], wp["lon"], wp.get("alt", self.target_altitude),
             yaw=wp.get("yaw", 0),
-            control_mode="POSITION",
         )
         self.current_target = {
             "lat": wp["lat"], "lon": wp["lon"],
@@ -683,7 +681,7 @@ class MissionManager:
         """
         [RETURNING] 发送 POSITION SETPOINT 到 HOME 点
 
-        v2: send_setpoint(x=home_lat, y=home_lon, z=target_alt)
+        POSITION SETPOINT 发送到 HOME 的 LOCAL_OFFSET_NED 相对偏移；
         到达判定使用 Haversine 球面距离。
         """
         home = state.get("home", {})
@@ -691,10 +689,9 @@ class MissionManager:
         home_lat = home.get("lat", 0.0)
         home_lon = home.get("lon", 0.0)
 
-        # v2: x=纬度, y=经度, z=相对高度
+        # 发送 POSITION SETPOINT（相对当前位置 LOCAL_OFFSET_NED）
-        self.proxy.send_setpoint(
+        self._send_relative_position_setpoint(
-            lat=home_lat, lon=home_lon, z=self.target_altitude, yaw=0,
+            state, home_lat, home_lon, self.target_altitude, yaw=0,
-            control_mode="POSITION",
         )
         self.current_target = {
             "lat": home_lat, "lon": home_lon,
@@ -742,7 +739,7 @@ class MissionManager:
         """
         [HOLD_FINAL] 末端悬停，保持最终航点位置
 
-        v2: x=lat, y=lon, z=alt
+        POSITION SETPOINT 发送 LOCAL_OFFSET_NED 相对偏移。
         """
         if self.waypoints:
             final_wp = self.waypoints[-1]
@@ -750,11 +747,11 @@ class MissionManager:
             final_wp = {"lat": 0.0, "lon": 0.0,
                         "alt": self.target_altitude, "yaw": 0}
 
-        self.proxy.send_setpoint(
+        self._send_relative_position_setpoint(
-            lat=final_wp["lat"], lon=final_wp["lon"],
+            state,
-            z=final_wp.get("alt", self.target_altitude),
+            final_wp["lat"], final_wp["lon"],
+            final_wp.get("alt", self.target_altitude),
             yaw=final_wp.get("yaw", 0),
-            control_mode="POSITION",
         )
         self.current_target = {
             "lat": final_wp["lat"], "lon": final_wp["lon"],
@@ -770,6 +767,25 @@ class MissionManager:
     #  辅助
     # ================================================================
 
+    def _send_relative_position_setpoint(self, state, target_lat, target_lon,
+                                         target_alt, yaw=0.0):
+        """按相对当前位置的 LOCAL_OFFSET_NED 发送 POSITION SETPOINT。"""
+        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,
+        )
+
+        return self.proxy.send_setpoint(
+            x=x, y=y, z=z, yaw=yaw,
+            frame="LOCAL_OFFSET_NED",
+            control_mode="POSITION",
+        )
+
     def _set_current_target_from_wp(self, index):
         """从航点列表更新 current_target"""
         if self.waypoints and index < len(self.waypoints):

drone_controller/router_proxy.py

@@ -173,7 +173,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)

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 公式计算两个经纬度点之间的球面距离（米）