refactor: enforce layered crates and Rust audit gates
This commit is contained in:
Horldsence
@@ -0,0 +1,431 @@
|
||||
//! 闭环集成测试：`EscBus` + 内存通道 + 一阶电机仿真。
|
||||
//!
|
||||
//! 全部可在任意平台（macOS / Linux / CI）运行，无需硬件。
|
||||
|
||||
#![allow(clippy::unwrap_used, clippy::expect_used)]
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use can::{CanFrame, CanLink};
|
||||
use esc_bus::{BusOptions, EscBus, EscBusError, EscConfig};
|
||||
use motor::{Disable, EffortDrive, MotorFeedback, VelocityDrive};
|
||||
use motor_control::{MotorPolicy, VelocityGains};
|
||||
use motor_protocol::{EscFeedback, encode_feedback_frame};
|
||||
use sim_harness::spawn_plant;
|
||||
use tokio::sync::mpsc;
|
||||
use tokio::task::JoinHandle;
|
||||
use tokio::time::{MissedTickBehavior, interval, sleep, timeout};
|
||||
|
||||
/// 与 `PlantParams::gm3508` 增益精确匹配的速度环参数（ff = 16384 / 9254）。
|
||||
fn tuned(esc_id: u8, direction: f32) -> EscConfig {
|
||||
EscConfig::gm3508(format!("m{esc_id}"), esc_id, direction)
|
||||
}
|
||||
|
||||
fn tuned_gains() -> VelocityGains {
|
||||
VelocityGains::new(8.0, 2.0, 0.0, 16384.0 / 9254.0)
|
||||
}
|
||||
|
||||
fn spawn_bus(
|
||||
link: CanLink,
|
||||
configs: Vec<EscConfig>,
|
||||
options: BusOptions,
|
||||
) -> Result<esc_bus::EscBusHandle<MotorPolicy>, EscBusError> {
|
||||
let mut policy = MotorPolicy::new();
|
||||
for config in &configs {
|
||||
policy.configure(config.esc_id, config.current_limit, tuned_gains());
|
||||
}
|
||||
EscBus::spawn(link, configs, options, policy)
|
||||
}
|
||||
|
||||
/// 轮询直到条件满足或超时（毫秒，仿真时间）。
|
||||
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()
|
||||
}
|
||||
|
||||
/// 与 [`wait_until`] 相同，但每次轮询都重新下发 `target_rpm`。
|
||||
///
|
||||
/// **命令是租约**：驱动在 [`BusOptions::command_timeout`]（默认 50ms）内未收到
|
||||
/// 新命令就强制 0 输出。真实底盘循环每 1ms 重发一次，这里以 5ms 节拍模拟。
|
||||
async fn hold_until<M: VelocityDrive>(
|
||||
total_ms: u64,
|
||||
motor: &M,
|
||||
target_rpm: f32,
|
||||
mut cond: impl FnMut() -> bool,
|
||||
) -> bool {
|
||||
let mut waited = 0u64;
|
||||
loop {
|
||||
motor.set_velocity(target_rpm); // 续租
|
||||
if cond() {
|
||||
return true;
|
||||
}
|
||||
if waited >= total_ms {
|
||||
return false;
|
||||
}
|
||||
sleep(Duration::from_millis(5)).await;
|
||||
waited += 5;
|
||||
}
|
||||
}
|
||||
|
||||
/// 保持 `target_rpm` 共 `ms` 毫秒（期间持续续租），仅推进仿真时间。
|
||||
async fn hold<M: VelocityDrive>(motor: &M, target_rpm: f32, ms: u64) {
|
||||
let _ = hold_until(ms, motor, target_rpm, || false).await;
|
||||
}
|
||||
|
||||
/// 手动向总线注入一帧所有电机的反馈（新鲜度 100ms 内有效）。
|
||||
async fn send_feedback(peer: &CanLink, esc_ids: &[u8]) {
|
||||
for &id in esc_ids {
|
||||
let fb = EscFeedback { angle_raw: 0, speed_rpm: 0, current_raw: 0, temperature_c: 30 };
|
||||
peer.outgoing_tx.send(encode_feedback_frame(id, &fb)).await.unwrap();
|
||||
}
|
||||
}
|
||||
|
||||
async fn recv_command(peer: &mut CanLink) -> CanFrame {
|
||||
timeout(Duration::from_millis(100), peer.incoming_rx.recv())
|
||||
.await
|
||||
.expect("timed out waiting for command frame")
|
||||
.expect("link closed")
|
||||
}
|
||||
|
||||
fn slot_current(frame: &CanFrame, slot: usize) -> i16 {
|
||||
i16::from_be_bytes([frame.data[slot * 2], frame.data[slot * 2 + 1]])
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- 收敛性
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn velocity_loop_converges() {
|
||||
let (bus_link, plant_link) = CanLink::pair(512);
|
||||
let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1));
|
||||
let bus = spawn_bus(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
|
||||
let motor = bus.motor(1).unwrap().clone();
|
||||
|
||||
motor.set_velocity(3000.0);
|
||||
let ok = hold_until(2000, &motor, 3000.0, || motor.state().rotor_speed_rpm >= 2950.0).await;
|
||||
assert!(ok, "not converged, rpm = {}", motor.state().rotor_speed_rpm);
|
||||
|
||||
// 稳态保持（速度无漂移、反馈持续流入）；期间必须继续续租命令。
|
||||
let count_before = motor.state().feedback_count;
|
||||
hold(&motor, 3000.0, 300).await;
|
||||
let st = motor.state();
|
||||
assert!((2950.0..=3050.0).contains(&st.rotor_speed_rpm), "steady state drifted: {st:?}");
|
||||
assert!(
|
||||
st.feedback_count >= count_before + 280,
|
||||
"feedback should keep flowing: {} → {}",
|
||||
count_before,
|
||||
st.feedback_count
|
||||
);
|
||||
|
||||
plant.abort();
|
||||
bus.shutdown().await;
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn reversed_motor_converges_in_physical_frame() {
|
||||
// direction = -1：物理 +2000 rpm ⇔ 电调侧 -2000 rpm。
|
||||
let (bus_link, plant_link) = CanLink::pair(512);
|
||||
let plant = spawn_plant(plant_link, &[2], Duration::from_millis(1));
|
||||
let bus = spawn_bus(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap();
|
||||
let motor = bus.motor(2).unwrap().clone();
|
||||
|
||||
motor.set_velocity(2000.0);
|
||||
let ok = hold_until(2000, &motor, 2000.0, || motor.state().rotor_speed_rpm >= 1950.0).await;
|
||||
assert!(ok, "not converged, rpm = {}", motor.state().rotor_speed_rpm);
|
||||
|
||||
let st = motor.state();
|
||||
assert!((1950.0..=2050.0).contains(&st.rotor_speed_rpm), "{st:?}");
|
||||
// 物理转速为正 ⇒ 电调电流为负 ⇒ 物理电流 = dir × esc > 0。
|
||||
assert!(st.effort_raw > 0, "{st:?}");
|
||||
|
||||
plant.abort();
|
||||
bus.shutdown().await;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- 安全
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn motor_without_feedback_commands_zero_current() {
|
||||
let (bus_link, mut peer) = CanLink::pair(64);
|
||||
// 不接 plant → 永远没有反馈。
|
||||
let bus = spawn_bus(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
|
||||
|
||||
bus.motor(1).unwrap().set_velocity(5000.0);
|
||||
for _ in 0..5 {
|
||||
let frame = recv_command(&mut peer).await;
|
||||
assert_eq!(frame.id, 0x200);
|
||||
assert_eq!(frame.data, [0; 8], "must send zero current while offline");
|
||||
}
|
||||
bus.shutdown().await;
|
||||
}
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn current_limit_clamps_saturation() {
|
||||
let (bus_link, mut peer) = CanLink::pair(64);
|
||||
let mut cfg = tuned(1, 1.0);
|
||||
cfg.current_limit = 5000;
|
||||
let bus = spawn_bus(bus_link, vec![cfg], BusOptions::default()).unwrap();
|
||||
|
||||
send_feedback(&peer, &[1]).await;
|
||||
bus.motor(1).unwrap().set_velocity(30000.0); // 不可达目标 → 深度饱和
|
||||
sleep(Duration::from_millis(5)).await;
|
||||
|
||||
let mut saw_saturated = false;
|
||||
for _ in 0..5 {
|
||||
bus.motor(1).unwrap().set_velocity(30000.0); // 续租，避免读到租约到期后的 0
|
||||
let frame = recv_command(&mut peer).await;
|
||||
let raw = slot_current(&frame, 0);
|
||||
assert!(raw <= 5000, "current {raw} exceeds limit 5000");
|
||||
saw_saturated |= raw == 5000;
|
||||
}
|
||||
assert!(saw_saturated, "loop should reach the configured limit");
|
||||
bus.shutdown().await;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- 命令租约
|
||||
|
||||
/// 反馈喂食器：每 5ms 注入一帧所有电机的反馈，让电机保持"在线"，
|
||||
/// 从而把「租约到期归零」与「离线保护归零」区分开。
|
||||
fn spawn_feedback(yield_tx: mpsc::Sender<CanFrame>, esc_ids: Vec<u8>) -> JoinHandle<()> {
|
||||
tokio::spawn(async move {
|
||||
let mut ticker = interval(Duration::from_millis(5));
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let fb = EscFeedback { angle_raw: 0, speed_rpm: 0, current_raw: 0, temperature_c: 30 };
|
||||
loop {
|
||||
ticker.tick().await;
|
||||
for &id in &esc_ids {
|
||||
if yield_tx.send(encode_feedback_frame(id, &fb)).await.is_err() {
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
})
|
||||
}
|
||||
|
||||
/// 一次性 `set_velocity` 在租约内出力，租约到期后必须自动归零；
|
||||
/// 重新下发又能恢复出力——这就是"命令是租约"契约的可执行规范。
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn command_lease_expires_and_renews() {
|
||||
let (bus_link, mut peer) = CanLink::pair(256);
|
||||
let feeder = spawn_feedback(peer.outgoing_tx.clone(), vec![1]);
|
||||
let bus = spawn_bus(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
|
||||
let motor = bus.motor(1).unwrap().clone();
|
||||
|
||||
// 1) 下发后 30ms 内（< 50ms 租约）应看到非零电流。
|
||||
motor.set_velocity(3000.0);
|
||||
let mut live = false;
|
||||
for _ in 0..30 {
|
||||
if slot_current(&recv_command(&mut peer).await, 0) != 0 {
|
||||
live = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
assert!(live, "租约内应输出非零电流");
|
||||
|
||||
// 2) 沉默 200ms（> 50ms 租约）→ 必须强制归零。
|
||||
sleep(Duration::from_millis(200)).await;
|
||||
// 排空沉默期间积压的帧：其中前段仍带着租约到期前的旧命令。
|
||||
while peer.incoming_rx.try_recv().is_ok() {}
|
||||
sleep(Duration::from_millis(10)).await;
|
||||
for _ in 0..5 {
|
||||
let raw = slot_current(&recv_command(&mut peer).await, 0);
|
||||
assert_eq!(raw, 0, "租约到期后必须归零，实测 {raw}");
|
||||
}
|
||||
|
||||
// 3) 续租 → 恢复出力。
|
||||
motor.set_velocity(3000.0);
|
||||
let mut renewed = false;
|
||||
for _ in 0..30 {
|
||||
if slot_current(&recv_command(&mut peer).await, 0) != 0 {
|
||||
renewed = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
assert!(renewed, "续租后应恢复非零电流");
|
||||
|
||||
feeder.abort();
|
||||
bus.shutdown().await;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- 组帧与路由
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn current_commands_route_to_group_slots() {
|
||||
let (bus_link, mut peer) = CanLink::pair(256);
|
||||
let configs = (1..=5).map(|id| tuned(id, 1.0)).collect();
|
||||
let bus = spawn_bus(bus_link, configs, BusOptions::default()).unwrap();
|
||||
|
||||
bus.motor(3).unwrap().set_effort_raw(1234);
|
||||
bus.motor(5).unwrap().set_effort_raw(-567);
|
||||
send_feedback(&peer, &(1..=5).collect::<Vec<_>>()).await;
|
||||
sleep(Duration::from_millis(5)).await;
|
||||
|
||||
let mut saw_200 = false;
|
||||
let mut saw_1ff = false;
|
||||
for _ in 0..20 {
|
||||
// 续租两条直发命令，避免读到租约到期后的 0 槽。
|
||||
bus.motor(3).unwrap().set_effort_raw(1234);
|
||||
bus.motor(5).unwrap().set_effort_raw(-567);
|
||||
let frame = recv_command(&mut peer).await;
|
||||
match frame.id {
|
||||
0x200 if !saw_200 => {
|
||||
assert_eq!(frame.data, [0, 0, 0, 0, 0x04, 0xD2, 0, 0], "esc3 slot = 1234");
|
||||
saw_200 = true;
|
||||
}
|
||||
0x1FF if !saw_1ff => {
|
||||
// -567 = 0xFDC9（i16 补码，大端）。
|
||||
assert_eq!(frame.data, [0xFD, 0xC9, 0, 0, 0, 0, 0, 0], "esc5 slot = -567");
|
||||
saw_1ff = true;
|
||||
}
|
||||
_ => {}
|
||||
}
|
||||
if saw_200 && saw_1ff {
|
||||
break;
|
||||
}
|
||||
}
|
||||
assert!(saw_200, "no 0x200 frame observed");
|
||||
assert!(saw_1ff, "no 0x1FF frame observed");
|
||||
|
||||
// disable 后槽位清零。
|
||||
bus.motor(3).unwrap().disable();
|
||||
let zeroed = wait_until(200, || {
|
||||
try_find_frame(&mut peer, |frame| frame.id == 0x200 && slot_current(frame, 2) == 0)
|
||||
})
|
||||
.await;
|
||||
assert!(zeroed, "esc3 slot should return to 0 after disable");
|
||||
bus.shutdown().await;
|
||||
}
|
||||
|
||||
/// 非阻塞地扫一眼通道里已有的帧。
|
||||
fn try_find_frame(peer: &mut CanLink, pred: impl Fn(&CanFrame) -> bool) -> bool {
|
||||
while let Ok(frame) = peer.incoming_rx.try_recv() {
|
||||
if pred(&frame) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
false
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- 反馈 → 状态
|
||||
|
||||
#[tokio::test]
|
||||
async fn feedback_updates_state_with_direction() {
|
||||
let (bus_link, peer) = CanLink::pair(64);
|
||||
let bus = spawn_bus(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap();
|
||||
|
||||
// 两帧正向跨圈：8000 → 100，电调侧圈数 ≈ 1.0122。
|
||||
let fb = EscFeedback { angle_raw: 8000, speed_rpm: 600, current_raw: -300, temperature_c: 47 };
|
||||
peer.outgoing_tx.send(encode_feedback_frame(2, &fb)).await.unwrap();
|
||||
let fb2 = EscFeedback { angle_raw: 100, ..fb };
|
||||
peer.outgoing_tx.send(encode_feedback_frame(2, &fb2)).await.unwrap();
|
||||
sleep(Duration::from_millis(50)).await;
|
||||
|
||||
let st = bus.motor(2).unwrap().state();
|
||||
assert_eq!(st.rotor_angle_raw, 100);
|
||||
assert!((st.rotor_speed_rpm + 600.0).abs() < 1e-3); // direction = -1
|
||||
assert_eq!(st.effort_raw, 300);
|
||||
assert_eq!(st.temperature_c, 47);
|
||||
assert_eq!(st.feedback_count, 2);
|
||||
let esc_rev = 1.0 + 100.0 / 8192.0;
|
||||
assert!(
|
||||
(st.rotor_revolutions + esc_rev).abs() < 1e-3,
|
||||
"revolutions = {}",
|
||||
st.rotor_revolutions
|
||||
);
|
||||
// 输出轴换算由 `OutputShaft` 负责（见 motor-core gearbox 单测）。
|
||||
bus.shutdown().await;
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- 生命周期
|
||||
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn shutdown_stops_tasks_and_closes_link() {
|
||||
let (bus_link, mut peer) = CanLink::pair(128);
|
||||
// 不接 plant，直接验证任务生命周期。
|
||||
let bus = spawn_bus(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
|
||||
bus.motor(1).unwrap().set_velocity(1000.0);
|
||||
sleep(Duration::from_millis(20)).await;
|
||||
// 有命令帧在流动。
|
||||
assert!(peer.incoming_rx.recv().await.is_some());
|
||||
|
||||
bus.shutdown().await;
|
||||
// 排空残留缓冲帧后应立即得到 None（发送端已全部退出）；
|
||||
// 若任务仍在运行，帧会持续流入并触发超时。
|
||||
loop {
|
||||
match timeout(Duration::from_millis(100), peer.incoming_rx.recv()).await {
|
||||
Ok(None) => break,
|
||||
Ok(Some(_)) => {} // 排空残留缓冲帧
|
||||
Err(elapsed) => panic!("bus tasks still running after shutdown (waited {elapsed:?})"),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn spawn_rejects_bad_configs() {
|
||||
let link = || CanLink::pair(8).0;
|
||||
assert_eq!(spawn_bus(link(), vec![], BusOptions::default()).err(), Some(EscBusError::NoMotors));
|
||||
assert_eq!(
|
||||
spawn_bus(link(), vec![tuned(2, 1.0), tuned(2, -1.0)], BusOptions::default()).err(),
|
||||
Some(EscBusError::DuplicateEscId(2))
|
||||
);
|
||||
let mut bad = tuned(1, 1.0);
|
||||
bad.direction = 0.0;
|
||||
let err = spawn_bus(link(), vec![bad], BusOptions::default()).err();
|
||||
assert!(
|
||||
matches!(err, Some(EscBusError::InvalidConfig { .. })),
|
||||
"expected InvalidConfig, got {err:?}"
|
||||
);
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------- 增益热更新
|
||||
|
||||
/// 自动整定的真机后端靠这条路径逐次试验：不重启总线，只换增益。
|
||||
/// 契约：① 读取即得到最新增益；② 换增益后控制器**状态复位**（无旧积分残留）；
|
||||
/// ③ 限幅等安全字段不受影响。
|
||||
#[tokio::test(start_paused = true)]
|
||||
async fn velocity_gains_can_be_hot_swapped() {
|
||||
let (bus_link, plant_link) = CanLink::pair(256);
|
||||
let plant = spawn_plant(plant_link, &[1], Duration::from_millis(1));
|
||||
let cfg = tuned(1, 1.0);
|
||||
let limit = i32::from(cfg.current_limit);
|
||||
let bus = spawn_bus(bus_link, vec![cfg.clone()], BusOptions::default()).unwrap();
|
||||
let motor = bus.motor(1).unwrap().clone();
|
||||
|
||||
// ① 初始增益来自 EscConfig（含前馈）。
|
||||
assert_eq!(bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap()), tuned_gains());
|
||||
|
||||
// 让旧增益的积分先积累起来（大静差 + 深度饱和）。
|
||||
motor.set_velocity(9000.0);
|
||||
hold(&motor, 9000.0, 200).await;
|
||||
let stale = bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap());
|
||||
|
||||
// ② 热更新：控制器被重建。
|
||||
let swapped = VelocityGains::new(1.0, 0.0, 0.0, 0.0);
|
||||
bus.with_policy(|policy| policy.set_velocity_gains(motor.id(), swapped));
|
||||
assert_eq!(bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap()), swapped);
|
||||
assert_ne!(bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap()), stale);
|
||||
|
||||
// 新增益生效：误差 e 时输出 = kp·e = e（无前馈、无积分），并仍被限幅夹住。
|
||||
motor.set_velocity(30_000.0);
|
||||
let saturated =
|
||||
hold_until(500, &motor, 30_000.0, || i32::from(motor.state().effort_raw) >= limit).await;
|
||||
assert!(saturated, "effort = {} 未达到限幅 {}", motor.state().effort_raw, limit);
|
||||
|
||||
// ③ 限幅仍是启动时的 current_limit：热更新不得放宽安全边界。
|
||||
let effort = i32::from(motor.state().effort_raw);
|
||||
assert_eq!(effort, limit, "饱和值必须正好是 current_limit");
|
||||
assert_eq!(
|
||||
bus.with_policy(|policy| policy.velocity_gains(motor.id()).unwrap()),
|
||||
swapped,
|
||||
"热更新后增益保持不变"
|
||||
);
|
||||
|
||||
plant.abort();
|
||||
bus.shutdown().await;
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
//! 语义一致性：同步评估 harness 与异步 `EscBus` 闭环必须给出同一个答案。
|
||||
//!
|
||||
//! 自动整定跑在 [`sim_harness::simulate`]（同步、无通道）上，而实车跑的是
|
||||
//! `EscBus` 的 1 kHz 发送循环。若两者的限幅 / 物理帧 / 控制器语义漂移，
|
||||
//! 整定出来的增益就只是"对仿真有效"。本测试把两条路径放在同一组参数与
|
||||
//! 同一条场景下比较稳态与 effort。
|
||||
//!
|
||||
//! 允许的差异来自**离散化细节**（异步侧每拍实测 `dt`、反馈比命令晚一拍），
|
||||
//! 因此比较的是稳态量级而不是逐点相等。
|
||||
|
||||
#![allow(clippy::unwrap_used, clippy::expect_used)]
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use can::CanLink;
|
||||
use esc_bus::{BusOptions, EscBus, EscConfig};
|
||||
use experiment::Scenario;
|
||||
use motor::{MotorFeedback, VelocityDrive};
|
||||
use motor_control::{MotorPolicy, VelocityGains};
|
||||
use motor_model::PlantParams;
|
||||
use sim_harness::{LoopLimits, simulate, spawn_plant};
|
||||
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.current_limit = LoopLimits::gm3508().current_limit_raw;
|
||||
let mut policy = MotorPolicy::new();
|
||||
policy.configure(cfg.esc_id, cfg.current_limit, GAINS);
|
||||
let bus =
|
||||
EscBus::spawn(bus_link, vec![cfg], BusOptions::default(), policy).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}"
|
||||
);
|
||||
}
|
||||
@@ -0,0 +1,132 @@
|
||||
//! vcan 集成测试：在真实 Linux 内核 CAN 栈上验证桥接。
|
||||
//!
|
||||
//! 仅 Linux 下编译；默认 `#[ignore]`，需要 vcan0（见 justfile `vcan-test`）。
|
||||
|
||||
#![cfg(target_os = "linux")]
|
||||
#![allow(clippy::unwrap_used, clippy::expect_used)]
|
||||
|
||||
use std::time::Duration;
|
||||
|
||||
use can::{CanFrame, CanLink};
|
||||
use motor::{MotorFeedback, VelocityDrive};
|
||||
use motor_control::{MotorPolicy, VelocityGains};
|
||||
use socketcan_rs::tokio::CanSocket;
|
||||
use socketcan_rs::{CanDataFrame, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _, StandardId};
|
||||
use tokio::time::timeout;
|
||||
|
||||
const IFACE: &str = "vcan0";
|
||||
|
||||
fn sc_data_frame(id: u16, data: [u8; 8]) -> ScFrame {
|
||||
ScFrame::Data(CanDataFrame::new(StandardId::new(id).unwrap(), &data).unwrap())
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
#[ignore = "requires vcan0 (see `just vcan-test`)"]
|
||||
async fn bridge_relays_command_to_bus() {
|
||||
let link = socketcan::open_link(IFACE).unwrap();
|
||||
let observer = CanSocket::open(IFACE).unwrap();
|
||||
|
||||
let command = CanFrame::new(0x200, [0x04, 0xD2, 0, 0, 0, 0, 0, 0]);
|
||||
link.outgoing_tx.send(command).await.unwrap();
|
||||
|
||||
let got = timeout(Duration::from_secs(2), observer.read_frame())
|
||||
.await
|
||||
.expect("timeout waiting for command frame")
|
||||
.unwrap();
|
||||
match got {
|
||||
ScFrame::Data(df) => {
|
||||
assert_eq!(df.raw_id(), 0x200);
|
||||
assert_eq!(df.data(), &[0x04, 0xD2, 0, 0, 0, 0, 0, 0]);
|
||||
}
|
||||
other => panic!("expected data frame, got {other:?}"),
|
||||
}
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
#[ignore = "requires vcan0 (see `just vcan-test`)"]
|
||||
async fn bridge_relays_feedback_from_bus() {
|
||||
let CanLink { outgoing_tx: _, mut incoming_rx } = socketcan::open_link(IFACE).unwrap();
|
||||
let simulator = CanSocket::open(IFACE).unwrap();
|
||||
|
||||
// 电调 1 反馈：角度 8191、-364rpm、电流 123、45℃。
|
||||
let feedback = sc_data_frame(0x201, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]);
|
||||
simulator.write_frame(feedback).await.unwrap();
|
||||
|
||||
let got = timeout(Duration::from_secs(2), incoming_rx.recv())
|
||||
.await
|
||||
.expect("timeout waiting for feedback frame")
|
||||
.unwrap();
|
||||
assert_eq!(got.id, 0x201);
|
||||
assert_eq!(got.data, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]);
|
||||
}
|
||||
|
||||
#[tokio::test]
|
||||
#[ignore = "requires vcan0 (see `just vcan-test`)"]
|
||||
async fn esc_bus_end_to_end_over_vcan() {
|
||||
// EscBus ↔ 真实内核 CAN 栈 ↔ 模拟电调（第二个 socket）。
|
||||
let link = socketcan::open_link(IFACE).unwrap();
|
||||
let mut esc = EscSim::new(IFACE, &[1]);
|
||||
|
||||
let config = esc_bus::EscConfig::gm3508("m1", 1, 1.0);
|
||||
let mut policy = MotorPolicy::new();
|
||||
policy.configure(1, config.current_limit, VelocityGains::new(8.0, 2.0, 0.0, 16384.0 / 9254.0));
|
||||
let bus =
|
||||
esc_bus::EscBus::spawn(link, vec![config], esc_bus::BusOptions::default(), policy).unwrap();
|
||||
bus.motor(1).unwrap().set_velocity(2000.0);
|
||||
|
||||
// 仿真 2s：模拟电调按命令回反馈，速度环应收敛。
|
||||
let motor = bus.motor(1).unwrap().clone();
|
||||
let deadline = tokio::time::Instant::now() + Duration::from_secs(2);
|
||||
loop {
|
||||
esc.step_once().await;
|
||||
if motor.state().rotor_speed_rpm > 1900.0 {
|
||||
break;
|
||||
}
|
||||
assert!(
|
||||
tokio::time::Instant::now() < deadline,
|
||||
"not converged: {}",
|
||||
motor.state().rotor_speed_rpm
|
||||
);
|
||||
tokio::time::sleep(Duration::from_millis(5)).await;
|
||||
}
|
||||
bus.shutdown().await;
|
||||
}
|
||||
|
||||
/// 最小电调模拟：收命令帧 → 更新转速 → 回反馈帧。
|
||||
struct EscSim {
|
||||
socket: CanSocket,
|
||||
speed_rpm: f32,
|
||||
angle: u16,
|
||||
}
|
||||
|
||||
impl EscSim {
|
||||
fn new(iface: &str, _esc_ids: &[u8]) -> Self {
|
||||
Self { socket: CanSocket::open(iface).unwrap(), speed_rpm: 0.0, angle: 0 }
|
||||
}
|
||||
|
||||
/// 排空当前命令帧并推进模型一步（dt = 5ms）。
|
||||
async fn step_once(&mut self) {
|
||||
const DT: f32 = 0.005;
|
||||
let mut current = 0i16;
|
||||
while let Ok(frame) = self.socket.try_read_frame() {
|
||||
if let ScFrame::Data(df) = frame {
|
||||
if df.raw_id() == 0x200 {
|
||||
current = i16::from_be_bytes([df.data()[0], df.data()[1]]);
|
||||
}
|
||||
}
|
||||
}
|
||||
let target = f32::from(current) * 9254.0 / 16384.0;
|
||||
self.speed_rpm += (target - self.speed_rpm) * (1.0 - (-DT / 0.12).exp());
|
||||
self.angle =
|
||||
(u32::from(self.angle) + (self.speed_rpm / 60.0 * 8192.0 * DT) as u32) as u16 % 8192;
|
||||
let data = {
|
||||
let mut d = [0u8; 8];
|
||||
d[0..2].copy_from_slice(&self.angle.to_be_bytes());
|
||||
d[2..4].copy_from_slice(&(self.speed_rpm as i16).to_be_bytes());
|
||||
d[4..6].copy_from_slice(¤t.to_be_bytes());
|
||||
d[6] = 30;
|
||||
d
|
||||
};
|
||||
self.socket.write_frame(sc_data_frame(0x201, data)).await.unwrap();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user