实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/SITL入口
- utils/task_runner: 6态状态机(WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL) + 编排器(快照采集/状态动作) + 双层日志(事件JSONL/遥测CSV) + 吊舱控制指令(0x07/0x0F/0x0E/0x08) - pod_bridge: ground_link 地面坐标UDP注入(超龄判无效,无激光替代表4); set_heading_servo_enabled() 运行时L1/L2切换(切换即reset清积分防输出跳变); pod_locked 提为公共门控; config 新增 ground_inject 段 - virtual_pod 模拟吊舱(无电机语义) + verify_px4 增量④仅飞控验证 + pod_sitl_test L1 SITL全流程(飞行编排不塞进 run_bridge, 保持纯offboard闭环入口) - 入口: task_runner_sitl(VirtualPod+虚拟目标, SITL端到端跑通约1m24s) / task_runner_real(真串口+UDP注入+吊舱控制) - 地面站 ground_station: 坐标注入 + 状态可视化(Flask) - S1标定回填(2026-10-09): beta_sign=+1 / alpha_sign=-1, 加标定回归测试守护 - 新增测试: task_runner x4 + ground_link/virtual_pod*/bridge/px4_gateway 增量; 全量 483 passed - 文档: 预定流程(需求) + 预定流程-操作指南(操作/待办) + L1方案补S1结论与实现状态
This commit is contained in:
keita
@@ -31,6 +31,7 @@ from visual_track_control.types import (ControlMode, ControlSnapshot,
|
||||
TrackState)
|
||||
|
||||
from .config import BridgeConfig
|
||||
from .ground_link import GroundTarget
|
||||
from .guidance import GuidanceInput, GuidanceLaw
|
||||
from .pod_serial import PodLink, PodSerial
|
||||
from .protocol import (CarrierAttitude, ControlCommand, TargetGeo, Telemetry,
|
||||
@@ -48,12 +49,15 @@ class PodBridge:
|
||||
"""px4：Px4Gateway（或等价 mock）；pod：PodLink（PodSerial 或内存 FakePod）。"""
|
||||
|
||||
def __init__(self, cfg: BridgeConfig, px4: Px4Gateway, pod: PodLink,
|
||||
now_ms_fn=_monotonic_ms, vision_target_fn=None) -> None:
|
||||
now_ms_fn=_monotonic_ms, vision_target_fn=None,
|
||||
ground_target_fn=None) -> None:
|
||||
self._cfg = cfg
|
||||
self._px4 = px4
|
||||
self._pod = pod
|
||||
self._now_ms = now_ms_fn
|
||||
self._vision_target_fn = vision_target_fn # §2.3 第二判据（可选，S3+ 接入我方视觉）
|
||||
self._ground_target_fn = ground_target_fn # 地面注入坐标访问器（A2，无激光替代表4）
|
||||
self._use_heading_servo = cfg.heading_servo.enabled # 运行时控制律选择（A3 状态机切换）
|
||||
self._guidance = GuidanceLaw(cfg.guidance)
|
||||
self._tracking = TrackingControlLaw(cfg.tracking) # L1 航向伺服（方案 §0）
|
||||
self._attitude_period_ms = max(
|
||||
@@ -114,22 +118,36 @@ class PodBridge:
|
||||
|
||||
def _active_frequency_hz(self) -> float:
|
||||
"""当前生效控制律的循环频率：L1 用 tracking.control，L2 用 guidance。"""
|
||||
if self._cfg.heading_servo.enabled:
|
||||
if self._use_heading_servo:
|
||||
return self._cfg.tracking.control.frequency_hz
|
||||
return self._cfg.guidance.frequency_hz
|
||||
|
||||
def set_heading_servo_enabled(self, enabled: bool) -> None:
|
||||
"""运行时切换控制律（任务状态机 GOTO(L2) ↔ TRACK(L1) 切换，A3）。
|
||||
|
||||
切换时 reset 即将启用的律（清积分/限幅器历史），使指令从零渐变，
|
||||
避免切换瞬间输出跳变。L1 用吊舱角度直入，L2 用地理引导（注入坐标/表4）。
|
||||
"""
|
||||
if enabled == self._use_heading_servo:
|
||||
return
|
||||
self._use_heading_servo = enabled
|
||||
if enabled:
|
||||
self._tracking.reset()
|
||||
else:
|
||||
self._guidance.reset()
|
||||
|
||||
async def step(self, now_ms: int):
|
||||
"""单控制周期（显式传入时钟，便于确定性单测）。返回 ControlOutput。
|
||||
|
||||
按 heading_servo.enabled 在 L1（TrackingControlLaw，吊舱角度直入）与
|
||||
L2（GuidanceLaw，表4 地理引导）间切换（方案 §7）。
|
||||
按运行时选择的控制律在 L1（TrackingControlLaw，吊舱角度直入）与
|
||||
L2（GuidanceLaw，表4/注入坐标地理引导）间切换（方案 §7）。
|
||||
"""
|
||||
self._ingest_pod_frames(now_ms)
|
||||
|
||||
own = self._px4.own_state()
|
||||
self._downlink_attitude(own, now_ms)
|
||||
|
||||
if self._cfg.heading_servo.enabled:
|
||||
if self._use_heading_servo:
|
||||
out = self._tracking.update(self._build_snapshot(own, now_ms), now_ms)
|
||||
else:
|
||||
out = self._guidance.update(
|
||||
@@ -175,6 +193,21 @@ class PodBridge:
|
||||
|
||||
# --- 飞控侧 -------------------------------------------------------------
|
||||
def _build_guidance_input(self, own, now_ms: int) -> GuidanceInput:
|
||||
injected = self._injected_target(now_ms)
|
||||
if injected is not None:
|
||||
# 地面注入坐标优先（本吊舱无激光 → 表4 永不出，目标坐标改走地面站）
|
||||
return GuidanceInput(
|
||||
own_lat_deg=own.latitude_deg,
|
||||
own_lon_deg=own.longitude_deg,
|
||||
own_yaw_deg=math.degrees(own.yaw_rad),
|
||||
own_position_valid=own.position_valid,
|
||||
own_position_time_ms=own.position_time_ms, # position 流专用时间戳
|
||||
target_lat_deg=injected.latitude_deg,
|
||||
target_lon_deg=injected.longitude_deg,
|
||||
target_valid=True, # 注入坐标已验证新鲜
|
||||
target_time_ms=injected.time_ms,
|
||||
)
|
||||
# 回退表4（有激光吊舱 / 未接入地面站）
|
||||
tgt = self._latest_target
|
||||
locked = self._pod_locked(now_ms)
|
||||
return GuidanceInput(
|
||||
@@ -189,6 +222,26 @@ class PodBridge:
|
||||
target_time_ms=tgt[1] if tgt else 0,
|
||||
)
|
||||
|
||||
def _injected_target(self, now_ms: int) -> Optional[GroundTarget]:
|
||||
"""地面注入坐标（新鲜有效则返回，否则 None → 回退表4 / 无目标）。
|
||||
|
||||
本吊舱无激光 → 表4 永不出，目标坐标改由地面站注入（预定流程「地面给坐标」）。
|
||||
未启用 / 未注入 / 超龄 → None（下游按无目标刹车）。"""
|
||||
if not self._cfg.ground_inject.enabled:
|
||||
return None
|
||||
if self._ground_target_fn is None:
|
||||
return None
|
||||
target = self._ground_target_fn()
|
||||
if target is None or target.time_ms <= 0:
|
||||
return None
|
||||
if is_stale(target.time_ms, now_ms, self._cfg.ground_inject.timeout_ms):
|
||||
return None
|
||||
return target
|
||||
|
||||
def pod_locked(self, now_ms: int) -> bool:
|
||||
"""吊舱目标门控（公共接口，供编排层/状态机复用，A3）。"""
|
||||
return self._pod_locked(now_ms)
|
||||
|
||||
def _pod_locked(self, now_ms: int) -> bool:
|
||||
"""吊舱目标门控：表2-2 遥测在 link.telemetry_timeout_ms 内、状态字节1
|
||||
bit2=锁定、且（若 link.require_laser_valid）激光测距有效，才算目标有效。
|
||||
|
||||
@@ -20,6 +20,7 @@ from visual_track_control.config import TrackingConfig
|
||||
from visual_track_control.config import load_from_dict as _load_tracking_dict
|
||||
|
||||
from .guidance import GuidanceConfig
|
||||
from .ground_link import GroundInjectConfig
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -77,6 +78,7 @@ class BridgeConfig:
|
||||
guidance: GuidanceConfig = field(default_factory=GuidanceConfig)
|
||||
heading_servo: HeadingServoConfig = field(default_factory=HeadingServoConfig)
|
||||
tracking: TrackingConfig = field(default_factory=TrackingConfig)
|
||||
ground_inject: GroundInjectConfig = field(default_factory=GroundInjectConfig)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
@@ -294,6 +296,23 @@ def _load_tracking(root: dict) -> TrackingConfig:
|
||||
return _load_tracking_dict(s)
|
||||
|
||||
|
||||
def _load_ground_inject(root: dict) -> GroundInjectConfig:
|
||||
"""地面坐标注入配置（预定流程「地面给坐标」，A2）。"""
|
||||
s = _require_section(root, "ground_inject")
|
||||
allowed = {f.name for f in dataclasses.fields(GroundInjectConfig)}
|
||||
_reject_unknown(s, allowed, "ground_inject")
|
||||
c = GroundInjectConfig()
|
||||
c.enabled = _get_bool(s, "ground_inject", "enabled", c.enabled)
|
||||
c.host = _get_string(s, "ground_inject", "host", c.host)
|
||||
c.port = int(_in_range(
|
||||
_get_integer(s, "ground_inject", "port", c.port),
|
||||
1, 65535, "ground_inject.port"))
|
||||
c.timeout_ms = int(_positive(
|
||||
_get_integer(s, "ground_inject", "timeout_ms", c.timeout_ms),
|
||||
"ground_inject.timeout_ms"))
|
||||
return c
|
||||
|
||||
|
||||
def load_from_json(path: str) -> BridgeConfig:
|
||||
"""从 JSON 文件一次性加载桥接配置（启动期调用，调参重启生效）。"""
|
||||
try:
|
||||
@@ -314,4 +333,5 @@ def load_from_json(path: str) -> BridgeConfig:
|
||||
cfg.guidance = _load_guidance(root)
|
||||
cfg.heading_servo = _load_heading_servo(root)
|
||||
cfg.tracking = _load_tracking(root)
|
||||
cfg.ground_inject = _load_ground_inject(root)
|
||||
return cfg
|
||||
|
||||
@@ -0,0 +1,135 @@
|
||||
"""ground_link.py —— 地面坐标注入通道（预定流程.md「地面给坐标」）。
|
||||
|
||||
本吊舱无激光 → 表4 目标经纬度永不出（部署文档 §10.0），目标坐标改由地面站注入。
|
||||
本模块负责接收地面站下发的目标坐标（UDP JSON），维护一份带本地打戳的注入目标
|
||||
缓存，供 PodBridge 的 L2 地理引导**优先消费**（回退表4）。
|
||||
|
||||
设计要点：
|
||||
* UDP 无连接监听，每包 JSON {"lat":..,"lon":..,"alt":..}（可选 "ts" 仅诊断，不用）。
|
||||
* 新鲜度用「本地接收时刻」打戳（now_ms_fn 单调时钟），**不信任地面站时钟**
|
||||
（两端时钟不同步）；超龄由下游按 GroundInjectConfig.timeout_ms 判无效。
|
||||
* parse_payload 为纯函数（可单测）；socket / 事件循环拆到 run()，且 socket
|
||||
延迟导入，模块 import 无副作用（bridge 仅复用 GroundTarget 类型）。
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import json
|
||||
import logging
|
||||
from dataclasses import dataclass
|
||||
from typing import Callable, Optional
|
||||
|
||||
from visual_track_control.staleness_checker import is_stale
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
def _is_number(value) -> bool:
|
||||
"""数值（排除 bool）。"""
|
||||
return isinstance(value, (int, float)) and not isinstance(value, bool)
|
||||
|
||||
|
||||
@dataclass
|
||||
class GroundTarget:
|
||||
"""地面注入的目标坐标（带本地接收时刻）。
|
||||
|
||||
字段与 protocol.TargetGeo 语义对齐（经纬度 °、绝对高度 m），但不含表4 的
|
||||
时间字段——那些由吊舱解算生成，地面注入不提供。
|
||||
"""
|
||||
|
||||
latitude_deg: float = 0.0
|
||||
longitude_deg: float = 0.0
|
||||
abs_alt_m: float = 0.0
|
||||
time_ms: int = 0 # 本地接收时刻（单调毫秒），0 = 从未注入
|
||||
|
||||
|
||||
@dataclass
|
||||
class GroundInjectConfig:
|
||||
"""地面坐标注入配置（全外置；数值占位，待落地时回填）。"""
|
||||
|
||||
enabled: bool = False # 是否启用地面注入（骨架期默认关，走表4 原路径）
|
||||
host: str = "0.0.0.0" # UDP 监听地址
|
||||
port: int = 14555 # UDP 监听端口（避开 MAVSDK SITL 14540）
|
||||
timeout_ms: int = 5000 # 注入坐标超龄门限（超龄判无效 → 下游刹车）
|
||||
|
||||
|
||||
class GroundLink:
|
||||
"""地面坐标注入接收器。
|
||||
|
||||
用法（在 runner 外部以 asyncio task 运行）：
|
||||
link = GroundLink(cfg, now_ms_fn)
|
||||
task = asyncio.create_task(link.run(stop_event))
|
||||
PodBridge 通过 `ground_target_fn=lambda: link.latest()` 读取注入坐标。
|
||||
"""
|
||||
|
||||
def __init__(self, cfg: GroundInjectConfig,
|
||||
now_ms_fn: Callable[[], int]) -> None:
|
||||
self._cfg = cfg
|
||||
self._now_ms = now_ms_fn
|
||||
self._target = GroundTarget()
|
||||
|
||||
def latest(self) -> GroundTarget:
|
||||
"""返回当前注入目标（可能 time_ms=0 表示从未注入）。"""
|
||||
return self._target
|
||||
|
||||
def inject(self, target: GroundTarget) -> None:
|
||||
"""直接注入目标并本地打戳（测试 / 地面站本地注入用，与 UDP run() 等价）。"""
|
||||
target.time_ms = self._now_ms()
|
||||
self._target = target
|
||||
|
||||
def is_valid(self, now_ms: int) -> bool:
|
||||
"""注入坐标是否新鲜有效（未注入 / 超龄 → False）。"""
|
||||
t = self._target
|
||||
if t.time_ms <= 0:
|
||||
return False
|
||||
return not is_stale(t.time_ms, now_ms, self._cfg.timeout_ms)
|
||||
|
||||
@staticmethod
|
||||
def parse_payload(payload: bytes) -> Optional[GroundTarget]:
|
||||
"""解析地面站 JSON 包（纯函数，可单测）。返回不含 time_ms 的目标；
|
||||
非法 JSON / 非对象 / 缺 lat 或 lon → None（静默丢弃）。"""
|
||||
try:
|
||||
obj = json.loads(payload.decode("utf-8"))
|
||||
except (UnicodeDecodeError, json.JSONDecodeError):
|
||||
return None
|
||||
if not isinstance(obj, dict):
|
||||
return None
|
||||
lat = obj.get("lat")
|
||||
lon = obj.get("lon")
|
||||
if not _is_number(lat) or not _is_number(lon):
|
||||
return None
|
||||
alt = obj.get("alt", 0.0)
|
||||
if not _is_number(alt):
|
||||
return None
|
||||
return GroundTarget(latitude_deg=float(lat), longitude_deg=float(lon),
|
||||
abs_alt_m=float(alt))
|
||||
|
||||
async def run(self, stop_event: asyncio.Event) -> None:
|
||||
"""UDP 接收循环：每收一包合法 JSON 更新注入目标并本地打戳。"""
|
||||
import socket # 延迟导入：仅真正跑 socket 时才需要
|
||||
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
sock.bind((self._cfg.host, self._cfg.port))
|
||||
sock.setblocking(False)
|
||||
loop = asyncio.get_running_loop()
|
||||
logger.info("[GroundLink] 监听 UDP %s:%d", self._cfg.host, self._cfg.port)
|
||||
try:
|
||||
while not stop_event.is_set():
|
||||
try:
|
||||
data, _addr = await loop.sock_recvfrom(sock, 1024)
|
||||
except asyncio.CancelledError:
|
||||
raise
|
||||
except OSError as exc:
|
||||
logger.warning("[GroundLink] 接收异常: %s", exc)
|
||||
await asyncio.sleep(0.05)
|
||||
continue
|
||||
target = self.parse_payload(data)
|
||||
if target is None:
|
||||
continue # 非法包静默丢弃（热路径不打日志）
|
||||
target.time_ms = self._now_ms()
|
||||
self._target = target
|
||||
logger.debug("[GroundLink] 更新注入目标 (%f, %f, %fm)",
|
||||
target.latitude_deg, target.longitude_deg,
|
||||
target.abs_alt_m)
|
||||
finally:
|
||||
sock.close()
|
||||
@@ -1,7 +1,7 @@
|
||||
"""px4_gateway.py —— PX4(MAVSDK) 侧网关：读飞控遥测 + 发 offboard 速度指令。
|
||||
|
||||
职责（桥接的飞控侧）：
|
||||
* 订阅 PX4 遥测（position / attitude_euler / gps_info / velocity_body），缓存为
|
||||
* 订阅 PX4 遥测（position / attitude_euler / gps_info / velocity_ned），缓存为
|
||||
OwnState 快照，供桥接层构造 表3(载机姿态下发) 与 GuidanceInput(引导输入)；
|
||||
* 复用 visual_track_control.MavsdkCommandGateway 发送 offboard 速度设定点
|
||||
（不重造三分支映射，直接喂 GuidanceLaw 的 ControlOutput）。
|
||||
@@ -98,9 +98,11 @@ class Px4Gateway:
|
||||
self._state.position_time_ms = now # 仅此流刷新，姿态高频流不得代刷
|
||||
|
||||
def update_attitude(self, e: Any) -> None:
|
||||
self._state.roll_rad = float(e.roll_rad)
|
||||
self._state.pitch_rad = float(e.pitch_rad)
|
||||
self._state.yaw_rad = float(e.yaw_rad)
|
||||
"""MAVSDK attitude_euler() 返回 EulerAngle(roll_deg/pitch_deg/yaw_deg，度)，
|
||||
转弧度存入 OwnState（下游控制律统一用弧度 NED）。"""
|
||||
self._state.roll_rad = math.radians(float(e.roll_deg))
|
||||
self._state.pitch_rad = math.radians(float(e.pitch_deg))
|
||||
self._state.yaw_rad = math.radians(float(e.yaw_deg))
|
||||
self._state.has_attitude = True
|
||||
self._state.time_ms = self._now_ms()
|
||||
|
||||
@@ -110,9 +112,11 @@ class Px4Gateway:
|
||||
self._recompute_valid()
|
||||
self._state.time_ms = self._now_ms()
|
||||
|
||||
def update_velocity_body(self, v: Any) -> None:
|
||||
def update_velocity_ned(self, v: Any) -> None:
|
||||
"""MAVSDK velocity_ned() 返回 VelocityNed(north/east/down_m_s)，
|
||||
水平合速度即地速（表3 ground_speed 语义）。"""
|
||||
self._state.ground_speed_mps = math.hypot(
|
||||
float(v.forward_m_s), float(v.right_m_s))
|
||||
float(v.north_m_s), float(v.east_m_s))
|
||||
self._state.time_ms = self._now_ms()
|
||||
|
||||
def own_state(self) -> OwnState:
|
||||
@@ -125,7 +129,9 @@ class Px4Gateway:
|
||||
|
||||
# --- 遥测订阅（异步胶水）----------------------------------------------
|
||||
async def start_telemetry(self) -> None:
|
||||
"""订阅四路遥测流，后台任务持续刷新缓存。"""
|
||||
"""订阅四路遥测流，后台任务持续刷新缓存。幂等：已订阅则直接返回。"""
|
||||
if self._tasks:
|
||||
return # 已订阅，避免重复起任务（runner 会先订阅，bridge.start 再触发时跳过）
|
||||
t = self._drone.telemetry
|
||||
self._tasks = [
|
||||
asyncio.ensure_future(self._consume(t.position(), self.update_position)),
|
||||
@@ -133,7 +139,7 @@ class Px4Gateway:
|
||||
self._consume(t.attitude_euler(), self.update_attitude)),
|
||||
asyncio.ensure_future(self._consume(t.gps_info(), self.update_gps)),
|
||||
asyncio.ensure_future(
|
||||
self._consume(t.velocity_body(), self.update_velocity_body)),
|
||||
self._consume(t.velocity_ned(), self.update_velocity_ned)),
|
||||
]
|
||||
logger.info("[Px4Gateway] 遥测订阅已启动")
|
||||
|
||||
|
||||
@@ -0,0 +1,86 @@
|
||||
"""verify_px4.py —— 仅飞控验证（不接吊舱）：连飞控 + 订阅遥测 + offboard 启停。
|
||||
|
||||
增量流程第④级「仅飞控」：确认飞控侧遥测与 offboard 正常，再进第⑤级地面联调。
|
||||
|
||||
用法：
|
||||
python utils/pod_bridge/verify_px4.py serial:///dev/serial0:57600
|
||||
python utils/pod_bridge/verify_px4.py serial:///dev/serial0:57600 --offboard --seconds 15
|
||||
|
||||
判读标准（每行遥测）：
|
||||
* 连接成功 → 打印「飞控已连接」
|
||||
* 位置流：has_position=True、position_valid=True（fix_type>=3）、lat/lon 非 0
|
||||
* 姿态流：has_attitude=True（yaw 有值）
|
||||
* GPS：fix_type>=3、num_satellites>0
|
||||
* 遥测持续刷新（time_ms / position_time_ms 每拍递增）→ 链路稳定
|
||||
* --offboard：未上锁(armed)时 offboard.start 可能返回 False 属正常，仅验证启停不报错
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import asyncio
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
sys.path.insert(0, str(Path(__file__).resolve().parents[1])) # utils/ 入 sys.path
|
||||
|
||||
from pod_bridge.px4_gateway import Px4Gateway # noqa: E402
|
||||
|
||||
|
||||
def _monotonic_ms() -> int:
|
||||
return int(time.monotonic() * 1000)
|
||||
|
||||
|
||||
async def run(conn: str, seconds: int, do_offboard: bool) -> int:
|
||||
px4 = await Px4Gateway.connect(conn, now_ms_fn=_monotonic_ms)
|
||||
await px4.start_telemetry()
|
||||
print(f"-- 遥测订阅已启动，连续打印 {seconds}s …")
|
||||
print(" (has_pos pos_valid fix sat | lat lon rel_alt | yaw | time_ms/pos_ms)")
|
||||
|
||||
for i in range(seconds):
|
||||
await asyncio.sleep(1.0)
|
||||
s = px4.own_state()
|
||||
print(f"[{i:2d}s] pos={int(s.has_position)} valid={int(s.position_valid)} "
|
||||
f"fix={s.fix_type} sat={s.num_satellites:2d} | "
|
||||
f"{s.latitude_deg:.6f} {s.longitude_deg:.6f} {s.relative_altitude_m:5.1f}m | "
|
||||
f"yaw={s.yaw_rad:+.2f} att={int(s.has_attitude)} | "
|
||||
f"t={s.time_ms} pos_t={s.position_time_ms}")
|
||||
|
||||
s = px4.own_state()
|
||||
print("\n-- 判读 --")
|
||||
ok = True
|
||||
if not s.has_position:
|
||||
print(" ✗ 位置流无数据（position 流未到达）"); ok = False
|
||||
if not s.position_valid:
|
||||
print(f" ✗ 位置无效（fix_type={s.fix_type} < 3，GPS 未 3D 定位）"); ok = False
|
||||
if not s.has_attitude:
|
||||
print(" ✗ 姿态流无数据（attitude_euler 未到达）"); ok = False
|
||||
if ok:
|
||||
print(" ✓ 遥测正常：位置/姿态/GPS 均有效，链路稳定")
|
||||
|
||||
if do_offboard:
|
||||
print("\n-- offboard 启停测试 --")
|
||||
started = await px4.start_offboard()
|
||||
print(f" start_offboard -> {started}")
|
||||
if started:
|
||||
await asyncio.sleep(2.0)
|
||||
await px4.send_brake_hover(_monotonic_ms())
|
||||
await px4.stop_offboard()
|
||||
print(" ✓ offboard 启停完成（已零速刹车）")
|
||||
else:
|
||||
print(" ⚠ offboard 启动失败（未 armed 属正常，armed 后重试）")
|
||||
|
||||
await px4.stop_telemetry()
|
||||
return 0 if ok else 1
|
||||
|
||||
|
||||
def main() -> int:
|
||||
ap = argparse.ArgumentParser(description="仅飞控验证（不接吊舱）")
|
||||
ap.add_argument("conn", help="飞控连接串，如 serial:///dev/serial0:57600 或 udp://:14540")
|
||||
ap.add_argument("--seconds", type=int, default=10, help="遥测打印时长（秒）")
|
||||
ap.add_argument("--offboard", action="store_true", help="额外测试 offboard 启停")
|
||||
args = ap.parse_args()
|
||||
return asyncio.run(run(args.conn, args.seconds, args.offboard))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
sys.exit(main())
|
||||
@@ -0,0 +1,143 @@
|
||||
"""virtual_pod.py —— 模拟吊舱（SITL/台架工具，非飞行组件；类比 sim_fc.py 模拟飞控）。
|
||||
|
||||
按"无电机吊舱"的实测语义合成 表2-2 遥测，实现 PodLink 接口可直插 PodBridge：
|
||||
* 相机刚性固连机体朝前 → 框架角(frame_roll/pitch/yaw)恒 0；
|
||||
* 无激光 → laser_range_m 恒 None；
|
||||
* off_target 按目标相对机体的视线角合成，符号与 S1 台架标定一致：
|
||||
目标偏右 → off_target_x_deg 为正（右正）
|
||||
目标偏下 → off_target_y_deg 为负（上正）
|
||||
* 目标不可见（身后 / 超视场 / 被 set_visible(False)）→ 状态字节1 bit2=0（搜索态）
|
||||
且脱靶量为 0（对齐真实吊舱"丢失时脱靶量恒 0、锁定位不复位"的坑）。
|
||||
|
||||
载机状态由注入的 own_state_fn 提供（CarrierView：NED 位置 + 航向），与 PodBridge
|
||||
解耦——SITL 脚本注入遥测 hub，单测注入固定值。
|
||||
"""
|
||||
|
||||
import math
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
|
||||
from .protocol import Telemetry
|
||||
|
||||
|
||||
@dataclass
|
||||
class CarrierView:
|
||||
"""虚拟吊舱所需的载机视角状态（NED 位置 + 航向）。"""
|
||||
|
||||
north_m: float = 0.0
|
||||
east_m: float = 0.0
|
||||
down_m: float = 0.0
|
||||
yaw_rad: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class VirtualTargetConfig:
|
||||
"""虚拟目标放置与运动参数（锚点机体系偏移 + 水平圆弧）。"""
|
||||
|
||||
ahead_m: float = 30.0
|
||||
right_m: float = 15.0
|
||||
down_m: float = 0.0
|
||||
arc_radius_m: float = 12.0
|
||||
arc_speed_mps: float = 1.0
|
||||
arc_ccw: bool = True
|
||||
|
||||
|
||||
class VirtualTarget:
|
||||
"""虚拟目标：按锚点机头朝向放置，随后在 NED 水平面做匀速圆弧运动。
|
||||
|
||||
几何与 test.py 的 VirtualTarget 一致（去掉了针孔投影部分——吊舱路径
|
||||
直接给角度，不需要检测框）。
|
||||
"""
|
||||
|
||||
def __init__(self, cfg: VirtualTargetConfig,
|
||||
anchor_ned: tuple[float, float, float],
|
||||
anchor_yaw_rad: float) -> None:
|
||||
cos_y = math.cos(anchor_yaw_rad)
|
||||
sin_y = math.sin(anchor_yaw_rad)
|
||||
|
||||
self._start_n = anchor_ned[0] + cfg.ahead_m * cos_y - cfg.right_m * sin_y
|
||||
self._start_e = anchor_ned[1] + cfg.ahead_m * sin_y + cfg.right_m * cos_y
|
||||
self._start_d = anchor_ned[2] + cfg.down_m
|
||||
|
||||
self._radius_m = max(0.1, float(cfg.arc_radius_m))
|
||||
self._omega = (float(cfg.arc_speed_mps) / self._radius_m
|
||||
* (1.0 if cfg.arc_ccw else -1.0))
|
||||
|
||||
right_n = -sin_y
|
||||
right_e = cos_y
|
||||
self._center_n = self._start_n + self._radius_m * right_n
|
||||
self._center_e = self._start_e + self._radius_m * right_e
|
||||
self._theta0 = math.atan2(self._start_e - self._center_e,
|
||||
self._start_n - self._center_n)
|
||||
|
||||
self._pos_n = self._start_n
|
||||
self._pos_e = self._start_e
|
||||
self._pos_d = self._start_d
|
||||
self._t0 = time.monotonic()
|
||||
self._visible = True
|
||||
|
||||
@property
|
||||
def visible(self) -> bool:
|
||||
return self._visible
|
||||
|
||||
def set_visible(self, visible: bool) -> None:
|
||||
self._visible = visible
|
||||
|
||||
def update_position(self) -> None:
|
||||
elapsed = max(0.0, time.monotonic() - self._t0)
|
||||
theta = self._theta0 + self._omega * elapsed
|
||||
self._pos_n = self._center_n + self._radius_m * math.cos(theta)
|
||||
self._pos_e = self._center_e + self._radius_m * math.sin(theta)
|
||||
self._pos_d = self._start_d
|
||||
|
||||
@property
|
||||
def pos_ned(self) -> tuple[float, float, float]:
|
||||
return self._pos_n, self._pos_e, self._pos_d
|
||||
|
||||
|
||||
class VirtualPod:
|
||||
"""模拟吊舱：poll() 按当前载机视角合成一帧 表2-2 遥测（锁定/搜索态）。"""
|
||||
|
||||
def __init__(self, target: VirtualTarget, own_state_fn,
|
||||
fov_h_deg: float = 60.0, fov_v_deg: float = 35.0) -> None:
|
||||
self._target = target
|
||||
self._own = own_state_fn
|
||||
self._fov_h = fov_h_deg
|
||||
self._fov_v = fov_v_deg
|
||||
self.attitude_frames_rx = 0 # 收到的表3 帧数（诊断）
|
||||
self.closed = False
|
||||
|
||||
def poll(self) -> list:
|
||||
self._target.update_position()
|
||||
own = self._own()
|
||||
n, e, d = self._target.pos_ned
|
||||
rel_n = n - own.north_m
|
||||
rel_e = e - own.east_m
|
||||
rel_d = d - own.down_m
|
||||
cos_y = math.cos(own.yaw_rad)
|
||||
sin_y = math.sin(own.yaw_rad)
|
||||
fwd = rel_n * cos_y + rel_e * sin_y
|
||||
right = -rel_n * sin_y + rel_e * cos_y
|
||||
|
||||
off_x = math.degrees(math.atan2(right, fwd)) # 右正
|
||||
off_y = math.degrees(math.atan2(-rel_d, fwd)) # 上正（S1 标定）
|
||||
locked = (self._target.visible
|
||||
and fwd > 0.5
|
||||
and abs(off_x) < self._fov_h / 2.0
|
||||
and abs(off_y) < self._fov_v / 2.0)
|
||||
|
||||
return [Telemetry(
|
||||
status1=0b00000100 if locked else 0, # bit2 锁定 / 搜索
|
||||
off_target_x_deg=off_x if locked else 0.0,
|
||||
off_target_y_deg=off_y if locked else 0.0,
|
||||
frame_roll_deg=0.0, # 无电机：框架角恒 0
|
||||
frame_pitch_deg=0.0,
|
||||
frame_yaw_deg=0.0,
|
||||
laser_range_m=None, # 无激光：恒无效
|
||||
)]
|
||||
|
||||
def send(self, data: bytes) -> None:
|
||||
self.attitude_frames_rx += 1 # 表3 载机姿态下发：计数即可，不解析
|
||||
|
||||
def close(self) -> None:
|
||||
self.closed = True
|
||||
Reference in New Issue
Block a user