fix: block fire briefly after armor jump

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

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 预瞄生成 |

docs/系统架构与发控主线.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`。它只告诉发控：目标是否有效，运动是否稳定，观测是否可信。
 
 ## 发控模块拆分

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
 }

lib/src/rbt_mod/rbt_estimator.rs

@@ -179,6 +179,8 @@ pub struct RbtEstimator {
     ypd_angle_tracker: YpdAngleTracker,
     latest_tracker_snapshot: Option<YpdTrackerSnapshot>,
     last_update_tp: Option<Instant>,
+    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<EnemyTrackSnapshot> {
@@ -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);
+    }
 }

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);
+    }
 }