feat: 初始化
This commit is contained in:
Horldsence
@@ -0,0 +1,20 @@
|
||||
[package]
|
||||
name = "dji-esc"
|
||||
version.workspace = true
|
||||
edition.workspace = true
|
||||
rust-version.workspace = true
|
||||
license.workspace = true
|
||||
description = "DJI RoboMaster ESC (C620/C610) CAN protocol: codec, multi-turn tracking, and async bus driver"
|
||||
|
||||
[lints]
|
||||
workspace = true
|
||||
|
||||
[dependencies]
|
||||
motor-core = { workspace = true }
|
||||
can-link = { workspace = true }
|
||||
tokio = { workspace = true }
|
||||
thiserror = { workspace = true }
|
||||
tracing = { workspace = true }
|
||||
|
||||
[dev-dependencies]
|
||||
tokio = { workspace = true, features = ["test-util"] }
|
||||
@@ -0,0 +1,512 @@
|
||||
//! `EscBus` 异步驱动：协议状态机与 1kHz 控制节拍。
|
||||
//!
|
||||
//! [`EscBus::spawn`] 消费一个 [`CanLink`]，启动收发两个任务：
|
||||
//!
|
||||
//! - 接收：解码反馈帧，更新各电机共享状态（含多圈累计与方向修正）；
|
||||
//! - 发送：按 `period` 节拍为每台电机求值当前命令（速度/位置环 PID / 直发
|
||||
//! effort），按 `0x200`/`0x1FF` 分组组帧发送。
|
||||
//!
|
||||
//! 电机离线（从未收到反馈或反馈超时）时强制 0 输出并复位控制器。
|
||||
//! 命令模式切换时同样复位控制器，避免积分冲击。
|
||||
|
||||
use std::sync::{Arc, Mutex, MutexGuard};
|
||||
use std::time::Duration;
|
||||
|
||||
use motor_core::{
|
||||
Command, CommandMode, Disable, EffortDrive, MotorFeedback, MotorState, Pid, PositionController,
|
||||
PositionDrive, VelocityController, VelocityDrive,
|
||||
};
|
||||
use thiserror::Error;
|
||||
use tokio::sync::{mpsc, watch};
|
||||
use tokio::task::JoinHandle;
|
||||
use tokio::time::{Instant, Interval, MissedTickBehavior};
|
||||
use tracing::{debug, info, warn};
|
||||
|
||||
use crate::codec::{
|
||||
EscFeedback, EscModel, clamp_current, command_frame_id, decode_feedback, encode_current_frame,
|
||||
};
|
||||
use crate::frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, CanLink};
|
||||
use crate::turn::TurnTracker;
|
||||
|
||||
/// 锁辅助：中毒锁直接恢复内部数据（锁中毒只意味着持锁线程恐慌，
|
||||
/// 状态本身仍一致，驱动机不应因此停摆）。
|
||||
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,
|
||||
/// 减速比（电机转子 : 输出轴）。
|
||||
pub gear_ratio: f32,
|
||||
/// 安装方向：`+1.0` 或 `-1.0`。反装电机的读写符号在此统一翻转，
|
||||
/// 上层永远以"输出轴正转 = 前进方向"工作。
|
||||
pub direction: f32,
|
||||
/// effort 限幅（raw，`1..=型号满量程`）。
|
||||
pub current_limit: i16,
|
||||
/// 速度环比例增益（raw / rpm）。
|
||||
pub kp: f32,
|
||||
/// 速度环积分增益（raw / (rpm·s)）。
|
||||
pub ki: f32,
|
||||
/// 速度环微分增益。
|
||||
pub kd: f32,
|
||||
/// 速度前馈（raw / rpm）。
|
||||
pub feedforward: f32,
|
||||
}
|
||||
|
||||
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,
|
||||
kp: 10.0,
|
||||
ki: 0.5,
|
||||
kd: 0.0,
|
||||
feedforward: 1.5,
|
||||
}
|
||||
}
|
||||
|
||||
/// 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,
|
||||
kp: 8.0,
|
||||
ki: 0.4,
|
||||
kd: 0.0,
|
||||
feedforward: 1.0,
|
||||
}
|
||||
}
|
||||
|
||||
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,
|
||||
}
|
||||
|
||||
impl Default for BusOptions {
|
||||
fn default() -> Self {
|
||||
Self { period: Duration::from_millis(1), feedback_timeout: Duration::from_millis(100) }
|
||||
}
|
||||
}
|
||||
|
||||
/// 启动错误。
|
||||
#[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>,
|
||||
last_mode: Mutex<CommandMode>,
|
||||
vel_ctrl: Mutex<VelocityController>,
|
||||
pos_ctrl: Mutex<PositionController>,
|
||||
turn: Mutex<TurnTracker>,
|
||||
last_feedback: Mutex<Option<Instant>>,
|
||||
offline_warned: std::sync::atomic::AtomicBool,
|
||||
}
|
||||
|
||||
impl MotorShared {
|
||||
fn new(cfg: EscConfig) -> Self {
|
||||
let limit = cfg.current_limit;
|
||||
let pid = Pid::new(cfg.kp, cfg.ki, cfg.kd, -f32::from(limit), f32::from(limit));
|
||||
let pos_pid = Pid::new(cfg.kp, cfg.ki, cfg.kd, -f32::from(limit), f32::from(limit));
|
||||
Self {
|
||||
vel_ctrl: Mutex::new(VelocityController::new(pid, cfg.feedforward)),
|
||||
pos_ctrl: Mutex::new(PositionController::direct(pos_pid)),
|
||||
cfg,
|
||||
state: Mutex::new(MotorState::default()),
|
||||
cmd: Mutex::new(Command::Disabled),
|
||||
last_mode: Mutex::new(CommandMode::Disabled),
|
||||
turn: Mutex::new(TurnTracker::new()),
|
||||
last_feedback: Mutex::new(None),
|
||||
offline_warned: std::sync::atomic::AtomicBool::new(false),
|
||||
}
|
||||
}
|
||||
|
||||
/// 应用一帧反馈（原始电调侧值 → 方向修正后的物理量）。
|
||||
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);
|
||||
}
|
||||
|
||||
/// 复位主机侧控制器（离线或模式切换时）。
|
||||
fn reset_controllers(&self) {
|
||||
lock(&self.vel_ctrl).reset();
|
||||
lock(&self.pos_ctrl).reset();
|
||||
}
|
||||
|
||||
/// 求值本节拍应下发的 effort 命令（电调侧 raw，含方向翻转与限幅）。
|
||||
fn compute_effort(&self, dt: f32, feedback_timeout: Duration, now: Instant) -> i16 {
|
||||
let online = matches!(*lock(&self.last_feedback), Some(t) if now.duration_since(t) <= feedback_timeout);
|
||||
if !online {
|
||||
if !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");
|
||||
}
|
||||
// 复位控制器，避免重新上线时旧积分冲击。
|
||||
self.reset_controllers();
|
||||
*lock(&self.last_mode) = CommandMode::Disabled;
|
||||
return 0;
|
||||
}
|
||||
let cmd = *lock(&self.cmd);
|
||||
// 模式切换复位：避免积分/微分记忆冲击。
|
||||
{
|
||||
let mut last_mode = lock(&self.last_mode);
|
||||
if *last_mode != cmd.mode() {
|
||||
debug!(motor = %self.cfg.name, from = ?*last_mode, to = ?cmd.mode(), "command mode switch, resetting controllers");
|
||||
self.reset_controllers();
|
||||
*last_mode = cmd.mode();
|
||||
}
|
||||
}
|
||||
let dir = self.cfg.direction;
|
||||
let raw = match cmd {
|
||||
Command::Disabled => 0,
|
||||
Command::EffortRaw(raw) => (f32::from(raw) * dir) as i16,
|
||||
Command::Velocity(target) => {
|
||||
let measured = lock(&self.state).rotor_speed_rpm;
|
||||
let out = lock(&self.vel_ctrl).update(target, measured, dt);
|
||||
(out * dir) as i16
|
||||
}
|
||||
Command::Position(target) => {
|
||||
let (measured_rev, measured_rpm) = {
|
||||
let st = lock(&self.state);
|
||||
(st.rotor_revolutions, st.rotor_speed_rpm)
|
||||
};
|
||||
let out = lock(&self.pos_ctrl).update(target, measured_rev, measured_rpm, dt);
|
||||
(out * dir) as i16
|
||||
}
|
||||
};
|
||||
let limit = self.cfg.current_limit;
|
||||
clamp_current(self.cfg.model, raw.clamp(-limit, limit))
|
||||
}
|
||||
}
|
||||
|
||||
/// 电机的公共句柄：可克隆，跨任务共享。
|
||||
#[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)
|
||||
}
|
||||
}
|
||||
|
||||
impl VelocityDrive for EscMotor {
|
||||
fn set_velocity(&self, rotor_rpm: f32) {
|
||||
*lock(&self.shared.cmd) = Command::Velocity(rotor_rpm);
|
||||
}
|
||||
}
|
||||
|
||||
impl EffortDrive for EscMotor {
|
||||
fn set_effort_raw(&self, raw: i16) {
|
||||
*lock(&self.shared.cmd) = Command::EffortRaw(raw);
|
||||
}
|
||||
}
|
||||
|
||||
impl PositionDrive for EscMotor {
|
||||
fn set_position(&self, rotor_rev: f32) {
|
||||
*lock(&self.shared.cmd) = Command::Position(rotor_rev);
|
||||
}
|
||||
}
|
||||
|
||||
impl Disable for EscMotor {
|
||||
fn disable(&self) {
|
||||
*lock(&self.shared.cmd) = Command::Disabled;
|
||||
}
|
||||
}
|
||||
|
||||
/// 运行中的电调总线。
|
||||
pub struct EscBus;
|
||||
|
||||
impl EscBus {
|
||||
/// 启动收发任务，返回句柄。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 配置为空、电调 ID 重复或单台配置非法时返回 [`EscBusError`]。
|
||||
pub fn spawn(
|
||||
link: CanLink,
|
||||
configs: Vec<EscConfig>,
|
||||
options: BusOptions,
|
||||
) -> Result<EscBusHandle, 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 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), options, shutdown_rx));
|
||||
|
||||
Ok(EscBusHandle { motors, shutdown_tx, tasks: vec![rx_handle, tx_handle] })
|
||||
}
|
||||
}
|
||||
|
||||
/// `EscBus` 的控制句柄。
|
||||
pub struct EscBusHandle {
|
||||
motors: Arc<Vec<EscMotor>>,
|
||||
shutdown_tx: watch::Sender<bool>,
|
||||
tasks: Vec<JoinHandle<()>>,
|
||||
}
|
||||
|
||||
impl EscBusHandle {
|
||||
/// 按电调 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
|
||||
}
|
||||
|
||||
/// 停止收发任务并等待退出。
|
||||
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(
|
||||
outgoing: mpsc::Sender<CanFrame>,
|
||||
motors: Arc<Vec<EscMotor>>,
|
||||
options: BusOptions,
|
||||
mut shutdown: watch::Receiver<bool>,
|
||||
) {
|
||||
let dt = options.period.as_secs_f32();
|
||||
let mut ticker = tick_every(options.period);
|
||||
loop {
|
||||
tokio::select! {
|
||||
_ = shutdown.changed() => break,
|
||||
_ = ticker.tick() => {}
|
||||
}
|
||||
let now = Instant::now();
|
||||
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);
|
||||
currents[slot] = motor.shared.compute_effort(dt, options.feedback_timeout, now);
|
||||
}
|
||||
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());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,249 @@
|
||||
//! DJI 电调（C620 / C610）CAN 协议编解码。
|
||||
//!
|
||||
//! 帧格式（大端）：
|
||||
//!
|
||||
//! - 控制（`0x200` 电调 1–4 / `0x1FF` 电调 5–8）：
|
||||
//! `[I1_H, I1_L, I2_H, I2_L, I3_H, I3_L, I4_H, I4_L]`，i16 电流命令，
|
||||
//! 槽位按组内电调顺序。
|
||||
//! - 反馈（`0x200 + 电调ID`）：
|
||||
//! `u16 角度(0..8191) + i16 转速rpm + i16 转矩电流 + i8 温度℃ + 保留`。
|
||||
//!
|
||||
//! 本模块只做纯编解码；主机侧速度/位置环由 [`motor_core`] 算法在
|
||||
//! [`crate::bus`] 中按节拍调用。
|
||||
|
||||
use crate::frame::{
|
||||
COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, FEEDBACK_FRAME_ID_BASE,
|
||||
};
|
||||
use thiserror::Error;
|
||||
|
||||
/// 电调 ID 下限。
|
||||
pub const MIN_ESC_ID: u8 = 1;
|
||||
/// 电调 ID 上限。
|
||||
pub const MAX_ESC_ID: u8 = 8;
|
||||
|
||||
/// 电调型号决定电流标度。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub enum EscModel {
|
||||
/// C620（配 GM3508）：±16384 ↔ ±20 A。
|
||||
C620,
|
||||
/// C610（配 M2006）：±10000 ↔ ±10 A。
|
||||
///
|
||||
/// 接入前需实测核对。
|
||||
C610,
|
||||
}
|
||||
|
||||
impl EscModel {
|
||||
/// 满量程电流命令（raw）。
|
||||
#[must_use]
|
||||
pub const fn max_current_raw(self) -> i16 {
|
||||
match self {
|
||||
Self::C620 => 16384,
|
||||
Self::C610 => 10_000,
|
||||
}
|
||||
}
|
||||
|
||||
/// 满量程电流（安培）。
|
||||
#[must_use]
|
||||
pub const fn max_current_amp(self) -> f32 {
|
||||
match self {
|
||||
Self::C620 => 20.0,
|
||||
Self::C610 => 10.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// 安培 → raw（限幅）。
|
||||
#[must_use]
|
||||
pub fn raw_from_amp(self, amp: f32) -> i16 {
|
||||
let raw = amp * f32::from(self.max_current_raw()) / self.max_current_amp();
|
||||
(raw.clamp(-f32::from(self.max_current_raw()), f32::from(self.max_current_raw())) as i16)
|
||||
.clamp(-self.max_current_raw(), self.max_current_raw())
|
||||
}
|
||||
|
||||
/// raw → 安培。
|
||||
#[must_use]
|
||||
pub fn amp_from_raw(self, raw: i16) -> f32 {
|
||||
f32::from(raw) * self.max_current_amp() / f32::from(self.max_current_raw())
|
||||
}
|
||||
}
|
||||
|
||||
/// 一帧电调反馈（原始、电调侧，未经方向修正）。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct EscFeedback {
|
||||
/// 转子机械角原始值（0..8191，对应 0..360°）。
|
||||
pub angle_raw: u16,
|
||||
/// 转子转速（rpm，电调侧符号）。
|
||||
pub speed_rpm: i16,
|
||||
/// 转矩电流 raw（电调侧符号）。
|
||||
pub current_raw: i16,
|
||||
/// 电机温度（℃）。
|
||||
pub temperature_c: i8,
|
||||
}
|
||||
|
||||
/// 帧解码错误。
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
|
||||
pub enum DecodeError {
|
||||
/// 帧不属于电调反馈范围（0x201..=0x208）。
|
||||
#[error("frame id 0x{id:03X} is not an ESC feedback frame (0x201..=0x208)")]
|
||||
NotFeedback {
|
||||
/// 收到的帧 ID。
|
||||
id: u32,
|
||||
},
|
||||
}
|
||||
|
||||
/// 电调 ID → 其电流命令所在的组控制帧 ID。
|
||||
///
|
||||
/// 返回 `None` 表示 ID 越界。
|
||||
#[must_use]
|
||||
pub const fn command_frame_id(esc_id: u8) -> Option<u32> {
|
||||
match esc_id {
|
||||
1..=4 => Some(COMMAND_FRAME_ID_PRIMARY),
|
||||
5..=8 => Some(COMMAND_FRAME_ID_SECONDARY),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
/// 将 raw 电流限幅到电调满量程。
|
||||
#[must_use]
|
||||
pub const fn clamp_current(model: EscModel, raw: i16) -> i16 {
|
||||
if raw > model.max_current_raw() {
|
||||
model.max_current_raw()
|
||||
} else if raw < -model.max_current_raw() {
|
||||
-model.max_current_raw()
|
||||
} else {
|
||||
raw
|
||||
}
|
||||
}
|
||||
|
||||
/// 组装一个控制组帧（`0x200` 或 `0x1FF`）。
|
||||
///
|
||||
/// `currents` 按组内槽位排列：主组对应电调 1–4，副组对应电调 5–8。
|
||||
/// 本函数只做纯编码；电流限幅由驱动层按各电机型号与限流配置完成
|
||||
/// （见 [`clamp_current`]）。
|
||||
#[must_use]
|
||||
pub fn encode_current_frame(frame_id: u32, currents: [i16; 4]) -> CanFrame {
|
||||
let mut data = [0u8; 8];
|
||||
for (slot, raw) in currents.iter().enumerate() {
|
||||
data[slot * 2] = (raw >> 8) as u8;
|
||||
data[slot * 2 + 1] = (raw & 0xFF) as u8;
|
||||
}
|
||||
CanFrame::new(frame_id, data)
|
||||
}
|
||||
|
||||
/// 组装一帧电调反馈（仿真器 / 测试用）。
|
||||
#[must_use]
|
||||
pub fn encode_feedback_frame(esc_id: u8, feedback: &EscFeedback) -> CanFrame {
|
||||
let mut data = [0u8; 8];
|
||||
data[0..2].copy_from_slice(&feedback.angle_raw.to_be_bytes());
|
||||
data[2..4].copy_from_slice(&feedback.speed_rpm.to_be_bytes());
|
||||
data[4..6].copy_from_slice(&feedback.current_raw.to_be_bytes());
|
||||
data[6] = feedback.temperature_c as u8;
|
||||
CanFrame::new(FEEDBACK_FRAME_ID_BASE + u32::from(esc_id), data)
|
||||
}
|
||||
|
||||
/// 解码一帧电调反馈，返回 `(电调ID, 反馈)`。
|
||||
///
|
||||
/// # Errors
|
||||
///
|
||||
/// 帧不属于反馈 ID 范围（0x201..=0x208）时返回 [`DecodeError::NotFeedback`]。
|
||||
pub fn decode_feedback(frame: &CanFrame) -> Result<(u8, EscFeedback), DecodeError> {
|
||||
if frame.id <= FEEDBACK_FRAME_ID_BASE
|
||||
|| frame.id > FEEDBACK_FRAME_ID_BASE + u32::from(MAX_ESC_ID)
|
||||
{
|
||||
return Err(DecodeError::NotFeedback { id: frame.id });
|
||||
}
|
||||
let esc_id = (frame.id - FEEDBACK_FRAME_ID_BASE) as u8;
|
||||
Ok((
|
||||
esc_id,
|
||||
EscFeedback {
|
||||
angle_raw: u16::from_be_bytes([frame.data[0], frame.data[1]]),
|
||||
speed_rpm: i16::from_be_bytes([frame.data[2], frame.data[3]]),
|
||||
current_raw: i16::from_be_bytes([frame.data[4], frame.data[5]]),
|
||||
temperature_c: frame.data[6] as i8,
|
||||
},
|
||||
))
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn command_frame_id_routing() {
|
||||
assert_eq!(command_frame_id(1), Some(0x200));
|
||||
assert_eq!(command_frame_id(4), Some(0x200));
|
||||
assert_eq!(command_frame_id(5), Some(0x1FF));
|
||||
assert_eq!(command_frame_id(8), Some(0x1FF));
|
||||
assert_eq!(command_frame_id(0), None);
|
||||
assert_eq!(command_frame_id(9), None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn encode_current_frame_bytes() {
|
||||
let frame = encode_current_frame(0x200, [16384, -16384, 0, 258]);
|
||||
assert_eq!(frame.id, 0x200);
|
||||
assert_eq!(frame.data, [0x40, 0x00, 0xC0, 0x00, 0x00, 0x00, 0x01, 0x02]);
|
||||
|
||||
let frame = encode_current_frame(0x1FF, [10000, -10000, 0, 0]);
|
||||
assert_eq!(frame.id, 0x1FF);
|
||||
assert_eq!(frame.data, [0x27, 0x10, 0xD8, 0xF0, 0x00, 0x00, 0x00, 0x00]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn decode_feedback_known_bytes() {
|
||||
let frame = CanFrame::new(0x205, [0x1F, 0xFF, 0xFE, 0x94, 0x00, 0x7B, 45, 0]);
|
||||
let (esc_id, fb) = decode_feedback(&frame).unwrap();
|
||||
assert_eq!(esc_id, 5);
|
||||
assert_eq!(fb.angle_raw, 8191);
|
||||
assert_eq!(fb.speed_rpm, -364);
|
||||
assert_eq!(fb.current_raw, 123);
|
||||
assert_eq!(fb.temperature_c, 45);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn decode_feedback_rejects_non_feedback_ids() {
|
||||
for id in [0x000, 0x1FF, 0x200, 0x209, 0x7FF] {
|
||||
let frame = CanFrame::new(id, [0; 8]);
|
||||
assert_eq!(
|
||||
decode_feedback(&frame),
|
||||
Err(DecodeError::NotFeedback { id }),
|
||||
"id = 0x{id:03X}"
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn feedback_roundtrip_all_ids() {
|
||||
let fb = EscFeedback {
|
||||
angle_raw: 8191,
|
||||
speed_rpm: -9254,
|
||||
current_raw: -16384,
|
||||
temperature_c: -40,
|
||||
};
|
||||
for esc_id in MIN_ESC_ID..=MAX_ESC_ID {
|
||||
let frame = encode_feedback_frame(esc_id, &fb);
|
||||
assert_eq!(frame.id, 0x200 + u32::from(esc_id));
|
||||
assert_eq!(decode_feedback(&frame).unwrap(), (esc_id, fb));
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn amp_conversion() {
|
||||
let c620 = EscModel::C620;
|
||||
assert!((c620.amp_from_raw(16384) - 20.0).abs() < 1e-4);
|
||||
assert!((c620.amp_from_raw(-8192) + 10.0).abs() < 1e-4);
|
||||
assert_eq!(c620.raw_from_amp(20.0), 16384);
|
||||
assert_eq!(c620.raw_from_amp(-5.0), -4096);
|
||||
// 超量程限幅。
|
||||
assert_eq!(c620.raw_from_amp(99.0), 16384);
|
||||
let c610 = EscModel::C610;
|
||||
assert!((c610.amp_from_raw(10000) - 10.0).abs() < 1e-4);
|
||||
assert_eq!(c610.raw_from_amp(10.0), 10000);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn clamp_current_respects_model() {
|
||||
assert_eq!(clamp_current(EscModel::C620, 20000), 16384);
|
||||
assert_eq!(clamp_current(EscModel::C610, 15000), 10000);
|
||||
assert_eq!(clamp_current(EscModel::C620, 1000), 1000);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
//! DJI 电调协议帧 ID 常量。
|
||||
//!
|
||||
//! 帧与链路类型（[`CanFrame`] / [`CanLink`]）由 [`can_link`] 提供，
|
||||
//! 本模块只保留 **C620/C610 电流协议族**的 ID 约定。
|
||||
|
||||
pub use can_link::{CanFrame, CanLink};
|
||||
|
||||
/// 控制帧 ID：电调 1–4 的电流命令（8 字节 = 4 × i16 大端）。
|
||||
pub const COMMAND_FRAME_ID_PRIMARY: u32 = 0x200;
|
||||
/// 控制帧 ID：电调 5–8 的电流命令。
|
||||
pub const COMMAND_FRAME_ID_SECONDARY: u32 = 0x1FF;
|
||||
/// 反馈帧 ID 基址：`0x200 + 电调ID`（0x201–0x208）。
|
||||
pub const FEEDBACK_FRAME_ID_BASE: u32 = 0x200;
|
||||
@@ -0,0 +1,29 @@
|
||||
//! DJI RoboMaster 电调（C620/C610）CAN 协议与异步驱动。
|
||||
//!
|
||||
//! 分层：
|
||||
//!
|
||||
//! - [`codec`]：纯函数帧编解码（C620 手册协议）；
|
||||
//! - [`turn`]：单圈编码器 → 连续多圈角；
|
||||
//! - [`frame`]：协议帧 ID 常量（帧/链路类型见 [`can_link`]）；
|
||||
//! - [`bus`]：[`EscBus`] 驱动——反馈分发、主机速度/位置环、分组组帧、离线安全；
|
||||
//! - [`sim`]：被控对象仿真（测试与 `--mock` 演示复用）。
|
||||
//!
|
||||
//! 电机以 [`motor_core`] 能力 trait 呈现，物理量均已按 `EscConfig::direction`
|
||||
//! 完成方向修正。
|
||||
#![cfg_attr(test, allow(clippy::unwrap_used, clippy::expect_used))]
|
||||
|
||||
pub mod bus;
|
||||
pub mod codec;
|
||||
pub mod frame;
|
||||
pub mod sim;
|
||||
pub mod turn;
|
||||
|
||||
pub use bus::{BusOptions, EscBus, EscBusError, EscBusHandle, EscConfig, EscMotor};
|
||||
pub use can_link::{CanFrame, CanLink};
|
||||
pub use codec::{
|
||||
DecodeError, EscFeedback, EscModel, MAX_ESC_ID, MIN_ESC_ID, clamp_current, command_frame_id,
|
||||
decode_feedback, encode_current_frame, encode_feedback_frame,
|
||||
};
|
||||
pub use frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, FEEDBACK_FRAME_ID_BASE};
|
||||
pub use sim::{PlantMotor, spawn_plant};
|
||||
pub use turn::{ENCODER_TICKS, TurnTracker};
|
||||
@@ -0,0 +1,116 @@
|
||||
//! 电机被控对象仿真：一阶转速模型 + 编码器积分。
|
||||
//!
|
||||
//! 与 `EscBus` 接在同一对 [`CanLink`] 上即可构成全链路闭环
|
||||
//! （测试、`chassisd --mock` 均复用）。
|
||||
|
||||
use std::collections::HashMap;
|
||||
use std::time::Duration;
|
||||
|
||||
use tokio::task::JoinHandle;
|
||||
use tokio::time::{MissedTickBehavior, interval};
|
||||
use tracing::trace;
|
||||
|
||||
use crate::codec::{EscFeedback, encode_feedback_frame};
|
||||
use crate::frame::{COMMAND_FRAME_ID_PRIMARY, COMMAND_FRAME_ID_SECONDARY, CanFrame, CanLink};
|
||||
|
||||
/// 单台电机被控对象（转子侧、电调坐标系：正电流 → 正转速）。
|
||||
///
|
||||
/// 模型：`rpm_target = current_raw × gain`（限幅于 `max_rpm`），转速以时间常数
|
||||
/// `tau` 一阶趋近目标；机械角按转速积分并按 8192 取模。
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct PlantMotor {
|
||||
gain_rpm_per_raw: f32,
|
||||
tau: f32,
|
||||
max_rpm: f32,
|
||||
speed_rpm: f32,
|
||||
angle_raw: f32,
|
||||
}
|
||||
|
||||
impl PlantMotor {
|
||||
/// GM3508 量级：16384 raw 空载约 9254 rpm，机电时间常数约 120ms。
|
||||
#[must_use]
|
||||
pub fn gm3508() -> Self {
|
||||
Self {
|
||||
gain_rpm_per_raw: 9254.0 / 16384.0,
|
||||
tau: 0.12,
|
||||
max_rpm: 9254.0,
|
||||
speed_rpm: 0.0,
|
||||
angle_raw: 0.0,
|
||||
}
|
||||
}
|
||||
|
||||
/// 当前转子转速（rpm，电调侧）。
|
||||
#[must_use]
|
||||
pub fn speed_rpm(&self) -> f32 {
|
||||
self.speed_rpm
|
||||
}
|
||||
|
||||
/// 推进一个时间步，返回本步反馈帧内容。
|
||||
pub fn step(&mut self, current_raw: i16, dt: f32) -> EscFeedback {
|
||||
let target =
|
||||
(f32::from(current_raw) * self.gain_rpm_per_raw).clamp(-self.max_rpm, self.max_rpm);
|
||||
let alpha = 1.0 - (-dt / self.tau).exp();
|
||||
self.speed_rpm += (target - self.speed_rpm) * alpha;
|
||||
self.angle_raw = (self.angle_raw + self.speed_rpm / 60.0 * 8192.0 * dt).rem_euclid(8192.0);
|
||||
EscFeedback {
|
||||
angle_raw: self.angle_raw as u16,
|
||||
speed_rpm: self.speed_rpm as i16,
|
||||
current_raw,
|
||||
temperature_c: 30,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// 仿真整条电调总线：消费命令帧（0x200/0x1FF），按 `tick` 节拍产出反馈帧。
|
||||
///
|
||||
/// 返回任务句柄，测试结束或退出演示时 `abort()` 即可。
|
||||
pub fn spawn_plant(mut link: CanLink, esc_ids: &[u8], tick: Duration) -> JoinHandle<()> {
|
||||
let mut plants: HashMap<u8, PlantMotor> =
|
||||
esc_ids.iter().map(|&id| (id, PlantMotor::gm3508())).collect();
|
||||
let mut commands: HashMap<u8, i16> = esc_ids.iter().map(|&id| (id, 0)).collect();
|
||||
let dt = tick.as_secs_f32();
|
||||
let mut ticker = interval(tick);
|
||||
ticker.set_missed_tick_behavior(MissedTickBehavior::Skip);
|
||||
|
||||
tokio::spawn(async move {
|
||||
loop {
|
||||
tokio::select! {
|
||||
frame = link.incoming_rx.recv() => match frame {
|
||||
Some(frame) => apply_command_frame(&frame, &mut commands),
|
||||
None => break,
|
||||
},
|
||||
_ = ticker.tick() => {
|
||||
for (&id, plant) in &mut plants {
|
||||
let cmd = commands.get(&id).copied().unwrap_or(0);
|
||||
let feedback = plant.step(cmd, dt);
|
||||
if link
|
||||
.outgoing_tx
|
||||
.send(encode_feedback_frame(id, &feedback))
|
||||
.await
|
||||
.is_err()
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
trace!("plant exiting");
|
||||
})
|
||||
}
|
||||
|
||||
fn apply_command_frame(frame: &CanFrame, commands: &mut HashMap<u8, i16>) {
|
||||
let group_start = match frame.id {
|
||||
COMMAND_FRAME_ID_PRIMARY => 1u8,
|
||||
COMMAND_FRAME_ID_SECONDARY => 5u8,
|
||||
_ => return,
|
||||
};
|
||||
for slot in 0..4u8 {
|
||||
if let Some(cmd) = commands.get_mut(&(group_start + slot)) {
|
||||
*cmd = i16::from_be_bytes([
|
||||
frame.data[(slot * 2) as usize],
|
||||
frame.data[(slot * 2 + 1) as usize],
|
||||
]);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,134 @@
|
||||
//! 多圈累计：单圈编码器原始角 → 连续圈数。
|
||||
|
||||
/// C620/C610 反馈编码器单圈计数（0..8191 对应 0..360°）。
|
||||
pub const ENCODER_TICKS: u16 = 8192;
|
||||
|
||||
/// 半圈阈值：帧间角变化超过它视为发生回绕。
|
||||
const HALF_TURN_TICKS: i32 = 4096;
|
||||
|
||||
/// 单圈机械角跟踪器：把 0..8191 的原始角序列转成连续累计角（单位：圈）。
|
||||
///
|
||||
/// 假设相邻两帧间转子转动不超过半圈。1kHz 反馈下 GM3508 转子上限约
|
||||
/// 9300 rpm ≈ 0.155 圈/ms，余量充足。
|
||||
#[derive(Debug, Clone, Default)]
|
||||
pub struct TurnTracker {
|
||||
revolutions: i32,
|
||||
prev_raw: Option<u16>,
|
||||
last_angle_rev: f32,
|
||||
}
|
||||
|
||||
impl TurnTracker {
|
||||
/// 创建跟踪器。
|
||||
#[must_use]
|
||||
pub const fn new() -> Self {
|
||||
Self { revolutions: 0, prev_raw: None, last_angle_rev: 0.0 }
|
||||
}
|
||||
|
||||
/// 送入新的原始角，返回连续累计角（圈）。
|
||||
pub fn update(&mut self, raw: u16) -> f32 {
|
||||
if let Some(prev) = self.prev_raw {
|
||||
let delta = i32::from(raw) - i32::from(prev);
|
||||
if delta > HALF_TURN_TICKS {
|
||||
// 正向跨越 8191 → 0。
|
||||
self.revolutions -= 1;
|
||||
} else if delta < -HALF_TURN_TICKS {
|
||||
// 反向跨越 0 → 8191。
|
||||
self.revolutions += 1;
|
||||
}
|
||||
}
|
||||
self.prev_raw = Some(raw);
|
||||
self.last_angle_rev = self.revolutions as f32 + f32::from(raw) / 8192.0;
|
||||
self.last_angle_rev
|
||||
}
|
||||
|
||||
/// 最近一次 [`TurnTracker::update`] 的返回值。
|
||||
#[must_use]
|
||||
pub const fn last_angle_rev(&self) -> f32 {
|
||||
self.last_angle_rev
|
||||
}
|
||||
|
||||
/// 清零回到初始状态。
|
||||
pub fn reset(&mut self) {
|
||||
*self = Self::new();
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn first_reading_is_raw_fraction() {
|
||||
let mut t = TurnTracker::new();
|
||||
assert!((t.update(4096) - 0.5).abs() < 1e-4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn forward_wrap_counts_up() {
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(8000);
|
||||
let after = t.update(100); // 8000 → 8191 → 0 → 100
|
||||
assert!((after - (8192.0 + 100.0) / 8192.0).abs() < 1e-4, "{after}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reverse_wrap_counts_down() {
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(100);
|
||||
let after = t.update(8000); // 100 → 0 → 8191 → 8000（反向）
|
||||
assert!((after - (8000.0 - 8192.0) / 8192.0).abs() < 1e-4, "{after}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn small_deltas_do_not_wrap() {
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(0);
|
||||
assert!((t.update(4000) - 4000.0 / 8192.0).abs() < 1e-4);
|
||||
assert!((t.update(4100) - 4100.0 / 8192.0).abs() < 1e-4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn multi_turn_forward_then_reverse_returns() {
|
||||
let mut t = TurnTracker::new();
|
||||
// 正向 3 整圈：每圈 0 → 4000 → 8000 → 100（跨 8191 回绕，净 +1）。
|
||||
for _ in 0..3 {
|
||||
t.update(0);
|
||||
t.update(4000);
|
||||
t.update(8000);
|
||||
t.update(100);
|
||||
}
|
||||
let fwd = t.last_angle_rev();
|
||||
let expected_fwd = 3.0 + 100.0 / 8192.0;
|
||||
assert!((fwd - expected_fwd).abs() < 1e-4, "{fwd} vs {expected_fwd}");
|
||||
|
||||
// 沿同一路径反向退回 3 圈：100 → 8000（反向回绕）→ 4000 → 0，净 -1/圈。
|
||||
for _ in 0..3 {
|
||||
t.update(8000);
|
||||
t.update(4000);
|
||||
t.update(0);
|
||||
}
|
||||
let back = t.last_angle_rev();
|
||||
assert!(back.abs() < 1e-4, "should return to 0, got {back}");
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn jitter_at_boundary_stays_put() {
|
||||
// 0 附近抖动不产生虚假圈数。
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(8191);
|
||||
assert!((t.update(0) - 1.0).abs() < 1e-4); // 真实跨越一次
|
||||
assert!((t.update(2) - (2.0 / 8192.0 + 1.0)).abs() < 1e-4);
|
||||
assert!((t.update(8190) - (8190.0 / 8192.0)).abs() < 1e-4); // 抖回
|
||||
assert!((t.update(5) - (1.0 + 5.0 / 8192.0)).abs() < 1e-4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reset_forgets_history() {
|
||||
let mut t = TurnTracker::new();
|
||||
t.update(8000);
|
||||
t.update(100);
|
||||
t.reset();
|
||||
assert_eq!(t.last_angle_rev().to_bits(), 0.0f32.to_bits());
|
||||
assert!((t.update(4096) - 0.5).abs() < 1e-4);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,275 @@
|
||||
//! 闭环集成测试：`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:?}"
|
||||
);
|
||||
}
|
||||
Reference in New Issue
Block a user