实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/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
+14
-2
@@ -77,6 +77,11 @@ distance_available = snapshot.distance_valid and distance>0 and 未超龄
|
||||
>
|
||||
> "装不装电机"只是翻配置，架构（姿态来自 PX4、角度来自吊舱）不变。**装电机后 §2.1
|
||||
> 的"脱靶量长期≈0、必须靠框架角"警告才重新生效。**
|
||||
>
|
||||
> **S1 标定结论（2026-10-09）**：厂商确认本版电机无用；实测目标在**左下**时
|
||||
> `off_target_x/y` **均为负** → 吊舱 X 轴右正（与控制律 β 同向）故 `beta_sign=+1`；
|
||||
> Y 轴**上正**（与控制律 α"下正"相反）故 `alpha_sign=-1`。已回填 `pod_bridge.json`
|
||||
> 并加标定回归测试（`test_deployed_config_has_calibrated_signs`）守护。
|
||||
|
||||
### 2.2 如何接入 TrackingControlLaw（三选一，需拍板）
|
||||
|
||||
@@ -224,6 +229,13 @@ L1 落地时就留好口子，将来 L2 只是"换个输入源"，不动结构
|
||||
| 7 | `tests/` | ✅ 新增 24 条（vtc 5 + config 9 + bridge L1 10）；全量 **396 passed** |
|
||||
| 8 | `pod_bridge/run_bridge.py` | ✅ 闭环 CLI 入口：`--dry-run`(S2 只接吊舱/不发 offboard/打印 β→yaw_rate) 与实飞(S3+，`PodBridge.connect`+信号优雅停机)；配套 `run_bridge_test.py` 5 条，全量 **401 passed** |
|
||||
| 9 | `pod_bridge/bridge.py:run()` | ✅ 循环周期改按生效律取频率（L1=tracking.control，L2=guidance），修 L1 live 频率错配 |
|
||||
| 10 | `pod_bridge/virtual_pod.py` | ✅ 模拟吊舱（SITL/台架工具）：按无电机语义合成表2-2（框架角恒0、无激光、不可见→bit2=0+脱靶量归0），符号与 S1 标定一致 |
|
||||
| 11 | `pod_sitl_test.py`（根目录） | ✅ L1 SITL 全流程（test.py 吊舱版）：解锁→起飞→锚点→offboard→L1 闭环(VirtualPod→PodBridge→控制律)→丢目标→刹车→RTL→落地→上锁，轨迹落 CSV；飞行编排不塞进 run_bridge（保持其纯 offboard 闭环入口） |
|
||||
| 12 | `tests/pod_bridge/virtual_pod*_test.py` | ✅ 15 条：S1 符号回归 + VirtualPod×PodBridge×控制律集成（右转/下降/慢速前近/丢目标刹车）；全量 **417 passed** |
|
||||
|
||||
**待办（非代码，台架）**：S1 标定 `beta_sign`/`alpha_sign` 与框架角定义；S2 干跑
|
||||
`run_bridge.py --dry-run` 验符号（目标偏左→yaw_rate 负、居中→趋 0），符号错就翻配置里的 sign。
|
||||
**待办（非代码，台架）**：~~S1 标定~~ **已完成（2026-10-09）**：目标在左下时 `off_target_x/y`
|
||||
均为负 → 吊舱 X 轴右正（与 β 同向）`beta_sign=+1`；Y 轴上正（与 α 下正相反）
|
||||
`alpha_sign=-1`（已回填 `pod_bridge.json`，并加标定回归测试守住）。厂商确认本版
|
||||
**电机无用** → `beta_from_frame/alpha_from_frame` 恒 false。剩 **S2 干跑**
|
||||
`run_bridge.py --dry-run` 复核方向与量级（目标偏左→yaw_rate 负、居中→趋 0），并标定 `heading.gain`；
|
||||
之后 **SITL 全流程** `python pod_sitl_test.py`（需 PX4 SITL，本机尚未实跑）→ S3 不装桨接飞控。
|
||||
|
||||
@@ -0,0 +1,139 @@
|
||||
"""ground_station/app.py —— 地面站页面（Flask，临时测试用）。
|
||||
|
||||
预定流程 C 阶段：地面坐标输入 + 状态可视化配套页面。
|
||||
|
||||
功能：
|
||||
* GET / 地面站页面（坐标输入 + 状态面板 + 模拟视野）
|
||||
* POST /api/inject 接收 {lat, lon, alt}，UDP 发到吊舱桥接的 ground_inject 端口
|
||||
（默认 14555，GroundLink.run() 接收）
|
||||
* GET /api/state 返回最新任务状态（读任务日志：事件最后状态 + 遥测最后一行）
|
||||
|
||||
与无人机 runner 解耦：本页面只做「发坐标（UDP）+ 读日志（只读）」，不侵入 runner。
|
||||
用法：
|
||||
python ground_station/app.py --udp-port 14555 --log-dir logs
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
import os
|
||||
import socket
|
||||
from pathlib import Path
|
||||
|
||||
from flask import Flask, jsonify, render_template, request
|
||||
|
||||
_GROUND_UDP_PORT = 14555
|
||||
_LOG_DIR = "logs"
|
||||
|
||||
|
||||
def _read_last_line(path: Path):
|
||||
"""读文件最后一行（tail，避免读全文）。"""
|
||||
try:
|
||||
with open(path, "rb") as f:
|
||||
f.seek(0, 2)
|
||||
size = f.tell()
|
||||
if size == 0:
|
||||
return None
|
||||
f.seek(max(0, size - 8192))
|
||||
lines = f.read().decode("utf-8", errors="replace").splitlines()
|
||||
return lines[-1] if lines else None
|
||||
except OSError:
|
||||
return None
|
||||
|
||||
|
||||
def _latest_state(log_dir: str) -> dict:
|
||||
"""从任务日志读出当前状态 + 最新遥测（只读，供 /api/state）。"""
|
||||
log_path = Path(log_dir)
|
||||
state = "unknown"
|
||||
events = []
|
||||
|
||||
# 事件日志：找最后一个 state 事件（当前任务状态）
|
||||
events_path = log_path / "task_events.jsonl"
|
||||
if events_path.exists():
|
||||
try:
|
||||
with open(events_path, encoding="utf-8") as f:
|
||||
for line in f:
|
||||
line = line.strip()
|
||||
if not line:
|
||||
continue
|
||||
try:
|
||||
ev = json.loads(line)
|
||||
except json.JSONDecodeError:
|
||||
continue
|
||||
events.append(ev)
|
||||
if ev.get("type") == "state":
|
||||
state = ev.get("to_state", state)
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
# 遥测最后一行
|
||||
telemetry = None
|
||||
tel_path = log_path / "task_telemetry.csv"
|
||||
last = _read_last_line(tel_path)
|
||||
if last:
|
||||
try:
|
||||
with open(tel_path, encoding="utf-8") as f:
|
||||
header = f.readline().strip().split(",")
|
||||
values = last.split(",")
|
||||
telemetry = dict(zip(header, values))
|
||||
except OSError:
|
||||
pass
|
||||
|
||||
return {"state": state, "telemetry": telemetry,
|
||||
"events": events[-20:], "log_dir": str(log_path)}
|
||||
|
||||
|
||||
def create_app(udp_port: int, log_dir: str) -> Flask:
|
||||
app = Flask(__name__)
|
||||
|
||||
@app.route("/")
|
||||
def index():
|
||||
return render_template("index.html", udp_port=udp_port)
|
||||
|
||||
@app.route("/api/inject", methods=["POST"])
|
||||
def inject():
|
||||
data = request.get_json(silent=True) or {}
|
||||
lat = data.get("lat")
|
||||
lon = data.get("lon")
|
||||
if lat is None or lon is None:
|
||||
return jsonify({"ok": False, "error": "需要 lat 和 lon"}), 400
|
||||
try:
|
||||
lat = float(lat)
|
||||
lon = float(lon)
|
||||
except (TypeError, ValueError):
|
||||
return jsonify({"ok": False, "error": "lat/lon 必须是数值"}), 400
|
||||
alt = float(data.get("alt", 0.0) or 0.0)
|
||||
|
||||
payload = json.dumps({"lat": lat, "lon": lon, "alt": alt})
|
||||
try:
|
||||
with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as s:
|
||||
s.sendto(payload.encode("utf-8"), ("127.0.0.1", udp_port))
|
||||
except OSError as exc:
|
||||
return jsonify({"ok": False, "error": f"UDP 发送失败: {exc}"}), 500
|
||||
return jsonify({"ok": True, "sent": {"lat": lat, "lon": lon, "alt": alt}})
|
||||
|
||||
@app.route("/api/state")
|
||||
def state():
|
||||
return jsonify(_latest_state(log_dir))
|
||||
|
||||
return app
|
||||
|
||||
|
||||
def main() -> int:
|
||||
ap = argparse.ArgumentParser(description="地面站页面（Flask）")
|
||||
ap.add_argument("--host", default="127.0.0.1")
|
||||
ap.add_argument("--port", type=int, default=5000, help="Flask 监听端口")
|
||||
ap.add_argument("--udp-port", type=int, default=_GROUND_UDP_PORT,
|
||||
help="坐标注入 UDP 端口（GroundLink 监听）")
|
||||
ap.add_argument("--log-dir", default=_LOG_DIR, help="任务日志目录")
|
||||
args = ap.parse_args()
|
||||
|
||||
app = create_app(args.udp_port, args.log_dir)
|
||||
print(f"[地面站] http://{args.host}:{args.port} | UDP 注入 → 127.0.0.1:{args.udp_port}"
|
||||
f" | 日志 → {args.log_dir}")
|
||||
app.run(host=args.host, port=args.port, debug=False)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,137 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="zh">
|
||||
<head>
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width, initial-scale=1">
|
||||
<title>地面站 · 预定流程</title>
|
||||
<style>
|
||||
:root { --bg:#f7f4ec; --card:#fffdf7; --ink:#2f3b2f; --mut:#6b756b;
|
||||
--green:#2f5d46; --green-ink:#eaf3de; --line:#e2ddcf; --red:#b04a3a; }
|
||||
* { box-sizing: border-box; }
|
||||
body { margin:0; font-family:-apple-system,"Segoe UI","Microsoft YaHei",sans-serif;
|
||||
background:var(--bg); color:var(--ink); }
|
||||
header { padding:16px 24px; border-bottom:1px solid var(--line); background:var(--card); }
|
||||
header h1 { margin:0; font-size:18px; font-weight:600; }
|
||||
header .sub { color:var(--mut); font-size:12px; margin-top:2px; }
|
||||
.wrap { display:flex; gap:16px; padding:16px 24px; flex-wrap:wrap; align-items:flex-start; }
|
||||
.col { background:var(--card); border:1px solid var(--line); border-radius:10px;
|
||||
padding:16px; min-width:280px; }
|
||||
.col h2 { margin:0 0 12px; font-size:14px; font-weight:600; color:var(--green); }
|
||||
label { display:block; font-size:12px; color:var(--mut); margin:8px 0 4px; }
|
||||
input { width:100%; padding:8px; border:1px solid var(--line); border-radius:6px;
|
||||
font-size:14px; background:#fff; color:var(--ink); }
|
||||
button { margin-top:12px; padding:9px 16px; border:none; border-radius:6px;
|
||||
background:var(--green); color:var(--green-ink); font-size:14px; cursor:pointer; }
|
||||
button:active { opacity:.85; }
|
||||
.kv { display:flex; justify-content:space-between; padding:5px 0;
|
||||
border-bottom:1px dashed var(--line); font-size:13px; }
|
||||
.kv:last-child { border-bottom:none; }
|
||||
.kv .k { color:var(--mut); }
|
||||
.kv .v { font-variant-numeric:tabular-nums; }
|
||||
.badge { display:inline-block; padding:2px 10px; border-radius:12px; font-size:12px;
|
||||
background:var(--green); color:var(--green-ink); }
|
||||
.badge.warn { background:var(--red); color:#fff; }
|
||||
canvas { background:#0e1410; border-radius:8px; display:block; }
|
||||
.hint { color:var(--mut); font-size:12px; margin-top:8px; }
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<header>
|
||||
<h1>地面站 · 预定流程</h1>
|
||||
<div class="sub">地面坐标注入 + 状态可视化（临时测试用）</div>
|
||||
</header>
|
||||
|
||||
<div class="wrap">
|
||||
<div class="col">
|
||||
<h2>坐标注入</h2>
|
||||
<label>纬度 lat</label>
|
||||
<input id="lat" type="text" value="47.398241" placeholder="如 47.398241">
|
||||
<label>经度 lon</label>
|
||||
<input id="lon" type="text" value="8.546363" placeholder="如 8.546363">
|
||||
<label>高度 alt (m)</label>
|
||||
<input id="alt" type="text" value="0">
|
||||
<button onclick="sendInject()">注入坐标</button>
|
||||
<div class="hint" id="inject-msg"></div>
|
||||
</div>
|
||||
|
||||
<div class="col">
|
||||
<h2>任务状态 <span class="badge" id="state">-</span></h2>
|
||||
<div class="kv"><span class="k">位置</span><span class="v" id="pos">-</span></div>
|
||||
<div class="kv"><span class="k">距目标</span><span class="v" id="dist">-</span></div>
|
||||
<div class="kv"><span class="k">锁定</span><span class="v" id="locked">-</span></div>
|
||||
<div class="kv"><span class="k">脱靶量 (x/y)</span><span class="v" id="off">-</span></div>
|
||||
<div class="kv"><span class="k">控制</span><span class="v" id="mode">-</span></div>
|
||||
<div class="kv"><span class="k">yaw_rate / vx</span><span class="v" id="cmd">-</span></div>
|
||||
<div class="hint" id="log-dir"></div>
|
||||
</div>
|
||||
|
||||
<div class="col">
|
||||
<h2>模拟视野（脱靶量）</h2>
|
||||
<canvas id="view" width="300" height="200"></canvas>
|
||||
<div class="hint">十字线 = 视野中心，红点 = 目标（按吊舱脱靶量映射）</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<script>
|
||||
const FOV_H = 60, FOV_V = 35;
|
||||
|
||||
function $(id){ return document.getElementById(id); }
|
||||
|
||||
async function sendInject(){
|
||||
const lat = $("lat").value, lon = $("lon").value, alt = $("alt").value || "0";
|
||||
const msg = $("inject-msg");
|
||||
try {
|
||||
const r = await fetch("/api/inject", {
|
||||
method:"POST", headers:{"Content-Type":"application/json"},
|
||||
body: JSON.stringify({lat:Number(lat), lon:Number(lon), alt:Number(alt)})
|
||||
});
|
||||
const j = await r.json();
|
||||
msg.textContent = j.ok ? `已注入 (${j.sent.lat}, ${j.sent.lon})` : `失败: ${j.error}`;
|
||||
msg.style.color = j.ok ? "var(--green)" : "var(--red)";
|
||||
} catch(e){ msg.textContent = "请求失败: "+e; msg.style.color = "var(--red)"; }
|
||||
}
|
||||
|
||||
function fmt(v, d){ return (v===null||v===undefined||v==="") ? "-" : Number(v).toFixed(d); }
|
||||
|
||||
function drawView(offX, offY){
|
||||
const cv = $("view"), ctx = cv.getContext("2d");
|
||||
const w = cv.width, h = cv.height, cx = w/2, cy = h/2;
|
||||
ctx.clearRect(0,0,w,h);
|
||||
ctx.strokeStyle = "#3d5343"; ctx.lineWidth = 1;
|
||||
ctx.strokeRect(0,0,w,h);
|
||||
ctx.strokeStyle = "#5f7a68"; ctx.setLineDash([4,4]);
|
||||
ctx.beginPath(); ctx.moveTo(cx,0); ctx.lineTo(cx,h);
|
||||
ctx.moveTo(0,cy); ctx.lineTo(w,cy); ctx.stroke();
|
||||
ctx.setLineDash([]);
|
||||
if(offX===null||offX===undefined){ return; }
|
||||
const x = cx + (offX / FOV_H) * w;
|
||||
const y = cy - (offY / FOV_V) * h; // 上正 → 屏幕减
|
||||
ctx.fillStyle = "#e0574a";
|
||||
ctx.beginPath(); ctx.arc(x, y, 5, 0, Math.PI*2); ctx.fill();
|
||||
}
|
||||
|
||||
async function poll(){
|
||||
try {
|
||||
const r = await fetch("/api/state");
|
||||
const j = await r.json();
|
||||
const t = j.telemetry || {};
|
||||
const state = j.state || "unknown";
|
||||
const s = $("state");
|
||||
s.textContent = state;
|
||||
s.className = "badge" + (state==="rtl"||state==="wait" ? " warn" : "");
|
||||
$("pos").textContent = t.latitude_deg ? `${fmt(t.latitude_deg,6)}, ${fmt(t.longitude_deg,6)}` : "-";
|
||||
$("dist").textContent = t.distance_to_target_m ? fmt(t.distance_to_target_m,1)+" m" : "-";
|
||||
$("locked").textContent = t.target_locked==="True" ? "锁定" : "未锁定";
|
||||
$("off").textContent = t.off_target_x_deg ? `${fmt(t.off_target_x_deg,2)}° / ${fmt(t.off_target_y_deg,2)}°` : "-";
|
||||
$("mode").textContent = t.mode || "-";
|
||||
$("cmd").textContent = t.yaw_rate_dps ? `${fmt(t.yaw_rate_dps,1)}°/s / ${fmt(t.vx_body_mps,2)} m/s` : "-";
|
||||
$("log-dir").textContent = "日志: " + (j.log_dir || "-");
|
||||
drawView(parseFloat(t.off_target_x_deg), parseFloat(t.off_target_y_deg));
|
||||
} catch(e){ /* 后端未起时静默 */ }
|
||||
}
|
||||
|
||||
poll();
|
||||
setInterval(poll, 1000);
|
||||
</script>
|
||||
</body>
|
||||
</html>
|
||||
@@ -0,0 +1,403 @@
|
||||
#!/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())
|
||||
@@ -0,0 +1,165 @@
|
||||
#!/usr/bin/env python3
|
||||
"""task_runner_real.py —— 真机任务编排入口（真实吊舱串口 + 地面站 UDP 注入 + PX4）。
|
||||
|
||||
与 task_runner_sitl.py（仿真）的差异：
|
||||
* 吊舱 = PodSerial（真实串口表2-2 遥测），非 VirtualPod；
|
||||
* 地面坐标 = GroundLink.run()（UDP 接收地面站 JSON），非 inject() 直接注入；
|
||||
* 吊舱目标识别/锁定 = PodController（GOTO 进入发 0x07 识别 + 0x0F 自动锁定，
|
||||
RTL 前发 0x0E 停止 + 0x08 识别关）；
|
||||
* PX4 连接 = cfg.px4.connection_string（真机串口/遥测），非 SITL 14540。
|
||||
|
||||
前置（真机联调增量：纯软单测 → 串口回环 → 仅吊舱 → 仅飞控 → 地面联调）：
|
||||
1. 填 utils/config/pod_bridge.json 的 pod_serial.port / px4.connection_string；
|
||||
2. 吊舱上电、串口连通（run_bridge.py --dry-run 已验证遥测）；
|
||||
3. 地面站往 ground_inject.port（默认 14555）发 JSON 坐标
|
||||
{"lat":..,"lon":..,"alt":..}（每 0.5~1s 一次）。
|
||||
|
||||
用法：
|
||||
python task_runner_real.py
|
||||
python task_runner_real.py --config utils/config/pod_bridge.json --target-type 1
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import asyncio
|
||||
import logging
|
||||
import sys
|
||||
import time
|
||||
from datetime import datetime
|
||||
from pathlib import Path
|
||||
|
||||
_ROOT = Path(__file__).resolve().parent
|
||||
_UTILS = _ROOT / "utils"
|
||||
for _p in (_UTILS, _ROOT):
|
||||
if str(_p) not in sys.path:
|
||||
sys.path.insert(0, str(_p))
|
||||
|
||||
from mavsdk import System
|
||||
|
||||
from pod_bridge.bridge import PodBridge
|
||||
from pod_bridge.config import load_from_json
|
||||
from pod_bridge.ground_link import GroundLink
|
||||
from pod_bridge.pod_serial import PodSerial
|
||||
from pod_bridge.px4_gateway import Px4Gateway
|
||||
from task_runner.log import TaskLogConfig, TaskLogger
|
||||
from task_runner.pod_control import PodController
|
||||
from task_runner.runner import TaskRunner
|
||||
from task_runner.state_machine import TaskStateConfig, TaskStateMachine
|
||||
from pod_sitl_test import now_ms, print_flight_mode, set_telemetry_rates, wait_until_ready
|
||||
|
||||
logger = logging.getLogger("task_real")
|
||||
|
||||
CONFIG_PATH = _UTILS / "config" / "pod_bridge.json"
|
||||
|
||||
TASK_TIMEOUT_S = 300.0 # 全流程总超时（真机放宽）
|
||||
|
||||
|
||||
def build_sm_config() -> TaskStateConfig:
|
||||
"""真机状态机参数。⚠ 全部为占位值，须台架标定后回填，标定前不得实飞。"""
|
||||
return TaskStateConfig(
|
||||
takeoff_altitude_m=5.0,
|
||||
switch_radius_m=30.0, # GOTO→TRACK 切换半径（待标定）
|
||||
goto_timeout_s=180.0,
|
||||
track_timeout_s=180.0,
|
||||
center_hold_s=3.0,
|
||||
center_threshold_deg=3.0,
|
||||
approach_hover_s=5.0,
|
||||
loop_frequency_hz=20.0,
|
||||
)
|
||||
|
||||
|
||||
async def run(args) -> int:
|
||||
cfg = load_from_json(args.config)
|
||||
if not cfg.pod_serial.port:
|
||||
print("! 未配置吊舱串口：在 pod_bridge.json 填 pod_serial.port，或用 --port 覆盖")
|
||||
return 2
|
||||
if args.port:
|
||||
cfg.pod_serial.port = args.port
|
||||
cfg.ground_inject.enabled = True # 地面注入必须开（真机无激光，表4 永不出）
|
||||
sm_cfg = build_sm_config()
|
||||
|
||||
drone = System()
|
||||
await drone.connect(system_address=args.conn or cfg.px4.connection_string)
|
||||
try:
|
||||
await wait_until_ready(drone)
|
||||
except RuntimeError as e:
|
||||
print(f"! {e}")
|
||||
return 1
|
||||
await set_telemetry_rates(drone)
|
||||
|
||||
# 装配：真吊舱串口 + 地面站 UDP 注入 + 吊舱识别/锁定控制
|
||||
px4 = Px4Gateway(drone)
|
||||
ground = GroundLink(cfg.ground_inject, now_ms_fn=now_ms)
|
||||
pod = PodSerial(cfg.pod_serial.port, cfg.pod_serial.baudrate)
|
||||
bridge = PodBridge(cfg, px4, pod, now_ms_fn=now_ms,
|
||||
ground_target_fn=lambda: ground.latest())
|
||||
sm = TaskStateMachine(sm_cfg)
|
||||
pod_control = PodController(bridge, target_type=args.target_type)
|
||||
log_dir = args.log_dir or f"logs/real_{datetime.now().strftime('%Y%m%d_%H%M%S')}"
|
||||
task_log = TaskLogger(TaskLogConfig(dir=log_dir), now_ms_fn=now_ms)
|
||||
runner = TaskRunner(cfg, sm_cfg, px4, bridge, sm, ground, drone,
|
||||
now_ms_fn=now_ms, logger=task_log, pod_control=pod_control)
|
||||
|
||||
stop = asyncio.Event()
|
||||
ground_task = asyncio.ensure_future(ground.run(stop)) # UDP 接收地面站坐标
|
||||
runner_task = asyncio.ensure_future(runner.run(stop))
|
||||
|
||||
print(f"-- 启动真机任务全流程（吊舱 {cfg.pod_serial.port}，"
|
||||
f"地面站 UDP {cfg.ground_inject.port}，目标类型 {args.target_type}）")
|
||||
print(f" 等待地面站注入坐标（每 0.5~1s 一次 JSON）… 日志 → {log_dir}")
|
||||
|
||||
started = time.monotonic()
|
||||
last_state = None
|
||||
try:
|
||||
while not runner.rtl_done:
|
||||
await asyncio.sleep(1.0)
|
||||
if runner.state is not last_state:
|
||||
last_state = runner.state
|
||||
print(f"[任务] 进入状态: {runner.state.value}")
|
||||
if runner_task.done():
|
||||
exc = runner_task.exception()
|
||||
if exc is not None:
|
||||
print(f"! runner 异常退出: {type(exc).__name__}: {exc}")
|
||||
break
|
||||
if time.monotonic() - started > TASK_TIMEOUT_S:
|
||||
print("! 全流程超时，强制返航")
|
||||
runner.request_rtl()
|
||||
if runner.rtl_done:
|
||||
print("-- 任务完成（RTL 落地）")
|
||||
except asyncio.CancelledError:
|
||||
raise
|
||||
finally:
|
||||
stop.set()
|
||||
ground_task.cancel()
|
||||
runner_task.cancel()
|
||||
try:
|
||||
await runner_task
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
try:
|
||||
await ground_task
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
|
||||
await print_flight_mode(drone)
|
||||
print("\n===== 真机任务全流程完成 =====")
|
||||
return 0
|
||||
|
||||
|
||||
def main() -> int:
|
||||
ap = argparse.ArgumentParser(description="真机任务状态机全流程")
|
||||
ap.add_argument("--config", default=str(CONFIG_PATH), help="桥接配置 JSON 路径")
|
||||
ap.add_argument("--conn", default=None, help="MAVSDK 连接串（覆盖 px4.connection_string）")
|
||||
ap.add_argument("--port", help="覆盖吊舱串口设备号（如 /dev/ttyAMA0）")
|
||||
ap.add_argument("--target-type", type=int, default=1,
|
||||
help="吊舱目标类型：0=人/车，1=无人机")
|
||||
ap.add_argument("--log-dir", default=None, help="日志输出目录（默认按时间戳）")
|
||||
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())
|
||||
@@ -0,0 +1,239 @@
|
||||
#!/usr/bin/env python3
|
||||
"""task_runner_sitl.py —— 任务级状态机 SITL 全流程集成测试（预定流程.md 天空侧）。
|
||||
|
||||
用状态机驱动完整流程：WAIT → ARM_TAKEOFF → GOTO(L2 坐标引导) → TRACK(L1 航向伺服)
|
||||
→ APPROACH(悬停) → RTL(返航落地)。相比 pod_sitl_test.py（固定场景、人工编排），
|
||||
本脚本把「飞行编排 + 控制律切换」交给 TaskRunner 状态机统一驱动。
|
||||
|
||||
场景：
|
||||
* 起飞前注入虚拟目标坐标（模拟地面站给坐标）——目标 = home 北 30m / 东 15m；
|
||||
* VirtualPod 按无电机语义合成表2-2（框架角恒0、无激光、锁定=目标在视场内）；
|
||||
* 后台任务持续刷新注入坐标（模拟地面站持续上报），坐标始终新鲜；
|
||||
* GOTO→TRACK 由状态机按「距目标 < switch_radius 且吊舱锁定」守卫切换。
|
||||
|
||||
运行前：启动 PX4 SITL（默认本机 14540）。用法：
|
||||
python task_runner_sitl.py
|
||||
python task_runner_sitl.py --conn udp://:14540 --config utils/config/pod_bridge.json
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import asyncio
|
||||
import logging
|
||||
import math
|
||||
import sys
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
_ROOT = Path(__file__).resolve().parent
|
||||
_UTILS = _ROOT / "utils"
|
||||
for _p in (_UTILS, _ROOT):
|
||||
if str(_p) not in sys.path:
|
||||
sys.path.insert(0, str(_p))
|
||||
|
||||
from mavsdk import System
|
||||
|
||||
from pod_bridge.bridge import PodBridge
|
||||
from pod_bridge.config import load_from_json
|
||||
from pod_bridge.ground_link import GroundInjectConfig, GroundLink, GroundTarget
|
||||
from pod_bridge.guidance import haversine_distance_m
|
||||
from pod_bridge.px4_gateway import Px4Gateway
|
||||
from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
|
||||
VirtualTargetConfig)
|
||||
from task_runner.log import TaskLogConfig, TaskLogger
|
||||
from task_runner.runner import TaskRunner
|
||||
from task_runner.state_machine import TaskStateConfig, TaskStateMachine
|
||||
from pod_sitl_test import (TelemetryHub, now_ms, print_flight_mode,
|
||||
set_telemetry_rates, wait_until_ready)
|
||||
|
||||
logger = logging.getLogger("task_sitl")
|
||||
|
||||
CONNECTION = "udpin://0.0.0.0:14540"
|
||||
CONFIG_PATH = _UTILS / "config" / "pod_bridge.json"
|
||||
|
||||
# 虚拟目标：相对 home（NED 原点）北 30m / 东 15m / 与悬停同高度（不追垂直）
|
||||
TARGET_NORTH_M = 30.0
|
||||
TARGET_EAST_M = 15.0
|
||||
TARGET_DOWN_M = -5.0 # 与 5m 悬停高度持平 → 垂直通道无动作
|
||||
|
||||
TASK_TIMEOUT_S = 180.0 # 全流程总超时
|
||||
INJECT_PERIOD_S = 0.5 # 注入坐标刷新周期（地面站上报快于 guidance.target_stale_ms）
|
||||
|
||||
_DEG_LAT_M = 111320.0
|
||||
|
||||
|
||||
def ned_offset_to_latlon(lat0: float, lon0: float, north_m: float,
|
||||
east_m: float) -> tuple[float, float]:
|
||||
"""NED 偏移（米）转经纬度（局部平面近似，SITL 小范围够用）。"""
|
||||
lat = lat0 + north_m / _DEG_LAT_M
|
||||
lon = lon0 + east_m / (_DEG_LAT_M * math.cos(math.radians(lat0)))
|
||||
return lat, lon
|
||||
|
||||
|
||||
async def read_home_gps(drone) -> tuple[float, float]:
|
||||
"""起飞前读 home 经纬度（放虚拟目标 + 注入坐标）。"""
|
||||
try:
|
||||
async with asyncio.timeout(10.0):
|
||||
async for pos in drone.telemetry.position():
|
||||
return pos.latitude_deg, pos.longitude_deg
|
||||
except TimeoutError:
|
||||
raise RuntimeError("10 s 内未取得 home GPS") from None
|
||||
|
||||
|
||||
def build_sm_config() -> TaskStateConfig:
|
||||
"""SITL 状态机参数（占位值，仅用于仿真验证链路）。"""
|
||||
return TaskStateConfig(
|
||||
takeoff_altitude_m=5.0,
|
||||
switch_radius_m=25.0, # 目标在 30m，飞进 25m 内切 TRACK
|
||||
goto_timeout_s=60.0,
|
||||
track_timeout_s=60.0,
|
||||
center_hold_s=3.0,
|
||||
center_threshold_deg=3.0, # 脱靶量 < 3° 判居中
|
||||
approach_hover_s=5.0, # 悬停 5s 后 RTL
|
||||
loop_frequency_hz=20.0,
|
||||
)
|
||||
|
||||
|
||||
async def inject_loop(ground: GroundLink, lat: float, lon: float,
|
||||
stop: asyncio.Event) -> None:
|
||||
"""后台持续刷新注入坐标（模拟地面站持续上报，保坐标始终新鲜）。"""
|
||||
first = True
|
||||
while not stop.is_set():
|
||||
ground.inject(GroundTarget(latitude_deg=lat, longitude_deg=lon,
|
||||
abs_alt_m=0.0))
|
||||
if first:
|
||||
print(f"-- 首次注入坐标 time_ms={ground.latest().time_ms}")
|
||||
first = False
|
||||
await asyncio.sleep(INJECT_PERIOD_S)
|
||||
|
||||
|
||||
async def run(args) -> int:
|
||||
cfg = load_from_json(args.config)
|
||||
cfg.ground_inject.enabled = True # 启用地面注入（骨架期 JSON 默认关）
|
||||
cfg.guidance.target_stale_ms = 5000 # 注入坐标超时放宽（表4 默认 1000 对低频注入太紧）
|
||||
sm_cfg = build_sm_config()
|
||||
|
||||
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)
|
||||
|
||||
home_lat, home_lon = await read_home_gps(drone)
|
||||
tgt_lat, tgt_lon = ned_offset_to_latlon(home_lat, home_lon,
|
||||
TARGET_NORTH_M, TARGET_EAST_M)
|
||||
print(f"-- home ({home_lat:.6f}, {home_lon:.6f})，"
|
||||
f"注入目标 ({tgt_lat:.6f}, {tgt_lon:.6f})（北 {TARGET_NORTH_M}m "
|
||||
f"东 {TARGET_EAST_M}m）")
|
||||
|
||||
# 装配：真 SITL 飞控 + 模拟吊舱 + 注入坐标 + 状态机编排器
|
||||
hub = TelemetryHub()
|
||||
hub.start(drone)
|
||||
target = VirtualTarget(
|
||||
VirtualTargetConfig(ahead_m=TARGET_NORTH_M, right_m=TARGET_EAST_M,
|
||||
down_m=TARGET_DOWN_M, arc_radius_m=0.1,
|
||||
arc_speed_mps=0.0, arc_ccw=True),
|
||||
(0.0, 0.0, 0.0), 0.0) # anchor=home NED 原点，yaw=0 → 北30/东15
|
||||
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)
|
||||
ground = GroundLink(cfg.ground_inject, now_ms_fn=now_ms)
|
||||
bridge = PodBridge(cfg, px4, pod, now_ms_fn=now_ms,
|
||||
ground_target_fn=lambda: ground.latest())
|
||||
sm = TaskStateMachine(sm_cfg)
|
||||
task_log = TaskLogger(TaskLogConfig(), now_ms_fn=now_ms)
|
||||
runner = TaskRunner(cfg, sm_cfg, px4, bridge, sm, ground, drone,
|
||||
now_ms_fn=now_ms, logger=task_log)
|
||||
|
||||
stop = asyncio.Event()
|
||||
if args.ground_station:
|
||||
# 地面站模式：UDP 接收地面站注入坐标（地面站页面 POST /api/inject → 14555）
|
||||
ground_task = asyncio.ensure_future(ground.run(stop))
|
||||
else:
|
||||
ground_task = asyncio.ensure_future(inject_loop(ground, tgt_lat, tgt_lon, stop))
|
||||
runner_task = asyncio.ensure_future(runner.run(stop))
|
||||
|
||||
print("-- 启动任务状态机全流程（WAIT→…→RTL）")
|
||||
if args.ground_station:
|
||||
print(f" [地面站模式] 等待 UDP {cfg.ground_inject.port} 注入坐标…")
|
||||
print(f" 状态机: takeoff={sm_cfg.takeoff_altitude_m}m "
|
||||
f"switch_radius={sm_cfg.switch_radius_m}m "
|
||||
f"center={sm_cfg.center_threshold_deg}°/{sm_cfg.center_hold_s}s "
|
||||
f"hover={sm_cfg.approach_hover_s}s")
|
||||
|
||||
# 状态打印 + 总超时保护
|
||||
started = time.monotonic()
|
||||
last_state = None
|
||||
diag_count = 0
|
||||
try:
|
||||
while not runner.rtl_done:
|
||||
await asyncio.sleep(1.0)
|
||||
if runner.state is not last_state:
|
||||
last_state = runner.state
|
||||
print(f"[任务] 进入状态: {runner.state.value}")
|
||||
if runner_task.done():
|
||||
exc = runner_task.exception()
|
||||
if exc is not None:
|
||||
print(f"! runner 异常退出: {type(exc).__name__}: {exc}")
|
||||
break
|
||||
if runner.state.value == "goto" and diag_count % 3 == 0:
|
||||
own = px4.own_state()
|
||||
g = ground.latest()
|
||||
dist = haversine_distance_m(own.latitude_deg, own.longitude_deg,
|
||||
g.latitude_deg, g.longitude_deg)
|
||||
try:
|
||||
async for m in drone.telemetry.flight_mode():
|
||||
mode = str(m)
|
||||
break
|
||||
except Exception:
|
||||
mode = "?"
|
||||
print(f"[诊断] pos_valid={own.position_valid} "
|
||||
f"pos_ms={own.position_time_ms} now={now_ms()} "
|
||||
f"mode={mode} yaw={own.yaw_rad:.2f} dist={dist:.1f}m "
|
||||
f"lat={own.latitude_deg:.5f} lon={own.longitude_deg:.5f}")
|
||||
diag_count += 1
|
||||
if time.monotonic() - started > TASK_TIMEOUT_S:
|
||||
print("! 全流程超时，强制返航")
|
||||
runner.request_rtl()
|
||||
if runner.rtl_done:
|
||||
print("-- 任务完成（RTL 落地）")
|
||||
except asyncio.CancelledError:
|
||||
raise
|
||||
finally:
|
||||
stop.set()
|
||||
ground_task.cancel()
|
||||
runner_task.cancel()
|
||||
try:
|
||||
await runner_task
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
try:
|
||||
await ground_task
|
||||
except asyncio.CancelledError:
|
||||
pass
|
||||
await hub.stop()
|
||||
|
||||
await print_flight_mode(drone)
|
||||
print("\n===== 任务状态机全流程完成 =====")
|
||||
return 0
|
||||
|
||||
|
||||
def main() -> int:
|
||||
ap = argparse.ArgumentParser(description="任务级状态机 SITL 全流程")
|
||||
ap.add_argument("--conn", default=CONNECTION, help="MAVSDK 连接串")
|
||||
ap.add_argument("--config", default=str(CONFIG_PATH), help="桥接配置 JSON 路径")
|
||||
ap.add_argument("--ground-station", action="store_true",
|
||||
help="地面站模式：UDP 接收地面站注入坐标（而非内置虚拟目标自动注入）")
|
||||
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())
|
||||
@@ -1,5 +1,5 @@
|
||||
{
|
||||
"_comment": "吊舱↔PX4 桥接配置。heading_servo.enabled=true → 走 L1 航向伺服(TrackingControlLaw)；tracking 段为 L1 控制律参数(与 visual_track_control 同结构，各分组须齐全)。全部数值为台架标定前占位，符号(beta_sign/alpha_sign)与增益须经 S1/S2 标定后回填，标定前不得实飞。",
|
||||
"_comment": "吊舱↔PX4 桥接配置。heading_servo.enabled=true → 走 L1 航向伺服(TrackingControlLaw)；tracking 段为 L1 控制律参数(与 visual_track_control 同结构，各分组须齐全)。S1 台架标定(2026-10-09)：目标在左下时 off_target_x/y 均为负 → 吊舱 X 轴右正(与控制律 β 同向)故 beta_sign=+1；Y 轴上正(与控制律 α 下正相反)故 alpha_sign=-1。厂商已确认本版电机无用 → beta_from_frame/alpha_from_frame 恒 false(框架角路径仅留作将来装电机的预留)。heading.gain 等增益仍为占位，须 S2 干跑验证方向与量级后方可实飞。",
|
||||
|
||||
"pod_serial": {
|
||||
"port": "",
|
||||
@@ -33,7 +33,7 @@
|
||||
"heading_servo": {
|
||||
"enabled": true,
|
||||
"beta_sign": 1,
|
||||
"alpha_sign": 1,
|
||||
"alpha_sign": -1,
|
||||
"beta_from_frame": false,
|
||||
"beta_from_off_target": true,
|
||||
"alpha_from_frame": false,
|
||||
@@ -83,5 +83,12 @@
|
||||
"ay_mps2": 1.0,
|
||||
"az_mps2": 0.5
|
||||
}
|
||||
},
|
||||
|
||||
"ground_inject": {
|
||||
"enabled": false,
|
||||
"host": "0.0.0.0",
|
||||
"port": 14555,
|
||||
"timeout_ms": 5000
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
|
||||
@@ -0,0 +1,9 @@
|
||||
"""task_runner —— 任务编排器（预定流程.md 的天空侧任务级状态机）。
|
||||
|
||||
把已通的 L1（航向伺服 TrackingControlLaw）与 L2（地理引导 GuidanceLaw）按任务阶段
|
||||
编排成一条可运行的全流程：WAIT → ARM_TAKEOFF → GOTO → TRACK → APPROACH → RTL。
|
||||
|
||||
模块划分：
|
||||
* state_machine.py 纯逻辑任务状态机（无 IO，可单测）
|
||||
* runner.py 编排器（MAVSDK 飞行编排 + 状态机驱动 + PodBridge 闭环，A3）
|
||||
"""
|
||||
@@ -0,0 +1,149 @@
|
||||
"""log.py —— 任务日志模块：结构化记录任务全流程（事件 + 周期遥测）。
|
||||
|
||||
预定流程 B 阶段：让任意一次 SITL / 实飞都能回溯出完整阶段轨迹与异常。
|
||||
|
||||
分层：
|
||||
* 事件日志（JSONL）：状态机转移、飞行编排事件（解锁/起飞/返航）、异常/紧急。
|
||||
低频、带语义、结构化，便于 grep / jq 定位「什么时候发生了什么」。
|
||||
* 周期遥测日志（CSV）：每控制周期的位姿 / GPS / 目标 / 控制输出，便于回放与画图。
|
||||
|
||||
时间戳：单调毫秒（now_ms，与运行时代码同源）+ 相对时间 t_s（自首个 now_ms 起）。
|
||||
文件 IO 集中在 open / close，热路径只追加一行，不做 flush 决策。
|
||||
"""
|
||||
|
||||
import csv
|
||||
import json
|
||||
import time
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Optional
|
||||
|
||||
_TELEMETRY_FIELDS = (
|
||||
"t_s", "state",
|
||||
"pos_valid", "fix_type", "num_satellites",
|
||||
"latitude_deg", "longitude_deg", "relative_altitude_m", "yaw_deg",
|
||||
"target_injected", "distance_to_target_m", "target_locked",
|
||||
"off_target_x_deg", "off_target_y_deg",
|
||||
"mode", "yaw_rate_dps", "vx_body_mps", "vz_mps",
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class TelemetryRecord:
|
||||
"""单周期遥测记录（字段顺序即 CSV 列序）。"""
|
||||
|
||||
t_s: float = 0.0
|
||||
state: str = ""
|
||||
pos_valid: bool = False
|
||||
fix_type: int = 0
|
||||
num_satellites: int = 0
|
||||
latitude_deg: float = 0.0
|
||||
longitude_deg: float = 0.0
|
||||
relative_altitude_m: float = 0.0
|
||||
yaw_deg: float = 0.0
|
||||
target_injected: bool = False
|
||||
distance_to_target_m: float = float("inf")
|
||||
target_locked: bool = False
|
||||
off_target_x_deg: float = 0.0
|
||||
off_target_y_deg: float = 0.0
|
||||
mode: str = ""
|
||||
yaw_rate_dps: float = 0.0
|
||||
vx_body_mps: float = 0.0
|
||||
vz_mps: float = 0.0
|
||||
|
||||
|
||||
@dataclass
|
||||
class TaskLogConfig:
|
||||
"""任务日志配置（全外置）。"""
|
||||
|
||||
enabled: bool = True
|
||||
dir: str = "logs" # 输出目录（相对工作目录）
|
||||
event_file: str = "task_events.jsonl" # 事件日志
|
||||
telemetry_file: str = "task_telemetry.csv" # 周期遥测日志
|
||||
telemetry_every_n: int = 1 # 每 N 周期记一次遥测（1 = 每周期）
|
||||
|
||||
|
||||
def _monotonic_ms() -> int:
|
||||
return int(time.monotonic() * 1000)
|
||||
|
||||
|
||||
class TaskLogger:
|
||||
"""结构化任务日志。用法：
|
||||
|
||||
logger = TaskLogger(cfg, now_ms_fn)
|
||||
logger.open() # 建目录 + 写 CSV 表头
|
||||
logger.log_event(now_ms, "state", from_state="wait", ...)
|
||||
logger.log_telemetry(now_ms, record)
|
||||
logger.close()
|
||||
|
||||
enabled=False 或未 open 时，所有 log 方法为空操作（不抛异常、不写文件）。
|
||||
"""
|
||||
|
||||
def __init__(self, cfg: TaskLogConfig = TaskLogConfig(),
|
||||
now_ms_fn=_monotonic_ms) -> None:
|
||||
self._cfg = cfg
|
||||
self._now_ms = now_ms_fn
|
||||
self._event_fp = None
|
||||
self._tel_fp = None
|
||||
self._tel_writer = None
|
||||
self._t0_ms: Optional[int] = None
|
||||
self._tick = 0
|
||||
|
||||
# --- 生命周期 -----------------------------------------------------------
|
||||
def open(self) -> None:
|
||||
"""创建输出目录并打开两个日志文件；disabled 时跳过。"""
|
||||
if not self._cfg.enabled:
|
||||
return
|
||||
out_dir = Path(self._cfg.dir)
|
||||
out_dir.mkdir(parents=True, exist_ok=True)
|
||||
self._event_fp = (out_dir / self._cfg.event_file).open(
|
||||
"a", encoding="utf-8")
|
||||
self._tel_fp = (out_dir / self._cfg.telemetry_file).open(
|
||||
"a", encoding="utf-8", newline="")
|
||||
self._tel_writer = csv.writer(self._tel_fp)
|
||||
if (out_dir / self._cfg.telemetry_file).stat().st_size == 0:
|
||||
self._tel_writer.writerow(_TELEMETRY_FIELDS)
|
||||
self._event_fp.flush()
|
||||
self._tel_fp.flush()
|
||||
|
||||
def close(self) -> None:
|
||||
"""flush 并关闭两个日志文件。"""
|
||||
if self._event_fp is not None:
|
||||
self._event_fp.flush()
|
||||
self._event_fp.close()
|
||||
self._event_fp = None
|
||||
if self._tel_fp is not None:
|
||||
self._tel_fp.flush()
|
||||
self._tel_fp.close()
|
||||
self._tel_fp = None
|
||||
self._tel_writer = None
|
||||
|
||||
# --- 记录 ---------------------------------------------------------------
|
||||
def log_event(self, now_ms: int, event_type: str, **fields) -> None:
|
||||
"""追加一条 JSONL 事件。"""
|
||||
if self._event_fp is None:
|
||||
return
|
||||
record = {"type": event_type, "ts_ms": now_ms,
|
||||
"t_s": self._rel_s(now_ms)}
|
||||
record.update(fields)
|
||||
self._event_fp.write(json.dumps(record, ensure_ascii=False) + "\n")
|
||||
|
||||
def log_telemetry(self, now_ms: int, rec: TelemetryRecord) -> None:
|
||||
"""追加一行遥测（按 telemetry_every_n 节流）。"""
|
||||
if self._tel_writer is None:
|
||||
return
|
||||
self._tick += 1
|
||||
if self._tick % max(1, self._cfg.telemetry_every_n) != 0:
|
||||
return
|
||||
rec.t_s = self._rel_s(now_ms)
|
||||
self._tel_writer.writerow([getattr(rec, f) for f in _TELEMETRY_FIELDS])
|
||||
|
||||
# --- 内部 ---------------------------------------------------------------
|
||||
def _rel_s(self, now_ms: int) -> float:
|
||||
if self._t0_ms is None:
|
||||
self._t0_ms = now_ms
|
||||
return (now_ms - self._t0_ms) / 1000.0
|
||||
|
||||
@property
|
||||
def fieldnames(self) -> tuple:
|
||||
return _TELEMETRY_FIELDS
|
||||
@@ -0,0 +1,39 @@
|
||||
"""pod_control.py —— 吊舱目标识别/锁定控制（真机，任务编排的一部分）。
|
||||
|
||||
真机吊舱不会自动输出"锁定"——需要先发控制指令让它进入目标识别 + 自动锁定模式：
|
||||
* 0x07 目标识别开（param_x = 目标类型：0=人/车，1=无人机）
|
||||
* 0x0F 发现目标自动锁定（常驻模式：目标进画面自动锁定，非一次性动作）
|
||||
结束（任务完成/返航）时：
|
||||
* 0x0E 停止跟踪（只停当前这一次跟踪）
|
||||
* 0x08 目标识别关（真正退出识别）
|
||||
|
||||
本模块把这些指令序列封装成 PodController，供 TaskRunner 在 GOTO / RTL 阶段调用。
|
||||
SITL 用 VirtualPod（直接合成锁定态），不注入 PodController。
|
||||
"""
|
||||
|
||||
from pod_bridge.protocol import ControlCode, ControlCommand
|
||||
|
||||
|
||||
class PodController:
|
||||
"""吊舱目标识别/锁定控制器。
|
||||
|
||||
arm_detection / stop_tracking 为同步方法，经 bridge.send_control 下发 表2-1
|
||||
控制帧（编码由 PodBridge 完成，本类只关心"发什么指令序列"）。
|
||||
"""
|
||||
|
||||
def __init__(self, bridge, target_type: int = 0) -> None:
|
||||
"""bridge：PodBridge（需有 send_control）；target_type：0=人/车，1=无人机。"""
|
||||
self._bridge = bridge
|
||||
self._target_type = int(target_type)
|
||||
|
||||
def arm_detection(self) -> None:
|
||||
"""开启目标识别 + 发现自动锁定（GOTO 阶段进入时调用）。"""
|
||||
self._bridge.send_control(ControlCommand(
|
||||
code=ControlCode.TARGET_DETECT_ON, param_x=self._target_type))
|
||||
self._bridge.send_control(ControlCommand(
|
||||
code=ControlCode.AUTO_LOCK_ON_FOUND))
|
||||
|
||||
def stop_tracking(self) -> None:
|
||||
"""停止跟踪 + 关闭目标识别（RTL 前调用）。"""
|
||||
self._bridge.send_control(ControlCommand(code=ControlCode.STOP_TRACK))
|
||||
self._bridge.send_control(ControlCommand(code=ControlCode.TARGET_DETECT_OFF))
|
||||
@@ -0,0 +1,296 @@
|
||||
"""runner.py —— 任务编排器：驱动任务状态机跑通 WAIT→…→RTL 全流程。
|
||||
|
||||
「预定流程.md」天空侧的落地：把已通的 L1（航向伺服 TrackingControlLaw）与
|
||||
L2（地理引导 GuidanceLaw）按任务阶段编排，配合地面坐标注入（GroundLink）与
|
||||
状态机（TaskStateMachine），串成一条可运行的全流程。
|
||||
|
||||
职责边界：
|
||||
* 状态机（TaskStateMachine）只做「状态 + 转移判据」决策；
|
||||
* 本编排器负责「采集快照 → 喂状态机 → 按状态执行动作」；
|
||||
* 闭环控制复用 PodBridge（L1/L2 运行时切换走 set_heading_servo_enabled）；
|
||||
* 飞行编排（解锁/起飞/返航）通过 duck-typed `drone`（MAVSDK System）完成，
|
||||
便于 mock 单测；真实 SITL 装配见仓库根 task_runner_sitl.py。
|
||||
|
||||
生命周期：run() 开头先订阅 PX4 遥测（幂等），用于起飞到位 / 距离计算；
|
||||
GOTO 进入时 start offboard；RTL 进入时安全停机 + 返航。
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import logging
|
||||
import math
|
||||
import time
|
||||
from typing import Callable, Optional
|
||||
|
||||
from pod_bridge.guidance import haversine_distance_m
|
||||
from visual_track_control.staleness_checker import is_stale
|
||||
|
||||
from .log import TaskLogger, TelemetryRecord
|
||||
from .state_machine import (TaskInputs, TaskState, TaskStateConfig,
|
||||
TaskStateMachine)
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
def _monotonic_ms() -> int:
|
||||
return int(time.monotonic() * 1000)
|
||||
|
||||
|
||||
class TaskRunner:
|
||||
"""任务编排器。依赖（除 sm 外均可注入 mock）：
|
||||
|
||||
cfg BridgeConfig —— ground_inject.timeout_ms 等
|
||||
sm_cfg TaskStateConfig —— 切换半径/超时/起飞高度/居中阈值/循环频率
|
||||
px4 Px4Gateway —— own_state()（姿态/位置/相对高度）
|
||||
bridge PodBridge —— step / set_heading_servo_enabled / pod_locked
|
||||
sm TaskStateMachine —— update()
|
||||
ground GroundLink —— latest()（地面注入坐标）
|
||||
drone MAVSDK System —— action.arm/takeoff/return_to_launch/land/disarm
|
||||
vision_fn callable->bool —— 我方视觉第二判据（可选，require_vision 时用）
|
||||
"""
|
||||
|
||||
def __init__(self, cfg, sm_cfg: TaskStateConfig, px4, bridge, sm: TaskStateMachine,
|
||||
ground, drone, vision_fn: Optional[Callable[[], bool]] = None,
|
||||
now_ms_fn: Callable[[], int] = _monotonic_ms,
|
||||
logger: Optional[TaskLogger] = None,
|
||||
pod_control=None) -> None:
|
||||
self._cfg = cfg
|
||||
self._sm_cfg = sm_cfg
|
||||
self._px4 = px4
|
||||
self._bridge = bridge
|
||||
self._sm = sm
|
||||
self._ground = ground
|
||||
self._drone = drone
|
||||
self._vision_fn = vision_fn
|
||||
self._now = now_ms_fn
|
||||
self._logger = logger # 结构化任务日志（可选，B 阶段）
|
||||
self._pod_control = pod_control # 吊舱识别/锁定控制（可选，真机）
|
||||
self._centered_since_ms: Optional[int] = None
|
||||
self._rtl_requested = False
|
||||
self._emergency = False
|
||||
self._rtl_done = False
|
||||
|
||||
# --- 外部控制 -----------------------------------------------------------
|
||||
def request_rtl(self) -> None:
|
||||
"""外部请求返航（如收到地面站返航命令 / 人工按键）。"""
|
||||
self._rtl_requested = True
|
||||
|
||||
def trigger_emergency(self) -> None:
|
||||
"""外部触发紧急终止（丢控 / RC 接管 / 失控）。"""
|
||||
self._emergency = True
|
||||
|
||||
@property
|
||||
def state(self) -> TaskState:
|
||||
return self._sm.state
|
||||
|
||||
@property
|
||||
def rtl_done(self) -> bool:
|
||||
return self._rtl_done
|
||||
|
||||
# --- 主循环 -------------------------------------------------------------
|
||||
async def run(self, stop_event: asyncio.Event) -> None:
|
||||
"""主循环：采集快照 → 状态机 → 进入动作/持续动作，直到 RTL 完成或停止。"""
|
||||
await self._px4.start_telemetry() # 幂等：先拿遥测，供起飞到位/距离计算
|
||||
if self._logger is not None:
|
||||
self._logger.open()
|
||||
period_s = 1.0 / self._sm_cfg.loop_frequency_hz
|
||||
try:
|
||||
while not stop_event.is_set():
|
||||
now_ms = self._now()
|
||||
inputs = self._collect_inputs(now_ms)
|
||||
tr = self._sm.update(inputs, now_ms)
|
||||
if tr.occurred:
|
||||
logger.info("[Task] %s -> %s (%s)",
|
||||
tr.from_state.value, tr.to_state.value, tr.reason)
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(
|
||||
now_ms, "state", from_state=tr.from_state.value,
|
||||
to_state=tr.to_state.value, reason=tr.reason)
|
||||
await self._on_enter(tr.to_state, now_ms)
|
||||
out = await self._tick(now_ms)
|
||||
if self._logger is not None:
|
||||
self._logger.log_telemetry(
|
||||
now_ms, self._build_telemetry_record(now_ms, inputs, out))
|
||||
if self._finished():
|
||||
break
|
||||
await asyncio.sleep(period_s)
|
||||
finally:
|
||||
await self._teardown()
|
||||
if self._logger is not None:
|
||||
self._logger.close()
|
||||
|
||||
# --- 输入采集 -----------------------------------------------------------
|
||||
def _collect_inputs(self, now_ms: int) -> TaskInputs:
|
||||
"""聚合 PX4 / 注入坐标 / 吊舱 / 视觉为一份 TaskInputs 快照。"""
|
||||
own = self._px4.own_state()
|
||||
|
||||
injected = self._ground.latest()
|
||||
target_injected = bool(
|
||||
injected.time_ms > 0
|
||||
and not is_stale(injected.time_ms, now_ms,
|
||||
self._cfg.ground_inject.timeout_ms))
|
||||
|
||||
distance = math.inf
|
||||
if target_injected and own.position_valid:
|
||||
distance = haversine_distance_m(
|
||||
own.latitude_deg, own.longitude_deg,
|
||||
injected.latitude_deg, injected.longitude_deg)
|
||||
|
||||
takeoff_reached = (own.relative_altitude_m
|
||||
>= self._sm_cfg.takeoff_altitude_m)
|
||||
target_locked = self._bridge.pod_locked(now_ms)
|
||||
vision = bool(self._vision_fn and self._vision_fn())
|
||||
centered, centered_s = self._update_centered(now_ms)
|
||||
|
||||
return TaskInputs(
|
||||
target_injected=target_injected,
|
||||
distance_to_target_m=distance,
|
||||
takeoff_reached=takeoff_reached,
|
||||
target_locked=target_locked,
|
||||
vision_has_target=vision,
|
||||
target_centered=centered,
|
||||
centered_duration_s=centered_s,
|
||||
rtl_requested=self._rtl_requested,
|
||||
emergency=self._emergency,
|
||||
)
|
||||
|
||||
def _update_centered(self, now_ms: int) -> tuple[bool, float]:
|
||||
"""脱靶量居中判定 + 持续时长累加（TRACK→APPROACH 判据）。
|
||||
|
||||
无遥测 / 任一轴脱靶量超出 center_threshold_deg → 清零累加。
|
||||
"""
|
||||
tel = self._bridge.latest_telemetry
|
||||
if tel is None:
|
||||
self._centered_since_ms = None
|
||||
return False, 0.0
|
||||
frame, _ = tel
|
||||
centered = (abs(frame.off_target_x_deg) < self._sm_cfg.center_threshold_deg
|
||||
and abs(frame.off_target_y_deg) < self._sm_cfg.center_threshold_deg)
|
||||
if not centered:
|
||||
self._centered_since_ms = None
|
||||
return False, 0.0
|
||||
if self._centered_since_ms is None:
|
||||
self._centered_since_ms = now_ms
|
||||
return True, (now_ms - self._centered_since_ms) / 1000.0
|
||||
|
||||
# --- 状态进入动作 -------------------------------------------------------
|
||||
async def _on_enter(self, state: TaskState, now_ms: int) -> None:
|
||||
if state is TaskState.ARM_TAKEOFF:
|
||||
await self._arm_takeoff()
|
||||
elif state is TaskState.GOTO:
|
||||
if not await self._bridge.start():
|
||||
logger.error("[Task] offboard 启动失败，紧急终止")
|
||||
self._emergency = True
|
||||
return
|
||||
self._bridge.set_heading_servo_enabled(False) # L2 地理引导
|
||||
if self._pod_control is not None:
|
||||
self._pod_control.arm_detection() # 真机：开启目标识别 + 自动锁定
|
||||
elif state is TaskState.TRACK:
|
||||
self._bridge.set_heading_servo_enabled(True) # L1 航向伺服
|
||||
elif state is TaskState.APPROACH:
|
||||
pass # 悬停由 _tick 持续发零速
|
||||
elif state is TaskState.RTL:
|
||||
if self._pod_control is not None:
|
||||
self._pod_control.stop_tracking() # 真机：停止跟踪 + 识别关（bridge 停机前）
|
||||
await self._start_rtl()
|
||||
|
||||
# --- 每周期持续动作 -----------------------------------------------------
|
||||
async def _tick(self, now_ms: int):
|
||||
"""每周期持续动作。返回本拍控制输出（GOTO/TRACK 有，其余 None）。"""
|
||||
state = self._sm.state
|
||||
if state in (TaskState.GOTO, TaskState.TRACK):
|
||||
return await self._bridge.step(now_ms)
|
||||
elif state is TaskState.APPROACH:
|
||||
await self._px4.send_brake_hover(now_ms) # 悬停目标上空（保 offboard 心跳）
|
||||
elif state is TaskState.RTL:
|
||||
await self._check_landed()
|
||||
return None
|
||||
# WAIT / ARM_TAKEOFF：无持续动作（待命 / 等起飞高度）
|
||||
|
||||
def _build_telemetry_record(self, now_ms: int, inputs: TaskInputs,
|
||||
out) -> TelemetryRecord:
|
||||
"""聚合本拍遥测为一帧 TelemetryRecord（供日志 / 页面复用）。"""
|
||||
own = self._px4.own_state()
|
||||
tel = self._bridge.latest_telemetry
|
||||
off_x = off_y = 0.0
|
||||
if tel is not None:
|
||||
frame, _ = tel
|
||||
off_x = frame.off_target_x_deg
|
||||
off_y = frame.off_target_y_deg
|
||||
return TelemetryRecord(
|
||||
state=self._sm.state.value,
|
||||
pos_valid=own.position_valid,
|
||||
fix_type=own.fix_type,
|
||||
num_satellites=own.num_satellites,
|
||||
latitude_deg=own.latitude_deg,
|
||||
longitude_deg=own.longitude_deg,
|
||||
relative_altitude_m=own.relative_altitude_m,
|
||||
yaw_deg=math.degrees(own.yaw_rad),
|
||||
target_injected=inputs.target_injected,
|
||||
distance_to_target_m=inputs.distance_to_target_m,
|
||||
target_locked=inputs.target_locked,
|
||||
off_target_x_deg=off_x,
|
||||
off_target_y_deg=off_y,
|
||||
mode=out.mode.name if out is not None else "",
|
||||
yaw_rate_dps=out.yaw_rate_dps if out is not None else 0.0,
|
||||
vx_body_mps=out.vx_body_mps if out is not None else 0.0,
|
||||
vz_mps=out.vz_mps if out is not None else 0.0,
|
||||
)
|
||||
|
||||
# --- 飞行编排 -----------------------------------------------------------
|
||||
async def _arm_takeoff(self) -> None:
|
||||
try:
|
||||
await self._drone.action.arm()
|
||||
await self._drone.action.set_takeoff_altitude(
|
||||
self._sm_cfg.takeoff_altitude_m)
|
||||
await self._drone.action.takeoff()
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(
|
||||
self._now(), "takeoff",
|
||||
altitude_m=self._sm_cfg.takeoff_altitude_m)
|
||||
except Exception as exc:
|
||||
logger.error("[Task] 起飞失败: %s", exc)
|
||||
self._emergency = True
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(self._now(), "takeoff_failed",
|
||||
error=str(exc))
|
||||
|
||||
async def _start_rtl(self) -> None:
|
||||
try:
|
||||
await self._bridge.stop() # 先零速刹车 + 停 offboard/遥测/串口
|
||||
except Exception as exc:
|
||||
logger.warning("[Task] 停机异常: %s", exc)
|
||||
try:
|
||||
await self._drone.action.return_to_launch()
|
||||
except Exception as exc:
|
||||
logger.warning("[Task] RTL 失败，尝试降落: %s", exc)
|
||||
try:
|
||||
await self._drone.action.land()
|
||||
except Exception:
|
||||
pass
|
||||
if self._logger is not None:
|
||||
self._logger.log_event(self._now(), "rtl")
|
||||
|
||||
async def _check_landed(self) -> None:
|
||||
"""RTL 状态每周期取一帧 in_air 遥测；落地后 disarm 并标记完成。"""
|
||||
try:
|
||||
async for is_in_air in self._drone.telemetry.in_air():
|
||||
if not is_in_air and not self._rtl_done:
|
||||
self._rtl_done = True
|
||||
try:
|
||||
await self._drone.action.disarm()
|
||||
except Exception:
|
||||
pass
|
||||
break # 只取最新一帧
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# --- 收尾 ---------------------------------------------------------------
|
||||
def _finished(self) -> bool:
|
||||
return self._sm.state is TaskState.RTL and self._rtl_done
|
||||
|
||||
async def _teardown(self) -> None:
|
||||
"""退出主循环（含异常/中途停止）时的安全收尾。"""
|
||||
try:
|
||||
await self._bridge.stop()
|
||||
except Exception:
|
||||
pass
|
||||
@@ -0,0 +1,178 @@
|
||||
"""state_machine.py —— 任务级状态机（纯逻辑，无 IO，可单测）。
|
||||
|
||||
「预定流程.md」天空侧的任务编排：把已通的 L1（航向伺服 TrackingControlLaw）与
|
||||
L2（地理引导 GuidanceLaw）按任务阶段串成一条全流程。本模块只做「状态 + 转移判据
|
||||
+ 守卫」，不驱动 MAVSDK/offboard —— 那些由 runner.py（编排器）负责。每周期吃一份
|
||||
TaskInputs 快照，返回是否转移 + 原因。
|
||||
|
||||
状态链：
|
||||
WAIT ──收到有效坐标──▶ ARM_TAKEOFF ──到起飞高度──▶ GOTO（L2 坐标引导）
|
||||
│ 距目标 < switch_radius
|
||||
│ 且锁定(+可选视觉)
|
||||
▼
|
||||
APPROACH ◀──居中稳定── TRACK（L1 航向伺服）
|
||||
│
|
||||
任意状态 ──超时 / 返航 / 紧急──▶ RTL
|
||||
|
||||
设计要点（铁律对齐）：
|
||||
* 纯逻辑：输入/输出都是 dataclass，时间用外部传入的 now_ms（单调毫秒），
|
||||
不碰时钟/IO/网络，可确定性单测。
|
||||
* 全局最高优先级：emergency（丢控/RC 接管/失控）与 rtl_requested（人工返航）
|
||||
→ 任何非 RTL 状态立即转 RTL（安全第一）。
|
||||
* GOTO→TRACK 守卫复用 L1 方案 §2.3：目标锁定 bit2 +（可选）我方视觉第二判据，
|
||||
避免 bit2 粘滞导致误入追踪。
|
||||
* TRACK 丢目标：若注入坐标仍有效 → 退回 GOTO 重新引导；否则 → RTL。
|
||||
* 阈值/超时全外置（TaskStateConfig），代码不写死数值。
|
||||
"""
|
||||
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum
|
||||
|
||||
|
||||
class TaskState(Enum):
|
||||
"""任务状态（.value 为小写字符串，供日志/序列化）。"""
|
||||
|
||||
WAIT = "wait" # 待命：等地面坐标
|
||||
ARM_TAKEOFF = "arm_takeoff" # 解锁 + 起飞到悬停高度
|
||||
GOTO = "goto" # L2 坐标引导（飞向注入坐标）
|
||||
TRACK = "track" # L1 航向伺服（吊舱角度追踪）
|
||||
APPROACH = "approach" # 接近/悬停目标上空
|
||||
RTL = "rtl" # 返航 + 落地 + 上锁（终态，reset() 才回 WAIT）
|
||||
|
||||
|
||||
@dataclass
|
||||
class TaskStateConfig:
|
||||
"""状态机配置（全外置；数值均为占位，须台架/实飞标定后回填）。"""
|
||||
|
||||
switch_radius_m: float = 30.0 # GOTO→TRACK 切换半径（米）
|
||||
require_vision: bool = False # GOTO→TRACK 是否需视觉第二判据（S3 起开）
|
||||
arm_takeoff_timeout_s: float = 60.0 # 起飞阶段超时
|
||||
goto_timeout_s: float = 120.0 # 坐标引导超时
|
||||
track_timeout_s: float = 120.0 # 追踪超时
|
||||
center_hold_s: float = 3.0 # TRACK→APPROACH 居中稳定时长（秒）
|
||||
approach_hover_s: float = 10.0 # APPROACH 悬停时长（到点 → RTL）
|
||||
takeoff_altitude_m: float = 5.0 # 起飞目标高度（runner 起飞 + takeoff_reached 判据）
|
||||
center_threshold_deg: float = 1.0 # 脱靶量居中阈值（|off_target| < 此值判居中）
|
||||
loop_frequency_hz: float = 20.0 # runner 主循环频率（bridge.step 节拍）
|
||||
|
||||
|
||||
@dataclass
|
||||
class TaskInputs:
|
||||
"""单周期任务快照（调用方在同一控制周期采集并打戳）。
|
||||
|
||||
各布尔量为「已合成好的判据」，由 runner 从 PX4 遥测 / 吊舱遥测 / 地面注入 /
|
||||
视觉源聚合后传入；状态机不做聚合，只做转移决策。
|
||||
"""
|
||||
|
||||
target_injected: bool = False # 地面坐标有效注入（新鲜、未超龄）
|
||||
distance_to_target_m: float = math.inf # 到注入目标的水平距离（GOTO 用）
|
||||
takeoff_reached: bool = False # 已到达起飞目标高度
|
||||
target_locked: bool = False # 吊舱锁定（bit2 + 遥测新鲜）
|
||||
vision_has_target: bool = False # 我方视觉第二判据（可选，require_vision 时必看）
|
||||
target_centered: bool = False # 目标居中（脱靶量长期≈0）
|
||||
centered_duration_s: float = 0.0 # 居中持续时长（外部累加，秒）
|
||||
rtl_requested: bool = False # 人工返航命令
|
||||
emergency: bool = False # 紧急（丢控 / RC 接管 / 失控）
|
||||
|
||||
|
||||
@dataclass
|
||||
class TaskTransition:
|
||||
"""状态机单周期转移结果。occurred=False 表示维持原状态。"""
|
||||
|
||||
from_state: TaskState = TaskState.WAIT
|
||||
to_state: TaskState = TaskState.WAIT
|
||||
occurred: bool = False
|
||||
reason: str = ""
|
||||
|
||||
|
||||
class TaskStateMachine:
|
||||
"""任务级状态机。update() 单周期返回 TaskTransition，维护内部 current 状态。"""
|
||||
|
||||
def __init__(self, cfg: TaskStateConfig) -> None:
|
||||
self._cfg = cfg
|
||||
self._state = TaskState.WAIT
|
||||
self._entered_ms = 0 # 进入当前状态的单调毫秒（超时/时长判定用）
|
||||
|
||||
@property
|
||||
def state(self) -> TaskState:
|
||||
return self._state
|
||||
|
||||
def reset(self) -> None:
|
||||
"""回到 WAIT 并清零计时（新任务 / 任务终止后复用）。"""
|
||||
self._state = TaskState.WAIT
|
||||
self._entered_ms = 0
|
||||
|
||||
def update(self, inputs: TaskInputs, now_ms: int) -> TaskTransition:
|
||||
"""吃一份快照，返回转移结果；无转移时 occurred=False、to_state 保持原状态。"""
|
||||
state = self._state
|
||||
|
||||
# 全局最高优先级：紧急 / 人工返航 → 任何非 RTL 状态立即终止。
|
||||
if state is not TaskState.RTL:
|
||||
if inputs.emergency:
|
||||
return self._goto(TaskState.RTL, "紧急（丢控/RC 接管）", now_ms)
|
||||
if inputs.rtl_requested:
|
||||
return self._goto(TaskState.RTL, "人工返航命令", now_ms)
|
||||
|
||||
if state is TaskState.WAIT:
|
||||
if inputs.target_injected:
|
||||
return self._goto(TaskState.ARM_TAKEOFF, "收到地面坐标", now_ms)
|
||||
|
||||
elif state is TaskState.ARM_TAKEOFF:
|
||||
if inputs.takeoff_reached:
|
||||
return self._goto(TaskState.GOTO, "已到起飞高度", now_ms)
|
||||
if self._elapsed_s(now_ms) >= self._cfg.arm_takeoff_timeout_s:
|
||||
return self._goto(TaskState.RTL, "起飞超时", now_ms)
|
||||
|
||||
elif state is TaskState.GOTO:
|
||||
if not inputs.target_injected:
|
||||
return self._goto(TaskState.RTL, "注入坐标失效", now_ms)
|
||||
if (inputs.distance_to_target_m <= self._cfg.switch_radius_m
|
||||
and self._lock_ok(inputs)):
|
||||
return self._goto(TaskState.TRACK, "进入追踪半径且目标锁定", now_ms)
|
||||
if self._elapsed_s(now_ms) >= self._cfg.goto_timeout_s:
|
||||
return self._goto(TaskState.RTL, "坐标引导超时", now_ms)
|
||||
|
||||
elif state is TaskState.TRACK:
|
||||
if not inputs.target_locked:
|
||||
# 丢目标：坐标仍有效 → 退回 GOTO 重新引导；否则终止返航。
|
||||
if inputs.target_injected:
|
||||
return self._goto(TaskState.GOTO, "目标丢失，退回坐标引导", now_ms)
|
||||
return self._goto(TaskState.RTL, "目标丢失且坐标失效", now_ms)
|
||||
if (inputs.target_centered
|
||||
and inputs.centered_duration_s >= self._cfg.center_hold_s):
|
||||
return self._goto(TaskState.APPROACH, "目标居中稳定", now_ms)
|
||||
if self._elapsed_s(now_ms) >= self._cfg.track_timeout_s:
|
||||
return self._goto(TaskState.RTL, "追踪超时", now_ms)
|
||||
|
||||
elif state is TaskState.APPROACH:
|
||||
if self._elapsed_s(now_ms) >= self._cfg.approach_hover_s:
|
||||
return self._goto(TaskState.RTL, "悬停完成", now_ms)
|
||||
|
||||
# RTL 为终态；无任何转移条件命中 → 维持原状态。
|
||||
return TaskTransition(from_state=state, to_state=state, occurred=False)
|
||||
|
||||
# --- 内部辅助 -----------------------------------------------------------
|
||||
def _lock_ok(self, inputs: TaskInputs) -> bool:
|
||||
"""GOTO→TRACK 的目标锁定判据（复用 L1 方案 §2.3）。
|
||||
|
||||
需吊舱锁定 bit2；若 require_vision 还需我方视觉第二判据（bit2 粘滞兜底）。
|
||||
"""
|
||||
if not inputs.target_locked:
|
||||
return False
|
||||
if self._cfg.require_vision and not inputs.vision_has_target:
|
||||
return False
|
||||
return True
|
||||
|
||||
def _elapsed_s(self, now_ms: int) -> float:
|
||||
"""进入当前状态的时长（秒）。_entered_ms=0（未打戳）时返回 0。"""
|
||||
if self._entered_ms <= 0:
|
||||
return 0.0
|
||||
return max(0.0, (now_ms - self._entered_ms) / 1000.0)
|
||||
|
||||
def _goto(self, target: TaskState, reason: str, now_ms: int) -> TaskTransition:
|
||||
prev = self._state
|
||||
self._state = target
|
||||
self._entered_ms = now_ms
|
||||
return TaskTransition(from_state=prev, to_state=target,
|
||||
occurred=True, reason=reason)
|
||||
@@ -13,6 +13,7 @@ import pytest
|
||||
|
||||
from pod_bridge.bridge import PodBridge
|
||||
from pod_bridge.config import BridgeConfig, load_from_json
|
||||
from pod_bridge.ground_link import GroundTarget
|
||||
from pod_bridge.protocol import (ControlCode, ControlCommand, TargetGeo,
|
||||
Telemetry, decode_attitude, decode_control)
|
||||
from pod_bridge.px4_gateway import OwnState
|
||||
@@ -523,3 +524,75 @@ def test_l1_require_vision_passes_with_source():
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
|
||||
|
||||
|
||||
# --- 地面坐标注入（A2）-------------------------------------------------------
|
||||
def _inject_config(timeout_ms=5000):
|
||||
cfg = BridgeConfig()
|
||||
cfg.ground_inject.enabled = True
|
||||
cfg.ground_inject.timeout_ms = timeout_ms
|
||||
return cfg
|
||||
|
||||
|
||||
def test_ground_inject_priority_over_table4():
|
||||
"""注入坐标优先于表4：吊舱无表4/无锁定（本设备常态），注入新鲜坐标照常引导。"""
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0, time_ms=1000)
|
||||
b = PodBridge(_inject_config(), px4, pod, now_ms_fn=lambda: 0,
|
||||
ground_target_fn=lambda: injected)
|
||||
out = asyncio.run(b.step(1000))
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert len(px4.velocity_calls) == 1
|
||||
|
||||
|
||||
def test_ground_inject_stale_falls_back_to_brake():
|
||||
"""注入坐标超龄 → 视为无目标 → 刹车（不得朝过期坐标飞）。"""
|
||||
px4 = FakePx4(_valid_state(2000))
|
||||
pod = FakePod()
|
||||
injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
|
||||
time_ms=1000) # 1000ms 前注入
|
||||
cfg = _inject_config(timeout_ms=500)
|
||||
b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0,
|
||||
ground_target_fn=lambda: injected)
|
||||
out = asyncio.run(b.step(2000)) # 差值 1000ms > 500ms → 超龄
|
||||
assert out.mode.name == "BRAKE_HOVER"
|
||||
assert px4.velocity_calls == []
|
||||
|
||||
|
||||
def test_ground_inject_disabled_ignores_injection():
|
||||
"""未启用地面注入 → 忽略注入坐标，回退表4 原路径（无表4 → 刹车）。"""
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
injected = GroundTarget(latitude_deg=0.001, longitude_deg=0.0, time_ms=1000)
|
||||
cfg = BridgeConfig() # ground_inject.enabled=False
|
||||
b = PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0,
|
||||
ground_target_fn=lambda: injected)
|
||||
out = asyncio.run(b.step(1000))
|
||||
assert out.mode.name == "BRAKE_HOVER"
|
||||
|
||||
|
||||
# --- 运行时切换控制律（A3：GOTO(L2) ↔ TRACK(L1)）-----------------------------
|
||||
def test_set_heading_servo_enabled_runtime_switch():
|
||||
"""运行时切换控制律：初始 L1 → 切 L2（刹车）→ 切回 L1（速度）。"""
|
||||
px4 = FakePx4(_valid_state(1000))
|
||||
pod = FakePod()
|
||||
b = _bridge(px4, pod, _l1_config()) # enabled=True → 初始 L1
|
||||
|
||||
# 初始 L1：锁定 + 脱靶量 → 速度
|
||||
pod.queue(_locked_off_target(x=10.0))
|
||||
out = asyncio.run(b.step(1000))
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
|
||||
|
||||
# 切 L2：无表4/无锁定 → 刹车
|
||||
b.set_heading_servo_enabled(False)
|
||||
out = asyncio.run(b.step(1100))
|
||||
assert out.mode.name == "BRAKE_HOVER"
|
||||
|
||||
# 切回 L1：重新喂锁定脱靶量 → 速度
|
||||
b.set_heading_servo_enabled(True)
|
||||
pod.queue(_locked_off_target(x=10.0))
|
||||
out = asyncio.run(b.step(1200))
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert out.yaw_rate_dps == pytest.approx(10.0, abs=1e-9)
|
||||
|
||||
|
||||
@@ -133,6 +133,17 @@ def test_heading_servo_loads():
|
||||
assert hs.vertical_enabled is True # 决策3：无人机 L1 开垂直
|
||||
|
||||
|
||||
def test_deployed_config_has_calibrated_signs():
|
||||
"""S1 台架标定回归(2026-10-09)：目标在左下时 off_target_x/y 均为负 →
|
||||
吊舱 X 轴右正(与控制律 β 同向) beta_sign=+1；Y 轴上正(与 α 下正相反) alpha_sign=-1。
|
||||
厂商确认本版电机无用 → 框架角路径必须关。改动这些值前必须重新台架标定。"""
|
||||
cfg = load_from_json(str(_DEFAULT_CONFIG))
|
||||
assert cfg.heading_servo.beta_sign == 1
|
||||
assert cfg.heading_servo.alpha_sign == -1
|
||||
assert cfg.heading_servo.beta_from_frame is False
|
||||
assert cfg.heading_servo.alpha_from_frame is False
|
||||
|
||||
|
||||
def test_heading_servo_partial_override(tmp_path):
|
||||
cfg = load_from_json(_write(tmp_path, {
|
||||
"heading_servo": {"beta_sign": -1, "beta_from_frame": True},
|
||||
|
||||
@@ -0,0 +1,94 @@
|
||||
"""ground_link_test.py —— 地面坐标注入通道单元测试。
|
||||
|
||||
覆盖：parse_payload 解析（合法/缺字段/非法 JSON/非对象/类型错误）、
|
||||
is_valid 新鲜度判定（未注入/新鲜/超龄边界）。
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from pod_bridge.ground_link import GroundInjectConfig, GroundLink, GroundTarget
|
||||
|
||||
|
||||
# --- parse_payload ----------------------------------------------------------
|
||||
def test_parse_valid_payload():
|
||||
t = GroundLink.parse_payload(
|
||||
b'{"lat": 39.9042, "lon": 116.4074, "alt": 50.0}')
|
||||
assert t is not None
|
||||
assert t.latitude_deg == pytest.approx(39.9042)
|
||||
assert t.longitude_deg == pytest.approx(116.4074)
|
||||
assert t.abs_alt_m == pytest.approx(50.0)
|
||||
assert t.time_ms == 0 # 打戳由接收方（run）负责
|
||||
|
||||
|
||||
def test_parse_default_alt_is_zero():
|
||||
t = GroundLink.parse_payload(b'{"lat": 1.0, "lon": 2.0}')
|
||||
assert t is not None
|
||||
assert t.abs_alt_m == 0.0
|
||||
|
||||
|
||||
def test_parse_missing_lat_returns_none():
|
||||
assert GroundLink.parse_payload(b'{"lon": 2.0}') is None
|
||||
|
||||
|
||||
def test_parse_missing_lon_returns_none():
|
||||
assert GroundLink.parse_payload(b'{"lat": 1.0}') is None
|
||||
|
||||
|
||||
def test_parse_invalid_json_returns_none():
|
||||
assert GroundLink.parse_payload(b'not json') is None
|
||||
|
||||
|
||||
def test_parse_non_object_returns_none():
|
||||
assert GroundLink.parse_payload(b'[1, 2, 3]') is None
|
||||
|
||||
|
||||
def test_parse_string_coords_returns_none():
|
||||
assert GroundLink.parse_payload(b'{"lat": "x", "lon": 2.0}') is None
|
||||
|
||||
|
||||
def test_parse_bool_coords_returns_none():
|
||||
# True 是 bool 不是数值 → 拒绝
|
||||
assert GroundLink.parse_payload(b'{"lat": true, "lon": 2.0}') is None
|
||||
|
||||
|
||||
def test_parse_ignores_ts_field():
|
||||
"""ts 字段仅诊断，不参与解析（新鲜度用本地接收时刻）。"""
|
||||
t = GroundLink.parse_payload(b'{"lat": 1.0, "lon": 2.0, "ts": 999}')
|
||||
assert t is not None
|
||||
assert t.latitude_deg == pytest.approx(1.0)
|
||||
|
||||
|
||||
# --- is_valid ---------------------------------------------------------------
|
||||
def _link(timeout_ms=5000):
|
||||
return GroundLink(GroundInjectConfig(timeout_ms=timeout_ms),
|
||||
now_ms_fn=lambda: 0)
|
||||
|
||||
|
||||
def _inject(link, time_ms):
|
||||
link._target = GroundTarget(latitude_deg=1.0, longitude_deg=2.0,
|
||||
time_ms=time_ms)
|
||||
|
||||
|
||||
def test_is_valid_false_when_never_injected():
|
||||
assert _link().is_valid(1000) is False
|
||||
|
||||
|
||||
def test_is_valid_true_when_fresh():
|
||||
link = _link(timeout_ms=5000)
|
||||
_inject(link, 1000)
|
||||
assert link.is_valid(1000) is True
|
||||
assert link.is_valid(4000) is True
|
||||
|
||||
|
||||
def test_is_valid_timeout_boundary():
|
||||
link = _link(timeout_ms=5000)
|
||||
_inject(link, 1000)
|
||||
assert link.is_valid(6000) is True # 恰好 5000ms，未超龄
|
||||
assert link.is_valid(6001) is False # 5001ms，超龄
|
||||
|
||||
|
||||
def test_latest_returns_injected_target():
|
||||
link = _link()
|
||||
_inject(link, 1000)
|
||||
assert link.latest().latitude_deg == pytest.approx(1.0)
|
||||
assert link.latest().time_ms == 1000
|
||||
@@ -5,6 +5,7 @@
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
import math
|
||||
from enum import Enum
|
||||
from types import SimpleNamespace
|
||||
|
||||
@@ -62,7 +63,7 @@ class FakeTelemetry:
|
||||
def gps_info(self):
|
||||
return self._stream(self._gps)
|
||||
|
||||
def velocity_body(self):
|
||||
def velocity_ned(self):
|
||||
return self._stream(self._velocity)
|
||||
|
||||
|
||||
@@ -77,8 +78,9 @@ def _pos(lat=31.0, lon=121.0, aalt=100.0, ralt=50.0):
|
||||
absolute_altitude_m=aalt, relative_altitude_m=ralt)
|
||||
|
||||
|
||||
def _att(roll=0.1, pitch=0.2, yaw=1.57):
|
||||
return SimpleNamespace(roll_rad=roll, pitch_rad=pitch, yaw_rad=yaw)
|
||||
def _att(roll_deg=5.0, pitch_deg=10.0, yaw_deg=90.0):
|
||||
"""MAVSDK EulerAngle 字段是度（roll_deg/pitch_deg/yaw_deg），网关转弧度。"""
|
||||
return SimpleNamespace(roll_deg=roll_deg, pitch_deg=pitch_deg, yaw_deg=yaw_deg)
|
||||
|
||||
|
||||
# --- 枚举转换 ---------------------------------------------------------------
|
||||
@@ -120,12 +122,14 @@ def test_update_attitude_sets_radians():
|
||||
gw.update_attitude(_att())
|
||||
s = gw.own_state()
|
||||
assert s.has_attitude is True
|
||||
assert s.yaw_rad == pytest.approx(1.57)
|
||||
assert s.roll_rad == pytest.approx(math.radians(5.0))
|
||||
assert s.pitch_rad == pytest.approx(math.radians(10.0))
|
||||
assert s.yaw_rad == pytest.approx(math.radians(90.0))
|
||||
|
||||
|
||||
def test_update_velocity_body_ground_speed_is_horizontal_magnitude():
|
||||
def test_update_velocity_ned_ground_speed_is_horizontal_magnitude():
|
||||
gw = Px4Gateway(FakeDrone())
|
||||
gw.update_velocity_body(SimpleNamespace(forward_m_s=3.0, right_m_s=4.0,
|
||||
gw.update_velocity_ned(SimpleNamespace(north_m_s=3.0, east_m_s=4.0,
|
||||
down_m_s=-9.9))
|
||||
assert gw.own_state().ground_speed_mps == pytest.approx(5.0) # 不含 down
|
||||
|
||||
@@ -173,9 +177,9 @@ def test_command_delegation_reaches_offboard():
|
||||
def test_start_telemetry_populates_cache_then_stop():
|
||||
telemetry = FakeTelemetry(
|
||||
position=[_pos(lat=22.0, lon=114.0)],
|
||||
attitude=[_att(yaw=0.5)],
|
||||
attitude=[_att(yaw_deg=30.0)],
|
||||
gps=[SimpleNamespace(fix_type=3, num_satellites=10)],
|
||||
velocity=[SimpleNamespace(forward_m_s=2.0, right_m_s=0.0, down_m_s=0.0)],
|
||||
velocity=[SimpleNamespace(north_m_s=2.0, east_m_s=0.0, down_m_s=0.0)],
|
||||
)
|
||||
gw = Px4Gateway(FakeDrone(telemetry))
|
||||
|
||||
@@ -184,7 +188,7 @@ def test_start_telemetry_populates_cache_then_stop():
|
||||
await asyncio.sleep(0.05) # 让后台任务消费预设消息
|
||||
s = gw.own_state()
|
||||
assert s.latitude_deg == 22.0 and s.longitude_deg == 114.0
|
||||
assert s.yaw_rad == pytest.approx(0.5)
|
||||
assert s.yaw_rad == pytest.approx(math.radians(30.0))
|
||||
assert s.position_valid is True
|
||||
assert s.ground_speed_mps == pytest.approx(2.0)
|
||||
await gw.stop_telemetry()
|
||||
@@ -210,7 +214,7 @@ def test_position_timestamp_not_refreshed_by_other_streams():
|
||||
clock[0] = 2000
|
||||
gw.update_attitude(_att())
|
||||
gw.update_gps(SimpleNamespace(fix_type=3, num_satellites=9))
|
||||
gw.update_velocity_body(SimpleNamespace(forward_m_s=1.0, right_m_s=0.0,
|
||||
gw.update_velocity_ned(SimpleNamespace(north_m_s=1.0, east_m_s=0.0,
|
||||
down_m_s=0.0))
|
||||
s = gw.own_state()
|
||||
assert s.time_ms == 2000 # 诊断时间戳照常刷新
|
||||
|
||||
@@ -0,0 +1,104 @@
|
||||
"""virtual_pod_bridge_test.py —— VirtualPod × PodBridge × TrackingControlLaw 集成测试。
|
||||
|
||||
用部署配置(pod_bridge.json)把"SITL 闭环段"在无飞控下跑通，端到端复核 S1 标定：
|
||||
目标偏右 → 控制律应输出正 yaw_rate（右转）；
|
||||
目标偏下 → 应输出正 vz（下降）；
|
||||
目标消失 → 应判丢零速刹车。
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
from dataclasses import replace
|
||||
from pathlib import Path
|
||||
|
||||
import pytest
|
||||
|
||||
from pod_bridge.bridge import PodBridge
|
||||
from pod_bridge.config import load_from_json
|
||||
from pod_bridge.px4_gateway import OwnState
|
||||
from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
|
||||
VirtualTargetConfig)
|
||||
|
||||
_DEFAULT_CONFIG = Path(__file__).resolve().parents[2] / "config" / "pod_bridge.json"
|
||||
|
||||
|
||||
class FakePx4:
|
||||
def __init__(self, yaw_rad=0.0):
|
||||
self._yaw = yaw_rad
|
||||
self.clock = 1000 # 测试循环每拍推进，与 step(now_ms) 同步
|
||||
self.velocity_calls = []
|
||||
self.brake_calls = []
|
||||
|
||||
def own_state(self):
|
||||
return OwnState(has_attitude=True, has_position=True, position_valid=True,
|
||||
fix_type=3, yaw_rad=self._yaw, time_ms=self.clock,
|
||||
position_time_ms=self.clock)
|
||||
|
||||
async def start_telemetry(self): pass
|
||||
async def stop_telemetry(self): pass
|
||||
async def start_offboard(self): return True
|
||||
async def stop_offboard(self): pass
|
||||
async def send_velocity_heading(self, out, time_boot_ms=None):
|
||||
self.velocity_calls.append(out); return True
|
||||
async def send_brake_hover(self, time_boot_ms=None):
|
||||
self.brake_calls.append(time_boot_ms); return True
|
||||
|
||||
|
||||
def _bridge(target, own):
|
||||
cfg = load_from_json(str(_DEFAULT_CONFIG)) # 部署配置：L1 enabled、S1 符号
|
||||
pod = VirtualPod(target, lambda: own)
|
||||
px4 = FakePx4(yaw_rad=own.yaw_rad)
|
||||
return PodBridge(cfg, px4, pod, now_ms_fn=lambda: 0), px4
|
||||
|
||||
|
||||
def _static_target(ahead=10.0, right=0.0, down=0.0):
|
||||
cfg = VirtualTargetConfig(ahead_m=ahead, right_m=right, down_m=down,
|
||||
arc_speed_mps=0.0)
|
||||
return VirtualTarget(cfg, (0.0, 0.0, 0.0), 0.0)
|
||||
|
||||
|
||||
def _run_ticks(bridge, px4, n, t0=1000, dt=50):
|
||||
"""跑 n 拍（每拍 50ms，节拍与 20Hz 一致），返回末拍输出。"""
|
||||
out = None
|
||||
for i in range(n):
|
||||
t = t0 + i * dt
|
||||
px4.clock = t
|
||||
out = asyncio.run(bridge.step(t))
|
||||
return out
|
||||
|
||||
|
||||
def test_deployed_config_turns_right_for_right_target():
|
||||
# 目标在右前方：S1 标定 beta_sign=+1 → yaw_rate 应为正（右转）
|
||||
tgt = _static_target(ahead=10.0, right=5.0)
|
||||
bridge, px4 = _bridge(tgt, CarrierView())
|
||||
out = _run_ticks(bridge, px4, 5)
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert out.yaw_rate_dps > 0.0
|
||||
assert px4.velocity_calls # 正常速度航向指令
|
||||
assert not px4.brake_calls
|
||||
|
||||
|
||||
def test_deployed_config_descends_for_below_target():
|
||||
# 目标偏下：S1 标定 alpha_sign=-1（吊舱上正→控制律下正）→ vz 应为正（下降）
|
||||
tgt = _static_target(ahead=10.0, down=2.0)
|
||||
bridge, px4 = _bridge(tgt, CarrierView())
|
||||
out = _run_ticks(bridge, px4, 10)
|
||||
assert out.mode.name == "VELOCITY_HEADING"
|
||||
assert out.vz_mps > 0.0
|
||||
|
||||
|
||||
def test_deployed_config_slow_approach_forward():
|
||||
# L1 无距离：slow_approach 0.2 m/s 起步（决策3 低限速）
|
||||
tgt = _static_target()
|
||||
bridge, px4 = _bridge(tgt, CarrierView())
|
||||
out = _run_ticks(bridge, px4, 10)
|
||||
assert 0.0 < out.vx_body_mps <= 0.2 + 1e-9
|
||||
|
||||
|
||||
def test_target_lost_brakes_through_pipeline():
|
||||
tgt = _static_target()
|
||||
bridge, px4 = _bridge(tgt, CarrierView())
|
||||
_run_ticks(bridge, px4, 3)
|
||||
tgt.set_visible(False) # 目标消失 → 吊舱转搜索态 → 判丢刹车
|
||||
out = _run_ticks(bridge, px4, 2, t0=1000 + 3 * 50)
|
||||
assert out.mode.name == "BRAKE_HOVER"
|
||||
assert px4.brake_calls
|
||||
@@ -0,0 +1,124 @@
|
||||
"""virtual_pod_test.py —— 模拟吊舱几何与 S1 符号约定回归测试。
|
||||
|
||||
钉死 S1 台架标定结论（2026-10-09）：目标在左下时 off_target_x/y 均为负，
|
||||
即 X 右正、Y 上正；并验证不可见/超视场 → 搜索态 + 脱靶量归 0（对齐真实
|
||||
吊舱"丢失时脱靶量恒 0"的行为）。
|
||||
"""
|
||||
|
||||
import math
|
||||
import time
|
||||
|
||||
import pytest
|
||||
|
||||
from pod_bridge.virtual_pod import (CarrierView, VirtualPod, VirtualTarget,
|
||||
VirtualTargetConfig)
|
||||
|
||||
|
||||
def _pod(target, own):
|
||||
return VirtualPod(target, lambda: own)
|
||||
|
||||
|
||||
def _static_target(ahead=30.0, right=0.0, down=0.0):
|
||||
"""放在锚点正前方（机头朝北）的静止目标。"""
|
||||
cfg = VirtualTargetConfig(ahead_m=ahead, right_m=right, down_m=down,
|
||||
arc_speed_mps=0.0)
|
||||
return VirtualTarget(cfg, (0.0, 0.0, 0.0), 0.0)
|
||||
|
||||
|
||||
def _carrier(yaw_deg=0.0):
|
||||
return CarrierView(north_m=0.0, east_m=0.0, down_m=0.0,
|
||||
yaw_rad=math.radians(yaw_deg))
|
||||
|
||||
|
||||
def _telemetry(pod):
|
||||
frames = pod.poll()
|
||||
assert len(frames) == 1
|
||||
return frames[0]
|
||||
|
||||
|
||||
# ---- S1 符号约定回归：目标位置 → 脱靶量符号 --------------------------------
|
||||
def test_target_right_gives_positive_off_x():
|
||||
# 目标在右前方：rel_e>0、机头朝北 → 脱靶量 X 应为正（右正）
|
||||
tgt = _static_target(ahead=10.0, right=5.0)
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert tel.off_target_x_deg > 0.0
|
||||
assert tel.off_target_x_deg == pytest.approx(math.degrees(math.atan2(5.0, 10.0)),
|
||||
abs=1e-9)
|
||||
|
||||
|
||||
def test_target_left_gives_negative_off_x():
|
||||
tgt = _static_target(ahead=10.0, right=-5.0)
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert tel.off_target_x_deg < 0.0
|
||||
|
||||
|
||||
def test_target_below_gives_negative_off_y():
|
||||
# S1 实测：目标偏下 → off_target_y 为负（上正）。11.3° 在垂直视场(35°)内
|
||||
tgt = _static_target(ahead=10.0, down=2.0)
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert tel.off_target_y_deg < 0.0
|
||||
assert tel.off_target_y_deg == pytest.approx(-math.degrees(math.atan2(2.0, 10.0)),
|
||||
abs=1e-9)
|
||||
|
||||
|
||||
def test_target_above_gives_positive_off_y():
|
||||
tgt = _static_target(ahead=10.0, down=-2.0)
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert tel.off_target_y_deg > 0.0
|
||||
|
||||
|
||||
def test_yaw_rotation_follows_body_frame():
|
||||
# 机头偏东 20°：目标在正北 → 目标在机体左侧 20° → off_x 为负
|
||||
tgt = _static_target(ahead=10.0)
|
||||
tel = _telemetry(_pod(tgt, _carrier(yaw_deg=20.0)))
|
||||
assert tel.off_target_x_deg == pytest.approx(-20.0, abs=1e-9)
|
||||
|
||||
|
||||
# ---- 无电机 / 无激光语义 ---------------------------------------------------
|
||||
def test_frame_angles_always_zero_and_laser_none():
|
||||
tgt = _static_target()
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert tel.frame_yaw_deg == 0.0
|
||||
assert tel.frame_pitch_deg == 0.0
|
||||
assert tel.frame_roll_deg == 0.0
|
||||
assert tel.laser_range_m is None
|
||||
|
||||
|
||||
# ---- 锁定门控 --------------------------------------------------------------
|
||||
def test_visible_target_locks():
|
||||
tel = _telemetry(_pod(_static_target(), _carrier()))
|
||||
assert tel.is_target_locked
|
||||
|
||||
|
||||
def test_hidden_target_searches_with_zero_off():
|
||||
tgt = _static_target()
|
||||
tgt.set_visible(False)
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert not tel.is_target_locked
|
||||
assert tel.off_target_x_deg == 0.0 and tel.off_target_y_deg == 0.0
|
||||
|
||||
|
||||
def test_target_behind_searches():
|
||||
# 机头朝北、目标在正南 10m → fwd<0 → 搜索态
|
||||
cfg = VirtualTargetConfig(ahead_m=-10.0, right_m=0.0, down_m=0.0,
|
||||
arc_speed_mps=0.0)
|
||||
tgt = VirtualTarget(cfg, (0.0, 0.0, 0.0), 0.0)
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert not tel.is_target_locked
|
||||
|
||||
|
||||
def test_target_out_of_fov_searches():
|
||||
# 目标几乎在正右方（β→90°）超出默认 FOV 60° → 搜索态
|
||||
tgt = _static_target(ahead=1.0, right=100.0)
|
||||
tel = _telemetry(_pod(tgt, _carrier()))
|
||||
assert not tel.is_target_locked
|
||||
|
||||
|
||||
# ---- PodLink 接口 ----------------------------------------------------------
|
||||
def test_send_counts_and_close_marks():
|
||||
pod = _pod(_static_target(), _carrier())
|
||||
pod.send(b"\x00" * 4)
|
||||
pod.send(b"\x00" * 4)
|
||||
assert pod.attitude_frames_rx == 2
|
||||
pod.close()
|
||||
assert pod.closed
|
||||
@@ -0,0 +1,123 @@
|
||||
"""log_test.py —— 任务日志模块单元测试。
|
||||
|
||||
覆盖：open 建目录/写表头、事件 JSONL、遥测 CSV、telemetry_every_n 节流、
|
||||
disabled 时零副作用、close 收尾。
|
||||
"""
|
||||
|
||||
import csv
|
||||
import json
|
||||
|
||||
import pytest
|
||||
|
||||
from task_runner.log import TaskLogConfig, TaskLogger, TelemetryRecord
|
||||
|
||||
|
||||
def _cfg(tmp_path, **kw):
|
||||
c = TaskLogConfig(dir=str(tmp_path), **kw)
|
||||
return c
|
||||
|
||||
|
||||
def test_open_creates_dir_and_csv_header(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path))
|
||||
logger.open()
|
||||
logger.close()
|
||||
|
||||
assert (tmp_path / "task_events.jsonl").exists()
|
||||
assert (tmp_path / "task_telemetry.csv").exists()
|
||||
|
||||
with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f:
|
||||
header = next(csv.reader(f))
|
||||
assert header[0] == "t_s"
|
||||
assert "state" in header
|
||||
assert "distance_to_target_m" in header
|
||||
|
||||
|
||||
def test_log_event_writes_jsonl(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path))
|
||||
logger.open()
|
||||
logger.log_event(1000, "state", from_state="wait",
|
||||
to_state="arm_takeoff", reason="收到地面坐标")
|
||||
logger.log_event(2000, "emergency", reason="RC 接管")
|
||||
logger.close()
|
||||
|
||||
with open(tmp_path / "task_events.jsonl", encoding="utf-8") as f:
|
||||
lines = [json.loads(x) for x in f if x.strip()]
|
||||
assert len(lines) == 2
|
||||
assert lines[0]["type"] == "state"
|
||||
assert lines[0]["from_state"] == "wait"
|
||||
assert lines[0]["to_state"] == "arm_takeoff"
|
||||
assert lines[0]["ts_ms"] == 1000
|
||||
assert lines[0]["t_s"] == 0.0 # 首个事件 t_s = 0
|
||||
assert lines[1]["t_s"] == 1.0 # 第二个事件 t_s = 1.0s
|
||||
|
||||
|
||||
def test_log_telemetry_writes_csv_rows(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path))
|
||||
logger.open()
|
||||
rec = TelemetryRecord(state="goto", pos_valid=True, latitude_deg=47.39,
|
||||
distance_to_target_m=33.5, mode="VELOCITY_HEADING",
|
||||
yaw_rate_dps=-30.0, vx_body_mps=1.7)
|
||||
logger.log_telemetry(1000, rec)
|
||||
logger.log_telemetry(1100, rec)
|
||||
logger.close()
|
||||
|
||||
with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f:
|
||||
rows = list(csv.DictReader(f))
|
||||
assert len(rows) == 2
|
||||
assert rows[0]["state"] == "goto"
|
||||
assert rows[0]["pos_valid"] == "True"
|
||||
assert rows[0]["distance_to_target_m"] == "33.5"
|
||||
assert rows[0]["t_s"] == "0.0"
|
||||
assert rows[1]["t_s"] == "0.1"
|
||||
|
||||
|
||||
def test_telemetry_throttled_by_every_n(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path, telemetry_every_n=5))
|
||||
logger.open()
|
||||
rec = TelemetryRecord(state="goto")
|
||||
for i in range(12):
|
||||
logger.log_telemetry(1000 + i * 100, rec)
|
||||
logger.close()
|
||||
|
||||
with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f:
|
||||
rows = list(csv.DictReader(f))
|
||||
assert len(rows) == 2 # 第 5、10 次各记一次
|
||||
|
||||
|
||||
def test_disabled_logs_nothing(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path, enabled=False))
|
||||
logger.open()
|
||||
logger.log_event(1000, "state", from_state="wait", to_state="arm_takeoff")
|
||||
logger.log_telemetry(1000, TelemetryRecord(state="goto"))
|
||||
logger.close()
|
||||
|
||||
# 未 open（disabled）→ 不建目录、不写文件
|
||||
assert not (tmp_path / "task_events.jsonl").exists()
|
||||
assert not (tmp_path / "task_telemetry.csv").exists()
|
||||
|
||||
|
||||
def test_log_before_open_is_noop(tmp_path):
|
||||
logger = TaskLogger(_cfg(tmp_path))
|
||||
logger.log_event(1000, "state", from_state="wait", to_state="arm_takeoff")
|
||||
logger.log_telemetry(1000, TelemetryRecord(state="goto"))
|
||||
logger.close()
|
||||
assert not (tmp_path / "task_events.jsonl").exists()
|
||||
|
||||
|
||||
def test_append_keeps_existing_rows(tmp_path):
|
||||
"""重复 open（追加模式）不清空既有内容，表头不重复写。"""
|
||||
logger = TaskLogger(_cfg(tmp_path))
|
||||
logger.open()
|
||||
logger.log_telemetry(1000, TelemetryRecord(state="wait"))
|
||||
logger.close()
|
||||
|
||||
logger2 = TaskLogger(_cfg(tmp_path))
|
||||
logger2.open()
|
||||
logger2.log_telemetry(2000, TelemetryRecord(state="goto"))
|
||||
logger2.close()
|
||||
|
||||
with open(tmp_path / "task_telemetry.csv", encoding="utf-8") as f:
|
||||
rows = list(csv.DictReader(f))
|
||||
assert len(rows) == 2
|
||||
assert rows[0]["state"] == "wait"
|
||||
assert rows[1]["state"] == "goto"
|
||||
@@ -0,0 +1,45 @@
|
||||
"""pod_control_test.py —— 吊舱目标识别/锁定控制器单元测试。
|
||||
|
||||
覆盖：arm_detection 发 0x07(识别,目标类型)+0x0F(自动锁定)、
|
||||
stop_tracking 发 0x0E(停止)+0x08(识别关)。
|
||||
"""
|
||||
|
||||
from pod_bridge.protocol import ControlCode
|
||||
from task_runner.pod_control import PodController
|
||||
|
||||
|
||||
class FakeBridge:
|
||||
def __init__(self):
|
||||
self.sent = []
|
||||
|
||||
def send_control(self, cmd):
|
||||
self.sent.append(cmd)
|
||||
|
||||
|
||||
def test_arm_detection_sends_detect_and_autolock():
|
||||
bridge = FakeBridge()
|
||||
pc = PodController(bridge, target_type=1)
|
||||
pc.arm_detection()
|
||||
|
||||
assert len(bridge.sent) == 2
|
||||
assert bridge.sent[0].code == ControlCode.TARGET_DETECT_ON
|
||||
assert bridge.sent[0].param_x == 1 # 目标类型 1 = 无人机
|
||||
assert bridge.sent[1].code == ControlCode.AUTO_LOCK_ON_FOUND
|
||||
|
||||
|
||||
def test_arm_detection_default_target_type_is_person_vehicle():
|
||||
bridge = FakeBridge()
|
||||
pc = PodController(bridge) # 默认 target_type=0 = 人/车
|
||||
pc.arm_detection()
|
||||
assert bridge.sent[0].code == ControlCode.TARGET_DETECT_ON
|
||||
assert bridge.sent[0].param_x == 0
|
||||
|
||||
|
||||
def test_stop_tracking_sends_stop_and_detect_off():
|
||||
bridge = FakeBridge()
|
||||
pc = PodController(bridge)
|
||||
pc.stop_tracking()
|
||||
|
||||
assert len(bridge.sent) == 2
|
||||
assert bridge.sent[0].code == ControlCode.STOP_TRACK
|
||||
assert bridge.sent[1].code == ControlCode.TARGET_DETECT_OFF
|
||||
@@ -0,0 +1,311 @@
|
||||
"""runner_test.py —— 任务编排器单元测试（mock px4/bridge/ground/drone，无硬件）。
|
||||
|
||||
覆盖：输入采集（注入坐标新鲜/超龄、起飞到位、锁定、居中累加）、
|
||||
状态进入动作（ARM_TAKEOFF 触发起飞、GOTO 切 L2、TRACK 切 L1）。
|
||||
"""
|
||||
|
||||
import asyncio
|
||||
from dataclasses import replace
|
||||
|
||||
import pytest
|
||||
|
||||
from pod_bridge.config import BridgeConfig
|
||||
from pod_bridge.ground_link import GroundTarget
|
||||
from pod_bridge.px4_gateway import OwnState
|
||||
from pod_bridge.protocol import Telemetry
|
||||
from task_runner.runner import TaskRunner
|
||||
from task_runner.state_machine import (TaskState, TaskStateConfig,
|
||||
TaskStateMachine)
|
||||
|
||||
|
||||
# --- mocks ------------------------------------------------------------------
|
||||
class FakePx4:
|
||||
def __init__(self, state=None):
|
||||
self._state = state or OwnState()
|
||||
self.telemetry_started = 0
|
||||
self.offboard_started = False
|
||||
self.brake_calls = []
|
||||
|
||||
def own_state(self):
|
||||
return replace(self._state)
|
||||
|
||||
async def start_telemetry(self):
|
||||
self.telemetry_started += 1
|
||||
|
||||
async def stop_telemetry(self):
|
||||
pass
|
||||
|
||||
async def start_offboard(self):
|
||||
self.offboard_started = True
|
||||
return True
|
||||
|
||||
async def stop_offboard(self):
|
||||
pass
|
||||
|
||||
async def send_velocity_heading(self, out, t=None):
|
||||
return True
|
||||
|
||||
async def send_brake_hover(self, t=None):
|
||||
self.brake_calls.append(t)
|
||||
return True
|
||||
|
||||
|
||||
class FakeBridge:
|
||||
def __init__(self):
|
||||
self.started = False
|
||||
self.stopped = False
|
||||
self.law = None # set_heading_servo_enabled 最后值
|
||||
self.steps = 0
|
||||
self._locked = False
|
||||
self._telemetry = None
|
||||
|
||||
async def start(self):
|
||||
self.started = True
|
||||
return True
|
||||
|
||||
async def stop(self):
|
||||
self.stopped = True
|
||||
|
||||
def set_heading_servo_enabled(self, enabled):
|
||||
self.law = enabled
|
||||
|
||||
def pod_locked(self, now_ms):
|
||||
return self._locked
|
||||
|
||||
async def step(self, now_ms):
|
||||
self.steps += 1
|
||||
|
||||
@property
|
||||
def latest_telemetry(self):
|
||||
return self._telemetry
|
||||
|
||||
|
||||
class FakeGround:
|
||||
def __init__(self, target=None):
|
||||
self._target = target
|
||||
|
||||
def latest(self):
|
||||
return self._target or GroundTarget()
|
||||
|
||||
|
||||
class FakeDrone:
|
||||
def __init__(self):
|
||||
self.armed = False
|
||||
self.took_off = False
|
||||
self.takeoff_alt = 0.0
|
||||
self.rtl = False
|
||||
self.landed = False
|
||||
self.disarmed = False
|
||||
|
||||
class _Action:
|
||||
def __init__(self, d):
|
||||
self._d = d
|
||||
|
||||
async def arm(self):
|
||||
self._d.armed = True
|
||||
|
||||
async def set_takeoff_altitude(self, a):
|
||||
self._d.takeoff_alt = a
|
||||
|
||||
async def takeoff(self):
|
||||
self._d.took_off = True
|
||||
|
||||
async def return_to_launch(self):
|
||||
self._d.rtl = True
|
||||
|
||||
async def land(self):
|
||||
self._d.landed = True
|
||||
|
||||
async def disarm(self):
|
||||
self._d.disarmed = True
|
||||
|
||||
@property
|
||||
def action(self):
|
||||
return FakeDrone._Action(self)
|
||||
|
||||
|
||||
def _state(now_ms, lat=0.0, lon=0.0, rel_alt=0.0):
|
||||
return OwnState(latitude_deg=lat, longitude_deg=lon,
|
||||
relative_altitude_m=rel_alt, fix_type=3, num_satellites=12,
|
||||
has_position=True, has_attitude=True, position_valid=True,
|
||||
time_ms=now_ms, position_time_ms=now_ms)
|
||||
|
||||
|
||||
def _make_runner(px4=None, bridge=None, ground=None, drone=None,
|
||||
sm_cfg=None, vision_fn=None, pod_control=None):
|
||||
cfg = BridgeConfig()
|
||||
cfg.ground_inject.enabled = True
|
||||
cfg.ground_inject.timeout_ms = 5000
|
||||
return TaskRunner(
|
||||
cfg, sm_cfg or TaskStateConfig(),
|
||||
px4 or FakePx4(_state(1000)),
|
||||
bridge or FakeBridge(),
|
||||
TaskStateMachine(sm_cfg or TaskStateConfig()),
|
||||
ground or FakeGround(),
|
||||
drone or FakeDrone(),
|
||||
vision_fn=vision_fn,
|
||||
now_ms_fn=lambda: 1000,
|
||||
pod_control=pod_control,
|
||||
)
|
||||
|
||||
|
||||
# --- 输入采集 ---------------------------------------------------------------
|
||||
def test_collect_injected_fresh_sets_distance():
|
||||
ground = FakeGround(GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
|
||||
time_ms=1000))
|
||||
px4 = FakePx4(_state(1000))
|
||||
r = _make_runner(px4=px4, ground=ground)
|
||||
|
||||
inp = r._collect_inputs(1000)
|
||||
assert inp.target_injected is True
|
||||
assert inp.distance_to_target_m == pytest.approx(111.19, rel=1e-2) # ~111m
|
||||
assert inp.takeoff_reached is False # rel_alt=0 < 5
|
||||
|
||||
|
||||
def test_collect_injected_stale_is_invalid():
|
||||
ground = FakeGround(GroundTarget(latitude_deg=0.001, longitude_deg=0.0,
|
||||
time_ms=1000))
|
||||
r = _make_runner(ground=ground) # timeout=5000
|
||||
|
||||
inp = r._collect_inputs(7000) # 6000ms 差值 > 5000 → 超龄
|
||||
assert inp.target_injected is False
|
||||
assert inp.distance_to_target_m == float("inf")
|
||||
|
||||
|
||||
def test_collect_takeoff_reached():
|
||||
px4 = FakePx4(_state(1000, rel_alt=5.0))
|
||||
sm_cfg = TaskStateConfig(takeoff_altitude_m=5.0)
|
||||
r = _make_runner(px4=px4, sm_cfg=sm_cfg)
|
||||
|
||||
assert r._collect_inputs(1000).takeoff_reached is True
|
||||
|
||||
|
||||
def test_collect_locked_from_bridge():
|
||||
bridge = FakeBridge()
|
||||
bridge._locked = True
|
||||
r = _make_runner(bridge=bridge)
|
||||
|
||||
assert r._collect_inputs(1000).target_locked is True
|
||||
|
||||
|
||||
def test_collect_vision_second_judgement():
|
||||
r = _make_runner(vision_fn=lambda: True)
|
||||
assert r._collect_inputs(1000).vision_has_target is True
|
||||
|
||||
r2 = _make_runner(vision_fn=lambda: False)
|
||||
assert r2._collect_inputs(1000).vision_has_target is False
|
||||
|
||||
|
||||
# --- 居中累加 ---------------------------------------------------------------
|
||||
def test_centered_accumulates_duration():
|
||||
bridge = FakeBridge()
|
||||
bridge._telemetry = (Telemetry(off_target_x_deg=0.1, off_target_y_deg=0.1), 1000)
|
||||
sm_cfg = TaskStateConfig(center_threshold_deg=1.0)
|
||||
r = _make_runner(bridge=bridge, sm_cfg=sm_cfg)
|
||||
|
||||
assert r._update_centered(1000) == (True, 0.0) # 开始居中
|
||||
assert r._update_centered(3000) == (True, 2.0) # 持续 2s
|
||||
assert r._update_centered(3500) == (True, 2.5) # 持续 2.5s
|
||||
|
||||
|
||||
def test_centered_resets_on_large_offset():
|
||||
bridge = FakeBridge()
|
||||
bridge._telemetry = (Telemetry(off_target_x_deg=0.1, off_target_y_deg=0.1), 1000)
|
||||
r = _make_runner(bridge=bridge)
|
||||
|
||||
r._update_centered(1000)
|
||||
bridge._telemetry = (Telemetry(off_target_x_deg=5.0, off_target_y_deg=0.1), 2000)
|
||||
assert r._update_centered(2000) == (False, 0.0) # 脱靶量大 → 清零
|
||||
|
||||
|
||||
def test_centered_no_telemetry_returns_false():
|
||||
r = _make_runner() # bridge.latest_telemetry is None
|
||||
assert r._update_centered(1000) == (False, 0.0)
|
||||
|
||||
|
||||
# --- 状态进入动作 -----------------------------------------------------------
|
||||
def test_on_enter_arm_takeoff_arms_and_takes_off():
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(drone=drone, sm_cfg=TaskStateConfig(takeoff_altitude_m=5.0))
|
||||
asyncio.run(r._on_enter(TaskState.ARM_TAKEOFF, 1000))
|
||||
assert drone.armed is True
|
||||
assert drone.took_off is True
|
||||
assert drone.takeoff_alt == pytest.approx(5.0)
|
||||
|
||||
|
||||
def test_on_enter_goto_switches_to_l2():
|
||||
bridge = FakeBridge()
|
||||
r = _make_runner(bridge=bridge)
|
||||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||||
assert bridge.started is True
|
||||
assert bridge.law is False # L2 地理引导
|
||||
|
||||
|
||||
def test_on_enter_track_switches_to_l1():
|
||||
bridge = FakeBridge()
|
||||
r = _make_runner(bridge=bridge)
|
||||
asyncio.run(r._on_enter(TaskState.TRACK, 1000))
|
||||
assert bridge.law is True # L1 航向伺服
|
||||
|
||||
|
||||
def test_on_enter_rtl_stops_bridge_and_returns():
|
||||
bridge = FakeBridge()
|
||||
drone = FakeDrone()
|
||||
r = _make_runner(bridge=bridge, drone=drone)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 1000))
|
||||
assert bridge.stopped is True
|
||||
assert drone.rtl is True
|
||||
|
||||
|
||||
# --- 持续动作 ---------------------------------------------------------------
|
||||
def test_tick_goto_steps_bridge():
|
||||
bridge = FakeBridge()
|
||||
r = _make_runner(bridge=bridge)
|
||||
asyncio.run(r._tick(1000))
|
||||
# WAIT 状态不 step；切到 GOTO 才 step
|
||||
r._sm._state = TaskState.GOTO
|
||||
asyncio.run(r._tick(1000))
|
||||
assert bridge.steps == 1
|
||||
|
||||
|
||||
def test_tick_approach_brakes():
|
||||
px4 = FakePx4(_state(1000))
|
||||
r = _make_runner(px4=px4)
|
||||
r._sm._state = TaskState.APPROACH
|
||||
asyncio.run(r._tick(1000))
|
||||
assert len(px4.brake_calls) == 1
|
||||
|
||||
|
||||
# --- 吊舱控制集成（真机）----------------------------------------------------
|
||||
class FakePodControl:
|
||||
def __init__(self):
|
||||
self.armed = False
|
||||
self.stopped = False
|
||||
|
||||
def arm_detection(self):
|
||||
self.armed = True
|
||||
|
||||
def stop_tracking(self):
|
||||
self.stopped = True
|
||||
|
||||
|
||||
def test_on_enter_goto_arms_detection():
|
||||
pod_control = FakePodControl()
|
||||
r = _make_runner(pod_control=pod_control)
|
||||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||||
assert pod_control.armed is True
|
||||
assert pod_control.stopped is False
|
||||
|
||||
|
||||
def test_on_enter_rtl_stops_tracking_before_teardown():
|
||||
pod_control = FakePodControl()
|
||||
r = _make_runner(pod_control=pod_control)
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 1000))
|
||||
assert pod_control.stopped is True
|
||||
|
||||
|
||||
def test_no_pod_control_is_safe():
|
||||
"""未注入 pod_control（SITL）→ GOTO/RTL 进入动作不触发吊舱控制、不抛异常。"""
|
||||
r = _make_runner(pod_control=None)
|
||||
asyncio.run(r._on_enter(TaskState.GOTO, 1000))
|
||||
asyncio.run(r._on_enter(TaskState.RTL, 1000))
|
||||
@@ -0,0 +1,233 @@
|
||||
"""state_machine_test.py —— 任务级状态机单元测试。
|
||||
|
||||
覆盖：完整状态链 WAIT→ARM_TAKEOFF→GOTO→TRACK→APPROACH→RTL、各状态超时、
|
||||
GOTO→TRACK 的锁定/视觉守卫、TRACK 丢目标退回 GOTO、紧急/返航全局终止、
|
||||
RTL 终态与 reset。
|
||||
"""
|
||||
|
||||
import pytest
|
||||
|
||||
from task_runner.state_machine import (TaskInputs, TaskState, TaskStateConfig,
|
||||
TaskStateMachine, TaskTransition)
|
||||
|
||||
|
||||
def _inp(**kw) -> TaskInputs:
|
||||
return TaskInputs(**kw)
|
||||
|
||||
|
||||
def _sm(cfg=None) -> TaskStateMachine:
|
||||
return TaskStateMachine(cfg or TaskStateConfig())
|
||||
|
||||
|
||||
# --- 初始态与 reset ---------------------------------------------------------
|
||||
def test_initial_state_is_wait():
|
||||
assert _sm().state is TaskState.WAIT
|
||||
|
||||
|
||||
def test_reset_returns_to_wait():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
assert sm.state is TaskState.ARM_TAKEOFF
|
||||
sm.reset()
|
||||
assert sm.state is TaskState.WAIT
|
||||
|
||||
|
||||
# --- WAIT -------------------------------------------------------------------
|
||||
def test_wait_without_coords_stays():
|
||||
tr = _sm().update(_inp(), 1000)
|
||||
assert not tr.occurred
|
||||
assert tr.to_state is TaskState.WAIT
|
||||
|
||||
|
||||
def test_wait_receives_coords_goes_arm_takeoff():
|
||||
sm = _sm()
|
||||
tr = sm.update(_inp(target_injected=True), 1000)
|
||||
assert tr.occurred
|
||||
assert tr.from_state is TaskState.WAIT
|
||||
assert tr.to_state is TaskState.ARM_TAKEOFF
|
||||
assert sm.state is TaskState.ARM_TAKEOFF
|
||||
|
||||
|
||||
# --- ARM_TAKEOFF ------------------------------------------------------------
|
||||
def test_arm_takeoff_reaches_altitude_goes_goto():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
tr = sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
assert tr.occurred and tr.to_state is TaskState.GOTO
|
||||
|
||||
|
||||
def test_arm_takeoff_timeout_goes_rtl():
|
||||
cfg = TaskStateConfig(arm_takeoff_timeout_s=60.0)
|
||||
sm = _sm(cfg)
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
# 60s 未到高度 → 超时返航
|
||||
tr = sm.update(_inp(target_injected=True), 1000 + 61_000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
# --- GOTO -------------------------------------------------------------------
|
||||
def test_goto_far_away_stays():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=100.0,
|
||||
target_locked=True), 3000)
|
||||
assert not tr.occurred
|
||||
assert sm.state is TaskState.GOTO
|
||||
|
||||
|
||||
def test_goto_within_radius_and_locked_goes_track():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True), 3000)
|
||||
assert tr.occurred and tr.to_state is TaskState.TRACK
|
||||
|
||||
|
||||
def test_goto_within_radius_but_unlocked_stays():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=False), 3000)
|
||||
assert not tr.occurred
|
||||
assert sm.state is TaskState.GOTO
|
||||
|
||||
|
||||
def test_goto_requires_vision_when_enabled():
|
||||
cfg = TaskStateConfig(require_vision=True)
|
||||
sm = _sm(cfg)
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
# 有锁定但无视觉第二判据 → 不切换
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True, vision_has_target=False), 3000)
|
||||
assert not tr.occurred
|
||||
# 补上视觉 → 切换
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True, vision_has_target=True), 4000)
|
||||
assert tr.occurred and tr.to_state is TaskState.TRACK
|
||||
|
||||
|
||||
def test_goto_loses_coords_goes_rtl():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
tr = sm.update(_inp(target_injected=False, distance_to_target_m=10.0), 3000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
def test_goto_timeout_goes_rtl():
|
||||
cfg = TaskStateConfig(goto_timeout_s=120.0)
|
||||
sm = _sm(cfg)
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
tr = sm.update(_inp(target_injected=True, distance_to_target_m=100.0), 2000 + 121_000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
# --- TRACK ------------------------------------------------------------------
|
||||
def test_track_centered_stable_goes_approach():
|
||||
sm = _sm(TaskStateConfig(center_hold_s=3.0))
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True), 3000)
|
||||
assert sm.state is TaskState.TRACK
|
||||
# 居中但未满时长 → 停留
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True,
|
||||
target_centered=True, centered_duration_s=1.0), 4000)
|
||||
assert not tr.occurred
|
||||
# 居中稳定满 3s → APPROACH
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True,
|
||||
target_centered=True, centered_duration_s=3.0), 5000)
|
||||
assert tr.occurred and tr.to_state is TaskState.APPROACH
|
||||
|
||||
|
||||
def test_track_lost_with_coords_goes_back_to_goto():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True), 3000)
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=False), 4000)
|
||||
assert tr.occurred and tr.to_state is TaskState.GOTO
|
||||
|
||||
|
||||
def test_track_lost_without_coords_goes_rtl():
|
||||
sm = _sm()
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True), 3000)
|
||||
tr = sm.update(_inp(target_injected=False, target_locked=False), 4000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
def test_track_timeout_goes_rtl():
|
||||
cfg = TaskStateConfig(track_timeout_s=120.0)
|
||||
sm = _sm(cfg)
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True), 3000)
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True), 3000 + 121_000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
# --- APPROACH ---------------------------------------------------------------
|
||||
def test_approach_hover_done_goes_rtl():
|
||||
cfg = TaskStateConfig(center_hold_s=0.0, approach_hover_s=10.0)
|
||||
sm = _sm(cfg)
|
||||
sm.update(_inp(target_injected=True), 1000)
|
||||
sm.update(_inp(target_injected=True, takeoff_reached=True), 2000)
|
||||
sm.update(_inp(target_injected=True, distance_to_target_m=10.0,
|
||||
target_locked=True), 3000)
|
||||
sm.update(_inp(target_injected=True, target_locked=True, target_centered=True,
|
||||
centered_duration_s=0.0), 4000)
|
||||
assert sm.state is TaskState.APPROACH
|
||||
# 悬停未满 10s → 停留
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True), 4000 + 5000)
|
||||
assert not tr.occurred
|
||||
# 满 10s → RTL
|
||||
tr = sm.update(_inp(target_injected=True, target_locked=True), 4000 + 10_000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
# --- 全局终止（紧急 / 返航）-------------------------------------------------
|
||||
@pytest.mark.parametrize("state_kw", [
|
||||
{}, # WAIT
|
||||
{"target_injected": True}, # 会先转 ARM_TAKEOFF
|
||||
])
|
||||
def test_emergency_goes_rtl_from_any_state(state_kw):
|
||||
sm = _sm()
|
||||
sm.update(_inp(**state_kw), 1000)
|
||||
tr = sm.update(_inp(emergency=True), 2000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
def test_rtl_requested_goes_rtl():
|
||||
sm = _sm()
|
||||
tr = sm.update(_inp(rtl_requested=True), 1000)
|
||||
assert tr.occurred and tr.to_state is TaskState.RTL
|
||||
|
||||
|
||||
# --- RTL 终态 ---------------------------------------------------------------
|
||||
def test_rtl_is_terminal():
|
||||
sm = _sm()
|
||||
sm.update(_inp(rtl_requested=True), 1000)
|
||||
assert sm.state is TaskState.RTL
|
||||
# 终态不再转移（连紧急/坐标也不动）
|
||||
tr = sm.update(_inp(target_injected=True, emergency=True), 2000)
|
||||
assert not tr.occurred
|
||||
assert sm.state is TaskState.RTL
|
||||
|
||||
|
||||
# --- 转移结果结构 -----------------------------------------------------------
|
||||
def test_transition_dataclass_defaults():
|
||||
tr = TaskTransition()
|
||||
assert tr.from_state is TaskState.WAIT
|
||||
assert tr.to_state is TaskState.WAIT
|
||||
assert tr.occurred is False
|
||||
assert tr.reason == ""
|
||||
+163
@@ -0,0 +1,163 @@
|
||||
# 预定流程 · 操作指南与待办事项
|
||||
|
||||
> 更新日期：2026-10-12
|
||||
> 覆盖：任务级状态机全链路（`utils/task_runner/`）＋ 地面站页面 ＋ 真机/仿真入口
|
||||
>
|
||||
> 本链路对应 `预定流程.md` 的「地面给坐标 → 天空解锁/接受坐标引导/追踪打击」，
|
||||
> 已在 **PX4 SITL 端到端跑通**。架构与铁律见 `.workbuddy/memory/MEMORY.md` / `ARCHITECTURE.md`。
|
||||
|
||||
---
|
||||
|
||||
## 一、本次落地了什么
|
||||
|
||||
| 模块 | 文件 | 作用 |
|
||||
|------|------|------|
|
||||
| 任务状态机 | `utils/task_runner/state_machine.py` | 6 状态纯逻辑转移：WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL |
|
||||
| 编排器 | `utils/task_runner/runner.py` | 状态机驱动 + 采集快照 + 状态动作（起飞/切律/返航） |
|
||||
| 日志模块 | `utils/task_runner/log.py` | 事件 JSONL + 遥测 CSV 双层结构化日志 |
|
||||
| 吊舱控制 | `utils/task_runner/pod_control.py` | 真机吊舱识别/锁定指令（0x07/0x0F/0x0E/0x08） |
|
||||
| 地面注入 | `utils/pod_bridge/ground_link.py` | UDP 接收地面站坐标（超龄判无效） |
|
||||
| SITL 入口 | `task_runner_sitl.py` | 仿真全流程（VirtualPod + 虚拟目标） |
|
||||
| 真机入口 | `task_runner_real.py` | 真串口吊舱 + UDP 注入 + 吊舱控制 |
|
||||
| 地面站 | `ground_station/app.py` + `templates/index.html` | 坐标注入 + 状态可视化（Flask） |
|
||||
|
||||
控制律本体复用既有 `visual_track_control.TrackingControlLaw`（L1 角度直入）与
|
||||
`pod_bridge.GuidanceLaw`（L2 地理引导），**运行时通过 `bridge.set_heading_servo_enabled()` 切换**。
|
||||
|
||||
---
|
||||
|
||||
## 二、操作指南
|
||||
|
||||
### 2.1 纯软测试（无硬件，最快验证）
|
||||
|
||||
```bash
|
||||
cd "D:/ProgramData/code_drone-test/utils"
|
||||
"C:/Users/yimiao/.workbuddy/binaries/python/envs/drone-test/Scripts/python.exe" -m pytest -q
|
||||
# 预期：483 passed
|
||||
```
|
||||
|
||||
> ⚠ conda `pytorch` 环境缺 pytest（见 §三 待办 P2-2），测试用隔离 venv `drone-test`。
|
||||
|
||||
### 2.2 SITL 仿真全流程（独立模式，内置虚拟目标自动注入坐标）
|
||||
|
||||
前置：WSL 里启动 PX4 SITL（监听 14540）。
|
||||
|
||||
```bash
|
||||
# 1. WSL 内启动 PX4 SITL
|
||||
cd ~/PX4-Autopilot && make px4_sitl jmavsim
|
||||
|
||||
# 2. Windows 侧跑全流程（内置虚拟目标 home 北30m/东15m）
|
||||
cd "D:/ProgramData/code_drone-test"
|
||||
"D:/program file/conda/envs/pytorch/python.exe" task_runner_sitl.py --conn udp://:14540
|
||||
```
|
||||
|
||||
预期：`wait → arm_takeoff → goto → track → approach → rtl`，全程约 1m24s。
|
||||
|
||||
### 2.3 SITL 仿真（地面站联动模式，坐标由地面站 UDP 注入）
|
||||
|
||||
```bash
|
||||
# 终端 A：无人机（--ground-station 表示坐标走 UDP 接收，不再内置自动注入）
|
||||
"D:/program file/conda/envs/pytorch/python.exe" task_runner_sitl.py --conn udp://:14540 --ground-station
|
||||
|
||||
# 终端 B：地面站页面
|
||||
"D:/program file/conda/envs/pytorch/python.exe" ground_station/app.py --udp-port 14555 --log-dir logs
|
||||
# 浏览器打开 http://127.0.0.1:5000，输入坐标点「注入坐标」
|
||||
```
|
||||
|
||||
⚠ 地面站需**持续**发坐标（每 0.5~1s 一次），否则 `ground_inject.timeout_ms`（默认 5s）
|
||||
超龄后 GOTO 判「坐标失效」返航。页面上手动点一次只发一次，联调时建议用脚本循环注入。
|
||||
|
||||
### 2.4 真机联调（增量流程，标定前不得实飞）
|
||||
|
||||
按顺序逐级验证，每级过了再进下一级：
|
||||
|
||||
| 级 | 动作 | 命令 | 通过判据 |
|
||||
|----|------|------|---------|
|
||||
| ① 纯软单测 | 全量 pytest | `pytest -q` | 483 passed |
|
||||
| ② 串口回环 | 吊舱自环 | 台架工具 | 收发一致 |
|
||||
| ③ 仅吊舱 | 干跑（不发 offboard） | `python utils/pod_bridge/run_bridge.py --config utils/config/pod_bridge.json --port /dev/ttyAMA0 --dry-run` | β→yaw_rate 符号/量级正确 |
|
||||
| ④ 仅飞控 | 飞控遥测/offboard | — | 遥测新鲜、offboard 可启停 |
|
||||
| ⑤ 地面联调 | 真机全流程 | `python task_runner_real.py --target-type 1` | 完整走完 |
|
||||
|
||||
真机全流程前置：
|
||||
1. 填 `utils/config/pod_bridge.json` 的 `pod_serial.port`（如 `/dev/ttyAMA0`）、`px4.connection_string`；
|
||||
2. 地面站往 `ground_inject.port`（默认 14555）持续发 `{"lat":..,"lon":..,"alt":..}`。
|
||||
|
||||
真机关键命令参数：
|
||||
```bash
|
||||
python task_runner_real.py --target-type 1 # 1=无人机，0=人/车
|
||||
python task_runner_real.py --port /dev/ttyAMA0 # 覆盖吊舱串口
|
||||
python task_runner_real.py --conn serial:///dev/ttyS1:57600 # 覆盖飞控连接串
|
||||
```
|
||||
|
||||
### 2.5 硬件铁律（真机必读，来自厂商确认 + 实测）
|
||||
|
||||
- 🔴 **本吊舱无激光 → 表4 目标经纬度永不出**，目标坐标必须走地面注入（`ground_link`），
|
||||
`guidance` 吃注入坐标而非表4。
|
||||
- **0x0F「发现目标自动锁定」是常驻模式**，`0x0E` 只停当前这次跟踪、不退出自动锁定；
|
||||
只有 `0x08 目标识别关` 才真正退出识别。
|
||||
- **bit2 锁定位在目标丢失后不复位**，且脱靶量无无效标志位（丢失表现为 0）——单靠 bit2
|
||||
会朝过期坐标继续飞，必须加第二判据（我方视觉，`require_vision`）。
|
||||
- **自动解锁（`--auto-unlock`）在本设备禁用**（表4 恒不发导致误解锁），解锁一律用按键 `u`。
|
||||
|
||||
---
|
||||
|
||||
## 三、待办事项（按优先级）
|
||||
|
||||
### P0（实飞前必须）
|
||||
|
||||
- [ ] **真机增量验证**：按 §2.4 的 ②~⑤ 逐级跑，尤其 ③ 干跑确认 β→yaw_rate 符号/量级。
|
||||
- [ ] **状态机参数标定**：`task_runner_real.py` 里 `build_sm_config()` 的
|
||||
`switch_radius_m`（GOTO→TRACK 切换半径）、`center_threshold_deg`（居中阈值）、
|
||||
`center_hold_s`、`takeoff_altitude_m` 全是占位值，须台架标定后回填。
|
||||
- [ ] **L1/L2 增益标定**：`pod_bridge.json` 里 `heading.gain`、`guidance.speed_kp/yaw_kp`
|
||||
等仍为占位，`beta_sign`/`alpha_sign` 已按 S1 标定（+1/-1），其余待标定。
|
||||
|
||||
### P1（落地后尽快）
|
||||
|
||||
- [ ] **摄像头可视化接真视频流**：当前地面站是「脱靶量模拟视野」，真机需接吊舱视频流
|
||||
（吊舱视频输出 → 采集 → 页面叠加追踪框）。
|
||||
- [ ] **任务级状态机配置 JSON 化**：`TaskStateConfig` 目前是 dataclass 默认值 + 脚本内构造，
|
||||
应外置到配置（铁律「配置全外置」）。
|
||||
- [ ] **地面站与 runner 状态实时打通**：当前地面站读日志文件（秒级延迟），可改为 runner
|
||||
直接 UDP 上报遥测到地面站（低延迟）。
|
||||
|
||||
### P2（工程收尾）
|
||||
|
||||
- [ ] **仓库补 `.gitignore`**：`logs/`、`__pycache__/`、`.codegraph/`、`models/*.pt`、
|
||||
`stereo_calib_params.npz` 等长期未跟踪项，提交时显式列路径、勿 `git add .`。
|
||||
- [ ] **conda `pytorch` 回填 pytest**：README 声明有 pytest 但实测缺失，测试目前走
|
||||
隔离 venv `drone-test`；回填后使 README 与实际一致。
|
||||
- [ ] **C++ 侧构建验证**：`reference-c++/target_tracker` 与 `visual_track_control` 无
|
||||
`build/`，若以 C++ 为验收基准需 `cmake .. && make && ctest` 跑 11 个测试。
|
||||
|
||||
---
|
||||
|
||||
## 四、关键配置速查（`utils/config/pod_bridge.json`）
|
||||
|
||||
| 段 | 关键字段 | 说明 |
|
||||
|----|---------|------|
|
||||
| `pod_serial` | `port` | 吊舱串口设备号（真机填 `/dev/ttyAMA0`） |
|
||||
| `px4` | `connection_string` | 飞控连接串（SITL `udp://:14540`，真机串口） |
|
||||
| `link` | `require_laser_valid` | 无激光吊舱必须 `false` |
|
||||
| `heading_servo` | `enabled` / `beta_sign` / `alpha_sign` | L1 开关 + 符号（±1，已标定 +1/-1） |
|
||||
| `ground_inject` | `enabled` / `port` / `timeout_ms` | 地面坐标注入开关 / UDP 端口 / 超龄门限 |
|
||||
|
||||
---
|
||||
|
||||
## 五、一键命令汇总
|
||||
|
||||
```bash
|
||||
# 测试
|
||||
cd utils && "C:/Users/yimiao/.workbuddy/binaries/python/envs/drone-test/Scripts/python.exe" -m pytest -q
|
||||
|
||||
# SITL 独立全流程
|
||||
"D:/program file/conda/envs/pytorch/python.exe" task_runner_sitl.py --conn udp://:14540
|
||||
|
||||
# SITL 地面站联动
|
||||
"D:/program file/conda/envs/pytorch/python.exe" task_runner_sitl.py --conn udp://:14540 --ground-station
|
||||
"D:/program file/conda/envs/pytorch/python.exe" ground_station/app.py --udp-port 14555 --log-dir logs
|
||||
|
||||
# 真机全流程（填好配置后）
|
||||
"D:/program file/conda/envs/pytorch/python.exe" task_runner_real.py --target-type 1
|
||||
```
|
||||
@@ -0,0 +1,24 @@
|
||||
预定流程：
|
||||
|
||||
|
||||
|
||||
地面：某处出现了疑似可疑单位----|给出坐标
|
||||
|
||||
天空：等待 |解锁，接受坐标引导，**追踪打击**
|
||||
|
||||
|
||||
|
||||
代码层描述：地面部分工作已经实现，天空部分test.py已经给了一个大概的过程。目前就是需要改进该部分。
|
||||
|
||||
|
||||
|
||||
硬件层描述：目前已经测试完得到结论应势产品在无激光部分的情况下本身约等于一个做了目标检测的开发板，本质仍然没距离输入。
|
||||
|
||||
|
||||
|
||||
现行工作，需要把天空上的集成做完（让没距离输入时也能正常工作）。
|
||||
|
||||
|
||||
|
||||
需实现：1.完整的全流程可运行runner；2.配套的完整日志记录模块；3.配套的临时测试用地面坐标输入与摄像头可视化配套页面
|
||||
|
||||
Reference in New Issue
Block a user