276 lines
9.9 KiB
Rust
276 lines
9.9 KiB
Rust
//! 闭环集成测试：`EscBus` + 内存通道 + 一阶电机仿真。
|
||
//!
|
||
//! 全部可在任意平台（macOS / Linux / CI）运行，无需硬件。
|
||
|
||
#![allow(clippy::unwrap_used, clippy::expect_used)]
|
||
|
||
use std::time::Duration;
|
||
|
||
use dji_esc::{
|
||
BusOptions, CanFrame, CanLink, EscBus, EscBusError, EscConfig, EscFeedback,
|
||
encode_feedback_frame, spawn_plant,
|
||
};
|
||
use motor_core::{Disable, EffortDrive, MotorFeedback, VelocityDrive};
|
||
use tokio::time::{sleep, timeout};
|
||
|
||
/// 与 `PlantMotor::gm3508` 增益精确匹配的速度环参数（ff = 16384 / 9254）。
|
||
fn tuned(esc_id: u8, direction: f32) -> EscConfig {
|
||
let mut cfg = EscConfig::gm3508(format!("m{esc_id}"), esc_id, direction);
|
||
cfg.kp = 8.0;
|
||
cfg.ki = 2.0;
|
||
cfg.kd = 0.0;
|
||
cfg.feedforward = 16384.0 / 9254.0;
|
||
cfg
|
||
}
|
||
|
||
/// 轮询直到条件满足或超时（毫秒，仿真时间）。
|
||
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()
|
||
}
|
||
|
||
/// 手动向总线注入一帧所有电机的反馈（新鲜度 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 = EscBus::spawn(bus_link, vec![tuned(1, 1.0)], BusOptions::default()).unwrap();
|
||
let motor = bus.motor(1).unwrap().clone();
|
||
|
||
motor.set_velocity(3000.0);
|
||
let ok = wait_until(2000, || 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;
|
||
sleep(Duration::from_millis(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 = EscBus::spawn(bus_link, vec![tuned(2, -1.0)], BusOptions::default()).unwrap();
|
||
let motor = bus.motor(2).unwrap().clone();
|
||
|
||
motor.set_velocity(2000.0);
|
||
let ok = wait_until(2000, || 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 = EscBus::spawn(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 = EscBus::spawn(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 {
|
||
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;
|
||
}
|
||
|
||
// ---------------------------------------------------------------- 组帧与路由
|
||
|
||
#[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 = EscBus::spawn(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 {
|
||
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 = EscBus::spawn(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 = EscBus::spawn(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!(
|
||
EscBus::spawn(link(), vec![], BusOptions::default()).err(),
|
||
Some(EscBusError::NoMotors)
|
||
);
|
||
assert_eq!(
|
||
EscBus::spawn(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 = EscBus::spawn(link(), vec![bad], BusOptions::default()).err();
|
||
assert!(
|
||
matches!(err, Some(EscBusError::InvalidConfig { .. })),
|
||
"expected InvalidConfig, got {err:?}"
|
||
);
|
||
}
|