100 lines
4.1 KiB
Rust
100 lines
4.1 KiB
Rust
//! 语义一致性：同步评估 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}"
|
|
);
|
|
}
|