feat: add armor ballistic pitch control

cfg/rbt_cfg.toml

@@ -47,3 +47,12 @@ 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
+ypd_geometry_recovery_window_frames = 24
+ypd_geometry_recovery_cooldown_frames = 12
+ypd_geometry_recovery_mismatch_required_streak = 2
+ypd_geometry_recovery_min_matched_count = 2
+ypd_geometry_recovery_z_sigma_threshold = 3.0
+ypd_geometry_recovery_xy_sigma_threshold = 2.0
+ypd_geometry_recovery_cov_inflation_scale = 48.0
+ypd_geometry_recovery_min_dr_variance = 0.0025
+ypd_geometry_recovery_min_h_variance = 0.000625

docs/vivsionn-gap-checklist.md

@@ -5,7 +5,7 @@ This checklist tracks the functional gaps found while comparing this Rust worksp
 | feature | target repository | current repository | notes |
 |---|---|---|---|
 | [x] P0 true CAN TX/RX loop | `Serial + SocketCAN` reads `0x203/0x204` feedback and sends `0x100` control frames | SocketCAN runtime task opens `can0`, pairs feedback frames, feeds `SensData`, and sends serialized `CtrlData` | Implemented in `rbt_comm_device` and wired into `auto_aim_async` |
-| [ ] P0 armor pitch ballistic control | Armor fire control computes gravity-compensated pitch and sends it with yaw | Armor route still needs target pitch output instead of holding feedback pitch | Required for full 3D armor aiming |
-| [ ] P0 YPD geometry recovery | Tracker recovers after armor jump/mismatch by gating windows and inflating geometry covariance | Rust tracker still needs online geometry recovery logic | Required for robust reacquisition |
-| [ ] P0 outpost specialization | Outpost path has height phase lock, radius prior, and outpost yaw recovery | Rust outpost handling still needs target-specific recovery and yaw logic | Required for stable outpost mode |
-| [ ] P0 energy mechanism R center / switch gate | Buff detector corrects R center, gates target switching, and has contour/template fallback | Rust energy mechanism decode still needs R center correction and switching gates | Required for stable energy mechanism detection |
+| [x] P0 armor pitch ballistic control | Armor fire control computes gravity-compensated pitch and sends it with yaw | Armor route now computes ballistic pitch from planner target position and sends it with yaw | Implemented in armor fire-control controller |
+| [x] P0 YPD geometry recovery | Tracker recovers after armor jump/mismatch by gating windows and inflating geometry covariance | Rust tracker now opens a recovery window after multi-armor observations and inflates `dr/h` covariance after consecutive geometry mismatches | Implemented in YPD tracker with configurable thresholds |
+| [x] P0 outpost specialization | Outpost path has height phase lock, radius prior, and outpost yaw recovery | Rust tracker now converts outpost observed/radial yaw, locks height phase, freezes locked height offsets, applies radius prior, and gates rejected updates | Implemented in YPD tracker with outpost-specific tests |
+| [x] P0 energy mechanism R center / switch gate | Buff detector corrects R center, gates target switching, and has contour/template fallback | Rust solve stage now corrects inconsistent R center geometry and tracker defers/rebinds large-buff target switches with phase state | Implemented minimal R-center correction and switch gate without adding image ROI/template dependencies |

lib/src/rbt_infra/rbt_cfg.rs

