feat: 添加了自动超参数整定

2026-09-23 00:42:26 +08:00
parent 7d87987e41
commit 680adae339
56 changed files with 13664 additions and 245 deletions
@@ -0,0 +1,99 @@
+//! 语义一致性：同步评估 harness 与异步 `EscBus` 闭环必须给出同一个答案。
+//!
+//! 自动整定跑在 [`dji_esc_sim::simulate`]（同步、无通道）上，而实车跑的是
+//! `EscBus` 的 1 kHz 发送循环。若两者的限幅 / 物理帧 / 控制器语义漂移，
+//! 整定出来的增益就只是"对仿真有效"。本测试把两条路径放在同一组参数与
+//! 同一条场景下比较稳态与 effort。
+//!
+//! 允许的差异来自**离散化细节**（异步侧每拍实测 `dt`、反馈比命令晚一拍），
+//! 因此比较的是稳态量级而不是逐点相等。
+
+#![allow(clippy::unwrap_used, clippy::expect_used)]
+
+use std::time::Duration;
+
+use dji_esc::{BusOptions, CanLink, EscBus, EscConfig};
+use dji_esc_sim::{LoopLimits, PlantParams, Scenario, simulate, spawn_plant};
+use motor_core::{MotorFeedback, VelocityDrive, VelocityGains};
+use tokio::time::sleep;
+
+/// 与 `PlantParams::gm3508` 匹配的增益（ff = 16384 / 9254）。
+const GAINS: VelocityGains = VelocityGains::new(8.0, 2.0, 0.0, 16384.0 / 9254.0);
+const TARGET_RPM: f32 = 3000.0;
+const HOLD_MS: u64 = 2000;
+/// 稳态采样窗口（虚拟时间，毫秒）。
+const SAMPLE_FROM_MS: u64 = 1500;
+
+/// 同步路径：最后 500 ms 的稳态统计。
+fn sync_steady_state() -> (f32, f32) {
+    let scenario = Scenario::step(TARGET_RPM, HOLD_MS as f32 / 1000.0);
+    let trace = simulate(GAINS, &PlantParams::gm3508(), &scenario, LoopLimits::gm3508())
+        .expect("simulation must succeed");
+    let window = trace.window(SAMPLE_FROM_MS as f32 / 1000.0, HOLD_MS as f32 / 1000.0);
+    let mean_effort =
+        window.effort_raw().iter().map(|&raw| f32::from(raw)).sum::<f32>() / window.len() as f32;
+    (window.mean_rpm(), mean_effort)
+}
+
+/// 异步路径：`EscBus` + 假电调，逐拍续租直到 2 秒（虚拟时间）。
+async fn async_steady_state() -> (f32, f32) {
+    let (bus_link, plant_link) = CanLink::pair(512);
+    let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1));
+    let mut cfg = EscConfig::gm3508("front_left", 1, 1.0);
+    cfg.kp = GAINS.kp;
+    cfg.ki = GAINS.ki;
+    cfg.kd = GAINS.kd;
+    cfg.feedforward = GAINS.feedforward;
+    cfg.current_limit = LoopLimits::gm3508().current_limit_raw;
+    let bus = EscBus::spawn(bus_link, vec![cfg], BusOptions::default()).expect("bus must start");
+    let motor = bus.motor(1).expect("motor 1 exists").clone();
+
+    // 命令是租约：逐拍重发（5 ms 节拍模拟 1 kHz 控制循环）。
+    let mut waited = 0u64;
+    let mut effort_sum = 0.0f32;
+    let mut rpm_sum = 0.0f32;
+    let mut samples = 0u32;
+    while waited < HOLD_MS {
+        motor.set_velocity(TARGET_RPM);
+        if waited >= SAMPLE_FROM_MS {
+            let state = motor.state();
+            effort_sum += f32::from(state.effort_raw);
+            rpm_sum += state.rotor_speed_rpm;
+            samples += 1;
+        }
+        sleep(Duration::from_millis(5)).await;
+        waited += 5;
+    }
+
+    plant.abort();
+    bus.shutdown().await;
+    (rpm_sum / samples as f32, effort_sum / samples as f32)
+}
+
+#[tokio::test(start_paused = true)]
+async fn synchronous_harness_matches_async_closed_loop() {
+    let (sync_rpm, sync_effort) = sync_steady_state();
+    let (async_rpm, async_effort) = async_steady_state().await;
+
+    // 稳态转速：两条路径都应收敛到目标附近（理想模型 + 精确前馈 → 稳态无差）。
+    let rpm_tolerance = 0.05 * TARGET_RPM;
+    assert!(
+        (sync_rpm - async_rpm).abs() < rpm_tolerance,
+        "steady rpm mismatch: sync {sync_rpm}, async {async_rpm}"
+    );
+    assert!(
+        (sync_rpm - TARGET_RPM).abs() < rpm_tolerance,
+        "sync steady state off target: {sync_rpm}"
+    );
+    assert!(
+        (async_rpm - TARGET_RPM).abs() < rpm_tolerance,
+        "async steady state off target: {async_rpm}"
+    );
+
+    // effort：前馈主导，两条路径量级必须接近（±20% 或 ±300 raw 取大者）。
+    let effort_tolerance = (0.2 * sync_effort.abs()).max(300.0);
+    assert!(
+        (sync_effort - async_effort).abs() < effort_tolerance,
+        "steady effort mismatch: sync {sync_effort}, async {async_effort}"
+    );
+}