133 lines
4.5 KiB
Rust
133 lines
4.5 KiB
Rust
//! 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 dji_esc::{CanFrame, CanLink};
|
|
use motor_core::Motor;
|
|
use socketcan::tokio::CanSocket;
|
|
use socketcan::{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 = dji_esc_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 } = dji_esc_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 = dji_esc_socketcan::open_link(IFACE).unwrap();
|
|
let mut esc = EscSim::new(IFACE, &[1]);
|
|
|
|
let bus = dji_esc::EscBus::spawn(
|
|
link,
|
|
vec![dji_esc::EscConfig::gm3508("m1", 1, 1.0)],
|
|
dji_esc::BusOptions::default(),
|
|
)
|
|
.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();
|
|
}
|
|
}
|