Skip to main content

continuous_blocks/
pmsm.rs

1//! A permanent-magnet synchronous motor (PMSM), in the rotor `d`/`q` reference frame — the
2//! standard textbook electromechanical model, built to close the loop around
3//! `coordinate_transforms`' Clarke/Park blocks for a field-oriented-control (FOC) motor-drive
4//! experiment: an inverter's three-phase output feeds this block (through `clarke_park`), an
5//! outer speed loop and inner `id`/`iq` current loops regulate it, and `clarke_park_inv` turns
6//! the resulting `d`/`q` voltage commands back into three-phase PWM references.
7//!
8//! Deliberately not a [`crate::StateSpace`]: the back-EMF/cross-coupling terms (`omega_e * l_q *
9//! iq`, `omega_e * l_d * id`) and the electromagnetic torque (`(l_d - l_q) * id * iq`) are
10//! *bilinear* — products of two of this block's own states (speed and current), not linear in
11//! the state vector — so this genuinely cannot be expressed as the descriptor system `A x + K
12//! dx/dt = B u` every other dynamic block in this crate compiles to. Same category as
13//! [`crate::Vco`]: a small, self-contained block with its own bespoke `step()`, integrated
14//! explicitly (here, RK4 over its own 4-state nonlinear vector field) rather than through the
15//! generic `StateSpace` machinery. Because the whole nonlinear vector field is evaluated
16//! *inside* `step()`, RK4's own intermediate stages naturally handle the bilinear coupling
17//! correctly (each stage evaluates the same [`Pmsm::derivative`] at its own trial state) — there
18//! is no cross-block algebraic loop to resolve the way there would be if `id`/`iq` dynamics were
19//! split across two separate blocks that each needed the other's current-step output.
20//!
21//! State is `(id, iq, omega_m, theta_e)`: stator current `d`/`q` components (A), mechanical
22//! speed (rad/s), and *unwrapped* electrical angle (rad) — kept unwrapped through the
23//! integration (so RK4 never has to reason about a discontinuity mid-step) and wrapped to `[0,
24//! 2*pi)` only by [`Pmsm::theta_e_wrapped`] for a caller that needs it as an input to
25//! [`crate::coordinate_transforms::park`]/[`crate::coordinate_transforms::clarke_park`].
26//! Electrical angular speed is `omega_e = pole_pairs * omega_m`; `theta_e` does not itself feed
27//! back into the `id`/`iq`/`omega_m` dynamics, only accumulates `omega_e`.
28
29use std::f64::consts::PI;
30
31/// Motor parameters (SI units): stator resistance `r_s` (ohm), `d`/`q`-axis inductance `l_d`/
32/// `l_q` (H — equal for a surface-mount PMSM, unequal for an interior-PM motor with reluctance
33/// torque), permanent-magnet flux linkage `lambda_pm` (Wb), `pole_pairs` (electrical cycles per
34/// mechanical revolution), rotor+load inertia `inertia` (kg*m^2), and viscous friction
35/// coefficient `friction` (N*m*s/rad).
36#[derive(Debug, Clone, Copy, PartialEq)]
37pub struct Pmsm {
38    pub r_s: f64,
39    pub l_d: f64,
40    pub l_q: f64,
41    pub lambda_pm: f64,
42    pub pole_pairs: f64,
43    pub inertia: f64,
44    pub friction: f64,
45}
46
47#[derive(Debug, Clone, Copy, PartialEq, Eq)]
48pub enum PmsmError {
49    NegativeResistance,
50    NonPositiveInductance,
51    NonPositivePolePairs,
52    NonPositiveInertia,
53    NegativeFriction,
54}
55
56impl Pmsm {
57    pub fn new(
58        r_s: f64,
59        l_d: f64,
60        l_q: f64,
61        lambda_pm: f64,
62        pole_pairs: f64,
63        inertia: f64,
64        friction: f64,
65    ) -> Result<Self, PmsmError> {
66        if r_s < 0.0 {
67            return Err(PmsmError::NegativeResistance);
68        }
69        if !(l_d > 0.0 && l_q > 0.0) {
70            return Err(PmsmError::NonPositiveInductance);
71        }
72        if pole_pairs <= 0.0 {
73            return Err(PmsmError::NonPositivePolePairs);
74        }
75        if inertia <= 0.0 {
76            return Err(PmsmError::NonPositiveInertia);
77        }
78        if friction < 0.0 {
79            return Err(PmsmError::NegativeFriction);
80        }
81        Ok(Pmsm {
82            r_s,
83            l_d,
84            l_q,
85            lambda_pm,
86            pole_pairs,
87            inertia,
88            friction,
89        })
90    }
91
92    /// Electromagnetic torque (N*m) at a given `id`/`iq` operating point — the standard
93    /// permanent-magnet-alignment term plus reluctance torque (zero when `l_d == l_q`, the
94    /// surface-mount case).
95    pub fn torque(&self, id: f64, iq: f64) -> f64 {
96        1.5 * self.pole_pairs * (self.lambda_pm * iq + (self.l_d - self.l_q) * id * iq)
97    }
98
99    /// This block's own electrical angle, wrapped to `[0, 2*pi)` — matches
100    /// [`crate::coordinate_transforms::anglewrap`]'s own convention, so a downstream `Park`/
101    /// `ClarkePark` block can consume either interchangeably.
102    pub fn theta_e_wrapped(theta_e: f64) -> f64 {
103        theta_e.rem_euclid(2.0 * PI)
104    }
105
106    /// The nonlinear vector field `dx/dt = f(x, vd, vq, t_load)`, `x = (id, iq, omega_m,
107    /// theta_e)`. `vd`/`vq`/`t_load` are held fixed across a call — [`Pmsm::step`]'s RK4 stages
108    /// evaluate this at several trial states within one timestep, not several trial inputs.
109    fn derivative(&self, x: [f64; 4], vd: f64, vq: f64, t_load: f64) -> [f64; 4] {
110        let [id, iq, omega_m, _theta_e] = x;
111        let omega_e = self.pole_pairs * omega_m;
112        let did = (vd - self.r_s * id + omega_e * self.l_q * iq) / self.l_d;
113        let diq =
114            (vq - self.r_s * iq - omega_e * self.l_d * id - omega_e * self.lambda_pm) / self.l_q;
115        let domega = (self.torque(id, iq) - t_load - self.friction * omega_m) / self.inertia;
116        let dtheta = omega_e;
117        [did, diq, domega, dtheta]
118    }
119
120    /// Advances the motor state by one step via classical 4th-order Runge-Kutta, with `vd`/
121    /// `vq`/`t_load` held fixed (zero-order hold) across the step — the same convention
122    /// [`crate::StateSpace::rk4_step`] uses for its own single-step input.
123    pub fn step(&self, x: [f64; 4], vd: f64, vq: f64, t_load: f64, dt: f64) -> [f64; 4] {
124        let f = |s: [f64; 4]| self.derivative(s, vd, vq, t_load);
125        let add = |a: [f64; 4], b: [f64; 4], s: f64| -> [f64; 4] {
126            std::array::from_fn(|i| a[i] + s * b[i])
127        };
128
129        let k1 = f(x);
130        let k2 = f(add(x, k1, dt / 2.0));
131        let k3 = f(add(x, k2, dt / 2.0));
132        let k4 = f(add(x, k3, dt));
133
134        std::array::from_fn(|i| x[i] + (dt / 6.0) * (k1[i] + 2.0 * k2[i] + 2.0 * k3[i] + k4[i]))
135    }
136}
137
138#[cfg(test)]
139mod tests {
140    use super::*;
141
142    #[test]
143    fn torque_matches_hand_derived_formula_including_reluctance_term() {
144        let motor = Pmsm::new(1.0, 2e-3, 3e-3, 0.05, 4.0, 1e-4, 0.0).unwrap();
145        // Te = 1.5 * p * (lambda_pm*iq + (Ld-Lq)*id*iq)
146        //    = 1.5 * 4 * (0.05*2.0 + (2e-3-3e-3)*1.5*2.0)
147        //    = 6 * (0.1 - 0.003) = 6 * 0.097 = 0.582
148        let te = motor.torque(1.5, 2.0);
149        assert!((te - 0.582).abs() < 1e-12, "Te={te}");
150
151        // Surface-mount case (Ld=Lq): reluctance term vanishes, torque is purely
152        // current-proportional.
153        let surface = Pmsm::new(1.0, 2e-3, 2e-3, 0.05, 4.0, 1e-4, 0.0).unwrap();
154        let te_surface = surface.torque(1.5, 2.0);
155        assert!((te_surface - 1.5 * 4.0 * 0.05 * 2.0).abs() < 1e-12);
156    }
157
158    /// With `iq(0) = 0` and `vq = 0`, both `iq` and `omega_m` stay identically zero for all
159    /// time (`diq/dt` and `Te` both vanish whenever `iq = 0`, `domega/dt` then has no torque
160    /// input and starts at `omega_m = 0`), which decouples the `d`-axis into an ordinary
161    /// R-L circuit driven by `vd` — a case with an exact closed-form solution,
162    /// `id(t) = (vd/R)*(1 - exp(-t*R/Ld))`, independent of this model's own RK4 implementation.
163    /// This is therefore a genuine hand-derived check on the electrical dynamics, not just
164    /// internal self-consistency.
165    #[test]
166    fn d_axis_step_response_matches_hand_derived_rl_circuit_when_decoupled() {
167        let motor = Pmsm::new(2.0, 5e-3, 5e-3, 0.05, 4.0, 1e-4, 0.0).unwrap();
168        let (vd, r, l) = (10.0, 2.0, 5e-3);
169        let tau = l / r;
170
171        let mut x = [0.0, 0.0, 0.0, 0.0];
172        let dt = tau / 200.0; // fine steps relative to the R-L time constant
173        let mut t = 0.0;
174        for _ in 0..400 {
175            x = motor.step(x, vd, 0.0, 0.0, dt);
176            t += dt;
177        }
178        let expected_id = (vd / r) * (1.0 - (-t * r / l).exp());
179        assert!(
180            (x[0] - expected_id).abs() < 1e-6,
181            "id={}, expected={}",
182            x[0],
183            expected_id
184        );
185        assert_eq!(x[1], 0.0, "iq must stay exactly zero when decoupled");
186        assert_eq!(x[2], 0.0, "omega_m must stay exactly zero when decoupled");
187    }
188
189    /// Convergence-order check on the full coupled nonlinear system (nonzero `vd`, `vq`,
190    /// `t_load`, and a nonzero initial speed, so every bilinear coupling term is actually
191    /// engaged) — the same technique `dae-runtime`'s own transient loop is verified with
192    /// (halving `dt` and checking the error shrinks at RK4's own 4th-order rate against a
193    /// converged reference), the standard way to validate a numerical integrator on a system
194    /// with no closed-form solution.
195    #[test]
196    fn full_coupled_system_converges_at_fourth_order() {
197        let motor = Pmsm::new(1.0, 3e-3, 5e-3, 0.08, 4.0, 2e-4, 1e-4).unwrap();
198        let x0 = [0.0, 0.0, 50.0, 0.0];
199        let (vd, vq, t_load) = (20.0, 40.0, 0.02);
200        let t_final = 2e-3;
201
202        let run = |steps: usize| -> [f64; 4] {
203            let dt = t_final / steps as f64;
204            let mut x = x0;
205            for _ in 0..steps {
206                x = motor.step(x, vd, vq, t_load, dt);
207            }
208            x
209        };
210
211        let reference = run(4096); // fine enough to stand in for the true solution
212        let coarse = run(64);
213        let fine = run(128);
214
215        let err = |a: [f64; 4], b: [f64; 4]| -> f64 {
216            a.iter()
217                .zip(b.iter())
218                .map(|(x, y)| (x - y).powi(2))
219                .sum::<f64>()
220                .sqrt()
221        };
222        let err_coarse = err(coarse, reference);
223        let err_fine = err(fine, reference);
224
225        // Halving dt should shrink RK4's error by roughly 2^4 = 16.
226        let ratio = err_coarse / err_fine;
227        assert!(
228            (8.0..32.0).contains(&ratio),
229            "expected roughly 16x error reduction halving dt, got ratio={ratio} \
230             (err_coarse={err_coarse}, err_fine={err_fine})"
231        );
232    }
233
234    #[test]
235    fn theta_e_wraps_to_0_2pi_matching_anglewrap_convention() {
236        assert!((Pmsm::theta_e_wrapped(0.5) - 0.5).abs() < 1e-12);
237        assert!((Pmsm::theta_e_wrapped(2.0 * PI + 0.5) - 0.5).abs() < 1e-12);
238        assert!((Pmsm::theta_e_wrapped(-0.5) - (2.0 * PI - 0.5)).abs() < 1e-12);
239    }
240
241    #[test]
242    fn invalid_parameters_are_rejected_not_a_panic() {
243        assert_eq!(
244            Pmsm::new(-1.0, 2e-3, 2e-3, 0.05, 4.0, 1e-4, 0.0),
245            Err(PmsmError::NegativeResistance)
246        );
247        assert_eq!(
248            Pmsm::new(1.0, 0.0, 2e-3, 0.05, 4.0, 1e-4, 0.0),
249            Err(PmsmError::NonPositiveInductance)
250        );
251        assert_eq!(
252            Pmsm::new(1.0, 2e-3, 2e-3, 0.05, 0.0, 1e-4, 0.0),
253            Err(PmsmError::NonPositivePolePairs)
254        );
255        assert_eq!(
256            Pmsm::new(1.0, 2e-3, 2e-3, 0.05, 4.0, 0.0, 0.0),
257            Err(PmsmError::NonPositiveInertia)
258        );
259        assert_eq!(
260            Pmsm::new(1.0, 2e-3, 2e-3, 0.05, 4.0, 1e-4, -1.0),
261            Err(PmsmError::NegativeFriction)
262        );
263    }
264}