"""
|
地理坐标与参考系转换工具。
|
|
提供以下能力：
|
- 解析 GGA/自定义格式的原点配置
|
- WGS-84 下的 LLA ↔ ECEF ↔ ENU 互转
|
- 将 ENU 坐标反算为经纬度
|
"""
|
|
from __future__ import annotations
|
|
import dataclasses
|
import math
|
from typing import Optional, Tuple
|
|
WGS84_A = 6378137.0 # 半长轴
|
WGS84_F = 1 / 298.257223563
|
WGS84_E2 = WGS84_F * (2 - WGS84_F)
|
WGS84_B = WGS84_A * (1 - WGS84_F)
|
WGS84_EP2 = (WGS84_A ** 2 - WGS84_B ** 2) / (WGS84_B ** 2)
|
|
|
def dm_to_decimal(dm_value: float) -> float:
|
"""度分转十进制度。"""
|
degrees = int(dm_value // 100)
|
minutes = dm_value - degrees * 100
|
return degrees + minutes / 60.0
|
|
|
@dataclasses.dataclass(frozen=True)
|
class Origin:
|
"""ENU 参考原点。"""
|
|
latitude_deg: float
|
longitude_deg: float
|
altitude_m: float
|
|
def __post_init__(self):
|
object.__setattr__(self, "latitude_rad", math.radians(self.latitude_deg))
|
object.__setattr__(self, "longitude_rad", math.radians(self.longitude_deg))
|
object.__setattr__(self, "ecef", geo_to_ecef(self.latitude_deg, self.longitude_deg, self.altitude_m))
|
|
|
def parse_origin(origin_geo: str) -> Origin:
|
"""
|
解析原点描述:
|
1) 纯 GGA 报文: "$GNGGA,..."
|
2) 简写: "3949.8890,N,11616.7555,E[,alt]"
|
"""
|
text = (origin_geo or "").strip()
|
if not text:
|
raise ValueError("原点字符串不能为空")
|
|
if text.startswith("$") and "GGA" in text.upper():
|
parts = text.split(",")
|
if len(parts) < 10:
|
raise ValueError("GGA 报文字段数量不足")
|
lat_dm = float(parts[2])
|
lat_dir = parts[3].upper()
|
lon_dm = float(parts[4])
|
lon_dir = parts[5].upper()
|
alt = float(parts[9])
|
|
lat = dm_to_decimal(lat_dm)
|
lon = dm_to_decimal(lon_dm)
|
if lat_dir == "S":
|
lat = -lat
|
if lon_dir == "W":
|
lon = -lon
|
return Origin(lat, lon, alt)
|
|
# 兼容 ",lat,N,lon,E" 或 "lat,N,lon,E"
|
text = text.lstrip(",")
|
parts = [p.strip() for p in text.split(",") if p.strip()]
|
if len(parts) not in (4, 5):
|
raise ValueError("原点格式应为 'lat,N,lon,E[,alt]' 或 GGA 报文")
|
lat_dm = float(parts[0])
|
lat_dir = parts[1].upper()
|
lon_dm = float(parts[2])
|
lon_dir = parts[3].upper()
|
alt = float(parts[4]) if len(parts) == 5 else 0.0
|
|
lat = dm_to_decimal(lat_dm)
|
lon = dm_to_decimal(lon_dm)
|
if lat_dir == "S":
|
lat = -lat
|
if lon_dir == "W":
|
lon = -lon
|
return Origin(lat, lon, alt)
|
|
|
def geo_to_ecef(lat_deg: float, lon_deg: float, alt_m: float) -> Tuple[float, float, float]:
|
"""经纬度 → ECEF。"""
|
lat_rad = math.radians(lat_deg)
|
lon_rad = math.radians(lon_deg)
|
sin_lat = math.sin(lat_rad)
|
cos_lat = math.cos(lat_rad)
|
sin_lon = math.sin(lon_rad)
|
cos_lon = math.cos(lon_rad)
|
|
N = WGS84_A / math.sqrt(1 - WGS84_E2 * sin_lat * sin_lat)
|
x = (N + alt_m) * cos_lat * cos_lon
|
y = (N + alt_m) * cos_lat * sin_lon
|
z = (N * (1 - WGS84_E2) + alt_m) * sin_lat
|
return x, y, z
|
|
|
def ecef_to_enu(dx: float, dy: float, dz: float, lat0_rad: float, lon0_rad: float) -> Tuple[float, float, float]:
|
"""ECEF 差值 → ENU。"""
|
sin_lat = math.sin(lat0_rad)
|
cos_lat = math.cos(lat0_rad)
|
sin_lon = math.sin(lon0_rad)
|
cos_lon = math.cos(lon0_rad)
|
|
east = -sin_lon * dx + cos_lon * dy
|
north = -sin_lat * cos_lon * dx - sin_lat * sin_lon * dy + cos_lat * dz
|
up = cos_lat * cos_lon * dx + cos_lat * sin_lon * dy + sin_lat * dz
|
return east, north, up
|
|
|
def enu_to_ecef(east: float, north: float, up: float, lat0_rad: float, lon0_rad: float) -> Tuple[float, float, float]:
|
"""ENU → ECEF 差值。"""
|
sin_lat = math.sin(lat0_rad)
|
cos_lat = math.cos(lat0_rad)
|
sin_lon = math.sin(lon0_rad)
|
cos_lon = math.cos(lon0_rad)
|
|
dx = -sin_lon * east - sin_lat * cos_lon * north + cos_lat * cos_lon * up
|
dy = cos_lon * east - sin_lat * sin_lon * north + cos_lat * sin_lon * up
|
dz = cos_lat * north + sin_lat * up
|
return dx, dy, dz
|
|
|
def ecef_to_lla(x: float, y: float, z: float) -> Tuple[float, float, float]:
|
"""ECEF → LLA。"""
|
p = math.hypot(x, y)
|
theta = math.atan2(WGS84_A * z, WGS84_B * p)
|
sin_theta = math.sin(theta)
|
cos_theta = math.cos(theta)
|
|
lon = math.atan2(y, x)
|
lat = math.atan2(
|
z + WGS84_EP2 * WGS84_B * sin_theta ** 3,
|
p - WGS84_E2 * WGS84_A * cos_theta ** 3,
|
)
|
|
N = WGS84_A / math.sqrt(1 - WGS84_E2 * math.sin(lat) ** 2)
|
alt = p / math.cos(lat) - N
|
return math.degrees(lat), math.degrees(lon), alt
|
|
|
def enu_to_lla(east: float, north: float, up: float, origin: Origin) -> Tuple[float, float, float]:
|
"""将 ENU 坐标转换为实时经纬度。"""
|
dx, dy, dz = enu_to_ecef(east, north, up, origin.latitude_rad, origin.longitude_rad)
|
x = origin.ecef[0] + dx
|
y = origin.ecef[1] + dy
|
z = origin.ecef[2] + dz
|
return ecef_to_lla(x, y, z)
|
|
|
def heading_math_to_nav(heading_rad: float) -> float:
|
"""
|
将数学坐标系 (东为0°, CCW为正) 的航向角转换为导航坐标系 (北为0°, 顺时针为正)。
|
与STM32代码保持一致：compass_deg = 90.0f - (heading_rad * RAD2DEG)
|
"""
|
heading_deg = math.degrees(heading_rad)
|
nav = 90.0 - heading_deg
|
# 与STM32代码保持一致：先计算，再包装到[0, 360)
|
while nav >= 360.0:
|
nav -= 360.0
|
while nav < 0.0:
|
nav += 360.0
|
return nav
|
