实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/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
@@ -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>
|
||||
Reference in New Issue
Block a user