feat: 初始化

2026-09-22 20:05:16 +08:00
commit 7d87987e41
54 changed files with 6446 additions and 0 deletions
@@ -0,0 +1,169 @@
+//! 守护进程端到端测试：`--mock` 同构栈（配置 → EscBus → 通道 → 仿真电机
+//! → 控制循环 → 麦轮运动学），全平台可跑。
+
+#![allow(clippy::unwrap_used, clippy::expect_used)]
+
+use std::time::Duration;
+
+use chassisd::config::Config;
+use chassisd::controller::{ChassisController, wrap_wheels};
+use dji_esc::{CanLink, EscBus, EscBusHandle, spawn_plant};
+use mecanum::Twist;
+use motor_core::MotorFeedback;
+use tokio::sync::watch;
+use tokio::time::sleep;
+
+/// 搭建与 `chassisd --mock` 完全同构的软件栈。
+///
+/// `command_timeout_ms` 由调用方指定：稳态测试用大值（模拟持续刷新的
+/// 指令源），超时测试用配置原值。
+fn stack(config: &Config, command_timeout_ms: u64) -> (EscBusHandle, watch::Sender<Twist>) {
+    let (bus_side, plant_side) = CanLink::pair(512);
+    let esc_ids: Vec<u8> = config.motors.iter().map(|m| m.esc_id).collect();
+    spawn_plant(plant_side, &esc_ids, config.bus_options().period);
+
+    let bus = EscBus::spawn(bus_side, config.to_esc_configs(), config.bus_options()).unwrap();
+
+    let (twist_tx, twist_rx) = watch::channel(Twist::ZERO);
+    let motors: [dji_esc::EscMotor; 4] =
+        bus.motors().to_vec().try_into().unwrap_or_else(|_| unreachable!("exactly 4 motors"));
+    let gears: [f32; 4] = config
+        .motors
+        .iter()
+        .map(|m| m.gear_ratio)
+        .collect::<Vec<_>>()
+        .try_into()
+        .unwrap_or_else(|_| unreachable!("exactly 4 motors"));
+    let controller = ChassisController::new(
+        config.kinematics(),
+        wrap_wheels(motors, gears),
+        (config.chassis.max_vx, config.chassis.max_vy, config.chassis.max_omega),
+        Duration::from_millis(command_timeout_ms),
+    );
+    controller.spawn(twist_rx, config.bus_options().period);
+    (bus, twist_tx)
+}
+
+async fn wait_until(total_ms: u64, mut cond: impl FnMut() -> bool) -> bool {
+    let mut waited = 0u64;
+    while waited < total_ms {
+        if cond() {
+            return true;
+        }
+        sleep(Duration::from_millis(5)).await;
+        waited += 5;
+    }
+    cond()
+}
+
+fn wheel_rpms(bus: &EscBusHandle) -> [f32; 4] {
+    bus.motors()
+        .iter()
+        .map(|m| m.state().rotor_speed_rpm / m.config().gear_ratio)
+        .collect::<Vec<_>>()
+        .try_into()
+        .unwrap()
+}
+
+/// mock 默认底盘（r = 0.0762 m）上 1 m/s 对应的轮转速。
+const RPM_PER_MPS: f32 = 60.0 / (std::f32::consts::TAU * 0.0762);
+
+#[tokio::test(start_paused = true)]
+async fn forward_command_drives_all_wheels_equally() {
+    let config = Config::mock_default();
+    let (bus, twist_tx) = stack(&config, 10_000);
+
+    twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
+    let target = RPM_PER_MPS;
+    let ok = wait_until(3000, || {
+        wheel_rpms(&bus).iter().all(|&rpm| (rpm - target).abs() < 0.05 * target)
+    })
+    .await;
+    let rpm = wheel_rpms(&bus);
+    assert!(ok, "未收敛到 {target} rpm: {rpm:?}");
+
+    // 停车：指令归零后各轮减速到停。
+    twist_tx.send_replace(Twist::ZERO);
+    let stopped = wait_until(2000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await;
+    let rpm = wheel_rpms(&bus);
+    assert!(stopped, "未停车: {rpm:?}");
+    bus.shutdown().await;
+}
+
+#[tokio::test(start_paused = true)]
+async fn rotation_command_respects_wheel_signs() {
+    let config = Config::mock_default();
+    let (bus, twist_tx) = stack(&config, 10_000);
+
+    twist_tx.send_replace(Twist { omega: 1.0, ..Twist::ZERO });
+    // a·ω = (0.2 + 0.25) m/s 的轮线速度。
+    let target = 0.45 * RPM_PER_MPS;
+    let ok = wait_until(3000, || {
+        let rpm = wheel_rpms(&bus);
+        // 逆时针：左侧（FL、RL）为负，右侧（FR、RR）为正。
+        [
+            (rpm[0] + target).abs(),
+            (rpm[1] - target).abs(),
+            (rpm[2] + target).abs(),
+            (rpm[3] - target).abs(),
+        ]
+        .iter()
+        .all(|e| *e < 0.05 * target)
+    })
+    .await;
+    let rpm = wheel_rpms(&bus);
+    assert!(ok, "旋转轮序符号错误, 期望 ±{target}: {rpm:?}");
+    bus.shutdown().await;
+}
+
+#[tokio::test(start_paused = true)]
+async fn command_timeout_stops_chassis() {
+    let config = Config::mock_default();
+    // 指令超时使用配置原值（200ms）。
+    let (bus, twist_tx) = stack(&config, config.chassis.command_timeout_ms);
+
+    // 只发一次指令，随后沉默 → 超过 command_timeout（200ms）后应停车。
+    twist_tx.send_replace(Twist { vx: 1.0, ..Twist::ZERO });
+    let moving =
+        wait_until(1000, || wheel_rpms(&bus).iter().any(|rpm| rpm.abs() > 0.2 * RPM_PER_MPS)).await;
+    assert!(moving, "指令后底盘应当运动");
+
+    sleep(Duration::from_millis(400)).await; // 超过 200ms 超时
+    let stopped = wait_until(1000, || wheel_rpms(&bus).iter().all(|rpm| rpm.abs() < 15.0)).await;
+    let rpm = wheel_rpms(&bus);
+    assert!(stopped, "指令超时后未停车: {rpm:?}");
+    bus.shutdown().await;
+}
+
+#[tokio::test(start_paused = true)]
+async fn speed_limits_scale_motion() {
+    let mut config = Config::mock_default();
+    config.chassis.max_vx = 1.0; // 限速 1 m/s
+    let (bus, twist_tx) = stack(&config, 10_000);
+
+    twist_tx.send_replace(Twist {
+        vx: 3.0, // 超限指令
+        vy: 2.0,
+        omega: 0.0,
+    });
+    // 限幅按比例收缩：(3, 2) → (1, 2/3)。四轮 = vx ∓ vy：
+    // FL/RR = (1 - 2/3) m/s，FR/RL = (1 + 2/3) m/s。
+    let diag_slow = (1.0 - 2.0 / 3.0) * RPM_PER_MPS;
+    let diag_fast = (1.0 + 2.0 / 3.0) * RPM_PER_MPS;
+    let tol = 0.08 * diag_fast;
+    let ok = wait_until(3000, || {
+        let rpm = wheel_rpms(&bus);
+        [
+            (rpm[0] - diag_slow).abs(),
+            (rpm[1] - diag_fast).abs(),
+            (rpm[2] - diag_fast).abs(),
+            (rpm[3] - diag_slow).abs(),
+        ]
+        .iter()
+        .all(|e| *e < tol)
+    })
+    .await;
+    let rpm = wheel_rpms(&bus);
+    assert!(ok, "限幅后未收敛到 [{diag_slow}, {diag_fast}, {diag_fast}, {diag_slow}] rpm: {rpm:?}");
+    bus.shutdown().await;
+}