192 lines
11 KiB
Python
192 lines
11 KiB
Python
# -*- coding: utf-8 -*-
|
||
"""
|
||
无人系统遥测数据模型
|
||
─────────────────────────────────────────────────────────────────
|
||
BaseTelemetry 通用遥测基类
|
||
UAVTelemetry 无人机遥测
|
||
RobotDogTelemetry 机器狗遥测
|
||
─────────────────────────────────────────────────────────────────
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import dataclasses
|
||
import time
|
||
from dataclasses import dataclass, field
|
||
from enum import Enum
|
||
from typing import Any, Dict, List
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 设备连接状态
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
class ConnectionState(Enum):
|
||
DISCONNECTED = "disconnected"
|
||
CONNECTING = "connecting"
|
||
CONNECTED = "connected"
|
||
DEGRADED = "degraded" # 连接质量差
|
||
LOST = "lost" # 信号丢失
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 通用遥测基类
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
@dataclass
|
||
class BaseTelemetry:
|
||
"""
|
||
无人系统通用遥测数据基类
|
||
|
||
所有遥测字段均有默认值，确保部分遥测数据缺失时不崩溃。
|
||
子类通过新增字段扩展特有遥测项。
|
||
"""
|
||
|
||
# ── 标识 ──────────────────────────────────────────────────────
|
||
device_id: str = ""
|
||
device_type: str = ""
|
||
timestamp: float = field(default_factory=time.time)
|
||
|
||
# ── 连接 ──────────────────────────────────────────────────────
|
||
connection_state: str = ConnectionState.DISCONNECTED.value
|
||
signal_strength: int = 0 # 信号强度（0~100）
|
||
latency_ms: float = 0.0 # 通信延迟（ms）
|
||
packet_loss_pct: float = 0.0 # 丢包率（%）
|
||
|
||
# ── 位置姿态 ──────────────────────────────────────────────────
|
||
latitude: float = 0.0 # 纬度（度）
|
||
longitude: float = 0.0 # 经度（度）
|
||
altitude: float = 0.0 # 高度（米，MSL）
|
||
relative_altitude: float = 0.0 # 相对高度（米，AGL）
|
||
heading: float = 0.0 # 航向（度，真北）
|
||
pitch: float = 0.0 # 俯仰角（度）
|
||
roll: float = 0.0 # 横滚角（度）
|
||
|
||
# ── 速度 ──────────────────────────────────────────────────────
|
||
ground_speed: float = 0.0 # 地速（m/s）
|
||
vx: float = 0.0 # 北向速度（m/s）
|
||
vy: float = 0.0 # 东向速度（m/s）
|
||
vz: float = 0.0 # 垂直速度（m/s，下为正）
|
||
|
||
# ── 电源 ──────────────────────────────────────────────────────
|
||
battery_voltage: float = 0.0 # 电池电压（V）
|
||
battery_current: float = 0.0 # 电池电流（A）
|
||
battery_percent: int = 100 # 电量百分比（%）
|
||
battery_remaining_s: int = 0 # 剩余续航（秒）
|
||
|
||
# ── 系统状态 ──────────────────────────────────────────────────
|
||
armed: bool = False # 是否解锁
|
||
mode: str = "" # 当前飞行/运动模式
|
||
system_status: str = "STANDBY"
|
||
error_flags: int = 0 # 错误标志位
|
||
|
||
# ── GPS ───────────────────────────────────────────────────────
|
||
gps_fix_type: int = 0 # 0=无,1=2D,2=3D,3=RTK
|
||
satellites_visible: int = 0 # 可见卫星数
|
||
hdop: float = 99.9 # 水平精度因子
|
||
vdop: float = 99.9 # 垂直精度因子
|
||
|
||
# ── 扩展 ──────────────────────────────────────────────────────
|
||
extra: Dict[str, Any] = field(default_factory=dict)
|
||
|
||
# ── 序列化 ────────────────────────────────────────────────────
|
||
|
||
def to_dict(self) -> Dict[str, Any]:
|
||
return dataclasses.asdict(self)
|
||
|
||
@property
|
||
def is_connected(self) -> bool:
|
||
return self.connection_state == ConnectionState.CONNECTED.value
|
||
|
||
@property
|
||
def gps_ok(self) -> bool:
|
||
return self.gps_fix_type >= 2 and self.satellites_visible >= 6
|
||
|
||
@property
|
||
def position(self) -> tuple:
|
||
return (self.latitude, self.longitude, self.altitude)
|
||
|
||
def __repr__(self) -> str:
|
||
return (f"{self.__class__.__name__}("
|
||
f"id={self.device_id}, "
|
||
f"pos=({self.latitude:.4f},{self.longitude:.4f},"
|
||
f"{self.altitude:.0f}m), "
|
||
f"hdg={self.heading:.1f}°, "
|
||
f"spd={self.ground_speed:.1f}m/s, "
|
||
f"bat={self.battery_percent}%)")
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 无人机遥测
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
@dataclass
|
||
class UAVTelemetry(BaseTelemetry):
|
||
"""无人机专有遥测数据"""
|
||
device_type: str = "controller"
|
||
|
||
# ── 飞行状态 ──────────────────────────────────────────────────
|
||
flight_mode: str = "STABILIZE"
|
||
flight_phase: str = "GROUND"
|
||
airspeed: float = 0.0 # 空速（m/s）
|
||
vertical_speed: float = 0.0 # 垂直速度（m/s，上为正）
|
||
climb_rate: float = 0.0 # 爬升率（m/s）
|
||
|
||
# ── 飞控状态 ──────────────────────────────────────────────────
|
||
throttle_pct: int = 0 # 油门百分比
|
||
autopilot_enabled: bool = False # 自动驾驶仪是否激活
|
||
mission_current: int = -1 # 当前任务点索引
|
||
mission_total: int = 0 # 任务点总数
|
||
|
||
# ── 发动机/电机 ───────────────────────────────────────────────
|
||
motor_count: int = 4 # 电机数量
|
||
motor_rpm: List[int] = field(default_factory=list)
|
||
|
||
# ── 载荷 ──────────────────────────────────────────────────────
|
||
gimbal_pitch: float = 0.0 # 云台俯仰（度）
|
||
gimbal_yaw: float = 0.0 # 云台偏航（度）
|
||
camera_recording: bool = False # 是否录像中
|
||
|
||
# ── 返航点 ────────────────────────────────────────────────────
|
||
home_lat: float = 0.0
|
||
home_lon: float = 0.0
|
||
home_alt: float = 0.0
|
||
distance_to_home: float = 0.0 # 距返航点距离（米）
|
||
|
||
|
||
# ─────────────────────────────────────────────────────────────────
|
||
# 机器狗遥测
|
||
# ─────────────────────────────────────────────────────────────────
|
||
|
||
@dataclass
|
||
class RobotDogTelemetry(BaseTelemetry):
|
||
"""机器狗专有遥测数据"""
|
||
device_type: str = "robot_dog"
|
||
|
||
# ── 运动状态 ──────────────────────────────────────────────────
|
||
gait_mode: str = "STAND" # STAND/WALK/TROT/BOUND
|
||
body_height: float = 0.5 # 机身高度（米）
|
||
step_frequency: float = 0.0 # 步频（Hz）
|
||
odometer_m: float = 0.0 # 里程计（米）
|
||
|
||
# ── 关节状态 ──────────────────────────────────────────────────
|
||
joint_positions: List[float] = field(default_factory=list)
|
||
joint_velocities: List[float] = field(default_factory=list)
|
||
joint_torques: List[float] = field(default_factory=list)
|
||
|
||
# ── 足端状态 ──────────────────────────────────────────────────
|
||
foot_contact: List[bool] = field(default_factory=list)
|
||
foot_force: List[float] = field(default_factory=list)
|
||
|
||
# ── IMU ───────────────────────────────────────────────────────
|
||
accel_x: float = 0.0
|
||
accel_y: float = 0.0
|
||
accel_z: float = 0.0
|
||
gyro_x: float = 0.0
|
||
gyro_y: float = 0.0
|
||
gyro_z: float = 0.0
|
||
|
||
# ── 环境感知 ──────────────────────────────────────────────────
|
||
obstacle_detected: bool = False
|
||
nearest_obstacle_m: float = 99.9
|
||
terrain_slope_deg: float = 0.0 |