# -*- 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