123 lines
4.5 KiB
Python
123 lines
4.5 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_phase1(alt_start, alt_end, vs):
|
||
# if alt_start < 500 and alt_end < 500:
|
||
# return FlightPhase.TAKEOFF if vs > 0.5 else FlightPhase.LAND
|
||
# if alt_start < 500: return FlightPhase.TAKEOFF
|
||
# if alt_end < 500: return FlightPhase.APPROACH
|
||
# if vs > 1.0: return FlightPhase.CLIMB
|
||
# if vs < -1.0: return FlightPhase.DESCENT
|
||
# return FlightPhase.CRUISE
|
||
#
|
||
# def determine_phase2(altitude, vs, airspeed):
|
||
# """判断飞行阶段（同 v1.0）"""
|
||
# if altitude < 50.0:
|
||
# return FlightPhase.LAND
|
||
# if vs > 2.0:
|
||
# return FlightPhase.CLIMB
|
||
# if vs < -2.0:
|
||
# if altitude < 3000.0 and airspeed < 103.0:
|
||
# return FlightPhase.APPROACH
|
||
# return FlightPhase.DESCENT
|
||
# if altitude < 3000.0:
|
||
# return FlightPhase.APPROACH
|
||
# return FlightPhase.CRUISE
|
||
|
||
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
|