feat: wire SocketCAN runtime loop

AGENTS.md

@@ -53,5 +53,6 @@
   - `cargo clippy --locked --workspace --all-targets -- -D warnings`
   - `cargo test --locked --workspace --all-targets`
 - CI 使用 `.github/workflows/ci.yml` 中同一套命令；本地结论应以这些命令为准。
+- 每次提交前，先检查 `.github/workflows/ci.yml` 当前声明的验证项目，并逐项确认已在本地运行或说明无法本地运行的原因。
 - 改模型 contract、decode、PnP、tracker、route 或发控时，需要补对应单元测试。
 - 改 `app/auto_aim_async` 主线时，确认 Armour route 不回归，并验证 EnergyMechanism route 的状态切换、队列清理和控制输出路径。

app/auto_aim_async/src/main.rs

@@ -3,17 +3,18 @@ extern crate rerun as rr;
 
 use crate::rbt_threads::{
     ArmorPipelineQueues, EnergyMechanismPipelineQueues, EnergyMechanismTrackPacket,
-    PlannerTrackSnapshot, RuntimePipelineCompletion, RuntimePipelineQueues, control_loop_250hz,
-    energy_mechanism_estimate_process, energy_mechanism_infer, energy_mechanism_post_process,
-    estimate_process, infer, post_process, pre_process, video_input_path,
+    PlannerTrackSnapshot, RuntimePipelineCompletion, RuntimePipelineQueues, can_io_process,
+    control_loop_250hz, energy_mechanism_estimate_process, energy_mechanism_infer,
+    energy_mechanism_post_process, estimate_process, infer, post_process, pre_process,
+    video_input_path,
 };
 use auto_aim_rust::rbt_infra::rbt_log;
 use lib as auto_aim_rust;
 use lib::rbt_infra::rbt_err::{RbtError, RbtResult};
 use lib::rbt_infra::rbt_global::GENERIC_RBT_CFG;
 use lib::rbt_infra::rbt_ort_ep::configure_session_builder;
 use lib::rbt_infra::rbt_queue_async::RbtSPSCQueueAsync;
-use lib::rbt_mod::rbt_comm::rbt_comm_frame::SensData;
+use lib::rbt_mod::rbt_comm::rbt_comm_frame::{CtrlData, SensData};
 use lib::rbt_mod::rbt_detector::rbt_frame::RbtFrame;
 use lib::rbt_mod::rbt_energy_mechanism::{EnergyMechanismFrame, EnergyMechanismSolvedFrame};
 use lib::rbt_mod::rbt_runtime_router::RuntimeRouter;