@@ -136,10 +136,64 @@ impl CamCfg {
 pub struct EstimatorCfg {
     armor_lost_wait_duration_ms: u64,
     enemy_lost_wait_duration_ms: u64,
+    #[serde(default = "default_ypd_geometry_recovery_window_frames")]
+    pub ypd_geometry_recovery_window_frames: usize,
+    #[serde(default = "default_ypd_geometry_recovery_cooldown_frames")]
+    pub ypd_geometry_recovery_cooldown_frames: usize,
+    #[serde(default = "default_ypd_geometry_recovery_mismatch_required_streak")]
+    pub ypd_geometry_recovery_mismatch_required_streak: usize,
+    #[serde(default = "default_ypd_geometry_recovery_min_matched_count")]
+    pub ypd_geometry_recovery_min_matched_count: usize,
+    #[serde(default = "default_ypd_geometry_recovery_z_sigma_threshold")]
+    pub ypd_geometry_recovery_z_sigma_threshold: f64,
+    #[serde(default = "default_ypd_geometry_recovery_xy_sigma_threshold")]
+    pub ypd_geometry_recovery_xy_sigma_threshold: f64,
+    #[serde(default = "default_ypd_geometry_recovery_cov_inflation_scale")]
+    pub ypd_geometry_recovery_cov_inflation_scale: f64,
+    #[serde(default = "default_ypd_geometry_recovery_min_dr_variance")]
+    pub ypd_geometry_recovery_min_dr_variance: f64,
+    #[serde(default = "default_ypd_geometry_recovery_min_h_variance")]
+    pub ypd_geometry_recovery_min_h_variance: f64,
     // top1_activate_w: f64,
     // top2_activate_w: f64,
 }
 
+fn default_ypd_geometry_recovery_window_frames() -> usize {
+    24
+}
+
+fn default_ypd_geometry_recovery_cooldown_frames() -> usize {
+    12
+}
+
+fn default_ypd_geometry_recovery_mismatch_required_streak() -> usize {
+    2
+}
+
+fn default_ypd_geometry_recovery_min_matched_count() -> usize {
+    2
+}
+
+fn default_ypd_geometry_recovery_z_sigma_threshold() -> f64 {
+    3.0
+}
+
+fn default_ypd_geometry_recovery_xy_sigma_threshold() -> f64 {
+    2.0
+}
+
+fn default_ypd_geometry_recovery_cov_inflation_scale() -> f64 {
+    48.0
+}
+
+fn default_ypd_geometry_recovery_min_dr_variance() -> f64 {
+    2.5e-3
+}
+
+fn default_ypd_geometry_recovery_min_h_variance() -> f64 {
+    6.25e-4
+}
+
 impl EstimatorCfg {
     #[inline(always)]
     pub fn lost_wait_duration_ms(&self) -> tokio::time::Duration {

lib/src/rbt_mod/rbt_energy_mechanism/fire_control.rs

@@ -234,6 +234,10 @@ mod tests {
             lost: false,
             track_valid: true,
             state_age_s: 0.0,
+            switch_deferred: false,
+            target_switched: false,
+            selected_phase_index: Some(0),
+            selected_roll_offset_rad: Some(0.0),
         };
 
         let control = controller.update(EnergyMechanismControlInput {

lib/src/rbt_mod/rbt_energy_mechanism/solved.rs

@@ -2,12 +2,15 @@ use crate::rbt_infra::rbt_err::{RbtError, RbtResult};
 
 use super::detected::{EnergyMechanismMode, EnergyMechanismObject};
 
+const R_CENTER_KEYPOINT_INDEX: usize = 4;
 const MODEL_TOP_M: na::Point3<f64> = na::Point3::new(0.0, 0.0, 0.827);
 const MODEL_LEFT_M: na::Point3<f64> = na::Point3::new(0.0, -0.127, 0.700);
 const MODEL_BOTTOM_M: na::Point3<f64> = na::Point3::new(0.0, 0.0, 0.573);
 const MODEL_RIGHT_M: na::Point3<f64> = na::Point3::new(0.0, 0.127, 0.700);
 const MODEL_BLADE_CENTER_M: na::Point3<f64> = na::Point3::new(0.0, 0.0, 0.700);
 const MODEL_R_CENTER_M: na::Point3<f64> = na::Point3::new(0.0, 0.0, 0.0);
+const MODEL_BLADE_RADIUS_M: f32 = 0.700;
+const R_CENTER_GEOMETRY_MAX_ERROR_RATIO: f32 = 0.35;
 
 #[derive(Debug, Clone, PartialEq)]
 pub struct EnergyMechanismPose {
@@ -25,9 +28,13 @@ pub struct EnergyMechanismSolvedTarget {
     pub pose: EnergyMechanismPose,
     pub image_r_center: na::Point2<f32>,
     pub image_target_center: na::Point2<f32>,
+    pub image_r_center_corrected: bool,
     pub confidence: f32,
     pub selected_phase_index: usize,
     pub observed_roll_rad: f64,
+    pub switch_deferred: bool,
+    pub target_switched: bool,
+    pub selected_roll_offset_rad: Option<f64>,
 }
 
 #[derive(Debug, Clone, PartialEq)]
@@ -43,18 +50,30 @@ pub fn solve_energy_mechanism(
     cam_k: &na::Matrix3<f64>,
 ) -> RbtResult<EnergyMechanismSolvedFrame> {
     let mut best = None;
-    for (index, candidate) in candidates.iter().enumerate() {
-        let Some(pose) = solve_candidate_pose(candidate, cam_k) else {
+    let mut corrected_candidates = Vec::with_capacity(candidates.len());
+    for candidate in &candidates {
+        let corrected = correct_candidate_r_center(candidate);
+        let Some(pose) = solve_candidate_pose(&corrected.object, cam_k) else {
+            corrected_candidates.push(corrected.object);
             continue;
         };
+        let observed_roll_rad = observed_roll(&corrected.object);
+        let selected_phase_index = phase_index_from_roll(observed_roll_rad);
         let target = EnergyMechanismSolvedTarget {
             mode,
-            observed_roll_rad: observed_roll(candidate),
+            observed_roll_rad,
             pose,
-            image_r_center: candidate.r_center(),
-            image_target_center: candidate.target_center(),
+            image_r_center: corrected.object.r_center(),
+            image_target_center: corrected.object.target_center(),
+            image_r_center_corrected: corrected.corrected,
             confidence: candidate.confidence,
-            selected_phase_index: index,
+            selected_phase_index,
+            switch_deferred: false,
+            target_switched: false,
+            selected_roll_offset_rad: Some(phase_offset_rad(
+                observed_roll_rad,
+                selected_phase_index,
+            )),
         };
         if best
             .as_ref()
@@ -65,15 +84,67 @@ pub fn solve_energy_mechanism(
         {
             best = Some(target);
         }
+        corrected_candidates.push(corrected.object);
     }
 
     Ok(EnergyMechanismSolvedFrame {
         mode,
         target: best,
-        candidates,
+        candidates: corrected_candidates,
     })
 }
 
+#[derive(Debug, Clone)]
+struct CorrectedCandidate {
+    object: EnergyMechanismObject,
+    corrected: bool,
+}
+
+fn correct_candidate_r_center(candidate: &EnergyMechanismObject) -> CorrectedCandidate {
+    let target = candidate.target_center();
+    let observed_r = candidate.r_center();
+    let geometry_r = geometry_r_center(candidate);
+    let observed_radius = (observed_r - target).norm();
+    let geometry_radius = geometry_radius(candidate);
+    let corrected = observed_radius <= 1e-3
+        || (observed_radius - geometry_radius).abs()
+            > geometry_radius * R_CENTER_GEOMETRY_MAX_ERROR_RATIO;
+    if corrected {
+        let mut object = candidate.clone();
+        object.keypoints[R_CENTER_KEYPOINT_INDEX] = geometry_r;
+        CorrectedCandidate { object, corrected }
+    } else {
+        CorrectedCandidate {
+            object: candidate.clone(),
+            corrected,
+        }
+    }
+}
+
+fn geometry_r_center(candidate: &EnergyMechanismObject) -> na::Point2<f32> {
+    let target = candidate.target_center();
+    let observed_r = candidate.r_center();
+    let radius = geometry_radius(candidate);
+    let direction = observed_r - target;
+    let direction = if direction.norm() > 1e-3 {
+        direction.normalize()
+    } else {
+        na::Vector2::new(0.0, -1.0)
+    };
+    target + direction * radius
+}
+
+fn geometry_radius(candidate: &EnergyMechanismObject) -> f32 {
+    let top = candidate.keypoints[0];
+    let left = candidate.keypoints[1];
+    let bottom = candidate.keypoints[2];
+    let right = candidate.keypoints[3];
+    let vertical = (top - bottom).norm();
+    let horizontal = (left - right).norm();
+    let blade_span_px = ((vertical + horizontal) * 0.5).max(1.0);
+    blade_span_px * MODEL_BLADE_RADIUS_M / 0.254
+}
+
 fn solve_candidate_pose(
     candidate: &EnergyMechanismObject,
     cam_k: &na::Matrix3<f64>,
@@ -225,6 +296,25 @@ fn observed_roll(candidate: &EnergyMechanismObject) -> f64 {
     (target.y - r_center.y).atan2(target.x - r_center.x) as f64
 }
 
+fn phase_index_from_roll(roll_rad: f64) -> usize {
+    let normalized = roll_rad.rem_euclid(std::f64::consts::TAU);
+    ((normalized / (std::f64::consts::TAU / 5.0)).round() as usize) % 5
+}
+
+fn phase_offset_rad(roll_rad: f64, phase_index: usize) -> f64 {
+    normalize_angle(roll_rad - phase_index as f64 * std::f64::consts::TAU / 5.0)
+}
+
+fn normalize_angle(mut angle: f64) -> f64 {
+    while angle > std::f64::consts::PI {
+        angle -= std::f64::consts::TAU;
+    }
+    while angle < -std::f64::consts::PI {
+        angle += std::f64::consts::TAU;
+    }
+    angle
+}
+
 fn to_f64(point: na::Point2<f32>) -> na::Point2<f64> {
     na::Point2::new(point.x as f64, point.y as f64)
 }
@@ -253,6 +343,21 @@ mod tests {
         )
     }
 
+    fn image_object_with_r_center(r_center: na::Point2<f32>) -> EnergyMechanismObject {
+        EnergyMechanismObject {
+            bbox: EnergyMechanismBBox::from_center_size(320.0, 320.0, 140.0, 140.0),
+            class: EnergyMechanismClass::RedTarget,
+            confidence: 0.9,
+            keypoints: [
+                na::Point2::new(320.0, 260.0),
+                na::Point2::new(290.0, 320.0),
+                na::Point2::new(320.0, 380.0),
+                na::Point2::new(350.0, 320.0),
+                r_center,
+            ],
+        }
+    }
+
     #[test]
     fn solves_valid_energy_mechanism_target() {
         let cam_k = camera_matrix();
@@ -303,4 +408,24 @@ mod tests {
 
         assert!(solved.target.is_none());
     }
+
+    #[test]
+    fn corrects_bad_r_center_to_geometry_radius() {
+        let object = image_object_with_r_center(na::Point2::new(320.0, 319.0));
+        let corrected = correct_candidate_r_center(&object);
+        let target = object.target_center();
+        let expected_radius = geometry_radius(&object);
+
+        assert!(corrected.corrected);
+        assert!(((corrected.object.r_center() - target).norm() - expected_radius).abs() < 1e-3);
+    }
+
+    #[test]
+    fn keeps_consistent_r_center_without_correction() {
+        let object = image_object_with_r_center(na::Point2::new(320.0, 650.0));
+        let corrected = correct_candidate_r_center(&object);
+
+        assert!(!corrected.corrected);
+        assert_eq!(corrected.object.r_center(), object.r_center());
+    }
 }