use std::f64::consts::PI;
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Pmsm {
pub r_s: f64,
pub l_d: f64,
pub l_q: f64,
pub lambda_pm: f64,
pub pole_pairs: f64,
pub inertia: f64,
pub friction: f64,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum PmsmError {
NegativeResistance,
NonPositiveInductance,
NonPositivePolePairs,
NonPositiveInertia,
NegativeFriction,
}
impl Pmsm {
pub fn new(
r_s: f64,
l_d: f64,
l_q: f64,
lambda_pm: f64,
pole_pairs: f64,
inertia: f64,
friction: f64,
) -> Result<Self, PmsmError> {
if r_s < 0.0 {
return Err(PmsmError::NegativeResistance);
}
if !(l_d > 0.0 && l_q > 0.0) {
return Err(PmsmError::NonPositiveInductance);
}
if pole_pairs <= 0.0 {
return Err(PmsmError::NonPositivePolePairs);
}
if inertia <= 0.0 {
return Err(PmsmError::NonPositiveInertia);
}
if friction < 0.0 {
return Err(PmsmError::NegativeFriction);
}
Ok(Pmsm {
r_s,
l_d,
l_q,
lambda_pm,
pole_pairs,
inertia,
friction,
})
}
pub fn torque(&self, id: f64, iq: f64) -> f64 {
1.5 * self.pole_pairs * (self.lambda_pm * iq + (self.l_d - self.l_q) * id * iq)
}
pub fn theta_e_wrapped(theta_e: f64) -> f64 {
theta_e.rem_euclid(2.0 * PI)
}
fn derivative(&self, x: [f64; 4], vd: f64, vq: f64, t_load: f64) -> [f64; 4] {
let [id, iq, omega_m, _theta_e] = x;
let omega_e = self.pole_pairs * omega_m;
let did = (vd - self.r_s * id + omega_e * self.l_q * iq) / self.l_d;
let diq =
(vq - self.r_s * iq - omega_e * self.l_d * id - omega_e * self.lambda_pm) / self.l_q;
let domega = (self.torque(id, iq) - t_load - self.friction * omega_m) / self.inertia;
let dtheta = omega_e;
[did, diq, domega, dtheta]
}
pub fn step(&self, x: [f64; 4], vd: f64, vq: f64, t_load: f64, dt: f64) -> [f64; 4] {
let f = |s: [f64; 4]| self.derivative(s, vd, vq, t_load);
let add = |a: [f64; 4], b: [f64; 4], s: f64| -> [f64; 4] {
std::array::from_fn(|i| a[i] + s * b[i])
};
let k1 = f(x);
let k2 = f(add(x, k1, dt / 2.0));
let k3 = f(add(x, k2, dt / 2.0));
let k4 = f(add(x, k3, dt));
std::array::from_fn(|i| x[i] + (dt / 6.0) * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]))
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn torque_matches_hand_derived_formula_including_reluctance_term() {
let motor = Pmsm::new(1.0, 2e-3, 3e-3, 0.05, 4.0, 1e-4, 0.0).unwrap();
let te = motor.torque(1.5, 2.0);
assert!((te - 0.582).abs() < 1e-12, "Te={te}");
let surface = Pmsm::new(1.0, 2e-3, 2e-3, 0.05, 4.0, 1e-4, 0.0).unwrap();
let te_surface = surface.torque(1.5, 2.0);
assert!((te_surface - 1.5 * 4.0 * 0.05 * 2.0).abs() < 1e-12);
}
#[test]
fn d_axis_step_response_matches_hand_derived_rl_circuit_when_decoupled() {
let motor = Pmsm::new(2.0, 5e-3, 5e-3, 0.05, 4.0, 1e-4, 0.0).unwrap();
let (vd, r, l) = (10.0, 2.0, 5e-3);
let tau = l / r;
let mut x = [0.0, 0.0, 0.0, 0.0];
let dt = tau / 200.0; let mut t = 0.0;
for _ in 0..400 {
x = motor.step(x, vd, 0.0, 0.0, dt);
t += dt;
}
let expected_id = (vd / r) * (1.0 - (-t * r / l).exp());
assert!(
(x[0] - expected_id).abs() < 1e-6,
"id={}, expected={}",
x[0],
expected_id
);
assert_eq!(x[1], 0.0, "iq must stay exactly zero when decoupled");
assert_eq!(x[2], 0.0, "omega_m must stay exactly zero when decoupled");
}
#[test]
fn full_coupled_system_converges_at_fourth_order() {
let motor = Pmsm::new(1.0, 3e-3, 5e-3, 0.08, 4.0, 2e-4, 1e-4).unwrap();
let x0 = [0.0, 0.0, 50.0, 0.0];
let (vd, vq, t_load) = (20.0, 40.0, 0.02);
let t_final = 2e-3;
let run = |steps: usize| -> [f64; 4] {
let dt = t_final / steps as f64;
let mut x = x0;
for _ in 0..steps {
x = motor.step(x, vd, vq, t_load, dt);
}
x
};
let reference = run(4096); let coarse = run(64);
let fine = run(128);
let err = |a: [f64; 4], b: [f64; 4]| -> f64 {
a.iter()
.zip(b.iter())
.map(|(x, y)| (x - y).powi(2))
.sum::<f64>()
.sqrt()
};
let err_coarse = err(coarse, reference);
let err_fine = err(fine, reference);
let ratio = err_coarse / err_fine;
assert!(
(8.0..32.0).contains(&ratio),
"expected roughly 16x error reduction halving dt, got ratio={ratio} \
(err_coarse={err_coarse}, err_fine={err_fine})"
);
}
#[test]
fn theta_e_wraps_to_0_2pi_matching_anglewrap_convention() {
assert!((Pmsm::theta_e_wrapped(0.5) - 0.5).abs() < 1e-12);
assert!((Pmsm::theta_e_wrapped(2.0 * PI + 0.5) - 0.5).abs() < 1e-12);
assert!((Pmsm::theta_e_wrapped(-0.5) - (2.0 * PI - 0.5)).abs() < 1e-12);
}
#[test]
fn invalid_parameters_are_rejected_not_a_panic() {
assert_eq!(
Pmsm::new(-1.0, 2e-3, 2e-3, 0.05, 4.0, 1e-4, 0.0),
Err(PmsmError::NegativeResistance)
);
assert_eq!(
Pmsm::new(1.0, 0.0, 2e-3, 0.05, 4.0, 1e-4, 0.0),
Err(PmsmError::NonPositiveInductance)
);
assert_eq!(
Pmsm::new(1.0, 2e-3, 2e-3, 0.05, 0.0, 1e-4, 0.0),
Err(PmsmError::NonPositivePolePairs)
);
assert_eq!(
Pmsm::new(1.0, 2e-3, 2e-3, 0.05, 4.0, 0.0, 0.0),
Err(PmsmError::NonPositiveInertia)
);
assert_eq!(
Pmsm::new(1.0, 2e-3, 2e-3, 0.05, 4.0, 1e-4, -1.0),
Err(PmsmError::NegativeFriction)
);
}
}