350 lines
14 KiB
Rust
350 lines
14 KiB
Rust
//! SLCAN 桥接：把串口 slcan 设备（如 CANable2 的 `/dev/ttyACM0`）接入 [`can::CanLink`]。
|
||
//!
|
||
//! 基于 [`slcan`] crate（Lawicel ASCII 协议）：`S8` = 1 Mbps（C620 标准），`O` 打开通道。
|
||
//! 阻塞串口 I/O 跑在专用线程上，与 SocketCAN 桥一样只暴露通道端点——对本 crate
|
||
//! 的调用方（`motor / esc-bus`）而言，看到的是一个普通 [`CanLink`]，与传输介质无关。
|
||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||
|
||
use std::io;
|
||
use std::sync::Arc;
|
||
use std::sync::atomic::{AtomicU64, Ordering};
|
||
use std::thread;
|
||
use std::time::{Duration, Instant};
|
||
|
||
use can::{CanFrame as LinkFrame, CanLink};
|
||
use serial::prelude::*;
|
||
use slcan::{BitRate, CanSocket};
|
||
use slcan_proto as slcan;
|
||
use tokio::sync::mpsc;
|
||
use tracing::{debug, error, info, warn};
|
||
|
||
/// 链路通道缓冲（**故意很浅**）。
|
||
///
|
||
/// 两个方向的需求完全相反：
|
||
///
|
||
/// - **命令方向**（bus → 桥）：必须浅。驱动按 1kHz 生产命令，而 slcan 一次 write
|
||
/// 在 CDC ACM 上要约 2ms，下行只能到几百帧/秒。若缓冲很深，队列会被"过时命令"
|
||
/// 灌满，ESC 实际执行的是**一秒多以前**算出来的电流——速度环等于开了 1.4s 的
|
||
/// 延迟，必然失控跑飞。取小值让 `send().await` 立刻回压，驱动自然降速到链路
|
||
/// 能承受的节拍，每一帧都是**按最新反馈现算**的。
|
||
/// - **反馈方向**（桥 → bus）：是采样流，来不及就丢（见 `pump` 的 `try_send`），
|
||
/// 因此不需要深队列。
|
||
const CHANNEL_BOUND: usize = 8;
|
||
|
||
/// 经典 CAN 标准帧 ID 上界（11 位）。超出者不是标准帧，一律丢弃：
|
||
/// SLCAN 的 `t` 命令只承载 3 位十六进制 ID，截断后会把扩展帧冒充成标准帧。
|
||
const MAX_STANDARD_ID: u32 = 0x7FF;
|
||
|
||
/// 反馈方向帧过滤：只接受「标准 ID + 8 字节」的数据帧。
|
||
///
|
||
/// C620 反馈固定 8 字节；DLC 不足的帧若放行，协议层会按越界读取拒绝，
|
||
/// 但更早丢弃可以省掉一次无谓的解码。
|
||
#[must_use]
|
||
fn accepted_rx(frame: &slcan::CanFrame) -> Option<LinkFrame> {
|
||
if frame.id > MAX_STANDARD_ID || frame.dlc != 8 {
|
||
return None;
|
||
}
|
||
Some(LinkFrame::new(frame.id, frame.data))
|
||
}
|
||
|
||
/// slcan CAN 波特率档位（对应协议 `Sn` 命令）。
|
||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
|
||
pub enum CanSpeed {
|
||
/// `S6`：500 kbps。
|
||
K500,
|
||
/// `S8`：1 Mbps（C620 标准）。
|
||
#[default]
|
||
M1,
|
||
}
|
||
|
||
impl CanSpeed {
|
||
fn to_slcan(self) -> BitRate {
|
||
match self {
|
||
Self::K500 => BitRate::Setup500Kbit,
|
||
Self::M1 => BitRate::Setup1Mbit,
|
||
}
|
||
}
|
||
}
|
||
|
||
/// 打开参数。
|
||
#[derive(Debug, Clone, Copy)]
|
||
pub struct SlcanOptions {
|
||
/// 串口波特率（USB CDC ACM 常见 2_000_000；部分适配器 115_200）。
|
||
pub serial_baud: u32,
|
||
/// CAN 波特率档位。
|
||
pub speed: CanSpeed,
|
||
/// 串口超时。**读写共用**（见 `pump` 的说明）：`serial` crate 的
|
||
/// `TTYPort::write` 也先按本超时 `poll(POLLOUT)`。
|
||
///
|
||
/// 太短会把正常的"这一拍写不出去"误判成写失败。总线繁忙时读几乎立即返回，
|
||
/// 因此放大它只影响"总线完全静默"时的下行延迟，默认 50ms。
|
||
pub read_timeout: Duration,
|
||
}
|
||
|
||
impl Default for SlcanOptions {
|
||
fn default() -> Self {
|
||
Self {
|
||
serial_baud: 2_000_000,
|
||
speed: CanSpeed::M1,
|
||
read_timeout: Duration::from_millis(50),
|
||
}
|
||
}
|
||
}
|
||
|
||
/// 打开 slcan 设备并桥接为 [`CanLink`]（默认 1 Mbps / 2 Mbaud）。
|
||
///
|
||
/// # Errors
|
||
///
|
||
/// 设备不存在、权限不足、波特率不被支持等 `io::Error`。
|
||
pub fn open_link(device: &str) -> io::Result<CanLink> {
|
||
open_link_with(device, SlcanOptions::default())
|
||
}
|
||
|
||
/// 按指定参数打开 slcan 设备。
|
||
///
|
||
/// # Errors
|
||
///
|
||
/// 同 [`open_link`]。
|
||
pub fn open_link_with(device: &str, options: SlcanOptions) -> io::Result<CanLink> {
|
||
let mut port = serial::open(device)?;
|
||
port.reconfigure(&|settings| {
|
||
// `serial::BaudOther` 以 `usize` 计数；u32 → usize 在 32/64 位目标上均无损。
|
||
settings.set_baud_rate(serial::BaudOther(options.serial_baud as usize))?;
|
||
Ok(())
|
||
})?;
|
||
port.set_timeout(options.read_timeout)?;
|
||
|
||
let mut socket = CanSocket::new(port);
|
||
socket.open(options.speed.to_slcan())?;
|
||
info!(device, serial_baud = options.serial_baud, speed = ?options.speed, "slcan opened");
|
||
|
||
let (bus_side, bridge_side) = CanLink::pair(CHANNEL_BOUND);
|
||
let CanLink { outgoing_tx, incoming_rx } = bridge_side;
|
||
let stats = Arc::new(LinkStats::default());
|
||
|
||
thread::Builder::new()
|
||
.name("slcan-bridge".into())
|
||
.spawn(move || pump(socket, outgoing_tx, incoming_rx, stats))?;
|
||
Ok(bus_side)
|
||
}
|
||
|
||
#[derive(Default)]
|
||
struct LinkStats {
|
||
rx: AtomicU64,
|
||
/// 因 bus 侧来不及消费而丢弃的反馈帧（反馈是采样流，丢弃是正常降级）。
|
||
rx_dropped: AtomicU64,
|
||
tx: AtomicU64,
|
||
/// 写通道未就绪而推迟到下一轮重试的命令帧次数（帧被保留，不丢）。
|
||
tx_retry: AtomicU64,
|
||
quiet_bucket: AtomicU64,
|
||
}
|
||
|
||
/// 每轮最多连续发送的命令帧数。
|
||
///
|
||
/// 电调总线按 1kHz 持续产出命令帧；若不加界，一次"排空"就可能永远排空——
|
||
/// 尤其在某次写 poll 超时之后，新帧仍在流入，接收方向会被**永久饿死**。
|
||
const TX_BURST: usize = 8;
|
||
|
||
/// 每轮最多读取的反馈帧数。
|
||
///
|
||
/// 一次 `write` 在 CDC ACM 上可能要 ~2ms，而发送上界是 8 帧，于是"每轮只读一帧"
|
||
/// 会让接收被发送压到几十 Hz。这里每轮把已缓冲的反馈尽量读完（无数据即刻返回），
|
||
/// 双向都不会饿死。
|
||
const RX_BURST: usize = 64;
|
||
|
||
/// 瞬时（可恢复）I/O 错误：错过这一拍不代表链路坏了。
|
||
///
|
||
/// `serial` crate 的 `TTYPort` 把**同一个** timeout 同时用于读和写——
|
||
/// `write()` 先做 `poll(POLLOUT, timeout)`，超时即返回 `TimedOut`。
|
||
/// 这类事件在 USB CDC ACM 上很常见，绝不能当成致命错误。
|
||
fn is_transient(e: &io::Error) -> bool {
|
||
matches!(
|
||
e.kind(),
|
||
io::ErrorKind::TimedOut | io::ErrorKind::WouldBlock | io::ErrorKind::Interrupted
|
||
)
|
||
}
|
||
|
||
/// 写一帧，并区分**瞬时**与**致命**错误。
|
||
///
|
||
/// 返回 `Ok(true)` = 已写出；`Ok(false)` = 通道暂时写不动，调用方应保留该帧、
|
||
/// **立刻让位给接收方向**，下一轮再试；`Err` = 真·致命（设备拔出、BrokenPipe 等）。
|
||
///
|
||
/// 关键：**写不动绝不能让接收方向一起停摆**。早期实现既无发送上界、又把
|
||
/// `TimedOut` 当致命错误直接 return，结果第一拍命令没写出去就把整条链路带停了。
|
||
fn write_frame(socket: &mut CanSocket<serial::SystemPort>, frame: &LinkFrame) -> io::Result<bool> {
|
||
match socket.write(frame.id, &frame.data) {
|
||
Ok(_) => Ok(true),
|
||
Err(e) if is_transient(&e) => {
|
||
debug!(error = %e, id = frame.id, "slcan write not ready, retry next round");
|
||
Ok(false)
|
||
}
|
||
Err(e) => Err(e),
|
||
}
|
||
}
|
||
|
||
/// 单线程双向泵：优先排空待发命令，随后读一帧反馈。
|
||
///
|
||
/// `serial::SystemPort` 即 `serial::open` 返回的平台原生端口类型
|
||
/// （Unix = `TTYPort`），因此本函数在 Linux 目标板与 macOS 开发机上同源。
|
||
///
|
||
/// 发送端与统计按值传入是有意的：它们随泵线程独占所有权，线程退出时
|
||
/// `tx` 被 drop，总线侧据此观察到链路关闭。
|
||
#[allow(clippy::needless_pass_by_value)]
|
||
fn pump(
|
||
mut socket: CanSocket<serial::SystemPort>,
|
||
tx: mpsc::Sender<LinkFrame>,
|
||
mut rx: mpsc::Receiver<LinkFrame>,
|
||
stats: Arc<LinkStats>,
|
||
) {
|
||
let mut last_rx_ok = Instant::now();
|
||
let mut last_stats = Instant::now();
|
||
// 上一轮没能写出去的帧：保留而非丢弃，下一轮优先重试。
|
||
let mut pending: Option<LinkFrame> = None;
|
||
loop {
|
||
// 写：**有界**排空待发命令；一旦通道写不动就交给接收方向。
|
||
for _ in 0..TX_BURST {
|
||
let frame = match pending.take() {
|
||
Some(frame) => frame,
|
||
None => match rx.try_recv() {
|
||
Ok(frame) => frame,
|
||
Err(mpsc::error::TryRecvError::Empty) => break,
|
||
Err(mpsc::error::TryRecvError::Disconnected) => {
|
||
debug!("bus side closed, slcan bridge exiting");
|
||
let _ = socket.close();
|
||
return;
|
||
}
|
||
},
|
||
};
|
||
// P-1：拒绝非标准 ID，避免 `as u16` 截断后冒充标准帧。
|
||
if frame.id > MAX_STANDARD_ID {
|
||
debug!(id = frame.id, "drop frame with non-standard CAN id");
|
||
continue;
|
||
}
|
||
match write_frame(&mut socket, &frame) {
|
||
Ok(true) => {
|
||
stats.tx.fetch_add(1, Ordering::Relaxed);
|
||
}
|
||
Ok(false) => {
|
||
stats.tx_retry.fetch_add(1, Ordering::Relaxed);
|
||
pending = Some(frame);
|
||
break;
|
||
}
|
||
Err(e) => {
|
||
error!(error = %e, "slcan write failed, bridge exiting");
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 周期性链路统计：现场判断"命令到底发出去没有"靠它。
|
||
if last_stats.elapsed() >= Duration::from_secs(5) {
|
||
last_stats = Instant::now();
|
||
debug!(
|
||
rx = stats.rx.load(Ordering::Relaxed),
|
||
rx_dropped = stats.rx_dropped.load(Ordering::Relaxed),
|
||
tx = stats.tx.load(Ordering::Relaxed),
|
||
tx_retry = stats.tx_retry.load(Ordering::Relaxed),
|
||
"slcan link stats"
|
||
);
|
||
}
|
||
|
||
// 读：**有界**批量读取已缓冲的反馈；无数据立即让位给发送方向。
|
||
for _ in 0..RX_BURST {
|
||
match socket.read() {
|
||
Ok(frame) => {
|
||
last_rx_ok = Instant::now();
|
||
stats.rx.fetch_add(1, Ordering::Relaxed);
|
||
let Some(frame) = accepted_rx(&frame) else {
|
||
debug!(
|
||
id = frame.id,
|
||
dlc = frame.dlc,
|
||
"drop non-standard or short slcan frame"
|
||
);
|
||
continue;
|
||
};
|
||
// 反馈是采样流：来不及就丢，绝不阻塞泵线程（否则又会饿死反方向）。
|
||
match tx.try_send(frame) {
|
||
Ok(()) => {}
|
||
Err(mpsc::error::TrySendError::Full(_)) => {
|
||
stats.rx_dropped.fetch_add(1, Ordering::Relaxed);
|
||
}
|
||
Err(mpsc::error::TrySendError::Closed(_)) => {
|
||
debug!("bus side closed, slcan rx exiting");
|
||
let _ = socket.close();
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
Err(e) if is_transient(&e) => {
|
||
// S-5：长时间无反馈 → 怀疑接线/bus-off/电调未上电。
|
||
let quiet = last_rx_ok.elapsed();
|
||
if quiet > Duration::from_secs(1) {
|
||
let bucket = quiet.as_secs();
|
||
if stats.quiet_bucket.swap(bucket, Ordering::Relaxed) != bucket {
|
||
warn!(
|
||
quiet_ms = quiet.as_millis() as u64,
|
||
rx = stats.rx.load(Ordering::Relaxed),
|
||
tx = stats.tx.load(Ordering::Relaxed),
|
||
tx_retry = stats.tx_retry.load(Ordering::Relaxed),
|
||
"slcan: no feedback for >1s (wiring / bus-off / powered ESC?)"
|
||
);
|
||
}
|
||
}
|
||
break; // 缓冲已空，回到发送方向
|
||
}
|
||
Err(e) if e.kind() == io::ErrorKind::InvalidData => {
|
||
// 单帧解析失败（噪声 / 半帧）不该杀掉整条链路。
|
||
debug!(error = %e, "slcan: malformed frame dropped");
|
||
}
|
||
Err(e) => {
|
||
error!(error = %e, "slcan read failed, bridge exiting");
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
#[cfg(test)]
|
||
mod tests {
|
||
use super::*;
|
||
|
||
fn slcan_frame(id: u32, dlc: usize) -> slcan::CanFrame {
|
||
slcan::CanFrame::new(id, dlc, &[0xAA; 8])
|
||
}
|
||
|
||
#[test]
|
||
fn accepts_standard_8_byte_frames() {
|
||
let frame = slcan::CanFrame::new(0x201, 8, &[1, 2, 3, 4, 5, 6, 7, 8]);
|
||
let link = accepted_rx(&frame).expect("0x201/8B must be accepted");
|
||
assert_eq!(link.id, 0x201);
|
||
assert_eq!(link.data, [1, 2, 3, 4, 5, 6, 7, 8]);
|
||
}
|
||
|
||
#[test]
|
||
fn rejects_extended_id() {
|
||
assert!(accepted_rx(&slcan_frame(0x800, 8)).is_none(), "11 位 ID 上界之外必须丢弃");
|
||
assert!(accepted_rx(&slcan_frame(0x1FFF_FFFF, 8)).is_none());
|
||
}
|
||
|
||
#[test]
|
||
fn rejects_short_frames() {
|
||
for dlc in 0..8 {
|
||
assert!(accepted_rx(&slcan_frame(0x201, dlc)).is_none(), "dlc {dlc} 必须丢弃");
|
||
}
|
||
}
|
||
|
||
#[test]
|
||
fn max_standard_id_is_boundary() {
|
||
assert!(accepted_rx(&slcan_frame(MAX_STANDARD_ID, 8)).is_some());
|
||
assert!(accepted_rx(&slcan_frame(MAX_STANDARD_ID + 1, 8)).is_none());
|
||
}
|
||
|
||
#[test]
|
||
fn speed_maps_to_lawicel_setup_command() {
|
||
assert_eq!(CanSpeed::M1.to_slcan() as u8, b'8');
|
||
assert_eq!(CanSpeed::K500.to_slcan() as u8, b'6');
|
||
assert_eq!(CanSpeed::default(), CanSpeed::M1);
|
||
}
|
||
}
|