实现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
+57
View File
@@ -1,4 +1,6 @@
{
"_comment": "吊舱↔PX4 桥接配置。heading_servo.enabled=true → 走 L1 航向伺服(TrackingControlLaw);tracking 段为 L1 控制律参数(与 visual_track_control 同结构,各分组须齐全)。全部数值为台架标定前占位,符号(beta_sign/alpha_sign)与增益须经 S1/S2 标定后回填,标定前不得实飞。",
"pod_serial": {
"port": "",
"baudrate": 115200
@@ -26,5 +28,60 @@
"max_yaw_rate_dps": 30.0,
"yaw_kp": 1.0,
"yaw_slew_dps2": 60.0
},
"heading_servo": {
"enabled": true,
"beta_sign": 1,
"alpha_sign": 1,
"beta_from_frame": false,
"beta_from_off_target": true,
"alpha_from_frame": false,
"alpha_from_off_target": true,
"vertical_enabled": true,
"require_vision": false
},
"tracking": {
"control": {
"frequency_hz": 20.0,
"visual_stale_ms": 200,
"distance_stale_ms": 500,
"attitude_stale_ms": 300
},
"camera": {
"image_width": 1920,
"image_height": 1080,
"fov_h_deg": 60.0,
"fov_v_deg": 35.0
},
"heading": {
"mode": "rate",
"gain": 1.0,
"yaw_rate_limit_dps": 20.0,
"yaw_slew_limit_dps2": 40.0
},
"vertical": {
"vz_gain_mps_per_deg": 0.05,
"vz_limit_mps": 0.5
},
"distance": {
"d_exp_m": 10.0,
"kp": 0.5,
"ki": 0.0,
"kd": 0.0,
"integral_limit": 0.0,
"derivative_filter_coef": 0.0,
"approach_velocity_limit_mps": 0.5,
"retreat_velocity_limit_mps": 0.3,
"no_distance_action": "slow_approach",
"no_distance_approach_limit_mps": 0.2,
"no_distance_exit_grace_ms": 3000
},
"accel_limit": {
"ax_mps2": 1.0,
"ay_mps2": 1.0,
"az_mps2": 0.5
}
}
}