139 lines
7.0 KiB
Python
139 lines
7.0 KiB
Python
|
# -*- coding: utf-8 -*-
|
|||
|
|
"""
|
|||
|
|
装备状态输出基类
|
|||
|
|
─────────────────────────────────────────────────────────────────
|
|||
|
|
BaseParams —— query(t) 的统一返回类型基类
|
|||
|
|
子类扩展示例：
|
|||
|
|
AirParams 增加 vertical_speed / bank_angle / phase
|
|||
|
|
GroundParams 增加 slope / road_type / wheel_speed
|
|||
|
|
WaterParams 增加 depth / pitch / roll / sea_state
|
|||
|
|
─────────────────────────────────────────────────────────────────
|
|||
|
|
"""
|
|||
|
|
|
|||
|
|
from __future__ import annotations
|
|||
|
|
|
|||
|
|
from dataclasses import dataclass, field
|
|||
|
|
from datetime import datetime
|
|||
|
|
from typing import Any, Dict
|
|||
|
|
|
|||
|
|
from uas.route.base_waypoint import EquipmentDomain
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ─────────────────────────────────────────────────────────────────
|
|||
|
|
# 状态基类
|
|||
|
|
# ─────────────────────────────────────────────────────────────────
|
|||
|
|
|
|||
|
|
@dataclass
|
|||
|
|
class BaseParams:
|
|||
|
|
"""
|
|||
|
|
装备运动状态公共基类
|
|||
|
|
|
|||
|
|
query(t) 的统一返回类型，包含所有域共有的运动状态字段。
|
|||
|
|
子类通过新增字段扩展特有状态，无需修改基类。
|
|||
|
|
|
|||
|
|
坐标约定与 BaseWaypoint 一致（WGS-84，高度单位米 MSL）。
|
|||
|
|
"""
|
|||
|
|
|
|||
|
|
# ── 时间 ──────────────────────────────────────────────────────
|
|||
|
|
timestamp: float = 0.0 # 查询时刻（Unix 秒）
|
|||
|
|
|
|||
|
|
# ── 位置 ──────────────────────────────────────────────────────
|
|||
|
|
latitude: float = 0.0 # 纬度（度）
|
|||
|
|
longitude: float = 0.0 # 经度（度）
|
|||
|
|
altitude: float = 0.0 # 高度/深度（米，MSL）
|
|||
|
|
|
|||
|
|
# ── 姿态 ──────────────────────────────────────────────────────
|
|||
|
|
heading: float = 0.0 # 航向角（度，真北顺时针 0~360）
|
|||
|
|
pitch: float = 0.0 # 俯仰角（度，抬头为正）
|
|||
|
|
roll: float = 0.0 # 横滚角（度，右滚为正）
|
|||
|
|
|
|||
|
|
# ── 速度 ──────────────────────────────────────────────────────
|
|||
|
|
speed: float = 0.0 # 地速（m/s）
|
|||
|
|
acceleration: float = 0.0 # 切向加速度（m/s²）
|
|||
|
|
|
|||
|
|
# ── 域标识 ────────────────────────────────────────────────────
|
|||
|
|
domain: EquipmentDomain = EquipmentDomain.AIR
|
|||
|
|
|
|||
|
|
# ── 扩展元数据 ────────────────────────────────────────────────
|
|||
|
|
extra: Dict[str, Any] = field(default_factory=dict)
|
|||
|
|
|
|||
|
|
# ── 属性 ──────────────────────────────────────────────────────
|
|||
|
|
|
|||
|
|
@property
|
|||
|
|
def position(self) -> tuple:
|
|||
|
|
"""返回 (lat, lon, alt) 三元组"""
|
|||
|
|
return (self.latitude, self.longitude, self.altitude)
|
|||
|
|
|
|||
|
|
@property
|
|||
|
|
def datetime(self) -> datetime:
|
|||
|
|
return datetime.fromtimestamp(self.timestamp)
|
|||
|
|
|
|||
|
|
@property
|
|||
|
|
def datetime_str(self) -> str:
|
|||
|
|
return self.datetime.strftime("%H:%M:%S.%f")[:-3]
|
|||
|
|
|
|||
|
|
def to_dict(self) -> dict:
|
|||
|
|
"""序列化为字典（含所有子类字段）"""
|
|||
|
|
import dataclasses
|
|||
|
|
return dataclasses.asdict(self)
|
|||
|
|
|
|||
|
|
def distance_to(self, other: "BaseParams") -> float:
|
|||
|
|
"""计算与另一状态点的水平距离（米，Haversine）"""
|
|||
|
|
import math
|
|||
|
|
R = 6_371_000.0
|
|||
|
|
φ1 = math.radians(self.latitude)
|
|||
|
|
φ2 = math.radians(other.latitude)
|
|||
|
|
dφ = math.radians(other.latitude - self.latitude)
|
|||
|
|
dλ = math.radians(other.longitude - self.longitude)
|
|||
|
|
a = (math.sin(dφ/2)**2
|
|||
|
|
+ math.cos(φ1) * math.cos(φ2) * math.sin(dλ/2)**2)
|
|||
|
|
return R * 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
|
|||
|
|
|
|||
|
|
def __repr__(self) -> str:
|
|||
|
|
return (f"{self.__class__.__name__}("
|
|||
|
|
f"t={self.datetime_str}, "
|
|||
|
|
f"pos=({self.latitude:.4f},{self.longitude:.4f},"
|
|||
|
|
f"{self.altitude:.0f}m), "
|
|||
|
|
f"hdg={self.heading:.1f}°, spd={self.speed:.1f}m/s)")
|
|||
|
|
|
|||
|
|
|
|||
|
|
# ─────────────────────────────────────────────────────────────────
|
|||
|
|
# 各域状态子类
|
|||
|
|
# ─────────────────────────────────────────────────────────────────
|
|||
|
|
|
|||
|
|
@dataclass
|
|||
|
|
class AirParams(BaseParams):
|
|||
|
|
"""空中装备状态"""
|
|||
|
|
domain: EquipmentDomain = EquipmentDomain.AIR
|
|||
|
|
|
|||
|
|
airspeed: float = 0.0 # 空速（m/s）
|
|||
|
|
vertical_speed: float = 0.0 # 垂直速度（m/s，上升为正）
|
|||
|
|
bank_angle: float = 0.0 # 坡度角（度）
|
|||
|
|
turn_rate: float = 0.0 # 转弯角速度（°/s）
|
|||
|
|
turn_radius: float = 0.0 # 转弯半径（米）
|
|||
|
|
phase: str = "CRUISE" # TAKEOFF/CLIMB/CRUISE/DESCENT/APPROACH/LAND
|
|||
|
|
|
|||
|
|
|
|||
|
|
@dataclass
|
|||
|
|
class GroundParams(BaseParams):
|
|||
|
|
"""陆地装备状态"""
|
|||
|
|
domain: EquipmentDomain = EquipmentDomain.GROUND
|
|||
|
|
|
|||
|
|
slope_deg: float = 0.0 # 当前坡度（度，上坡为正）
|
|||
|
|
cross_slope_deg: float = 0.0 # 横向坡度（度）
|
|||
|
|
road_type: str = "road" # road/offroad/track
|
|||
|
|
is_stopped: bool = False # 是否停车
|
|||
|
|
odometer_m: float = 0.0 # 里程计（米）
|
|||
|
|
|
|||
|
|
|
|||
|
|
@dataclass
|
|||
|
|
class WaterParams(BaseParams):
|
|||
|
|
"""水中装备状态"""
|
|||
|
|
domain: EquipmentDomain = EquipmentDomain.WATER
|
|||
|
|
|
|||
|
|
depth: float = 0.0 # 水下深度（米，正值向下）
|
|||
|
|
is_subsurface: bool = False # 是否在水下
|
|||
|
|
sea_state: int = 0 # 海况等级（0~9）
|
|||
|
|
current_speed: float = 0.0 # 水流速度（m/s）
|
|||
|
|
current_heading: float = 0.0 # 水流方向（度）
|
|||
|
|
rudder_angle: float = 0.0 # 舵角（度）
|