237 lines
7.8 KiB
Rust
237 lines
7.8 KiB
Rust
//! 场景定义：设定值曲线 + 采样节拍。
|
||
//!
|
||
//! 场景与具体被控对象解耦：同一场景可在 [`crate::PlantMotor`]（离线）、
|
||
//! `CanLink` 上的 `EscBus` 闭环（在线）与真机台架（`chassis-bench`）上重放，
|
||
//! 因此整定结果可以跨后端比较。
|
||
|
||
/// 设定值曲线。
|
||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||
pub enum SetpointProfile {
|
||
/// 阶跃：`t = 0` 直接给目标（忽略 `ramp_s`）。
|
||
Step,
|
||
/// 斜坡 + 保持：`ramp_s` 内线性升到目标，随后保持 `hold_s`。
|
||
RampAndHold,
|
||
/// 阶梯：在 `ramp_s + hold_s` 内均分 `levels` 级，逐级跳变到目标。
|
||
Staircase {
|
||
/// 阶梯数（≥ 1）。
|
||
levels: u32,
|
||
},
|
||
/// 升 → 保持 → 降 → 保持（回零），总时长 `2 × (ramp_s + hold_s)`。
|
||
UpDown,
|
||
}
|
||
|
||
/// 一次试验的设定值与时间规格。
|
||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||
pub struct Scenario {
|
||
/// 控制 / 采样节拍（秒）。
|
||
pub dt_s: f32,
|
||
/// 斜坡时间（秒）；`0` 表示瞬时。
|
||
pub ramp_s: f32,
|
||
/// 保持时间（秒）。
|
||
pub hold_s: f32,
|
||
/// 目标转子转速（rpm）。
|
||
pub target_rpm: f32,
|
||
/// 设定值曲线。
|
||
pub profile: SetpointProfile,
|
||
}
|
||
|
||
impl Scenario {
|
||
/// 默认控制节拍（1 kHz，与 C620 命令周期一致）。
|
||
pub const DEFAULT_DT_S: f32 = 0.001;
|
||
|
||
/// 阶跃 + 保持。
|
||
#[must_use]
|
||
pub const fn step(target_rpm: f32, hold_s: f32) -> Self {
|
||
Self {
|
||
dt_s: Self::DEFAULT_DT_S,
|
||
ramp_s: 0.0,
|
||
hold_s,
|
||
target_rpm,
|
||
profile: SetpointProfile::Step,
|
||
}
|
||
}
|
||
|
||
/// 斜坡 + 保持（台架默认整定场景）。
|
||
#[must_use]
|
||
pub const fn ramp_and_hold(target_rpm: f32, ramp_s: f32, hold_s: f32) -> Self {
|
||
Self {
|
||
dt_s: Self::DEFAULT_DT_S,
|
||
ramp_s,
|
||
hold_s,
|
||
target_rpm,
|
||
profile: SetpointProfile::RampAndHold,
|
||
}
|
||
}
|
||
|
||
/// 换一个采样节拍。
|
||
#[must_use]
|
||
pub const fn with_dt_s(mut self, dt_s: f32) -> Self {
|
||
self.dt_s = dt_s;
|
||
self
|
||
}
|
||
|
||
/// 换一条设定值曲线。
|
||
#[must_use]
|
||
pub const fn with_profile(mut self, profile: SetpointProfile) -> Self {
|
||
self.profile = profile;
|
||
self
|
||
}
|
||
|
||
/// 场景总时长（秒）。
|
||
#[must_use]
|
||
pub fn total_s(&self) -> f32 {
|
||
match self.profile {
|
||
SetpointProfile::Step => self.hold_s,
|
||
SetpointProfile::RampAndHold | SetpointProfile::Staircase { .. } => {
|
||
self.ramp_s + self.hold_s
|
||
}
|
||
SetpointProfile::UpDown => 2.0 * (self.ramp_s + self.hold_s),
|
||
}
|
||
}
|
||
|
||
/// 场景采样点数（`dt_s` 非法时返回 1，由调用方做参数校验）。
|
||
#[must_use]
|
||
pub fn samples(&self) -> usize {
|
||
let total = self.total_s();
|
||
let dt_ok = self.dt_s.is_finite() && self.dt_s > 0.0;
|
||
let total_ok = total.is_finite() && total > 0.0;
|
||
if !dt_ok || !total_ok {
|
||
return 1;
|
||
}
|
||
((total / self.dt_s).round() as usize).max(1)
|
||
}
|
||
|
||
/// `t_s` 时刻的设定值（rpm）。
|
||
#[must_use]
|
||
pub fn setpoint_at(&self, t_s: f32) -> f32 {
|
||
let target = self.target_rpm;
|
||
let ramp = self.ramp_s;
|
||
match self.profile {
|
||
SetpointProfile::Step => target,
|
||
SetpointProfile::RampAndHold => {
|
||
if ramp > 0.0 && t_s < ramp {
|
||
target * (t_s / ramp)
|
||
} else {
|
||
target
|
||
}
|
||
}
|
||
SetpointProfile::Staircase { levels } => {
|
||
let levels = levels.max(1);
|
||
let total = ramp + self.hold_s;
|
||
if total <= 0.0 {
|
||
return target;
|
||
}
|
||
let rung_s = total / levels as f32;
|
||
let index = (t_s / rung_s).floor().max(0.0) as u32;
|
||
target * ((index.min(levels - 1) + 1) as f32 / levels as f32)
|
||
}
|
||
SetpointProfile::UpDown => {
|
||
let up_end = ramp;
|
||
let hold_end = up_end + self.hold_s;
|
||
let down_end = hold_end + ramp;
|
||
if t_s < up_end {
|
||
if ramp > 0.0 { target * (t_s / ramp) } else { target }
|
||
} else if t_s < hold_end {
|
||
target
|
||
} else if t_s < down_end {
|
||
let progress = (t_s - hold_end) / ramp;
|
||
target * (1.0 - progress)
|
||
} else {
|
||
0.0
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/// 稳态评价窗口的起点（秒）：斜坡 / 阶梯结束、进入最后一段保持时。
|
||
#[must_use]
|
||
pub fn steady_start_s(&self) -> f32 {
|
||
match self.profile {
|
||
SetpointProfile::Step => 0.0,
|
||
SetpointProfile::RampAndHold => self.ramp_s,
|
||
SetpointProfile::Staircase { levels } => {
|
||
let level_s = self.level_s(levels);
|
||
((levels.max(1) - 1) as f32) * level_s
|
||
}
|
||
SetpointProfile::UpDown => self.ramp_s + self.hold_s,
|
||
}
|
||
}
|
||
|
||
/// 稳态评价窗口的终点（秒）。
|
||
#[must_use]
|
||
pub fn steady_end_s(&self) -> f32 {
|
||
match self.profile {
|
||
// 升降场景只评价上升段后的保持段。
|
||
SetpointProfile::UpDown => self.ramp_s + self.hold_s,
|
||
_ => self.total_s(),
|
||
}
|
||
}
|
||
|
||
/// 阶梯的单级时长（秒）。
|
||
fn level_s(&self, levels: u32) -> f32 {
|
||
let levels = levels.max(1);
|
||
let total = self.ramp_s + self.hold_s;
|
||
if total > 0.0 { total / levels as f32 } else { 0.0 }
|
||
}
|
||
}
|
||
|
||
#[cfg(test)]
|
||
mod tests {
|
||
use super::*;
|
||
|
||
/// 浮点断言辅助：避免 `float_cmp`，同时给出可读的失败信息。
|
||
#[track_caller]
|
||
fn assert_close(actual: f32, expected: f32) {
|
||
assert!((actual - expected).abs() < 1e-3, "actual {actual}, expected {expected}");
|
||
}
|
||
|
||
#[test]
|
||
fn step_is_immediate_and_lasts_hold() {
|
||
let s = Scenario::step(1000.0, 0.5);
|
||
assert!((s.total_s() - 0.5).abs() < 1e-6);
|
||
assert_close(s.setpoint_at(0.0), 1000.0);
|
||
assert_close(s.setpoint_at(0.4), 1000.0);
|
||
assert_eq!(s.samples(), 500);
|
||
}
|
||
|
||
#[test]
|
||
fn ramp_is_linear_then_holds() {
|
||
let s = Scenario::ramp_and_hold(1000.0, 0.2, 1.0);
|
||
assert_close(s.setpoint_at(0.0), 0.0);
|
||
assert!((s.setpoint_at(0.1) - 500.0).abs() < 1e-3);
|
||
assert_close(s.setpoint_at(0.2), 1000.0);
|
||
assert_close(s.setpoint_at(1.1), 1000.0);
|
||
assert!((s.total_s() - 1.2).abs() < 1e-6);
|
||
}
|
||
|
||
#[test]
|
||
fn staircase_rises_in_equal_levels() {
|
||
let s = Scenario::ramp_and_hold(1000.0, 0.0, 1.0)
|
||
.with_profile(SetpointProfile::Staircase { levels: 4 });
|
||
assert!((s.setpoint_at(0.0) - 250.0).abs() < 1e-3);
|
||
assert!((s.setpoint_at(0.3) - 500.0).abs() < 1e-3);
|
||
assert!((s.setpoint_at(0.6) - 750.0).abs() < 1e-3);
|
||
assert!((s.setpoint_at(0.99) - 1000.0).abs() < 1e-3);
|
||
// 越界采样被夹在最后一级。
|
||
assert_close(s.setpoint_at(5.0), 1000.0);
|
||
}
|
||
|
||
#[test]
|
||
fn up_down_returns_to_zero() {
|
||
let s = Scenario::ramp_and_hold(600.0, 0.1, 0.2).with_profile(SetpointProfile::UpDown);
|
||
assert!((s.total_s() - 0.6).abs() < 1e-6);
|
||
assert!((s.setpoint_at(0.05) - 300.0).abs() < 1e-3);
|
||
assert_close(s.setpoint_at(0.2), 600.0);
|
||
assert!((s.setpoint_at(0.35) - 300.0).abs() < 1e-3);
|
||
assert_close(s.setpoint_at(0.59), 0.0);
|
||
}
|
||
|
||
#[test]
|
||
fn degenerate_scenario_reports_one_sample() {
|
||
let s = Scenario::step(1.0, 0.0);
|
||
assert_eq!(s.samples(), 1);
|
||
let s = Scenario::step(1.0, 1.0).with_dt_s(0.0);
|
||
assert_eq!(s.samples(), 1);
|
||
}
|
||
}
|