base_agent/uas/route/routes/arinc424.py

448 lines
17 KiB
Python
Raw Normal View History

2026-06-18 03:28:14 +00:00
"""
ARINC 424 航线格式解析器
支持
- 标准 ARINC 424-18 固定宽度格式解析
- 简化 CSV 格式用于测试
- 路径终止符IF / TF / CF / DF / RF / AF / HF
"""
import csv
import io
import math
from typing import List, Optional, Dict
from uas.route.base_waypoint import AirWaypoint
DEG2RAD = math.pi / 180.0
# ─────────────────────────────────────────────
# ARINC 424 字段位置常量Section Code 4.1
# ─────────────────────────────────────────────
class ARINC424Fields:
"""ARINC 424-18 固定宽度字段偏移列索引0-based"""
RECORD_TYPE = (0, 1) # 记录类型
AREA_CODE = (1, 4) # 区域代码
SEC_CODE = (4, 5) # 节代码
SUB_CODE = (5, 6) # 子节代码
AIRPORT_IDENT = (6, 10) # 机场标识
ICAO_CODE = (10, 12) # ICAO 区域代码
SID_STAR_IDENT = (13, 19) # SID/STAR/APPR 标识
ROUTE_TYPE = (19, 20) # 航路类型
TRANS_IDENT = (20, 25) # 过渡标识
SEQ_NUMBER = (26, 29) # 序列号
WP_IDENT = (29, 34) # 航路点标识
WP_ICAO = (34, 36) # 航路点 ICAO
WP_SEC_CODE = (36, 37) # 航路点节代码
WP_SUB_CODE = (37, 38) # 航路点子节代码
DESC_CODE = (39, 43) # 描述代码
TURN_DIR = (43, 44) # 转弯方向 L/R
RNP = (44, 47) # RNP 值
PATH_TERM = (47, 49) # 路径终止符
TDV = (49, 50) # 时间/距离/速度限制
RECD_NAV = (50, 54) # 推荐导航台
ARC_RADIUS = (56, 62) # 圆弧半径RF 航段)
THETA = (62, 66) # 磁方位角
RHO = (66, 70) # 导航台距离
OUTBOUND_MAG_CRS = (70, 74) # 出航磁航道
ROUTE_HOLD_DIST = (74, 78) # 航路/等待距离/时间
ALT_DESC = (82, 83) # 高度描述符
ALT1 = (84, 89) # 高度 1
ALT2 = (89, 94) # 高度 2
TRANS_ALT = (94, 99) # 过渡高度
SPEED_LIMIT_DESC = (99, 100) # 速度限制描述符
SPEED_LIMIT = (100, 103) # 速度限制(节)
VERT_ANGLE = (103, 107) # 垂直角
CENTER_WP = (106, 111) # 圆弧中心航路点RF
MULTI_CD = (115, 116) # 多码
GPS_FMS_IND = (116, 117) # GPS/FMS 指示
RT_TYPE = (117, 118) # 航路类型
CYCLE_DATA = (118, 122) # 周期数据
# ─────────────────────────────────────────────
# 坐标解析工具
# ─────────────────────────────────────────────
def _parse_lat(s: str) -> float:
"""
解析 ARINC 424 纬度字段9字符
格式N/S + DDMMSS.S DDMMSSHH=半球
示例N400447.0 40.0797°
"""
s = s.strip()
if not s or len(s) < 8:
return 0.0
try:
hemi = s[0].upper()
deg = int(s[1:3])
mn = int(s[3:5])
sec = float(s[5:])
val = deg + mn / 60.0 + sec / 3600.0
return -val if hemi == 'S' else val
except (ValueError, IndexError):
return 0.0
def _parse_lon(s: str) -> float:
"""
解析 ARINC 424 经度字段10字符
格式E/W + DDDMMSS.S
示例E1163612.0 116.6033°
"""
s = s.strip()
if not s or len(s) < 9:
return 0.0
try:
hemi = s[0].upper()
deg = int(s[1:4])
mn = int(s[4:6])
sec = float(s[6:])
val = deg + mn / 60.0 + sec / 3600.0
return -val if hemi == 'W' else val
except (ValueError, IndexError):
return 0.0
def _parse_altitude(s: str) -> float:
"""解析高度字段(英尺 → 米)"""
s = s.strip()
if not s:
return 0.0
try:
# FL飞行高度层格式FL350 = 35000 英尺
if s.startswith('FL'):
ft = float(s[2:]) * 100
else:
ft = float(s)
return ft * 0.3048 # 英尺 → 米
except ValueError:
return 0.0
def _parse_speed_kt(s: str) -> float:
"""解析速度字段(节 → m/s"""
s = s.strip()
if not s:
return 0.0
try:
return float(s) * 0.514444 # 节 → m/s
except ValueError:
return 0.0
# ─────────────────────────────────────────────
# ARINC 424 解析器
# ─────────────────────────────────────────────
class ARINC424Parser:
"""
ARINC 424 格式解析器
支持两种输入
1. 标准 ARINC 424 固定宽度文本132字符/
2. 简化 CSV 格式用于测试和手动输入
CSV 格式列定义
identifier, lat, lon, altitude_ft, speed_kt,
path_terminator, alt_constraint, speed_limit_kt,
turn_direction, arc_radius_nm, fly_over
"""
# 路径终止符说明
PATH_TERMINATORS = {
"IF": "Initial Fix - 起始定位点",
"TF": "Track to Fix - 飞向定位点",
"CF": "Course to Fix - 指定航道飞向定位点",
"DF": "Direct to Fix - 直飞定位点",
"RF": "Radius to Fix - 圆弧飞向定位点",
"AF": "Arc to Fix - DME 圆弧飞向定位点",
"HF": "Hold to Fix - 等待后飞向定位点",
"HM": "Hold to Manual - 等待至人工终止",
"HA": "Hold to Altitude - 等待至高度",
"CA": "Course to Altitude - 指定航道飞至高度",
"CD": "Course to DME - 指定航道飞至DME距离",
"CI": "Course to Intercept - 指定航道截获",
"CR": "Course to Radial - 指定航道截获径向线",
"FA": "Fix to Altitude - 从定位点飞至高度",
"FC": "Fix to Distance - 从定位点飞至距离",
"FD": "Fix to DME Distance - 从定位点飞至DME距离",
"FM": "Fix to Manual - 从定位点飞至人工终止",
"PI": "Procedure Turn - 程序转弯",
"VA": "Heading to Altitude - 指定航向飞至高度",
"VD": "Heading to DME - 指定航向飞至DME距离",
"VI": "Heading to Intercept - 指定航向截获",
"VM": "Heading to Manual - 指定航向飞至人工终止",
"VR": "Heading to Radial - 指定航向截获径向线",
}
def __init__(self, default_speed_ms: float = 200.0,
default_altitude_m: float = 8000.0):
self.default_speed = default_speed_ms
self.default_altitude = default_altitude_m
# 航路点数据库(标识符 → 坐标)
self._wp_database: Dict[str, tuple] = {}
def load_waypoint_database(self, db: Dict[str, tuple]):
"""
加载航路点坐标数据库
db 格式{"ZBAA": (40.0799, 116.6031), ...}
"""
self._wp_database.update(db)
# ── ARINC 424 固定宽度格式解析 ──────────────
def parse_arinc424(self, text: str) -> List[AirWaypoint]:
"""
解析标准 ARINC 424 固定宽度文本
Args:
text: ARINC 424 格式文本每行 132 字符
Returns:
航路点列表
"""
waypoints = []
for line_no, line in enumerate(text.splitlines(), 1):
# 跳过空行和注释行
if not line.strip() or line.startswith('#'):
continue
# 补齐到 132 字符
line = line.ljust(132)
try:
wp = self._parse_arinc424_line(line)
if wp:
waypoints.append(wp)
except Exception as e:
print(f" [警告] 第 {line_no} 行解析失败: {e}")
return waypoints
def _parse_arinc424_line(self, line: str) -> Optional[AirWaypoint]:
"""解析单行 ARINC 424 记录"""
def field(f): return line[f[0]:f[1]].strip()
# 仅处理程序记录(节代码 P=机场D=SIDE=STARF=进近)
sec_code = field(ARINC424Fields.SEC_CODE)
if sec_code not in ('P', 'D', 'E', 'F', 'R'):
return None
wp_ident = field(ARINC424Fields.WP_IDENT)
if not wp_ident:
return None
path_term = field(ARINC424Fields.PATH_TERM)
turn_dir = field(ARINC424Fields.TURN_DIR)
alt_desc = field(ARINC424Fields.ALT_DESC)
alt1_str = field(ARINC424Fields.ALT1)
alt2_str = field(ARINC424Fields.ALT2)
spd_str = field(ARINC424Fields.SPEED_LIMIT)
arc_str = field(ARINC424Fields.ARC_RADIUS)
crs_str = field(ARINC424Fields.OUTBOUND_MAG_CRS)
# 坐标:优先从数据库查找,否则尝试解析行内坐标
lat, lon = self._lookup_or_parse_coords(wp_ident, line)
# 高度解析
alt = _parse_altitude(alt1_str) if alt1_str else self.default_altitude
alt_constraint = None
if alt_desc == '@': alt_constraint = "AT"
elif alt_desc == '+': alt_constraint = "AT_OR_ABOVE"
elif alt_desc == '-': alt_constraint = "AT_OR_BELOW"
elif alt_desc == 'B': alt_constraint = "BETWEEN"
# 速度解析
speed_limit = _parse_speed_kt(spd_str) if spd_str else None
speed = speed_limit if speed_limit else self.default_speed
# 圆弧半径RF 航段,单位:海里 → 米)
arc_radius = 0.0
if arc_str:
try:
arc_radius = float(arc_str) * 1852.0 # NM → m
except ValueError:
pass
# 指定航道
course = None
if crs_str:
try:
course = float(crs_str) / 10.0 # ARINC 424 精度 0.1°
except ValueError:
pass
# Fly-over 判断描述代码第4位 'Y'
desc_code = line[ARINC424Fields.DESC_CODE[0]:ARINC424Fields.DESC_CODE[1]]
fly_over = len(desc_code) >= 4 and desc_code[3] == 'Y'
return AirWaypoint(
identifier=wp_ident,
latitude=lat,
longitude=lon,
altitude=alt,
speed=speed,
path_terminator=path_term if path_term else "TF",
alt_constraint=alt_constraint,
alt_value=_parse_altitude(alt1_str) if alt1_str else None,
speed_limit=speed_limit,
turn_radius=arc_radius,
fly_over=fly_over,
course_angle=course,
)
def _lookup_or_parse_coords(self, ident: str, line: str
) -> tuple:
"""从数据库或行内解析坐标"""
if ident in self._wp_database:
return self._wp_database[ident]
# 尝试从行内解析(如果有坐标字段)
# ARINC 424 标准中坐标在 32-41 列(纬度)和 41-51 列(经度)
try:
lat_str = line[32:41].strip()
lon_str = line[41:51].strip()
if lat_str and lon_str:
return _parse_lat(lat_str), _parse_lon(lon_str)
except IndexError:
pass
return 0.0, 0.0
# ── CSV 简化格式解析 ─────────────────────────
def parse_csv(self, csv_text: str) -> List[AirWaypoint]:
"""
解析简化 CSV 格式航线
CSV 含表头
identifier, lat, lon, altitude_ft, speed_kt,
path_terminator, alt_constraint, speed_limit_kt,
turn_direction, arc_radius_nm, fly_over
示例
identifier,lat,lon,altitude_ft,speed_kt,path_terminator,...
ZBAA,40.0799,116.6031,1640,156,IF,,,,,False
WP001,39.5,117.5,26247,428,TF,AT_OR_ABOVE,250,,, False
"""
waypoints = []
reader = csv.DictReader(io.StringIO(csv_text.strip()))
for row in reader:
try:
wp = self._parse_csv_row(row)
if wp:
waypoints.append(wp)
except Exception as e:
print(f" [警告] CSV 行解析失败 ({row.get('identifier','?')}): {e}")
return waypoints
def _parse_csv_row(self, row: dict) -> Optional[AirWaypoint]:
"""解析单行 CSV 记录"""
def get(key, default=""): return row.get(key, default).strip()
ident = get('identifier')
if not ident:
return None
lat = float(get('lat', '0'))
lon = float(get('lon', '0'))
# 高度(英尺 → 米)
alt_ft_str = get('altitude_ft', '0')
alt = float(alt_ft_str) * 0.3048 if alt_ft_str else self.default_altitude
# 速度(节 → m/s
spd_kt_str = get('speed_kt', '0')
speed = float(spd_kt_str) * 0.514444 if spd_kt_str else self.default_speed
# 速度限制
sl_str = get('speed_limit_kt', '')
speed_limit = float(sl_str) * 0.514444 if sl_str else None
# 圆弧半径(海里 → 米)
arc_str = get('arc_radius_nm', '')
arc_radius = float(arc_str) * 1852.0 if arc_str else 0.0
fly_over_str = get('fly_over', 'False').lower()
fly_over = fly_over_str in ('true', '1', 'yes')
return AirWaypoint(
identifier=ident,
latitude=lat,
longitude=lon,
altitude=alt,
speed=speed,
name=get('name', ident),
path_terminator=get('path_terminator', 'TF'),
alt_constraint=get('alt_constraint') or None,
alt_value=alt if get('alt_constraint') else None,
speed_limit=speed_limit,
turn_radius=arc_radius,
fly_over=fly_over,
)
# ── 导出 ─────────────────────────────────────
def export_csv(self, waypoints: List[AirWaypoint]) -> str:
"""将航路点列表导出为 CSV 格式"""
lines = ["identifier,lat,lon,altitude_ft,speed_kt,"
"path_terminator,alt_constraint,speed_limit_kt,"
"arc_radius_nm,fly_over"]
for wp in waypoints:
alt_ft = wp.altitude / 0.3048
spd_kt = wp.speed / 0.514444
sl_kt = f"{wp.speed_limit/0.514444:.1f}" if wp.speed_limit else ""
arc_nm = f"{wp.turn_radius/1852.0:.3f}" if wp.turn_radius > 0 else ""
lines.append(
f"{wp.identifier},{wp.latitude:.6f},{wp.longitude:.6f},"
f"{alt_ft:.0f},{spd_kt:.1f},{wp.path_terminator},"
f"{wp.alt_constraint or ''},{sl_kt},{arc_nm},{wp.fly_over}"
)
return "\n".join(lines)
def export_arinc424(self, waypoints: List[AirWaypoint],
airport_ident: str = "ZBAA",
procedure_ident: str = "TESTRT") -> str:
"""
将航路点列表导出为简化 ARINC 424 格式文本
用于系统间数据交换
"""
lines = []
for i, wp in enumerate(waypoints):
seq = (i + 1) * 10
alt_ft = int(wp.altitude / 0.3048)
spd_kt = int(wp.speed / 0.514444)
# 高度描述符
alt_desc = ' '
if wp.alt_constraint == "AT": alt_desc = '@'
elif wp.alt_constraint == "AT_OR_ABOVE": alt_desc = '+'
elif wp.alt_constraint == "AT_OR_BELOW": alt_desc = '-'
# 构造 132 字符记录(简化版)
line = (
f"S" # 1: 记录类型
f"PENA" # 2-5: 区域代码
f"P " # 5-7: 节/子节代码
f"{airport_ident:<4s}" # 7-11: 机场标识
f"K9" # 11-13: ICAO
f" " # 13: 空
f"{procedure_ident:<6s}" # 14-20: 程序标识
f"1" # 20: 航路类型
f" " # 21-26: 过渡标识
f"{seq:03d}" # 26-29: 序列号
f"{wp.identifier:<5s}" # 29-34: 航路点标识
f"K9" # 34-36: ICAO
f"W " # 36-38: 节/子节
f" " # 38: 空
f" " # 39-43: 描述代码
f" " # 43: 转弯方向
f" " # 44-47: RNP
f"{wp.path_terminator:<2s}" # 47-49: 路径终止符
f" " # 50-83: 填充
f"{alt_desc}" # 83: 高度描述符
f"{alt_ft:05d}" # 84-89: 高度1
f" " # 89-94: 高度2
f" " # 94-99: 过渡高度
f" " # 99: 速度限制描述
f"{spd_kt:03d}" # 100-103: 速度限制
f" " * 29 # 103-132: 填充
)
lines.append(line[:132])
return "\n".join(lines)