2026.6.24实现 QGC_WAYPOINTS → 系统内部航点格式的转换链路

2026-06-24 12:50:31 +08:00
parent a4c3f3510b
commit 25882a5f8b
4 changed files with 157 additions and 7 deletions
@@ -0,0 +1,31 @@
+import math
+
+def latlon_to_ned(lat, lon, alt, ref_lat, ref_lon, ref_alt):
+    """
+    将 WGS-84 经纬高 (LLA) 转换为局部 NED 坐标 (x, y, z)
+
+    Args:
+        lat, lon, alt:   目标点的纬度(deg)、经度(deg)、海拔(m)
+        ref_lat, ref_lon, ref_alt: 参考点（起飞点）的纬度、经度、海拔
+
+    Returns:
+        (x, y, z): NED 坐标 (米)
+            x: 北向距离 (正北为正)
+            y: 东向距离 (正东为正)
+            z: 向下距离 (向下为正，即向上为负)
+    """
+    # WGS-84 椭球参数（赤道半径，单位：米）
+    EARTH_RADIUS = 6378137.0
+    # 1. 纬度差 -> 北向距离 (x)
+    dlat = math.radians(lat - ref_lat)
+    x = dlat * EARTH_RADIUS
+
+    # 2. 经度差 -> 东向距离 (y)，需乘以纬度的余弦值修正
+    dlon = math.radians(lon - ref_lon)
+    y = dlon * EARTH_RADIUS * math.cos(math.radians(ref_lat))
+
+    # 3. 高度差 -> NED Z (向下为正)
+    # 若目标海拔高于参考点，则 z 为负（向上）
+    z = -(alt - ref_alt)
+
+    return x, y, z