keita
ded17d7995
- 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结论与实现状态
404 lines
15 KiB
Python
404 lines
15 KiB
Python
#!/usr/bin/env python3
|
||
"""pod_sitl_test.py —— L1 航向伺服 SITL 全流程集成测试（test.py 的吊舱版）。
|
||
|
||
连接 PX4 SITL，流程：
|
||
解锁 → 起飞 → 记录悬停锚点/航向 → 进 offboard
|
||
→ L1 航向伺服闭环 20 Hz（VirtualPod 按虚拟目标几何合成表2-2 遥测
|
||
→ PodBridge → TrackingControlLaw 角度直入 → offboard 速度设定点）
|
||
→ 目标消失 → 判丢零速刹车 → 退出 offboard → RTL → 落地 → 上锁
|
||
|
||
与 test.py 的差异：
|
||
* 走 L1 角度直入（吊舱脱靶量 off_target），无像素/检测框/追踪器；
|
||
* 无距离（distance_valid=False）→ 距离通道按 no_distance_action 降级
|
||
（slow_approach，0.2 m/s 极低限速）；
|
||
* 无电机 → 框架角恒 0；无激光 → laser=None；
|
||
* 符号使用部署配置（S1 标定：beta_sign=+1 / alpha_sign=-1），
|
||
本脚本即对该标定的端到端复核。
|
||
|
||
运行前：启动 PX4 SITL（默认本机 14540）。用法：
|
||
python pod_sitl_test.py
|
||
python pod_sitl_test.py --conn udp://:14540 --config utils/config/pod_bridge.json
|
||
"""
|
||
|
||
import argparse
|
||
import asyncio
|
||
import csv
|
||
import logging
|
||
import math
|
||
import sys
|
||
import time
|
||
from pathlib import Path
|
||
|
||
_UTILS_DIR = Path(__file__).resolve().parent / "utils"
|
||
if str(_UTILS_DIR) not in sys.path:
|
||
sys.path.insert(0, str(_UTILS_DIR))
|
||
|
||
from mavsdk import System
|
||
from mavsdk.action import ActionError
|
||
|
||
from pod_bridge.bridge import PodBridge
|
||
from pod_bridge.config import load_from_json
|
||
from pod_bridge.px4_gateway import Px4Gateway
|
||
from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
|
||
VirtualTargetConfig)
|
||
|
||
logger = logging.getLogger("pod_sitl")
|
||
|
||
# ==================== 配置 ====================
|
||
CONNECTION = "udpin://0.0.0.0:14540"
|
||
CONFIG_PATH = _UTILS_DIR / "config" / "pod_bridge.json"
|
||
TAKEOFF_ALTITUDE = 5.0 # 起飞高度 (m)
|
||
|
||
# 虚拟目标：相对悬停锚点、按锚点机头朝向放置（前/右/下），随后水平圆弧运动。
|
||
TARGET_AHEAD_M = 30.0
|
||
TARGET_RIGHT_M = 15.0
|
||
TARGET_DOWN_M = 0.0 # 正 = 目标更低（地面目标场景，垂直通道会下降）
|
||
TARGET_ARC_RADIUS_M = 12.0
|
||
TARGET_ARC_SPEED_MPS = 1.0
|
||
TARGET_ARC_CCW = True
|
||
|
||
TRACK_NORMAL_SECS = 20.0 # 目标保持可见时长（之后模拟丢失）
|
||
TRACK_BRAKE_SECS = 5.0 # 判丢后零速刹车时长
|
||
TRACK_TIMEOUT_SECS = 120.0 # 跟踪段整体安全超时
|
||
STATUS_EVERY_TICKS = 10 # 20 Hz 下每 0.5 s 打印一行状态
|
||
CSV_PATH = Path(__file__).resolve().parent / "pod_sitl_trajectory.csv"
|
||
|
||
|
||
def now_ms() -> int:
|
||
return int(time.monotonic() * 1000.0)
|
||
|
||
|
||
class TelemetryHub:
|
||
"""后台订阅 NED 位置/姿态，供 VirtualPod 几何合成与锚点记录（同 test.py）。"""
|
||
|
||
def __init__(self) -> None:
|
||
self.yaw_rad = 0.0
|
||
self.north_m = 0.0
|
||
self.east_m = 0.0
|
||
self.down_m = 0.0
|
||
self.attitude_time_ms = 0
|
||
self._tasks: list = []
|
||
|
||
def start(self, drone) -> None:
|
||
self._tasks.append(asyncio.create_task(self._attitude_loop(drone)))
|
||
self._tasks.append(asyncio.create_task(self._position_loop(drone)))
|
||
|
||
async def _attitude_loop(self, drone) -> None:
|
||
async for euler in drone.telemetry.attitude_euler():
|
||
self.yaw_rad = math.radians(euler.yaw_deg)
|
||
self.attitude_time_ms = now_ms()
|
||
|
||
async def _position_loop(self, drone) -> None:
|
||
async for pos_vel in drone.telemetry.position_velocity_ned():
|
||
pos = pos_vel.position
|
||
self.north_m = pos.north_m
|
||
self.east_m = pos.east_m
|
||
self.down_m = pos.down_m
|
||
|
||
async def stop(self) -> None:
|
||
for task in self._tasks:
|
||
task.cancel()
|
||
for task in self._tasks:
|
||
try:
|
||
await task
|
||
except asyncio.CancelledError:
|
||
pass
|
||
self._tasks.clear()
|
||
|
||
def view(self) -> CarrierView:
|
||
return CarrierView(north_m=self.north_m, east_m=self.east_m,
|
||
down_m=self.down_m, yaw_rad=self.yaw_rad)
|
||
|
||
|
||
async def print_flight_mode(drone):
|
||
async for flight_mode in drone.telemetry.flight_mode():
|
||
print(f"[模式] 当前飞行模式: {flight_mode}")
|
||
return flight_mode
|
||
|
||
|
||
async def wait_until_ready(drone):
|
||
"""等待连接 + 位置估计就绪（各阶段限时 30 s，同 test.py）。"""
|
||
print("等待无人机连接...")
|
||
try:
|
||
async with asyncio.timeout(30.0):
|
||
async for state in drone.core.connection_state():
|
||
if state.is_connected:
|
||
print("-- 已连接")
|
||
break
|
||
except TimeoutError:
|
||
raise RuntimeError("30 s 内未建立 MAVSDK 连接（检查 SITL 是否在运行）") from None
|
||
|
||
print("等待全球位置估计就绪...")
|
||
try:
|
||
async with asyncio.timeout(30.0):
|
||
async for health in drone.telemetry.health():
|
||
if health.is_global_position_ok and health.is_home_position_ok:
|
||
print("-- 位置估计 OK")
|
||
break
|
||
except TimeoutError:
|
||
raise RuntimeError("30 s 内位置估计未就绪") from None
|
||
|
||
|
||
async def set_telemetry_rates(drone):
|
||
"""显式设定遥测流速率（失败沿用默认并告警，同 test.py）。"""
|
||
for stream, rate in (("position", 50.0),
|
||
("position_velocity_ned", 30.0),
|
||
("attitude_euler", 50.0),
|
||
("gps_info", 10.0),
|
||
("velocity_ned", 10.0),
|
||
("in_air", 10.0)):
|
||
try:
|
||
await getattr(drone.telemetry, f"set_rate_{stream}")(rate)
|
||
except Exception as e:
|
||
print(f"! 设置 {stream} 速率失败（沿用默认值）: {e}")
|
||
|
||
|
||
async def takeoff(drone) -> bool:
|
||
"""解锁 + 起飞到 TAKEOFF_ALTITUDE。失败返回 False（调用方执行降落兜底）。"""
|
||
print("-- 解锁")
|
||
try:
|
||
await drone.action.arm()
|
||
except ActionError as e:
|
||
print(f"解锁失败: {e}")
|
||
return False
|
||
|
||
print(f"-- 起飞到 {TAKEOFF_ALTITUDE} m")
|
||
await drone.action.set_takeoff_altitude(TAKEOFF_ALTITUDE)
|
||
await drone.action.takeoff()
|
||
|
||
# 全速排水等待爬升（test.py 问题一的经验：循环内不 sleep，避免队列积压）
|
||
print("-- 等待起飞完成...")
|
||
reached = False
|
||
try:
|
||
async with asyncio.timeout(30.0):
|
||
async for position in drone.telemetry.position():
|
||
if position.relative_altitude_m >= TAKEOFF_ALTITUDE * 0.90:
|
||
print(f"-- 已到达起飞高度: {position.relative_altitude_m:.2f} m")
|
||
reached = True
|
||
break
|
||
except TimeoutError:
|
||
pass
|
||
if not reached:
|
||
print("! 30 s 内未达到起飞高度")
|
||
return reached
|
||
|
||
|
||
async def land_fallback(drone):
|
||
try:
|
||
await drone.action.land()
|
||
except ActionError as e:
|
||
print(f"! 降落指令失败: {e}")
|
||
|
||
|
||
async def read_anchor(drone):
|
||
"""读取悬停锚点 NED 与机头航向（虚拟目标按机头朝向放置）。"""
|
||
try:
|
||
async with asyncio.timeout(10.0):
|
||
async for pos_vel in drone.telemetry.position_velocity_ned():
|
||
anchor_ned = (pos_vel.position.north_m,
|
||
pos_vel.position.east_m,
|
||
pos_vel.position.down_m)
|
||
break
|
||
async with asyncio.timeout(10.0):
|
||
async for euler in drone.telemetry.attitude_euler():
|
||
anchor_yaw_deg = euler.yaw_deg
|
||
break
|
||
except TimeoutError:
|
||
raise RuntimeError("10 s 内未取得悬停锚点/航向") from None
|
||
print(f" 悬停锚点 NED: N={anchor_ned[0]:.2f}, E={anchor_ned[1]:.2f}, "
|
||
f"D={anchor_ned[2]:.2f}，机头 {anchor_yaw_deg:.1f}°")
|
||
return anchor_ned, anchor_yaw_deg
|
||
|
||
|
||
async def run_tracking(bridge, hub, target, started_ms: int) -> bool:
|
||
"""L1 航向伺服闭环段：按生效律频率循环 bridge.step()，返回是否正常走完场景。"""
|
||
period_s = 1.0 / bridge._active_frequency_hz()
|
||
target_gone_ms = started_ms + TRACK_NORMAL_SECS * 1000.0
|
||
lost_at_ms = None
|
||
next_deadline = time.monotonic() + period_s
|
||
tick = 0
|
||
rows: list = []
|
||
try:
|
||
while True:
|
||
now = now_ms()
|
||
if now - started_ms >= TRACK_TIMEOUT_SECS * 1000.0:
|
||
logger.warning("跟踪段超时（%.0f s），提前收尾", TRACK_TIMEOUT_SECS)
|
||
break
|
||
|
||
if target.visible and now >= target_gone_ms:
|
||
target.set_visible(False)
|
||
print("-- 模拟目标消失，吊舱转搜索态 → 应判丢刹车")
|
||
|
||
try:
|
||
out = await bridge.step(now)
|
||
except Exception as exc:
|
||
# 对齐 bridge.run() 的护栏：异常不终止，补发刹车保 offboard 心跳
|
||
logger.warning("[SITL] 控制周期异常: %s", exc)
|
||
try:
|
||
await bridge._px4.send_brake_hover(now)
|
||
except Exception:
|
||
pass
|
||
await asyncio.sleep(period_s)
|
||
continue
|
||
|
||
if tick % STATUS_EVERY_TICKS == 0:
|
||
t_s = (now - started_ms) / 1000.0
|
||
tel = bridge.latest_telemetry
|
||
off = f"off=({tel[0].off_target_x_deg:+.1f},{tel[0].off_target_y_deg:+.1f})°" \
|
||
if tel else "off=(无遥测)"
|
||
print(f"[跟踪 t={t_s:5.1f}s] {off} | [{out.mode.name}] "
|
||
f"yaw_rate={out.yaw_rate_dps:+6.2f}°/s "
|
||
f"vx_body={out.vx_body_mps:+.2f} vz={out.vz_mps:+.2f}")
|
||
rows.append((t_s, hub.north_m, hub.east_m, hub.down_m,
|
||
*target.pos_ned, out.yaw_rate_dps,
|
||
out.vx_body_mps, out.vz_mps, out.mode.name))
|
||
tick += 1
|
||
|
||
if out.mode.name == "BRAKE_HOVER":
|
||
if lost_at_ms is None:
|
||
lost_at_ms = now
|
||
print("-- 判丢，执行零速刹车")
|
||
elif now - lost_at_ms >= TRACK_BRAKE_SECS * 1000.0:
|
||
print("-- 判丢刹车完成，退出跟踪段")
|
||
break
|
||
|
||
next_deadline += period_s
|
||
sleep_s = next_deadline - time.monotonic()
|
||
if sleep_s > 0:
|
||
await asyncio.sleep(sleep_s)
|
||
else:
|
||
next_deadline = time.monotonic() + period_s # 节拍落后，重锚
|
||
finally:
|
||
_write_csv(rows)
|
||
return True
|
||
|
||
|
||
def _write_csv(rows) -> None:
|
||
if not rows:
|
||
return
|
||
try:
|
||
with open(CSV_PATH, "w", newline="", encoding="utf-8") as fp:
|
||
w = csv.writer(fp)
|
||
w.writerow(["t_s", "drone_n", "drone_e", "drone_d",
|
||
"tgt_n", "tgt_e", "tgt_d",
|
||
"yaw_rate_dps", "vx_body", "vz", "mode"])
|
||
w.writerows(rows)
|
||
print(f"-- 轨迹 CSV 已保存: {CSV_PATH}")
|
||
except OSError as e:
|
||
print(f"! CSV 写入失败: {e}")
|
||
|
||
|
||
async def run(args) -> int:
|
||
cfg = load_from_json(args.config)
|
||
if not cfg.heading_servo.enabled:
|
||
print("! 配置 heading_servo.enabled=false：本脚本跑的是 L1 路径，"
|
||
"请用 pod_bridge.json（enabled=true）")
|
||
return 2
|
||
|
||
drone = System()
|
||
await drone.connect(system_address=args.conn)
|
||
|
||
try:
|
||
await wait_until_ready(drone)
|
||
except RuntimeError as e:
|
||
print(f"! {e}")
|
||
return 1
|
||
|
||
await set_telemetry_rates(drone)
|
||
initial_mode = await print_flight_mode(drone)
|
||
|
||
if not await takeoff(drone):
|
||
await land_fallback(drone)
|
||
return 1
|
||
|
||
await asyncio.sleep(1)
|
||
try:
|
||
anchor_ned, anchor_yaw_deg = await read_anchor(drone)
|
||
except RuntimeError as e:
|
||
print(f"! {e}")
|
||
await land_fallback(drone)
|
||
return 1
|
||
|
||
# ---- L1 闭环组件装配：真 SITL 飞控 + 模拟吊舱，全部复用生产代码路径 ----
|
||
hub = TelemetryHub()
|
||
hub.start(drone)
|
||
target = VirtualTarget(
|
||
VirtualTargetConfig(ahead_m=TARGET_AHEAD_M, right_m=TARGET_RIGHT_M,
|
||
down_m=TARGET_DOWN_M,
|
||
arc_radius_m=TARGET_ARC_RADIUS_M,
|
||
arc_speed_mps=TARGET_ARC_SPEED_MPS,
|
||
arc_ccw=TARGET_ARC_CCW),
|
||
anchor_ned, math.radians(anchor_yaw_deg))
|
||
pod = VirtualPod(target, hub.view,
|
||
fov_h_deg=cfg.tracking.camera.fov_h_deg,
|
||
fov_v_deg=cfg.tracking.camera.fov_v_deg)
|
||
px4 = Px4Gateway(drone)
|
||
bridge = PodBridge(cfg, px4, pod, now_ms_fn=now_ms)
|
||
|
||
print(f"-- 进入 L1 航向伺服：虚拟目标按机头朝向放置于 "
|
||
f"前 {TARGET_AHEAD_M:.0f} m / 右 {TARGET_RIGHT_M:.0f} m / "
|
||
f"下 {TARGET_DOWN_M:.0f} m（世界 NED: N={target.pos_ned[0]:.1f}, "
|
||
f"E={target.pos_ned[1]:.1f}, D={target.pos_ned[2]:.1f}）")
|
||
print(f" 场景: 正常跟踪 {TRACK_NORMAL_SECS:.0f}s → 目标消失 → "
|
||
f"判丢刹车 {TRACK_BRAKE_SECS:.0f}s")
|
||
print(f" 符号(部署配置): beta_sign={cfg.heading_servo.beta_sign:+d} "
|
||
f"alpha_sign={cfg.heading_servo.alpha_sign:+d} | 目标偏右→yaw_rate 正，"
|
||
f"目标偏下→vz 正")
|
||
|
||
if not await bridge.start():
|
||
print("! offboard 启动失败，放弃跟踪段")
|
||
await bridge.stop()
|
||
await hub.stop()
|
||
await land_fallback(drone)
|
||
return 1
|
||
|
||
await run_tracking(bridge, hub, target, now_ms())
|
||
await bridge.stop()
|
||
await hub.stop()
|
||
|
||
# ---- 退出 offboard 后回到 Hold，稍候确认模式 ----
|
||
await asyncio.sleep(1)
|
||
await print_flight_mode(drone)
|
||
|
||
# ---- RTL → 等待落地 → 上锁 ----
|
||
print("-- 执行 RTL")
|
||
await drone.action.return_to_launch()
|
||
|
||
print("-- 等待落地...")
|
||
try:
|
||
async with asyncio.timeout(240.0):
|
||
async for is_in_air in drone.telemetry.in_air():
|
||
if not is_in_air:
|
||
print("-- 已落地")
|
||
break
|
||
except TimeoutError:
|
||
print("! 240 s 内未检测到落地")
|
||
|
||
await asyncio.sleep(2)
|
||
print("-- 上锁")
|
||
try:
|
||
await drone.action.disarm()
|
||
print("-- 上锁成功")
|
||
except ActionError as e:
|
||
print(f"上锁失败（可能已自动上锁）: {e}")
|
||
|
||
print(f"-- 初始模式是: {initial_mode}")
|
||
print("\n===== L1 全流程测试完成 =====")
|
||
return 0
|
||
|
||
|
||
def main() -> int:
|
||
ap = argparse.ArgumentParser(description="L1 航向伺服 SITL 全流程集成测试")
|
||
ap.add_argument("--conn", default=CONNECTION, help="MAVSDK 连接串（默认 SITL 14540）")
|
||
ap.add_argument("--config", default=str(CONFIG_PATH), help="桥接配置 JSON 路径")
|
||
args = ap.parse_args()
|
||
|
||
logging.basicConfig(level=logging.INFO,
|
||
format="%(asctime)s.%(msecs)03d %(levelname)s %(message)s",
|
||
datefmt="%H:%M:%S")
|
||
return asyncio.run(run(args))
|
||
|
||
|
||
if __name__ == "__main__":
|
||
sys.exit(main())
|