99 lines
3.6 KiB
Python
99 lines
3.6 KiB
Python
|
"""
|
|||
|
|
地理坐标工具集 —— 球面计算 + 矢量分解
|
|||
|
|
"""
|
|||
|
|
import math
|
|||
|
|
from typing import Tuple
|
|||
|
|
|
|||
|
|
from uas.model.state import FlightPhase
|
|||
|
|
|
|||
|
|
EARTH_RADIUS = 6_371_000.0
|
|||
|
|
DEG2RAD = math.pi / 180.0
|
|||
|
|
RAD2DEG = 180.0 / math.pi
|
|||
|
|
|
|||
|
|
def determine_phase(alt: float, vs: float,
|
|||
|
|
min_alt: float, max_alt: float):
|
|||
|
|
if alt <= min_alt + 50:
|
|||
|
|
return (FlightPhase.TAKEOFF if vs > 0.5
|
|||
|
|
else FlightPhase.GROUND)
|
|||
|
|
if alt >= max_alt - 200:
|
|||
|
|
return FlightPhase.CRUISE
|
|||
|
|
if vs > 2.0: return FlightPhase.CLIMB
|
|||
|
|
if vs < -2.0: return FlightPhase.DESCENT
|
|||
|
|
return FlightPhase.CRUISE
|
|||
|
|
|
|||
|
|
class GeoUtils:
|
|||
|
|
@staticmethod
|
|||
|
|
def offset_position(lat: float, lon: float,
|
|||
|
|
bearing_deg: float, distance_m: float
|
|||
|
|
) -> Tuple[float, float]:
|
|||
|
|
"""球面偏移：从 (lat,lon) 沿 bearing 移动 distance_m"""
|
|||
|
|
if distance_m == 0:
|
|||
|
|
return lat, lon
|
|||
|
|
lat_r = lat * DEG2RAD
|
|||
|
|
lon_r = lon * DEG2RAD
|
|||
|
|
brg_r = bearing_deg * DEG2RAD
|
|||
|
|
delta = distance_m / EARTH_RADIUS
|
|||
|
|
|
|||
|
|
new_lat_r = math.asin(
|
|||
|
|
math.sin(lat_r) * math.cos(delta) +
|
|||
|
|
math.cos(lat_r) * math.sin(delta) * math.cos(brg_r)
|
|||
|
|
)
|
|||
|
|
new_lon_r = lon_r + math.atan2(
|
|||
|
|
math.sin(brg_r) * math.sin(delta) * math.cos(lat_r),
|
|||
|
|
math.cos(delta) - math.sin(lat_r) * math.sin(new_lat_r)
|
|||
|
|
)
|
|||
|
|
return new_lat_r * RAD2DEG, (new_lon_r * RAD2DEG + 540) % 360 - 180
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def haversine(lat1: float, lon1: float,
|
|||
|
|
lat2: float, lon2: float) -> float:
|
|||
|
|
"""大圆距离（米）"""
|
|||
|
|
dlat = (lat2 - lat1) * DEG2RAD
|
|||
|
|
dlon = (lon2 - lon1) * DEG2RAD
|
|||
|
|
a = (math.sin(dlat / 2) ** 2 +
|
|||
|
|
math.cos(lat1 * DEG2RAD) * math.cos(lat2 * DEG2RAD) *
|
|||
|
|
math.sin(dlon / 2) ** 2)
|
|||
|
|
return 2 * EARTH_RADIUS * math.asin(math.sqrt(max(0, min(1, a))))
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def bearing(lat1: float, lon1: float,
|
|||
|
|
lat2: float, lon2: float) -> float:
|
|||
|
|
"""初始方位角（度，真北顺时针）"""
|
|||
|
|
lat1_r, lat2_r = lat1 * DEG2RAD, lat2 * DEG2RAD
|
|||
|
|
dlon_r = (lon2 - lon1) * DEG2RAD
|
|||
|
|
x = math.sin(dlon_r) * math.cos(lat2_r)
|
|||
|
|
y = (math.cos(lat1_r) * math.sin(lat2_r) -
|
|||
|
|
math.sin(lat1_r) * math.cos(lat2_r) * math.cos(dlon_r))
|
|||
|
|
return (math.atan2(x, y) * RAD2DEG) % 360.0
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def normalize_heading(hdg: float) -> float:
|
|||
|
|
return hdg % 360.0
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def heading_diff(h1: float, h2: float) -> float:
|
|||
|
|
"""h2 - h1 的最短路径差值，范围 (-180, 180]"""
|
|||
|
|
return ((h2 - h1 + 540) % 360) - 180
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def wind_to_components(wind_dir_deg: float, wind_speed_ms: float
|
|||
|
|
) -> Tuple[float, float]:
|
|||
|
|
"""
|
|||
|
|
风向（气象风向：风从该方向吹来）→ 北向/东向分量（m/s）
|
|||
|
|
北向分量为正表示向北，东向分量为正表示向东
|
|||
|
|
"""
|
|||
|
|
# 气象风向转数学方向：风从 wind_dir 吹来，速度向量方向为 wind_dir + 180
|
|||
|
|
math_dir = (wind_dir_deg + 180.0) % 360.0
|
|||
|
|
rad = math_dir * DEG2RAD
|
|||
|
|
v_north = wind_speed_ms * math.cos(rad)
|
|||
|
|
v_east = wind_speed_ms * math.sin(rad)
|
|||
|
|
return v_north, v_east
|
|||
|
|
|
|||
|
|
@staticmethod
|
|||
|
|
def components_to_heading_speed(v_north: float, v_east: float
|
|||
|
|
) -> Tuple[float, float]:
|
|||
|
|
"""北向/东向速度分量 → 航向（度）+ 合速度（m/s）"""
|
|||
|
|
speed = math.sqrt(v_north ** 2 + v_east ** 2)
|
|||
|
|
heading = (math.atan2(v_east, v_north) * RAD2DEG) % 360.0
|
|||
|
|
return heading, speed
|