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结论与实现状态
87 lines
3.4 KiB
Python
87 lines
3.4 KiB
Python
"""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())
|