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

utils/coord.py

@@ -1,5 +1,9 @@
 import math
 
+# WGS-84 赤道半径（米）
+EARTH_RADIUS = 6378137.0
+
+
 def latlon_to_ned(lat, lon, alt, ref_lat, ref_lon, ref_alt):
     """
     将 WGS-84 经纬高 (LLA) 转换为局部 NED 坐标 (x, y, z)
@@ -14,8 +18,6 @@ def latlon_to_ned(lat, lon, alt, ref_lat, ref_lon, ref_alt):
             y: 东向距离 (正东为正)
             z: 向下距离 (向下为正，即向上为负)
     """
-    # WGS-84 椭球参数（赤道半径，单位：米）
-    EARTH_RADIUS = 6378137.0
     # 1. 纬度差 -> 北向距离 (x)
     dlat = math.radians(lat - ref_lat)
     x = dlat * EARTH_RADIUS
@@ -28,4 +30,44 @@ def latlon_to_ned(lat, lon, alt, ref_lat, ref_lon, ref_alt):
     # 若目标海拔高于参考点，则 z 为负（向上）
     z = -(alt - ref_alt)
 
-    return x, y, z
+    return x, y, z
+
+
+def haversine(lat1, lon1, lat2, lon2):
+    """
+    使用 Haversine 公式计算两个经纬度点之间的球面距离（米）
+
+    Args:
+        lat1, lon1: 起点纬度、经度（度）
+        lat2, lon2: 终点纬度、经度（度）
+
+    Returns:
+        水平距离（米）
+    """
+    dlat = math.radians(lat2 - lat1)
+    dlon = math.radians(lon2 - lon1)
+    a = (math.sin(dlat / 2) ** 2 +
+         math.cos(math.radians(lat1)) *
+         math.cos(math.radians(lat2)) *
+         math.sin(dlon / 2) ** 2)
+    c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
+    return EARTH_RADIUS * c
+
+
+def dist_3d_latlon(lat1, lon1, alt1, lat2, lon2, alt2):
+    """
+    计算两个经纬高坐标之间的 3D 距离（米）
+
+    水平距离使用 Haversine 公式，垂直距离使用高度差。
+    此处的 alt 为相对高度（正值向上），与 SETPOINT 协议一致。
+
+    Args:
+        lat1, lon1, alt1: 起点纬度(度)、经度(度)、相对高度(米)
+        lat2, lon2, alt2: 终点纬度(度)、经度(度)、相对高度(米)
+
+    Returns:
+        3D 距离（米）
+    """
+    horizontal = haversine(lat1, lon1, lat2, lon2)
+    vertical = alt1 - alt2
+    return math.sqrt(horizontal ** 2 + vertical ** 2)