@@ -64,6 +65,7 @@ async fn main() -> RbtResult<()> {
     let energy_solved_queue = Arc::new(RbtSPSCQueueAsync::<EnergyMechanismSolvedFrame>::new(1));
     let energy_track_queue = Arc::new(RbtSPSCQueueAsync::<EnergyMechanismTrackPacket>::new(1));
     let feedback_queue = Arc::new(RbtSPSCQueueAsync::<SensData>::new(1));
+    let control_tx_queue = Arc::new(RbtSPSCQueueAsync::<CtrlData>::new(1));
     let armor_queues = ArmorPipelineQueues::new(
         pre_infer_queue.clone(),
         infer_post_queue.clone(),
@@ -170,22 +172,26 @@ async fn main() -> RbtResult<()> {
     let control_task_handler = control_loop_250hz(
         track_queue.clone(),
         energy_track_queue.clone(),
-        feedback_queue,
+        feedback_queue.clone(),
+        control_tx_queue.clone(),
         runtime_router,
         runtime_queues,
-        runtime_completion,
+        runtime_completion.clone(),
     );
+    let can_task_handler =
+        can_io_process(control_tx_queue, feedback_queue, cfg, runtime_completion);
 
     let tim = std::time::Instant::now();
-    let (_, _, _, _, _, _, _, _) = tokio::join!(
+    let (_, _, _, _, _, _, _, _, _) = tokio::join!(
         pre_task_handler,
         infer_task_handler,
         post_task_handler,
         energy_infer_task_handler,
         energy_post_task_handler,
         energy_estimate_task_handler,
         estimate_task_handler,
-        control_task_handler
+        control_task_handler,
+        can_task_handler
     );
     tokio::time::sleep(tokio::time::Duration::from_millis(100)).await; // wait for post process to finish
     info!("multi_thread_pipeline finished in {:?}", tim.elapsed());

app/auto_aim_async/src/rbt_threads.rs

@@ -30,6 +30,9 @@ use lib::{
         rbt_queue_async::RbtSPSCQueueAsync,
     },
     rbt_mod::{
+        rbt_comm::rbt_comm_device::rbt_can::{
+            FeedbackPairDecoder, SocketCanDevice, control_to_can_payload,
+        },
         rbt_comm::rbt_comm_frame::{
             AimingState, CAN_FRAME_SIZE, CONTROL_LOOP_PERIOD_MS, CtrlData,
             DEFAULT_BULLET_SPEED_MPS, FEEDBACK_STALE_TIMEOUT_MS, SelfFraction, SensData,
@@ -53,6 +56,7 @@ const DEFAULT_VIDEO_FILE: &str = "offline_capture_bundle_outpost_rot180.avi";
 const RAW_RGB_CHANNELS: usize = 3;
 const FIRE_CONTROL_SNAPSHOT_STALE_MS: f64 = 180.0;
 const CONTROL_STATUS_LOG_PERIOD_TICKS: u64 = 50;
+const CAN_IO_POP_TIMEOUT_MS: u64 = 20;
 const PIPELINE_POP_TIMEOUT_MS: u64 = 100;
 const RERUN_FILTER_RAW_CENTER_COLOR: u32 = 0xFFBE14FF;
 const RERUN_FILTER_RAW_ARMOR_COLOR: u32 = 0xFF5014FF;
@@ -1302,6 +1306,7 @@ pub fn control_loop_250hz(
     track_queue: Arc<RbtSPSCQueueAsync<PlannerTrackSnapshot>>,
     energy_track_queue: Arc<RbtSPSCQueueAsync<EnergyMechanismTrackPacket>>,
     feedback_queue: Arc<RbtSPSCQueueAsync<SensData>>,
+    control_tx_queue: Arc<RbtSPSCQueueAsync<CtrlData>>,
     runtime_router: RuntimeRouter,
     runtime_queues: RuntimePipelineQueues,
     completion: RuntimePipelineCompletion,
@@ -1393,6 +1398,7 @@ pub fn control_loop_250hz(
                 if let Err(err) = control_data.serialize_with_seq(frame_seq, &mut payload) {
                     warn!("control_loop_250hz: failed to serialize energy mechanism frame: {err}");
                 }
+                control_tx_queue.push_latest(control_data);
                 tick_count = tick_count.wrapping_add(1);
                 frame_seq = frame_seq.wrapping_add(1);
                 if tick_count == 1 || tick_count.is_multiple_of(CONTROL_STATUS_LOG_PERIOD_TICKS) {
@@ -1423,6 +1429,7 @@ pub fn control_loop_250hz(
                 if let Err(err) = control_data.serialize_with_seq(frame_seq, &mut payload) {
                     warn!("control_loop_250hz: failed to serialize disabled-route frame: {err}");
                 }
+                control_tx_queue.push_latest(control_data);
                 tick_count = tick_count.wrapping_add(1);
                 frame_seq = frame_seq.wrapping_add(1);
                 if tick_count == 1 || tick_count.is_multiple_of(CONTROL_STATUS_LOG_PERIOD_TICKS) {
@@ -1453,6 +1460,7 @@ pub fn control_loop_250hz(
             if let Err(err) = control_data.serialize_with_seq(frame_seq, &mut payload) {
                 warn!("control_loop_250hz: failed to serialize control frame: {err}");
             }
+            control_tx_queue.push_latest(control_data);
 
             tick_count = tick_count.wrapping_add(1);
             frame_seq = frame_seq.wrapping_add(1);
@@ -1500,6 +1508,90 @@ pub fn control_loop_250hz(
     })
 }
 
+pub fn can_io_process(
+    control_tx_queue: Arc<RbtSPSCQueueAsync<CtrlData>>,
+    feedback_queue: Arc<RbtSPSCQueueAsync<SensData>>,
+    cfg: RbtCfg,
+    completion: RuntimePipelineCompletion,
+) -> JoinHandle<()> {
+    tokio::spawn(async move {
+        if !cfg.general_cfg.can_enabled {
+            info!("can_io_process: CAN disabled by config");
+            return;
+        }
+
+        let interface = cfg.general_cfg.can_interface.clone();
+        let device = match SocketCanDevice::open(&interface) {
+            Ok(device) => device,
+            Err(err) => {
+                error!("can_io_process: failed to open SocketCAN interface {interface}: {err}");
+                IS_RUNNING.store(false, Ordering::SeqCst);
+                return;
+            }
+        };
+        info!("can_io_process: opened SocketCAN interface {interface}");
+
+        let mut feedback_decoder = FeedbackPairDecoder::new();
+        let mut frame_seq = 0_u8;
+        let mut rx_count = 0_u64;
+        let mut tx_count = 0_u64;
+
+        loop {
+            if completion.armor_estimate_done()
+                && completion.energy_estimate_done()
+                && control_tx_queue.is_empty()
+            {
+                info!(
+                    "can_io_process: stopping after tx={} rx={}",
+                    tx_count, rx_count
+                );
+                break;
+            }
+
+            tokio::select! {
+                frame = device.receive() => {
+                    match frame {
+                        Ok(frame) => {
+                            match feedback_decoder.push(frame, StdInstant::now()) {
+                                Ok(Some(feedback)) => {
+                                    rx_count = rx_count.wrapping_add(1);
+                                    feedback_queue.push_latest(feedback);
+                                }
+                                Ok(None) => {}
+                                Err(err) => warn!("can_io_process: dropped corrupted feedback pair: {err}"),
+                            }
+                        }
+                        Err(err) => {
+                            error!("can_io_process: CAN receive failed: {err}");
+                            IS_RUNNING.store(false, Ordering::SeqCst);
+                            break;
+                        }
+                    }
+                }
+                control = pop_latest_until_running(
+                    &control_tx_queue,
+                    Duration::from_millis(CAN_IO_POP_TIMEOUT_MS),
+                ) => {
+                    if let Some(control) = control {
+                        match control_to_can_payload(control, frame_seq) {
+                            Ok(payload) => {
+                                if let Err(err) = device.send(payload).await {
+                                    error!("can_io_process: CAN send failed: {err}");
+                                    IS_RUNNING.store(false, Ordering::SeqCst);
+                                    break;
+                                }
+                                tx_count = tx_count.wrapping_add(1);
+                                frame_seq = frame_seq.wrapping_add(1);
+                            }
+                            Err(err) => warn!("can_io_process: failed to serialize control frame: {err}"),
+                        }
+                    }
+                }
+            }
+        }
+    })
+}
+
 fn format_status_value(value: f64) -> String {
     if value.is_finite() {
         format!("{value:.2}")

cfg/rbt_cfg.toml

@@ -16,6 +16,8 @@ file_log_enable = true
 [general_cfg]
 img_dbg = false
 bullet_speed = 24.0
+can_interface = "can0"
+can_enabled = true
 
 [detector_cfg]
 # 神经网络模型分别放在 model/armor 和 model/engine_mechanism 目录下
@@ -45,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

@@ -0,0 +1,11 @@
+# vivsionn Gap Checklist
+
+This checklist tracks the functional gaps found while comparing this Rust workspace with `/Users/flamingo/Projects/robomaster/vivsionn`.
+
+| 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` |
+| [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 |