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
|