实现预定流程全链路: 任务状态机 + 编排器 + 地面站 + 真机/SITL入口

- utils/task_runner: 6态状态机(WAIT→ARM_TAKEOFF→GOTO(L2)→TRACK(L1)→APPROACH→RTL)
  + 编排器(快照采集/状态动作) + 双层日志(事件JSONL/遥测CSV) + 吊舱控制指令(0x07/0x0F/0x0E/0x08)
- pod_bridge: ground_link 地面坐标UDP注入(超龄判无效,无激光替代表4);
  set_heading_servo_enabled() 运行时L1/L2切换(切换即reset清积分防输出跳变);
  pod_locked 提为公共门控; config 新增 ground_inject 段
- virtual_pod 模拟吊舱(无电机语义) + verify_px4 增量④仅飞控验证 +
  pod_sitl_test L1 SITL全流程(飞行编排不塞进 run_bridge, 保持纯offboard闭环入口)
- 入口: task_runner_sitl(VirtualPod+虚拟目标, SITL端到端跑通约1m24s) /
  task_runner_real(真串口+UDP注入+吊舱控制)
- 地面站 ground_station: 坐标注入 + 状态可视化(Flask)
- S1标定回填(2026-10-09): beta_sign=+1 / alpha_sign=-1, 加标定回归测试守护
- 新增测试: task_runner x4 + ground_link/virtual_pod*/bridge/px4_gateway 增量; 全量 483 passed
- 文档: 预定流程(需求) + 预定流程-操作指南(操作/待办) + L1方案补S1结论与实现状态
This commit is contained in:
keita
2026-10-13 15:50:32 +08:00
parent 5a944af4f5
commit ded17d7995
30 changed files with 3552 additions and 27 deletions
+73
View File
@@ -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)
+11
View File
@@ -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
+14 -10
View File
@@ -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
+124
View File
@@ -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