实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/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
2026-10-13 15:50:32 +08:00
parent 5a944af4f5
commit ded17d7995
30 changed files with 3552 additions and 27 deletions
+86
View File
@@ -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())