From 16b0b34f58d306e0f93675ba0884277035e56186 Mon Sep 17 00:00:00 2001 From: XiaoPengYouCode <120481176+XiaoPengYouCode@users.noreply.github.com> Date: Mon, 15 Jun 2026 15:54:23 +0800 Subject: [PATCH] fix: block fire briefly after armor jump --- cfg/rbt_cfg.toml | 2 + docs/vivsionn-gap-checklist.md | 2 +- ...21\346\216\247\344\270\273\347\272\277.md" | 2 + lib/src/rbt_infra/rbt_cfg.rs | 12 ++ lib/src/rbt_mod/rbt_estimator.rs | 165 +++++++++++++++++- .../rbt_mod/rbt_fire_control/controller.rs | 14 ++ 6 files changed, 195 insertions(+), 2 deletions(-) diff --git a/cfg/rbt_cfg.toml b/cfg/rbt_cfg.toml index 580f977..5192386 100644 --- a/cfg/rbt_cfg.toml +++ b/cfg/rbt_cfg.toml @@ -83,6 +83,8 @@ cam_k = [1600.0, 0.0, 320.0, 0.0, 1705.7, 192.0, 0.0, 0.0, 1.0] [estimator_cfg] armor_lost_wait_duration_ms = 100 enemy_lost_wait_duration_ms = 1000 +fire_block_on_armor_jump = true +fire_armor_jump_block_frames = 3 ypd_geometry_recovery_window_frames = 24 ypd_geometry_recovery_cooldown_frames = 12 ypd_geometry_recovery_mismatch_required_streak = 2 diff --git a/docs/vivsionn-gap-checklist.md b/docs/vivsionn-gap-checklist.md index d37e13f..ffda047 100644 --- a/docs/vivsionn-gap-checklist.md +++ b/docs/vivsionn-gap-checklist.md @@ -8,7 +8,7 @@ This checklist tracks the functional gaps found while comparing this Rust worksp | [ ] P0 相机/视频源 | 海康相机、模式切曝光、离线 `.avi + .csv` 回放 | 固定 ffmpeg 读离线视频 | 必补。不上这个很难常驻上车 | | [ ] P0 常驻机器人入口 | `supervisor + YoloDetect()` 常驻运行 | 视频结束进程退出 | 实车稳定性缺口 | | [x] P0 装甲板 pitch 弹道控制 | 发控会算重力补偿并下发 pitch | 装甲板路线已根据 planner 目标位置计算弹道 pitch，并随 yaw 一起下发 | 已补齐装甲板 3D 发控输出 | -| [x] P0 YPD geometry recovery | 有 armor jump 后几何恢复、协方差膨胀 | tracker 在多装甲板观测后打开 recovery window，并在连续几何 mismatch 后膨胀 `dr/h` covariance | 已补齐跳板、错配、重获相关基础恢复逻辑 | +| [x] P0 YPD geometry recovery | 有 armor jump 后几何恢复、协方差膨胀，jump 后短时禁火 | tracker 在多装甲板观测后打开 recovery window，并在连续几何 mismatch 后膨胀 `dr/h` covariance；估计器在 jump 后短时拉低 `observation_stable`，让发控保持瞄准但不自动开火 | 已补齐跳板、错配、重获、协方差恢复和开火观测稳定门控 | | [x] P0 前哨站特化 | outpost 高度相位锁定、半径先验、yaw 恢复 | tracker 已做 outpost observed/radial yaw 转换、高度相位锁定、锁定高度冻结、半径先验和 rejected update 门控 | 已补齐前哨站专用 tracker 路径 | | [x] P0 能量机关 R 圆心/切换门控 | `Buff_Detector` 有 R 圆心修正、模板/轮廓 fallback、锁定门控 | solve stage 已修正不一致 R 圆心几何，tracker 已对大符目标切换做 defer/rebind phase gate | 本轮补了 R 圆心和切换门控；模板/轮廓 fallback 仍未纳入 | | [x] P0 能量机关 tracker/aimer | 相位 EKF、大小符曲线模型、相位化预瞄、pitch lead | 大符曲线 EKF（基于共享不定长 EKF）+ 两轮飞行时间迭代 + yaw preview horizon + pitch lead + 配置化偏置 | 大符走 `BigBuffCurveEskf` 曲线预测（`speed=a·sin(phase)+base-a`），小符保留常速；aimer 两轮弹道迭代、yaw MPC horizon 由 tracker 预瞄生成 | diff --git "a/docs/\347\263\273\347\273\237\346\236\266\346\236\204\344\270\216\345\217\221\346\216\247\344\270\273\347\272\277.md" "b/docs/\347\263\273\347\273\237\346\236\266\346\236\204\344\270\216\345\217\221\346\216\247\344\270\273\347\272\277.md" index 714accf..97e994a 100644 --- "a/docs/\347\263\273\347\273\237\346\236\266\346\236\204\344\270\216\345\217\221\346\216\247\344\270\273\347\272\277.md" +++ "b/docs/\347\263\273\347\273\237\346\236\266\346\236\204\344\270\216\345\217\221\346\216\247\344\270\273\347\272\277.md" @@ -184,6 +184,8 @@ flowchart TD 2. planner 输入：`planner_state()` 对齐 `YawPlanner` 的 11D 状态。 3. 发控门控信号：`motion_state`、`motion_uniform`、`observation_stable`、运动突变指标、`track_valid`、`fire_permit`。 +其中 `observation_stable` 不只是 tracker 收敛状态。装甲板 jump 后，估计器会短时拉低这个信号，让发控继续瞄准但暂停自动开火，避免刚跳板时用不稳定观测触发发弹。 + 这样做的好处是估计器不用知道当前是 `AimOnly`、`ShotOnce` 还是 `AutoFire`。它只告诉发控：目标是否有效，运动是否稳定，观测是否可信。 ## 发控模块拆分 diff --git a/lib/src/rbt_infra/rbt_cfg.rs b/lib/src/rbt_infra/rbt_cfg.rs index 33ca29d..4a285a0 100644 --- a/lib/src/rbt_infra/rbt_cfg.rs +++ b/lib/src/rbt_infra/rbt_cfg.rs @@ -351,6 +351,10 @@ impl CamCfg { pub struct EstimatorCfg { armor_lost_wait_duration_ms: u64, enemy_lost_wait_duration_ms: u64, + #[serde(default = "default_fire_block_on_armor_jump")] + pub fire_block_on_armor_jump: bool, + #[serde(default = "default_fire_armor_jump_block_frames")] + pub fire_armor_jump_block_frames: usize, #[serde(default = "default_ypd_geometry_recovery_window_frames")] pub ypd_geometry_recovery_window_frames: usize, #[serde(default = "default_ypd_geometry_recovery_cooldown_frames")] @@ -373,6 +377,14 @@ pub struct EstimatorCfg { // top2_activate_w: f64, } +fn default_fire_block_on_armor_jump() -> bool { + true +} + +fn default_fire_armor_jump_block_frames() -> usize { + 3 +} + fn default_ypd_geometry_recovery_window_frames() -> usize { 24 } diff --git a/lib/src/rbt_mod/rbt_estimator.rs b/lib/src/rbt_mod/rbt_estimator.rs index 509f59b..81ad87f 100644 --- a/lib/src/rbt_mod/rbt_estimator.rs +++ b/lib/src/rbt_mod/rbt_estimator.rs @@ -179,6 +179,8 @@ pub struct RbtEstimator { ypd_angle_tracker: YpdAngleTracker, latest_tracker_snapshot: Option, last_update_tp: Option, + fire_observation_hold_frames: usize, + previous_multi_armor_observation: bool, pub enemy_id: EnemyId, pub fire: bool, // 当前是否开火 pub single_or_double: bool, // 当前帧是否有多装甲板观测 @@ -191,6 +193,8 @@ impl RbtEstimator { ypd_angle_tracker: YpdAngleTracker::new(), latest_tracker_snapshot: None, last_update_tp: None, + fire_observation_hold_frames: 0, + previous_multi_armor_observation: false, enemy_id, fire: false, single_or_double: false, @@ -208,10 +212,13 @@ impl RbtEstimator { ) { self.ypd_angle_tracker.reset(); self.latest_tracker_snapshot = None; + self.reset_fire_observation_hold(); } self.update_global_vars(solved_enemy); + let tracker_was_initialized = self.ypd_angle_tracker.is_initialized(); self.update_tracker(cfg, solved_enemy.as_ref(), dt_s); + self.update_fire_observation_hold(cfg, solved_enemy.is_some(), tracker_was_initialized); } pub fn snapshot(&self) -> Option { @@ -245,7 +252,8 @@ impl RbtEstimator { fire_permit: self.fire, motion_state: target_motion_state(&state), motion_uniform: motion_uniform(tracker_snapshot), - observation_stable: observation_stable(tracker_snapshot), + observation_stable: observation_stable(tracker_snapshot) + && self.fire_observation_hold_frames == 0, motion_translation_burst_metric: metric_mm_to_m( tracker_snapshot.motion_translation_burst_metric, ), @@ -268,6 +276,37 @@ impl RbtEstimator { .unwrap_or(false); } + fn update_fire_observation_hold( + &mut self, + cfg: &EstimatorCfg, + has_solution: bool, + tracker_was_initialized: bool, + ) { + if !has_solution { + self.reset_fire_observation_hold(); + return; + } + + let entering_multi_armor_observation = + self.single_or_double && !self.previous_multi_armor_observation; + if cfg.fire_block_on_armor_jump + && tracker_was_initialized + && entering_multi_armor_observation + { + self.fire_observation_hold_frames = self + .fire_observation_hold_frames + .max(cfg.fire_armor_jump_block_frames); + } else if self.fire_observation_hold_frames > 0 { + self.fire_observation_hold_frames -= 1; + } + self.previous_multi_armor_observation = self.single_or_double; + } + + fn reset_fire_observation_hold(&mut self) { + self.fire_observation_hold_frames = 0; + self.previous_multi_armor_observation = false; + } + pub fn tracker_snapshot(&self) -> Option<&YpdTrackerSnapshot> { self.latest_tracker_snapshot.as_ref() } @@ -500,6 +539,8 @@ mod tests { "\ armor_lost_wait_duration_ms = 100 enemy_lost_wait_duration_ms = {enemy_lost_wait_duration_ms} +fire_block_on_armor_jump = true +fire_armor_jump_block_frames = 3 " )) .unwrap() @@ -527,6 +568,33 @@ enemy_lost_wait_duration_ms = {enemy_lost_wait_duration_ms} } } + fn solved_enemy_with_armors(centers: &[(f32, f32)]) -> RbtSolvedResult { + let mut armors = Vec::with_capacity(centers.len()); + for (idx, (center_x, center_y)) in centers.iter().copied().enumerate() { + let detected_armor = DetectedArmor::new( + RbtImgPoint2::new_screen_pixel(center_x, center_y), + RbtImgPoint2::new_screen_pixel(center_x - 10.0, center_y - 5.0), + RbtImgPoint2::new_screen_pixel(center_x - 10.0, center_y + 5.0), + RbtImgPoint2::new_screen_pixel(center_x + 10.0, center_y + 5.0), + RbtImgPoint2::new_screen_pixel(center_x + 10.0, center_y - 5.0), + idx, + ); + let pose = Isometry3::translation(200.0 + idx as f64 * 20.0, idx as f64 * 200.0, 100.0); + armors.push(SolvedArmor::new( + detected_armor, + pose, + idx as f64 * 90.0, + 0.0, + 200.0, + )); + } + + RbtSolvedResult { + coord: RbtCylindricalPoint2::new(1_000.0, 0.0), + armors, + } + } + fn frame(targets: &[(EnemyId, (f32, f32))]) -> RbtSolvedResults { let mut solved_enemies = RbtSolvedResults::default(); for (enemy_id, (x, y)) in targets { @@ -535,6 +603,35 @@ enemy_lost_wait_duration_ms = {enemy_lost_wait_duration_ms} solved_enemies } + fn stable_tracker_snapshot() -> YpdTrackerSnapshot { + let mut state11d = [0.0; 11]; + state11d[8] = 200.0; + + YpdTrackerSnapshot { + state11d, + state9: [0.0; 9], + tracked_id: 0, + armor_num: 4, + tracked_armor_xyza: [200.0, 0.0, 0.0, 0.0], + predicted_armors_xyza: Vec::new(), + last_nis: 0.0, + converged: true, + diverged: false, + recent_nis_failures: 0, + motion_translation_burst_metric: 0.0, + motion_translation_drift_metric: 0.0, + motion_yaw_accel_metric: 0.0, + } + } + + fn estimator_with_stable_snapshot() -> RbtEstimator { + let mut estimator = RbtEstimator::new(EnemyId::Hero1); + estimator.state = EstimatorStateMachine::Track { jump: false }; + estimator.fire = true; + estimator.latest_tracker_snapshot = Some(stable_tracker_snapshot()); + estimator + } + #[test] fn handler_poll_feeds_only_the_selected_estimator() { let cfg = estimator_cfg(1_000); @@ -626,4 +723,70 @@ enemy_lost_wait_duration_ms = {enemy_lost_wait_duration_ms} assert!(handler_poll.selected_snapshot().is_none()); } + + #[test] + fn fire_hold_blocks_stable_tracker_observation() { + let mut estimator = estimator_with_stable_snapshot(); + + assert!(estimator.snapshot().unwrap().observation_stable); + + estimator.fire_observation_hold_frames = 1; + + assert!(!estimator.snapshot().unwrap().observation_stable); + } + + #[test] + fn armor_jump_starts_and_releases_fire_hold_for_configured_frames() { + let cfg = estimator_cfg(1_000); + let mut estimator = RbtEstimator::new(EnemyId::Hero1); + let single = Some(solved_enemy(320.0, 192.0)); + let double = Some(solved_enemy_with_armors(&[(320.0, 192.0), (350.0, 192.0)])); + + estimator.update(&cfg, &single); + estimator.update(&cfg, &single); + assert_eq!(estimator.fire_observation_hold_frames, 0); + + estimator.update(&cfg, &double); + assert_eq!(estimator.fire_observation_hold_frames, 3); + + estimator.update(&cfg, &single); + assert_eq!(estimator.fire_observation_hold_frames, 2); + estimator.update(&cfg, &single); + assert_eq!(estimator.fire_observation_hold_frames, 1); + estimator.update(&cfg, &single); + assert_eq!(estimator.fire_observation_hold_frames, 0); + } + + #[test] + fn armor_jump_fire_block_can_be_disabled() { + let mut cfg = estimator_cfg(1_000); + cfg.fire_block_on_armor_jump = false; + let mut estimator = RbtEstimator::new(EnemyId::Hero1); + let single = Some(solved_enemy(320.0, 192.0)); + let double = Some(solved_enemy_with_armors(&[(320.0, 192.0), (350.0, 192.0)])); + + estimator.update(&cfg, &single); + estimator.update(&cfg, &single); + estimator.update(&cfg, &double); + + assert_eq!(estimator.fire_observation_hold_frames, 0); + } + + #[test] + fn no_target_resets_armor_jump_fire_hold() { + let cfg = estimator_cfg(1_000); + let mut estimator = RbtEstimator::new(EnemyId::Hero1); + let single = Some(solved_enemy(320.0, 192.0)); + let double = Some(solved_enemy_with_armors(&[(320.0, 192.0), (350.0, 192.0)])); + + estimator.update(&cfg, &single); + estimator.update(&cfg, &single); + estimator.update(&cfg, &double); + assert!(estimator.fire_observation_hold_frames > 0); + + estimator.update(&cfg, &None); + + assert_eq!(estimator.fire_observation_hold_frames, 0); + assert!(!estimator.previous_multi_armor_observation); + } } diff --git a/lib/src/rbt_mod/rbt_fire_control/controller.rs b/lib/src/rbt_mod/rbt_fire_control/controller.rs index abf98d5..3950032 100644 --- a/lib/src/rbt_mod/rbt_fire_control/controller.rs +++ b/lib/src/rbt_mod/rbt_fire_control/controller.rs @@ -684,4 +684,18 @@ mod tests { assert_eq!(stale_feedback.shot_mode, ShotMode::AimOnly); assert!(!controller.last_stats().gate_mcu); } + + #[test] + fn unstable_observation_blocks_fire_but_keeps_aiming() { + let mut controller = FireControlController::new().unwrap(); + let mut target = target(TargetMotionState::Static); + target.observation_stable = false; + + let control = controller.update(input(Some(target), true, true)); + let stats = controller.last_stats(); + + assert_eq!(control.shot_mode, ShotMode::AimOnly); + assert!(stats.gate_mcu); + assert!(!stats.gate_observation); + } }