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 # 舵角（度） |