实现L1航向伺服(路线A1): 吊舱角度直入控制律 + 桥接胶水 + 干跑入口

- visual_track_control: ControlSnapshot 加可选 los_beta_deg/los_alpha_deg,
  第④步角度直入优先否则回退像素路径(向后兼容); config 拆出 load_from_dict 供内嵌复用
- pod_bridge: BridgeConfig 内嵌 TrackingConfig + 新 HeadingServoConfig;
  bridge._build_snapshot 合成 β/α(姿态来自PX4/角度来自吊舱,信号双路适配无电机),
  step 按 heading_servo.enabled 切 L1/L2, DEGRADED_HOLD 补发刹车保 offboard 心跳;
  run() 循环频率按生效律取(修 L1 频率错配)
- run_bridge.py: 闭环 CLI 入口, --dry-run(S2 只接吊舱不发offboard) 与实飞优雅停机
- 配置落 L1 首飞值(slow_approach/0.2mps、垂直压低、无电机仅用脱靶量)
- 新增测试 29 条(vtc5+config9+bridgeL1 10+runner5); 全量 401 passed
This commit is contained in:
keita
2026-10-09 12:52:14 +08:00
parent b147925d58
commit 5a944af4f5
12 changed files with 827 additions and 43 deletions
@@ -7,7 +7,8 @@
① 超龄判定(姿态优先于视觉,超龄 → 降级保持)
② 判丢 → 零速刹车(进入时清积分与限幅器历史)
③ Coast(is_predicted)→ 清距离积分防饱和
④ 像素偏移 → 视线角 β/α(全程针孔模型)
④ 视线角 β/α:默认像素偏移→针孔模型换算;若 snapshot 直入角度(los_beta_deg/
los_alpha_deg,L1 航向伺服路线 A1)则优先采用,绕过 FOV 换算
⑤ 水平航向(速率式/位置式,A2:位置式合成绝对 NED 航向)
⑥ 垂直统一速度支路(A1:不切姿态微调模式)
⑦ 距离通道 PID → 前向速度;无距离按配置降级(hold/slow_approach/exit,
@@ -130,12 +131,20 @@ class TrackingControlLaw:
# 航向通道为纯比例增益映射(§4.2),无积分项,无需清理
out.coast_active = True
# ---- ④ 像素偏移 → 视线角(§4.1 水平 β / §5.1 垂直 α) ----
# x_offset = cx − W/2(右正),y_offset = cy − H/2(下正)
x_offset = snapshot.track.center[0] - self._cfg.camera.image_width * 0.5
y_offset = snapshot.track.center[1] - self._cfg.camera.image_height * 0.5
beta_deg = self._pixel_to_beta.convert(x_offset) * _K_RAD_TO_DEG
alpha_deg = self._pixel_to_alpha.convert(y_offset) * _K_RAD_TO_DEG
# ---- ④ 视线角 β/α(§4.1 水平 / §5.1 垂直) ----
# 优先角度直入(L1 航向伺服,路线 A1):信源已给角度量则跳过像素换算,
# 避免吊舱变焦后 FOV 变化导致失真;未提供(None)则回退既有像素路径。
# 像素路径:x_offset = cx − W/2(右正),y_offset = cy − H/2(下正)
if snapshot.los_beta_deg is not None:
beta_deg = snapshot.los_beta_deg
else:
x_offset = snapshot.track.center[0] - self._cfg.camera.image_width * 0.5
beta_deg = self._pixel_to_beta.convert(x_offset) * _K_RAD_TO_DEG
if snapshot.los_alpha_deg is not None:
alpha_deg = snapshot.los_alpha_deg
else:
y_offset = snapshot.track.center[1] - self._cfg.camera.image_height * 0.5
alpha_deg = self._pixel_to_alpha.convert(y_offset) * _K_RAD_TO_DEG
# ---- ⑤ 水平航向通道(§4.2:增益 → slew → 角速度限幅) ----
if self._cfg.heading.mode is HeadingMode.RATE: