541 lines
18 KiB
Rust
541 lines
18 KiB
Rust
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||
|
||
//! `EscBus` 异步驱动：协议状态机与 1kHz 控制节拍。
|
||
//!
|
||
//! [`EscBus::spawn`] 消费一个 [`CanLink`]，启动收发两个任务：
|
||
//!
|
||
//! - 接收：解码反馈帧，更新各电机共享状态（含多圈累计与方向修正）；
|
||
//! - 发送：按 `period` 节拍为每台电机求值当前命令（速度/位置环 PID / 直发
|
||
//! effort），按 `0x200`/`0x1FF` 分组组帧发送。
|
||
//!
|
||
//! 电机离线（从未收到反馈或反馈超时）时强制 0 输出并复位控制器。
|
||
//! 命令模式切换时同样复位控制器，避免积分冲击。
|
||
|
||
use std::sync::{Arc, Mutex, MutexGuard};
|
||
use std::time::Duration;
|
||
|
||
use can::{CanFrame, CanLink};
|
||
use motor_protocol::common::{
|
||
EscFeedback, EscModel, clamp_current, command_frame_id, decode_feedback, encode_current_frame,
|
||
};
|
||
use motor_protocol::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, TurnTracker};
|
||
use thiserror::Error;
|
||
use tokio::sync::{mpsc, watch};
|
||
use tokio::task::JoinHandle;
|
||
use tokio::time::{Instant, Interval, MissedTickBehavior};
|
||
use tracing::{debug, info, warn};
|
||
|
||
use motor::{
|
||
Command, Disable, EffortDrive, EffortPolicy, MotorFeedback, MotorState, PolicyInput,
|
||
PolicySample, PositionDrive, VelocityDrive,
|
||
};
|
||
|
||
/// 锁辅助：中毒锁直接恢复内部数据（锁中毒只意味着持锁线程恐慌，
|
||
/// 状态本身仍一致，驱动机不应因此停摆）。
|
||
fn lock<T>(mutex: &Mutex<T>) -> MutexGuard<'_, T> {
|
||
mutex.lock().unwrap_or_else(std::sync::PoisonError::into_inner)
|
||
}
|
||
|
||
/// 单台电机的配置。
|
||
#[derive(Debug, Clone)]
|
||
pub struct EscConfig {
|
||
/// 便于日志辨认的名字（如 `front_left`）。
|
||
pub name: String,
|
||
/// 电调 ID（1–8，即 CAN 拨码）。
|
||
pub esc_id: u8,
|
||
/// 电调型号。
|
||
pub model: EscModel,
|
||
/// 减速比（电机转子 : 输出轴）。
|
||
///
|
||
/// 驱动层**不使用**此字段；由上层 `crate::OutputShaft` 应用。
|
||
/// 保留于此作为配置元数据，`chassisd` 包装轮子时必须读取本字段（单一来源）。
|
||
pub gear_ratio: f32,
|
||
/// 安装方向：`+1.0` 或 `-1.0`。反装电机的读写符号在此统一翻转，
|
||
/// 上层永远以"输出轴正转 = 前进方向"工作。
|
||
pub direction: f32,
|
||
/// effort 限幅（raw，`1..=型号满量程`）。
|
||
pub current_limit: i16,
|
||
}
|
||
|
||
impl EscConfig {
|
||
/// GM3508 + C620 的常用配置。`kp/ki/ff` 为起步值，需按整车整定。
|
||
#[must_use]
|
||
pub fn gm3508(name: impl Into<String>, esc_id: u8, direction: f32) -> Self {
|
||
Self {
|
||
name: name.into(),
|
||
esc_id,
|
||
model: EscModel::C620,
|
||
// 3591 / 187 ≈ 19.203
|
||
gear_ratio: 3591.0 / 187.0,
|
||
direction,
|
||
current_limit: 12_000,
|
||
}
|
||
}
|
||
|
||
/// M2006 + C610（拨弹轮）的常用配置。
|
||
///
|
||
/// 减速比按 P36 起步（36.0），接硬件前需核对；电流标度见 [`EscModel::C610`]。
|
||
#[must_use]
|
||
pub fn m2006(name: impl Into<String>, esc_id: u8, direction: f32) -> Self {
|
||
Self {
|
||
name: name.into(),
|
||
esc_id,
|
||
model: EscModel::C610,
|
||
// M2006 P36 减速比（接入前核对）
|
||
gear_ratio: 36.0,
|
||
direction,
|
||
current_limit: 8_000,
|
||
}
|
||
}
|
||
|
||
fn validate(&self) -> Result<(), String> {
|
||
if command_frame_id(self.esc_id).is_none() {
|
||
return Err(format!("esc_id {} out of range 1..=8", self.esc_id));
|
||
}
|
||
if !(self.gear_ratio.is_finite() && self.gear_ratio > 0.0) {
|
||
return Err(format!("gear_ratio must be > 0, got {}", self.gear_ratio));
|
||
}
|
||
// 逐位比较：direction 只允许精确的 ±1.0。
|
||
let dir_bits = self.direction.to_bits();
|
||
if dir_bits != 1.0f32.to_bits() && dir_bits != (-1.0f32).to_bits() {
|
||
return Err(format!("direction must be +1.0 or -1.0, got {}", self.direction));
|
||
}
|
||
if !(1..=self.model.max_current_raw()).contains(&self.current_limit) {
|
||
return Err(format!(
|
||
"current_limit {} out of range 1..={} for {:?}",
|
||
self.current_limit,
|
||
self.model.max_current_raw(),
|
||
self.model
|
||
));
|
||
}
|
||
Ok(())
|
||
}
|
||
}
|
||
|
||
/// 总线运行参数。
|
||
#[derive(Debug, Clone)]
|
||
pub struct BusOptions {
|
||
/// 控制节拍（C620 期望 1kHz 命令）。
|
||
pub period: Duration,
|
||
/// 反馈离线阈值：超过该时长未收到某电机反馈 → 强制 0 输出。
|
||
pub feedback_timeout: Duration,
|
||
/// **命令租约**：上层超过该时长未重新下发命令 → 视为 `Disabled`（0 输出）。
|
||
///
|
||
/// 命令是**租约**而非一次性设定值：`set_velocity` / `set_effort_raw` /
|
||
/// `set_position` 每次调用都会续租。上层必须按自己的控制节拍周期性下发
|
||
/// （`chassisd` 的 1kHz 循环天然满足；台架脚本每拍重发即可），
|
||
/// 否则驱动会判定"上层任务已死"并强制零输出——这是防失控的安全设计。
|
||
///
|
||
/// 设置为 `Duration::MAX` 可退化为纯设定值语义（仅建议在无失控风险的台架场景）。
|
||
pub command_timeout: Duration,
|
||
}
|
||
|
||
impl Default for BusOptions {
|
||
fn default() -> Self {
|
||
Self {
|
||
period: Duration::from_millis(1),
|
||
feedback_timeout: Duration::from_millis(100),
|
||
// 1kHz 下约 50 拍未刷新即失效。
|
||
command_timeout: Duration::from_millis(50),
|
||
}
|
||
}
|
||
}
|
||
|
||
/// 启动错误。
|
||
#[derive(Debug, Clone, PartialEq, Eq, Error)]
|
||
pub enum EscBusError {
|
||
/// 两台电机配置了同一电调 ID。
|
||
#[error("duplicate ESC id {0}")]
|
||
DuplicateEscId(u8),
|
||
/// 电机配置非法（原因见 `reason`）。
|
||
#[error("invalid config for motor {name}: {reason}")]
|
||
InvalidConfig {
|
||
/// 电机名。
|
||
name: String,
|
||
/// 非法原因。
|
||
reason: String,
|
||
},
|
||
/// 一台电机都没有配置。
|
||
#[error("no motors configured")]
|
||
NoMotors,
|
||
}
|
||
|
||
/// 单电机共享状态（驱动任务与 [`EscMotor`] 句柄之间）。
|
||
struct MotorShared {
|
||
cfg: EscConfig,
|
||
state: Mutex<MotorState>,
|
||
cmd: Mutex<Command>,
|
||
/// 命令写入时刻（看门狗用）。
|
||
cmd_at: Mutex<Option<Instant>>,
|
||
turn: Mutex<TurnTracker>,
|
||
last_feedback: Mutex<Option<Instant>>,
|
||
offline_warned: std::sync::atomic::AtomicBool,
|
||
}
|
||
|
||
impl MotorShared {
|
||
fn new(cfg: EscConfig) -> Self {
|
||
Self {
|
||
cfg,
|
||
state: Mutex::new(MotorState::default()),
|
||
cmd: Mutex::new(Command::Disabled),
|
||
cmd_at: Mutex::new(None),
|
||
turn: Mutex::new(TurnTracker::new()),
|
||
last_feedback: Mutex::new(None),
|
||
offline_warned: std::sync::atomic::AtomicBool::new(false),
|
||
}
|
||
}
|
||
|
||
fn store_command(&self, cmd: Command) {
|
||
*lock(&self.cmd) = cmd;
|
||
*lock(&self.cmd_at) = Some(Instant::now());
|
||
}
|
||
|
||
/// 应用一帧反馈（原始电调侧值 → 方向修正后的物理量）。
|
||
fn apply_feedback(&self, fb: EscFeedback, now: Instant) {
|
||
let esc_rev = lock(&self.turn).update(fb.angle_raw);
|
||
let dir = self.cfg.direction;
|
||
{
|
||
let mut st = lock(&self.state);
|
||
st.rotor_angle_raw = fb.angle_raw;
|
||
st.rotor_revolutions = dir * esc_rev;
|
||
st.rotor_speed_rpm = dir * f32::from(fb.speed_rpm);
|
||
st.effort_raw = (dir * f32::from(fb.current_raw)) as i16;
|
||
st.temperature_c = fb.temperature_c;
|
||
st.feedback_count = st.feedback_count.saturating_add(1);
|
||
}
|
||
*lock(&self.last_feedback) = Some(now);
|
||
self.offline_warned.store(false, std::sync::atomic::Ordering::Relaxed);
|
||
}
|
||
|
||
/// Produce one policy sample while enforcing feedback and command leases.
|
||
fn sample(
|
||
&self,
|
||
feedback_timeout: Duration,
|
||
command_timeout: Duration,
|
||
now: Instant,
|
||
) -> PolicySample {
|
||
let online = matches!(
|
||
*lock(&self.last_feedback),
|
||
Some(t) if now.saturating_duration_since(t) <= feedback_timeout
|
||
);
|
||
if !online && !self.offline_warned.swap(true, std::sync::atomic::Ordering::Relaxed) {
|
||
warn!(motor = %self.cfg.name, esc_id = self.cfg.esc_id, "feedback offline, forcing zero current");
|
||
}
|
||
let fresh = matches!(
|
||
*lock(&self.cmd_at),
|
||
Some(t) if now.saturating_duration_since(t) <= command_timeout
|
||
);
|
||
let command = if online && fresh { *lock(&self.cmd) } else { Command::Disabled };
|
||
PolicySample { id: self.cfg.esc_id, state: *lock(&self.state), command, online }
|
||
}
|
||
}
|
||
|
||
/// 电机的公共句柄：可克隆，跨任务共享。
|
||
///
|
||
/// # 命令租约
|
||
///
|
||
/// 下发命令（[`VelocityDrive::set_velocity`] / [`EffortDrive::set_effort_raw`] /
|
||
/// [`PositionDrive::set_position`]）即续租，租期见 [`BusOptions::command_timeout`]。
|
||
/// 只在启动时下发一次、之后不再刷新的脚本会在租约到期后自动失电（0 输出），
|
||
/// 因此台架工具必须在控制循环里逐拍重发同一个设定值。
|
||
#[derive(Clone)]
|
||
pub struct EscMotor {
|
||
shared: Arc<MotorShared>,
|
||
}
|
||
|
||
impl EscMotor {
|
||
/// 电机名。
|
||
#[must_use]
|
||
pub fn name(&self) -> &str {
|
||
&self.shared.cfg.name
|
||
}
|
||
|
||
/// 电机配置。
|
||
#[must_use]
|
||
pub fn config(&self) -> &EscConfig {
|
||
&self.shared.cfg
|
||
}
|
||
|
||
/// 当前命令。
|
||
#[must_use]
|
||
pub fn command(&self) -> Command {
|
||
*lock(&self.shared.cmd)
|
||
}
|
||
|
||
/// 最近一帧反馈的时刻（诊断用）。
|
||
#[must_use]
|
||
pub fn last_feedback(&self) -> Option<Instant> {
|
||
*lock(&self.shared.last_feedback)
|
||
}
|
||
}
|
||
|
||
impl MotorFeedback for EscMotor {
|
||
fn id(&self) -> u8 {
|
||
self.shared.cfg.esc_id
|
||
}
|
||
|
||
fn state(&self) -> MotorState {
|
||
*lock(&self.shared.state)
|
||
}
|
||
|
||
fn feedback_age(&self) -> Option<std::time::Duration> {
|
||
let t = *lock(&self.shared.last_feedback);
|
||
Some(Instant::now().saturating_duration_since(t?))
|
||
}
|
||
}
|
||
|
||
impl VelocityDrive for EscMotor {
|
||
/// 设定目标转子转速（**租约**，见 [`EscMotor`] 的命令租约说明）。
|
||
fn set_velocity(&self, rotor_rpm: f32) {
|
||
self.shared.store_command(Command::Velocity(rotor_rpm));
|
||
}
|
||
}
|
||
|
||
impl EffortDrive for EscMotor {
|
||
/// 直发开环 raw（**租约**，见 [`EscMotor`] 的命令租约说明）。
|
||
fn set_effort_raw(&self, raw: i16) {
|
||
self.shared.store_command(Command::EffortRaw(raw));
|
||
}
|
||
}
|
||
|
||
impl PositionDrive for EscMotor {
|
||
/// 设定目标转子多圈位置（**租约**，见 [`EscMotor`] 的命令租约说明）。
|
||
fn set_position(&self, rotor_rev: f32) {
|
||
self.shared.store_command(Command::Position(rotor_rev));
|
||
}
|
||
}
|
||
|
||
impl Disable for EscMotor {
|
||
/// 停用（0 输出）；同样受租约保护——上层若已死，租约到期后依旧归零。
|
||
fn disable(&self) {
|
||
self.shared.store_command(Command::Disabled);
|
||
}
|
||
}
|
||
|
||
/// 运行中的电调总线。
|
||
pub struct EscBus;
|
||
|
||
impl EscBus {
|
||
/// 启动收发任务，返回句柄。
|
||
///
|
||
/// # Errors
|
||
///
|
||
/// 配置为空、电调 ID 重复或单台配置非法时返回 [`EscBusError`]。
|
||
pub fn spawn<P: EffortPolicy>(
|
||
link: CanLink,
|
||
configs: Vec<EscConfig>,
|
||
options: BusOptions,
|
||
policy: P,
|
||
) -> Result<EscBusHandle<P>, EscBusError> {
|
||
if configs.is_empty() {
|
||
return Err(EscBusError::NoMotors);
|
||
}
|
||
let mut motors = Vec::with_capacity(configs.len());
|
||
let mut seen = std::collections::HashSet::new();
|
||
for cfg in configs {
|
||
cfg.validate()
|
||
.map_err(|reason| EscBusError::InvalidConfig { name: cfg.name.clone(), reason })?;
|
||
if !seen.insert(cfg.esc_id) {
|
||
return Err(EscBusError::DuplicateEscId(cfg.esc_id));
|
||
}
|
||
motors.push(EscMotor { shared: Arc::new(MotorShared::new(cfg)) });
|
||
}
|
||
|
||
let (shutdown_tx, shutdown_rx) = watch::channel(false);
|
||
let CanLink { outgoing_tx, incoming_rx } = link;
|
||
let motors = Arc::new(motors);
|
||
let policy = Arc::new(Mutex::new(policy));
|
||
let rx_handle =
|
||
tokio::spawn(rx_loop(incoming_rx, Arc::clone(&motors), shutdown_rx.clone()));
|
||
let tx_handle = tokio::spawn(tx_loop(
|
||
outgoing_tx,
|
||
Arc::clone(&motors),
|
||
Arc::clone(&policy),
|
||
options,
|
||
shutdown_rx,
|
||
));
|
||
|
||
Ok(EscBusHandle { motors, policy, shutdown_tx, tasks: vec![rx_handle, tx_handle] })
|
||
}
|
||
}
|
||
|
||
/// `EscBus` 的控制句柄。
|
||
pub struct EscBusHandle<P> {
|
||
motors: Arc<Vec<EscMotor>>,
|
||
policy: Arc<Mutex<P>>,
|
||
shutdown_tx: watch::Sender<bool>,
|
||
tasks: Vec<JoinHandle<()>>,
|
||
}
|
||
|
||
impl<P: EffortPolicy> EscBusHandle<P> {
|
||
/// 按电调 ID 取电机句柄。
|
||
#[must_use]
|
||
pub fn motor(&self, esc_id: u8) -> Option<&EscMotor> {
|
||
self.motors.iter().find(|m| m.id() == esc_id)
|
||
}
|
||
|
||
/// 全部电机（按配置顺序）。
|
||
#[must_use]
|
||
pub fn motors(&self) -> &[EscMotor] {
|
||
&self.motors
|
||
}
|
||
|
||
/// Mutate the composed control policy between bus ticks.
|
||
pub fn with_policy<R>(&self, update: impl FnOnce(&mut P) -> R) -> R {
|
||
update(&mut lock(&self.policy))
|
||
}
|
||
|
||
/// 停止收发任务并等待退出。
|
||
pub async fn shutdown(self) {
|
||
let _ = self.shutdown_tx.send(true);
|
||
for task in self.tasks {
|
||
let _ = task.await;
|
||
}
|
||
}
|
||
}
|
||
|
||
async fn rx_loop(
|
||
mut incoming: mpsc::Receiver<CanFrame>,
|
||
motors: Arc<Vec<EscMotor>>,
|
||
mut shutdown: watch::Receiver<bool>,
|
||
) {
|
||
loop {
|
||
let frame = tokio::select! {
|
||
_ = shutdown.changed() => break,
|
||
frame = incoming.recv() => match frame {
|
||
Some(frame) => frame,
|
||
None => break,
|
||
},
|
||
};
|
||
match decode_feedback(&frame) {
|
||
Ok((esc_id, fb)) => {
|
||
if let Some(motor) = motors.iter().find(|m| m.id() == esc_id) {
|
||
motor.shared.apply_feedback(fb, Instant::now());
|
||
}
|
||
}
|
||
Err(e) => {
|
||
debug!(error = %e, id = frame.id, "ignore non-feedback frame");
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
async fn tx_loop<P: EffortPolicy>(
|
||
outgoing: mpsc::Sender<CanFrame>,
|
||
motors: Arc<Vec<EscMotor>>,
|
||
policy: Arc<Mutex<P>>,
|
||
options: BusOptions,
|
||
mut shutdown: watch::Receiver<bool>,
|
||
) {
|
||
let nominal_dt = options.period.as_secs_f32();
|
||
let mut ticker = tick_every(options.period);
|
||
let mut last_tick = Instant::now();
|
||
loop {
|
||
tokio::select! {
|
||
_ = shutdown.changed() => break,
|
||
_ = ticker.tick() => {}
|
||
}
|
||
let now = Instant::now();
|
||
let raw_dt = now.saturating_duration_since(last_tick).as_secs_f32();
|
||
last_tick = now;
|
||
let dt = raw_dt.clamp(nominal_dt * 0.5, nominal_dt * 4.0);
|
||
let mut samples: [PolicySample; 8] = std::array::from_fn(|index| PolicySample {
|
||
id: u8::try_from(index + 1).unwrap_or(0),
|
||
state: MotorState::default(),
|
||
command: Command::Disabled,
|
||
online: false,
|
||
});
|
||
for motor in motors.iter() {
|
||
let sample =
|
||
motor.shared.sample(options.feedback_timeout, options.command_timeout, now);
|
||
if let Some(slot) = samples.get_mut(usize::from(sample.id.saturating_sub(1))) {
|
||
*slot = sample;
|
||
}
|
||
}
|
||
let efforts = lock(&policy).step(&PolicyInput { samples, dt });
|
||
for (frame_id, group_start) in
|
||
[(COMMAND_FRAME_ID_PRIMARY, 1u8), (COMMAND_FRAME_ID_SECONDARY, 5u8)]
|
||
{
|
||
let group: Vec<&EscMotor> = motors
|
||
.iter()
|
||
.filter(|m| (group_start..group_start + 4).contains(&m.id()))
|
||
.collect();
|
||
if group.is_empty() {
|
||
continue;
|
||
}
|
||
let mut currents = [0i16; 4];
|
||
for motor in &group {
|
||
let slot = usize::from(motor.id() - group_start);
|
||
let policy_index = usize::from(motor.id().saturating_sub(1));
|
||
let physical = efforts.get(policy_index).copied().unwrap_or(0);
|
||
let directed = (f32::from(physical) * motor.shared.cfg.direction) as i16;
|
||
currents[slot] = clamp_current(
|
||
motor.shared.cfg.model,
|
||
directed.clamp(-motor.shared.cfg.current_limit, motor.shared.cfg.current_limit),
|
||
);
|
||
}
|
||
if outgoing.send(encode_current_frame(frame_id, currents)).await.is_err() {
|
||
info!("can link closed, tx loop exiting");
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
fn tick_every(period: Duration) -> Interval {
|
||
let mut ticker = tokio::time::interval(period);
|
||
// 错过节拍后直接跳到当前节拍，避免补发帧风暴。
|
||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||
ticker
|
||
}
|
||
|
||
#[cfg(test)]
|
||
mod tests {
|
||
use super::*;
|
||
|
||
#[test]
|
||
fn config_validate_rejects_bad_values() {
|
||
let mut cfg = EscConfig::gm3508("m", 1, 1.0);
|
||
|
||
cfg.esc_id = 0;
|
||
assert!(cfg.validate().is_err());
|
||
cfg.esc_id = 9;
|
||
assert!(cfg.validate().is_err());
|
||
cfg.esc_id = 5;
|
||
|
||
cfg.gear_ratio = 0.0;
|
||
assert!(cfg.validate().is_err());
|
||
cfg.gear_ratio = 19.2;
|
||
|
||
cfg.direction = 0.5;
|
||
assert!(cfg.validate().is_err());
|
||
cfg.direction = -1.0;
|
||
|
||
cfg.current_limit = 0;
|
||
assert!(cfg.validate().is_err());
|
||
cfg.current_limit = 16385;
|
||
assert!(cfg.validate().is_err());
|
||
cfg.current_limit = 12_000;
|
||
|
||
assert!(cfg.validate().is_ok());
|
||
}
|
||
|
||
#[test]
|
||
fn gm3508_defaults_are_sane() {
|
||
let cfg = EscConfig::gm3508("front_left", 1, -1.0);
|
||
assert_eq!(cfg.model, EscModel::C620);
|
||
assert!((cfg.gear_ratio - 19.2).abs() < 0.01);
|
||
assert_eq!(cfg.current_limit, 12_000);
|
||
assert_eq!(cfg.direction.to_bits(), (-1.0f32).to_bits());
|
||
assert!(cfg.validate().is_ok());
|
||
}
|
||
|
||
#[test]
|
||
fn m2006_defaults_are_sane() {
|
||
let cfg = EscConfig::m2006("feeder", 5, 1.0);
|
||
assert_eq!(cfg.model, EscModel::C610);
|
||
assert!((cfg.gear_ratio - 36.0).abs() < 0.01);
|
||
assert!(cfg.validate().is_ok());
|
||
}
|
||
}
|