541 lines
20 KiB
Python
541 lines
20 KiB
Python
# -*- coding: utf-8 -*-
|
||
"""
|
||
空中航线（AirRoute）
|
||
─────────────────────────────────────────────────────────────────
|
||
继承 BaseRoute，适配已有 FlightRoute 的实现逻辑。
|
||
FlightRoute 只需 `class FlightRoute(AirRoute): pass` 即可适配。
|
||
─────────────────────────────────────────────────────────────────
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import math
|
||
import os
|
||
import sys
|
||
from dataclasses import dataclass, field
|
||
from typing import List, Optional
|
||
|
||
from PyQt5.QtCore import QObject
|
||
|
||
from uas.route.base_params import AirParams
|
||
from uas.route.base_route import BaseRoute
|
||
from uas.route.base_segment import BaseSegment, BaseTransition, SegmentKind
|
||
from uas.route.base_waypoint import EquipmentDomain
|
||
from uas.route.routes.arinc424 import ARINC424Parser
|
||
from uas.utils.geo_utils import GeoUtils
|
||
|
||
DEG2RAD = math.pi / 180.0
|
||
RAD2DEG = 180.0 / math.pi
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 空中直线段
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
@dataclass
|
||
class AirSegment(BaseSegment):
|
||
"""
|
||
空中直线航段
|
||
完全兼容原 RouteSegment，可直接替换。
|
||
"""
|
||
domain: EquipmentDomain = field(default=EquipmentDomain.AIR, repr=False)
|
||
kind: SegmentKind = field(default=SegmentKind.STRAIGHT, repr=False)
|
||
|
||
def query(self, t: float) -> AirParams:
|
||
return _air_seg_interpolate(self, t)
|
||
|
||
def interpolate(self, ratio: float) -> AirParams:
|
||
t = self.t_start + max(0.0, min(1.0, ratio)) * self.duration
|
||
return _air_seg_interpolate(self, t)
|
||
|
||
def validate(self) -> list:
|
||
errors = []
|
||
if self.distance < 0:
|
||
errors.append("距离不能为负")
|
||
if self.duration < 0:
|
||
errors.append("时长不能为负")
|
||
if self.v_start < 0 or self.v_end < 0:
|
||
errors.append("速度不能为负")
|
||
return errors
|
||
|
||
# ── 衍生属性（覆盖基类，使用 alt_start/alt_end 字段名）────────
|
||
@property
|
||
def alt_start(self) -> float:
|
||
return self.start_alt
|
||
|
||
@property
|
||
def alt_end(self) -> float:
|
||
return self.end_alt
|
||
|
||
@property
|
||
def avg_speed(self) -> float:
|
||
return (self.v_start + self.v_end) / 2.0
|
||
|
||
@property
|
||
def vertical_speed(self) -> float:
|
||
if self.duration < 1e-6:
|
||
return 0.0
|
||
return (self.end_alt - self.start_alt) / self.duration
|
||
|
||
|
||
def _air_seg_interpolate(seg: AirSegment, t: float) -> AirParams:
|
||
"""空中直线段内匀加速插值"""
|
||
dt = t - seg.t_start
|
||
ratio = dt / seg.duration if seg.duration > 1e-6 else 1.0
|
||
ratio = max(0.0, min(1.0, ratio))
|
||
|
||
# 匀加速位移：s = v0*t + 0.5*a*t²
|
||
dist = seg.v_start * dt + 0.5 * seg.acceleration * dt * dt
|
||
dist = max(0.0, min(dist, seg.distance))
|
||
|
||
new_lat, new_lon = GeoUtils.offset_position(
|
||
seg.start_lat, seg.start_lon, seg.bearing, dist
|
||
)
|
||
speed = max(seg.v_start + seg.acceleration * dt, 0.0)
|
||
alt = seg.start_alt + (seg.end_alt - seg.start_alt) * ratio
|
||
vs = seg.vertical_speed
|
||
|
||
#TODO: 计算phase
|
||
# phase = determine_phase(...)
|
||
|
||
return AirParams(
|
||
timestamp = t,
|
||
latitude = new_lat,
|
||
longitude = new_lon,
|
||
altitude = alt,
|
||
heading = seg.bearing,
|
||
speed = speed,
|
||
acceleration = seg.acceleration,
|
||
airspeed = speed,
|
||
vertical_speed = vs,
|
||
# phase = phase,
|
||
domain = EquipmentDomain.AIR,
|
||
)
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 空中转弯过渡段（Fly-by 圆弧）
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
@dataclass
|
||
class AirTurnSegment(BaseTransition):
|
||
"""
|
||
空中 Fly-by 圆弧转弯段
|
||
完全兼容原 TurnSegment，可直接替换。
|
||
"""
|
||
turn_radius: float = 2000.0 # 转弯半径（米）
|
||
domain: EquipmentDomain = field(
|
||
default=EquipmentDomain.AIR, repr=False
|
||
)
|
||
kind: SegmentKind = field(
|
||
default=SegmentKind.TURN, repr=False
|
||
)
|
||
|
||
# 圆心坐标（__post_init__ 计算）
|
||
_center_lat: float = field(default=0.0, init=False, repr=False)
|
||
_center_lon: float = field(default=0.0, init=False, repr=False)
|
||
_exit_lat: float = field(default=0.0, init=False, repr=False)
|
||
_exit_lon: float = field(default=0.0, init=False, repr=False)
|
||
|
||
def __post_init__(self):
|
||
self._compute_geometry()
|
||
|
||
def _compute_geometry(self):
|
||
"""计算圆弧圆心、出口坐标和时长"""
|
||
angle = self.turn_angle # 有符号转弯角（度）
|
||
r = self.turn_radius
|
||
|
||
# 圆心方向：转弯方向的垂直方向
|
||
if self.turn_direction >= 0:
|
||
center_bearing = (self.entry_heading + 90) % 360 # 右转
|
||
else:
|
||
center_bearing = (self.entry_heading - 90) % 360 # 左转
|
||
|
||
self._center_lat, self._center_lon = GeoUtils.offset_position(
|
||
self.entry_lat, self.entry_lon, center_bearing, r
|
||
)
|
||
|
||
# 出口坐标：圆心到出口的方向
|
||
if self.turn_direction >= 0:
|
||
exit_bearing = (center_bearing + 180 + angle) % 360
|
||
else:
|
||
exit_bearing = (center_bearing + 180 + angle) % 360
|
||
|
||
self._exit_lat, self._exit_lon = GeoUtils.offset_position(
|
||
self._center_lat, self._center_lon,
|
||
(exit_bearing + 180) % 360, r
|
||
)
|
||
|
||
# 圆弧时长
|
||
arc_len = abs(math.radians(angle)) * r
|
||
self.duration = arc_len / self.speed if self.speed > 1e-6 else 0.0
|
||
|
||
@property
|
||
def exit_lat(self) -> float:
|
||
return self._exit_lat
|
||
|
||
@property
|
||
def exit_lon(self) -> float:
|
||
return self._exit_lon
|
||
|
||
def query(self, t: float) -> AirParams:
|
||
"""圆弧段内按角速度插值"""
|
||
dt = t - self.t_start
|
||
ratio = dt / self.duration if self.duration > 1e-6 else 1.0
|
||
ratio = max(0.0, min(1.0, ratio))
|
||
|
||
# 已转过的角度
|
||
swept_angle = self.turn_angle * ratio
|
||
|
||
# 当前位置：从圆心出发，按已转角度计算
|
||
if self.turn_direction >= 0:
|
||
center_bearing = (self.entry_heading + 90) % 360
|
||
else:
|
||
center_bearing = (self.entry_heading - 90) % 360
|
||
|
||
entry_from_center = (center_bearing + 180) % 360
|
||
current_from_center = (entry_from_center + swept_angle) % 360
|
||
|
||
cur_lat, cur_lon = GeoUtils.offset_position(
|
||
self._center_lat, self._center_lon,
|
||
current_from_center, self.turn_radius
|
||
)
|
||
|
||
# 当前航向 = 入口航向 + 已转角度
|
||
cur_heading = (self.entry_heading + swept_angle) % 360
|
||
|
||
# 转弯率（°/s）
|
||
turn_rate = (self.turn_angle / self.duration
|
||
if self.duration > 1e-6 else 0.0)
|
||
|
||
# 坡度角（bank angle）
|
||
g = 9.81
|
||
bank_rad = math.atan(
|
||
self.speed * abs(math.radians(turn_rate)) / g
|
||
)
|
||
bank_angle = math.degrees(bank_rad) * self.turn_direction
|
||
|
||
return AirParams(
|
||
timestamp = t,
|
||
latitude = cur_lat,
|
||
longitude = cur_lon,
|
||
altitude = self.altitude,
|
||
heading = cur_heading,
|
||
roll = bank_angle,
|
||
speed = self.speed,
|
||
airspeed = self.speed,
|
||
vertical_speed = 0.0,
|
||
turn_rate = turn_rate,
|
||
turn_radius = self.turn_radius,
|
||
bank_angle = bank_angle,
|
||
phase = "CRUISE",
|
||
domain = EquipmentDomain.AIR,
|
||
)
|
||
|
||
def validate(self) -> list:
|
||
errors = []
|
||
if self.turn_radius <= 0:
|
||
errors.append(f"转弯半径必须为正: {self.turn_radius}")
|
||
if self.speed <= 0:
|
||
errors.append(f"速度必须为正: {self.speed}")
|
||
return errors
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 空中航线（AirRoute）
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
class AirRoute(BaseRoute):
|
||
"""
|
||
空中飞行航线
|
||
|
||
实现 BaseRoute 所有抽象方法，
|
||
算法逻辑与原 FlightRoute._rebuild() 完全一致。
|
||
|
||
适配方式：
|
||
原 FlightRoute 只需改为：
|
||
class FlightRoute(AirRoute):
|
||
pass
|
||
即可纳入 BaseRoute 继承体系，无需其他改动。
|
||
"""
|
||
|
||
def __init__(
|
||
self,
|
||
start_time: float = 0.0,
|
||
default_turn_radius: float = 2000.0,
|
||
min_turn_radius: float = 500.0,
|
||
parent: QObject = None,
|
||
):
|
||
super().__init__(start_time=start_time, parent=parent)
|
||
self.default_turn_radius = default_turn_radius
|
||
self.min_turn_radius = min_turn_radius
|
||
|
||
# ── 域标识 ────────────────────────────────────────────────────
|
||
|
||
@property
|
||
def domain(self) -> EquipmentDomain:
|
||
return EquipmentDomain.AIR
|
||
|
||
# ── 核心重建算法 ──────────────────────────────────────────────
|
||
|
||
def _rebuild(self) -> None:
|
||
"""
|
||
重建所有直线段和圆弧转弯段
|
||
算法与原 FlightRoute._rebuild() 完全一致
|
||
"""
|
||
self._segments.clear()
|
||
self._transitions.clear()
|
||
|
||
n = len(self._waypoints)
|
||
if n < 2:
|
||
self.total_duration = 0.0
|
||
return
|
||
|
||
# Step 1：预计算所有段方位角
|
||
bearings: List[float] = []
|
||
for i in range(n - 1):
|
||
brg = GeoUtils.bearing(
|
||
self._waypoints[i].latitude, self._waypoints[i].longitude,
|
||
self._waypoints[i+1].latitude, self._waypoints[i+1].longitude,
|
||
)
|
||
bearings.append(brg)
|
||
|
||
t_cursor = self.start_time
|
||
prev_exit_lat = self._waypoints[0].latitude
|
||
prev_exit_lon = self._waypoints[0].longitude
|
||
|
||
# Step 2-5：逐段构建
|
||
for i in range(n - 1):
|
||
wp_a = self._waypoints[i]
|
||
wp_b = self._waypoints[i + 1]
|
||
brg_in = bearings[i]
|
||
brg_out = bearings[i + 1] if (i + 1) < len(bearings) else brg_in
|
||
|
||
# 确定转弯半径
|
||
r = getattr(wp_b, "turn_radius", 0.0)
|
||
if r <= 0:
|
||
r = self.default_turn_radius
|
||
r = max(r, self.min_turn_radius)
|
||
|
||
# 转弯角
|
||
turn_angle = GeoUtils.heading_diff(brg_in, brg_out)
|
||
|
||
# 判断是否需要转弯
|
||
fly_over = getattr(wp_b, "fly_over", False)
|
||
need_turn = (
|
||
i < n - 2
|
||
and not fly_over
|
||
and abs(turn_angle) > 0.5
|
||
)
|
||
|
||
# 计算 TIP（提前转弯点）
|
||
if need_turn:
|
||
tip_dist_m = r * abs(math.tan(math.radians(turn_angle / 2)))
|
||
tip_lat, tip_lon = GeoUtils.offset_position(
|
||
wp_b.latitude, wp_b.longitude,
|
||
(brg_in + 180) % 360, tip_dist_m
|
||
)
|
||
# 安全检查：TIP 不能超过航路点
|
||
seg_total_dist = GeoUtils.haversine(
|
||
prev_exit_lat, prev_exit_lon,
|
||
wp_b.latitude, wp_b.longitude,
|
||
)
|
||
tip_check_dist = GeoUtils.haversine(
|
||
wp_b.latitude, wp_b.longitude,
|
||
tip_lat, tip_lon,
|
||
)
|
||
if tip_check_dist > seg_total_dist * 0.95:
|
||
need_turn = False
|
||
tip_lat, tip_lon = wp_b.latitude, wp_b.longitude
|
||
else:
|
||
tip_lat, tip_lon = wp_b.latitude, wp_b.longitude
|
||
|
||
# 构建直线段
|
||
seg_dist = GeoUtils.haversine(
|
||
prev_exit_lat, prev_exit_lon, tip_lat, tip_lon
|
||
)
|
||
v0 = wp_a.speed
|
||
v1 = wp_b.speed
|
||
v_avg = (v0 + v1) / 2.0
|
||
if v_avg < 1e-6:
|
||
v_avg = 1.0
|
||
duration = seg_dist / v_avg if v_avg > 1e-6 else 0.0
|
||
acc = (v1 - v0) / duration if duration > 1e-6 else 0.0
|
||
|
||
seg = self._make_segment(
|
||
wp_from = wp_a,
|
||
wp_to = wp_b,
|
||
start_lat = prev_exit_lat,
|
||
start_lon = prev_exit_lon,
|
||
start_alt = wp_a.altitude,
|
||
end_lat = tip_lat,
|
||
end_lon = tip_lon,
|
||
end_alt = wp_b.altitude,
|
||
bearing = brg_in,
|
||
t_start = t_cursor,
|
||
v_start = v0,
|
||
v_end = v1,
|
||
acc = acc,
|
||
dist = seg_dist,
|
||
duration = duration,
|
||
)
|
||
self._segments.append(seg)
|
||
t_cursor += duration
|
||
|
||
# 构建转弯段
|
||
if need_turn:
|
||
trans = self._make_transition(
|
||
entry_lat = tip_lat,
|
||
entry_lon = tip_lon,
|
||
entry_alt = wp_b.altitude,
|
||
entry_heading = brg_in,
|
||
exit_heading = brg_out,
|
||
speed = wp_b.speed,
|
||
altitude = wp_b.altitude,
|
||
t_start = t_cursor,
|
||
turn_radius = r,
|
||
)
|
||
self._transitions.append(trans)
|
||
if trans is not None:
|
||
t_cursor += trans.duration
|
||
prev_exit_lat = trans.exit_lat
|
||
prev_exit_lon = trans.exit_lon
|
||
else:
|
||
prev_exit_lat = tip_lat
|
||
prev_exit_lon = tip_lon
|
||
else:
|
||
self._transitions.append(None)
|
||
prev_exit_lat = tip_lat
|
||
prev_exit_lon = tip_lon
|
||
|
||
self._update_total_duration()
|
||
|
||
# ── 工厂方法 ──────────────────────────────────────────────────
|
||
|
||
def _make_segment(
|
||
self,
|
||
wp_from, wp_to,
|
||
start_lat, start_lon, start_alt,
|
||
end_lat, end_lon, end_alt,
|
||
bearing, t_start,
|
||
v_start=0.0, v_end=0.0, acc=0.0,
|
||
dist=0.0, duration=0.0,
|
||
**kwargs,
|
||
) -> AirSegment:
|
||
return AirSegment(
|
||
wp_from = wp_from,
|
||
wp_to = wp_to,
|
||
start_lat = start_lat,
|
||
start_lon = start_lon,
|
||
start_alt = start_alt,
|
||
end_lat = end_lat,
|
||
end_lon = end_lon,
|
||
end_alt = end_alt,
|
||
distance = dist,
|
||
bearing = bearing,
|
||
t_start = t_start,
|
||
t_end = t_start + duration,
|
||
duration = duration,
|
||
v_start = v_start,
|
||
v_end = v_end,
|
||
acceleration = acc,
|
||
)
|
||
|
||
def _make_transition(
|
||
self,
|
||
entry_lat, entry_lon, entry_alt,
|
||
entry_heading, exit_heading,
|
||
speed, altitude, t_start,
|
||
turn_radius=2000.0,
|
||
**kwargs,
|
||
) -> Optional[AirTurnSegment]:
|
||
return AirTurnSegment(
|
||
entry_lat = entry_lat,
|
||
entry_lon = entry_lon,
|
||
entry_alt = entry_alt,
|
||
entry_heading = entry_heading,
|
||
exit_heading = exit_heading,
|
||
speed = speed,
|
||
altitude = altitude,
|
||
t_start = t_start,
|
||
turn_radius = turn_radius,
|
||
)
|
||
|
||
def _interpolate_segment(
|
||
self, seg: BaseSegment, t: float
|
||
) -> AirParams:
|
||
return _air_seg_interpolate(seg, t) # type: ignore[arg-type]
|
||
|
||
def _state_at_start(self) -> AirParams:
|
||
seg = self._segments[0]
|
||
wp = self._waypoints[0]
|
||
vs = seg.vertical_speed
|
||
return AirParams(
|
||
timestamp = self.t_start,
|
||
latitude = seg.start_lat,
|
||
longitude = seg.start_lon,
|
||
altitude = seg.start_alt,
|
||
heading = seg.bearing,
|
||
speed = seg.v_start,
|
||
airspeed = seg.v_start,
|
||
vertical_speed = vs,
|
||
# phase = determine_phase(
|
||
# seg.start_alt, seg.end_alt, vs
|
||
# ),
|
||
domain = EquipmentDomain.AIR,
|
||
)
|
||
|
||
def _state_at_end(self) -> AirParams:
|
||
last_trans = self._transitions[-1] if self._transitions else None
|
||
if last_trans is not None:
|
||
s = last_trans.query(last_trans.t_end)
|
||
s.timestamp = self.t_end
|
||
s.phase = "LAND"
|
||
return s
|
||
last_seg = self._segments[-1]
|
||
return AirParams(
|
||
timestamp = self.t_end,
|
||
latitude = last_seg.end_lat,
|
||
longitude = last_seg.end_lon,
|
||
altitude = last_seg.end_alt,
|
||
heading = last_seg.bearing,
|
||
speed = last_seg.v_end,
|
||
airspeed = last_seg.v_end,
|
||
vertical_speed = 0.0,
|
||
phase = "LAND",
|
||
domain = EquipmentDomain.AIR,
|
||
)
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# FlightRoute 适配（最小改动）
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
class FlightRoute(AirRoute):
|
||
"""
|
||
原 FlightRoute 适配类
|
||
继承 AirRoute，保持与原有代码的完全兼容。
|
||
原有代码中所有对 FlightRoute 的引用无需修改。
|
||
"""
|
||
@staticmethod
|
||
def load_from_arinc424(route_file, start_time: float = 0.0,
|
||
default_turn_radius: float = 2000.0,
|
||
min_turn_radius: float = 500.0,) -> FlightRoute:
|
||
"""加载航线：优先从文件，否则使用内置示例"""
|
||
parser = ARINC424Parser(default_speed_ms=220.0, default_altitude_m=10000.0)
|
||
if not os.path.isfile(route_file):
|
||
print(f"[错误] 航线文件不存在: {route_file}", file=sys.stderr)
|
||
sys.exit(1)
|
||
with open(route_file, "r", encoding="utf-8") as f:
|
||
content = f.read()
|
||
ext = os.path.splitext(route_file)[1].lower()
|
||
if ext == ".csv":
|
||
waypoints = parser.parse_csv(content)
|
||
else:
|
||
waypoints = parser.parse_arinc424(content)
|
||
print(f" [航线] 已从文件加载: {route_file}（{len(waypoints)} 个航路点）")
|
||
route = FlightRoute(start_time=start_time,
|
||
default_turn_radius=default_turn_radius,
|
||
min_turn_radius=min_turn_radius)
|
||
route.add_waypoints(waypoints)
|
||
return route |