feat: 初始化
This commit is contained in:
Horldsence
@@ -0,0 +1,25 @@
|
||||
[package]
|
||||
name = "dji-esc-socketcan"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
description = "SocketCAN backend bridging real Linux CAN interfaces to can-link channels"
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
can-link = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
|
||||
[target.'cfg(target_os = "linux")'.dependencies]
|
||||
socketcan = { workspace = true }
|
||||
|
||||
[target.'cfg(target_os = "linux")'.dev-dependencies]
|
||||
can-link = { workspace = true }
|
||||
dji-esc = { workspace = true }
|
||||
motor-core = { workspace = true }
|
||||
socketcan = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
@@ -0,0 +1,110 @@
|
||||
//! 单电机验证工具：让一个 C620 / GM3508 转起来，验证电机抽象全链路。
|
||||
//!
|
||||
//! ```sh
|
||||
//! spin [interface] [esc_id] [rotor_rpm] [duration_secs]
|
||||
//! # 例：can0 上电调 1 以 1000 rpm（转子）转 10 秒
|
||||
//! cargo run --release -p dji-esc-socketcan --example spin -- can0 1 1000 10
|
||||
//! ```
|
||||
//!
|
||||
//! 关键点在 [`spin_one`]：它只依赖 `motor_core::Motor` 抽象，完全不知道
|
||||
//! 背后是 C620 协议还是别的电机——换任何实现该 trait 的电机，这段代码
|
||||
//! 原样可用。抽象与协议实现（`dji-esc`）、总线后端（本 crate）在此汇合。
|
||||
//!
|
||||
//! 仅 Linux（需要 SocketCAN）；其他平台编译为报错桩。
|
||||
|
||||
#![allow(clippy::unwrap_used, clippy::expect_used)]
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
use std::time::Duration;
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
use dji_esc::{BusOptions, EscBus, EscConfig};
|
||||
#[cfg(target_os = "linux")]
|
||||
use dji_esc_socketcan::open_link;
|
||||
#[cfg(target_os = "linux")]
|
||||
use motor_core::{Disable, EffortDrive, Motor, MotorFeedback, VelocityDrive};
|
||||
#[cfg(target_os = "linux")]
|
||||
use tokio::time::{MissedTickBehavior, interval};
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
#[tokio::main]
|
||||
async fn main() {
|
||||
let mut args = std::env::args().skip(1);
|
||||
let interface = args.next().unwrap_or_else(|| "can0".into());
|
||||
let esc_id: u8 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1);
|
||||
let rotor_rpm: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(1000.0);
|
||||
let duration_secs: f32 = args.next().and_then(|s| s.parse().ok()).unwrap_or(10.0);
|
||||
|
||||
println!("=== 单电机抽象验证 ===");
|
||||
println!(
|
||||
"接口 {interface} | 电调 {esc_id} | 目标 {rotor_rpm} rpm（转子）| 时长 {duration_secs}s"
|
||||
);
|
||||
|
||||
let link = open_link(&interface).unwrap_or_else(|e| {
|
||||
eprintln!(
|
||||
"打开 {interface} 失败：{e}（先 ip link set {interface} up type can bitrate 1000000）"
|
||||
);
|
||||
std::process::exit(1);
|
||||
});
|
||||
let config = EscConfig::gm3508("spin_test", esc_id, 1.0);
|
||||
println!(
|
||||
"电机配置：{:?} | 减速比 {:.3} | 电流限幅 {}",
|
||||
config.model, config.gear_ratio, config.current_limit
|
||||
);
|
||||
|
||||
let bus = EscBus::spawn(link, vec![config], BusOptions::default()).expect("EscBus 配置非法");
|
||||
let motor = bus.motor(esc_id).expect("刚配置的电机必然存在");
|
||||
|
||||
spin_one(motor, rotor_rpm, Duration::from_secs_f32(duration_secs)).await;
|
||||
|
||||
bus.shutdown().await;
|
||||
println!("已退出");
|
||||
}
|
||||
|
||||
/// 验证目标：本函数只面向 [`Motor`] 抽象——设定转速、周期读状态、停机。
|
||||
/// 它不知道 CAN，不知道 C620，换任何 Motor 实现都原样工作。
|
||||
#[cfg(target_os = "linux")]
|
||||
async fn spin_one(motor: &impl Motor, target_rpm: f32, duration: Duration) {
|
||||
motor.set_velocity(target_rpm);
|
||||
println!("已下发目标转速，开始观测（100ms 周期）……\n");
|
||||
|
||||
let mut ticker = interval(Duration::from_millis(100));
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
let deadline = tokio::time::Instant::now() + duration;
|
||||
|
||||
loop {
|
||||
tokio::select! {
|
||||
_ = tokio::signal::ctrl_c() => {
|
||||
println!("\n收到 ctrl-c");
|
||||
break;
|
||||
}
|
||||
_ = ticker.tick() => {}
|
||||
}
|
||||
if tokio::time::Instant::now() >= deadline {
|
||||
break;
|
||||
}
|
||||
let st = motor.state();
|
||||
if st.has_feedback() {
|
||||
println!(
|
||||
"转子 {:>8.1} rpm | effort {:>6} raw | 温度 {:>3} ℃ | 累计角度 {:>8.3} 圈 | 反馈 {} 帧",
|
||||
st.rotor_speed_rpm,
|
||||
st.effort_raw,
|
||||
st.temperature_c,
|
||||
st.rotor_revolutions,
|
||||
st.feedback_count
|
||||
);
|
||||
} else {
|
||||
println!("⚠ 尚未收到任何反馈——检查电调供电与 CAN 接线");
|
||||
}
|
||||
}
|
||||
|
||||
motor.disable();
|
||||
// 让 0 电流帧至少发出一拍再关总线。
|
||||
tokio::time::sleep(Duration::from_millis(50)).await;
|
||||
println!("已停机（0 电流）");
|
||||
}
|
||||
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
fn main() {
|
||||
eprintln!("spin 仅支持 Linux（需要 SocketCAN）");
|
||||
}
|
||||
@@ -0,0 +1,96 @@
|
||||
//! 真实总线桥接实现（Linux）。
|
||||
|
||||
use std::io;
|
||||
use std::sync::Arc;
|
||||
|
||||
use can_link::{CanFrame as LinkFrame, CanLink};
|
||||
use socketcan::tokio::CanSocket;
|
||||
use socketcan::{
|
||||
CanDataFrame, CanFilter, CanFrame as ScFrame, EmbeddedFrame as _, Frame as _,
|
||||
SocketOptions as _,
|
||||
};
|
||||
use tokio::sync::mpsc;
|
||||
use tracing::{debug, error};
|
||||
|
||||
/// 单个链路的通道缓冲（1kHz × 8 电调反馈余量）。
|
||||
const CHANNEL_BOUND: usize = 512;
|
||||
|
||||
/// 打开 CAN 接口并桥接为 [`CanLink`]，返回面向总线驱动的端点。
|
||||
///
|
||||
/// 内部：
|
||||
/// - 硬件过滤只放行电调反馈 ID 段（`0x200..=0x20F`，覆盖 0x201–0x208，
|
||||
/// 非反馈帧再由协议层解码拒绝，双保险）；
|
||||
/// - 两个桥接任务：socket → 通道（反馈）、通道 → socket（命令）。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 接口不存在 / 权限不足等 `io::Error`。
|
||||
pub fn open_link(interface: &str) -> io::Result<CanLink> {
|
||||
let socket = CanSocket::open(interface)?;
|
||||
socket.set_filters(&[CanFilter::new(0x200, 0x7F0)])?;
|
||||
let socket = Arc::new(socket);
|
||||
|
||||
let (bus_side, bridge_side) = CanLink::pair(CHANNEL_BOUND);
|
||||
let CanLink { outgoing_tx, incoming_rx } = bridge_side;
|
||||
|
||||
tokio::spawn(rx_bridge(Arc::clone(&socket), outgoing_tx));
|
||||
tokio::spawn(tx_bridge(socket, incoming_rx));
|
||||
Ok(bus_side)
|
||||
}
|
||||
|
||||
/// socket → 通道：真实总线反馈进入协议 bus。
|
||||
async fn rx_bridge(socket: Arc<CanSocket>, tx: mpsc::Sender<LinkFrame>) {
|
||||
loop {
|
||||
match socket.read_frame().await {
|
||||
Ok(frame) => {
|
||||
if let Some(frame) = to_link_frame(&frame) {
|
||||
if tx.send(frame).await.is_err() {
|
||||
debug!("bus side closed, rx bridge exiting");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
Err(e) => {
|
||||
// 接口脱落（如 USB 转 CAN 拔出）等致命错误：退出守护进程，
|
||||
// 由 systemd 拉起重连。
|
||||
error!(error = %e, "CAN read failed, rx bridge exiting");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 通道 → socket：协议 bus 命令写入真实总线。
|
||||
async fn tx_bridge(socket: Arc<CanSocket>, mut rx: mpsc::Receiver<LinkFrame>) {
|
||||
while let Some(frame) = rx.recv().await {
|
||||
match to_socketcan_frame(&frame) {
|
||||
Some(sc_frame) => {
|
||||
if let Err(e) = socket.write_frame(sc_frame).await {
|
||||
error!(error = %e, "CAN write failed, tx bridge exiting");
|
||||
return;
|
||||
}
|
||||
}
|
||||
None => {
|
||||
debug!(id = frame.id, "drop frame with non-standard CAN id");
|
||||
}
|
||||
}
|
||||
}
|
||||
debug!("bus side closed, tx bridge exiting");
|
||||
}
|
||||
|
||||
/// socketcan 帧 → 链路帧（仅 8 字节数据帧）。
|
||||
fn to_link_frame(frame: &ScFrame) -> Option<LinkFrame> {
|
||||
match frame {
|
||||
ScFrame::Data(df) if df.data().len() == 8 => {
|
||||
Some(LinkFrame::new(df.raw_id(), df.data().try_into().ok()?))
|
||||
}
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
/// 链路帧 → socketcan 帧（ID 必须落在 11 位标准 ID 范围内）。
|
||||
fn to_socketcan_frame(frame: &LinkFrame) -> Option<ScFrame> {
|
||||
let id = socketcan::StandardId::new(frame.id as u16)?;
|
||||
let df = CanDataFrame::new(id, &frame.data)?;
|
||||
Some(ScFrame::Data(df))
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
//! SocketCAN 桥接：把真实 Linux CAN 接口接入 [`can_link::CanLink`] 通道对。
|
||||
//!
|
||||
//! 本 crate 主体仅在 Linux 下编译（依赖 socketcan）。其他平台编译为空壳，
|
||||
//! 使整个 workspace 可以在任意开发机上构建与测试；实车部署目标板均为
|
||||
//! Linux。
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
mod bridge;
|
||||
|
||||
#[cfg(target_os = "linux")]
|
||||
pub use bridge::open_link;
|
||||
@@ -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 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();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user