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
|