Skip to main content

continuous_blocks/
coordinate_transforms.rs

1//! Clarke/Park coordinate transforms — the standard change of basis between a three-phase
2//! `abc` signal, its stationary two-axis `alpha`/`beta` (plus zero-sequence) projection, and a
3//! rotating `d`/`q` (plus zero-sequence) frame locked to a reference angle `theta`. Used to turn
4//! a three-phase grid or motor-drive quantity into DC-like `d`/`q` values a `Pid` can regulate
5//! directly, and the matching angle-tracking utility (`anglewrap`) that closes the loop
6//! (a synchronous-reference-frame PLL is `Clarke -> anglewrap -> Park`, feeding a `Pid` on
7//! the `q` output back into the tracked angle).
8//!
9//! Every function here is a pure, stateless function of its instantaneous inputs (no
10//! integration, no memory) — the same category as [`crate::math_ops`] and
11//! [`crate::waveform_arithmetic`], just multi-output. [`CoordinateTransform`] groups them the
12//! same way [`crate::MathFn1`]/[`crate::MathFn2`] group the single-output functions, so
13//! `dae-runtime`'s block graph can dispatch by one enum instead of one bespoke `BlockKind`
14//! variant per transform.
15//!
16//! [`clarke_park`]/[`clarke_park_inv`] are convenience compositions of [`clarke`]/[`park`] (and
17//! their inverses) directly against `abc`, skipping the `alpha`/`beta` intermediate — see
18//! `clarke_park_matches_clarke_then_park` below for the equivalence this relies on. Amplitude
19//! convention throughout is the standard "2/3" (non-power-invariant) scaling: a balanced
20//! three-phase signal of peak amplitude `A` transforms to a `d`/`q` pair of magnitude `A`, not
21//! `A * sqrt(3/2)`.
22
23use std::f64::consts::PI;
24
25const TWO_THIRDS: f64 = 2.0 / 3.0;
26
27/// `abc -> (alpha, beta, zero)`.
28pub fn clarke(a: f64, b: f64, c: f64) -> [f64; 3] {
29    let alpha = TWO_THIRDS * (a - 0.5 * b - 0.5 * c);
30    let beta = (b - c) / 3.0_f64.sqrt();
31    let zero = (a + b + c) / 3.0;
32    [alpha, beta, zero]
33}
34
35/// `(alpha, beta, zero) -> abc`. Exact inverse of [`clarke`].
36pub fn clarke_inv(alpha: f64, beta: f64, zero: f64) -> [f64; 3] {
37    let a = alpha + zero;
38    let b = -0.5 * alpha + 0.5 * 3.0_f64.sqrt() * beta + zero;
39    let c = -0.5 * alpha - 0.5 * 3.0_f64.sqrt() * beta + zero;
40    [a, b, c]
41}
42
43/// `(alpha, beta, zero, theta) -> (d, q, zero)` — projects the stationary frame onto a frame
44/// rotating at angle `theta` (radians). Zero-sequence passes through unchanged.
45pub fn park(alpha: f64, beta: f64, zero: f64, theta: f64) -> [f64; 3] {
46    let d = alpha * theta.cos() + beta * theta.sin();
47    let q = -alpha * theta.sin() + beta * theta.cos();
48    [d, q, zero]
49}
50
51/// `(d, q, zero, theta) -> (alpha, beta, zero)`. Exact inverse of [`park`] at the same `theta`.
52pub fn park_inv(d: f64, q: f64, zero: f64, theta: f64) -> [f64; 3] {
53    let alpha = d * theta.cos() - q * theta.sin();
54    let beta = d * theta.sin() + q * theta.cos();
55    [alpha, beta, zero]
56}
57
58/// `(a, b, c, theta) -> (d, q, zero)` — a convenience composition of [`clarke`] then [`park`],
59/// skipping the `alpha`/`beta` intermediate for the common case of going straight from a
60/// three-phase signal to its rotating-frame `d`/`q` value. Kept as its own function purely for
61/// netlist/call-site ergonomics (one call instead of wiring `Clarke` into `Park`) — calls
62/// through to `clarke`/`park` directly rather than re-deriving the trig, since a family exactly
63/// like this one (a combined transform hand-expanded separately from its two-step composition)
64/// is documented to have shipped with `d`/`q` swapped once already in the upstream library this
65/// crate's formulas were adapted from; there is exactly one real implementation of each
66/// direction now, not two that could silently drift apart.
67pub fn clarke_park(a: f64, b: f64, c: f64, theta: f64) -> [f64; 3] {
68    let [alpha, beta, zero] = clarke(a, b, c);
69    park(alpha, beta, zero, theta)
70}
71
72/// `(d, q, zero, theta) -> (a, b, c)` — the same convenience-composition treatment as
73/// [`clarke_park`], calling through to [`park_inv`] then [`clarke_inv`]. Exact inverse of
74/// [`clarke_park`].
75pub fn clarke_park_inv(d: f64, q: f64, zero: f64, theta: f64) -> [f64; 3] {
76    let [alpha, beta, zero] = park_inv(d, q, zero, theta);
77    clarke_inv(alpha, beta, zero)
78}
79
80/// `(alpha, beta) -> theta`, wrapped to `[0, 2*pi)` — the angle-tracking half of a
81/// synchronous-reference-frame PLL: `atan2(beta, alpha)`, shifted by a full turn when negative
82/// instead of the usual `(-pi, pi]` range, so `theta` is a monotonically-sensible input to
83/// [`park`]/[`clarke_park`] across a full mechanical/electrical revolution.
84pub fn anglewrap(alpha: f64, beta: f64) -> f64 {
85    let theta = beta.atan2(alpha);
86    if theta >= 0.0 {
87        theta
88    } else {
89        theta + 2.0 * PI
90    }
91}
92
93/// One of the six Clarke/Park coordinate transforms, grouped for dispatch by
94/// `dae-runtime`'s block graph the same way [`crate::MathFn1`]/[`crate::MathFn2`] group the
95/// single-output waveform-arithmetic functions.
96#[derive(Debug, Clone, Copy, PartialEq, Eq)]
97pub enum CoordinateTransform {
98    Clarke,
99    ClarkeInv,
100    Park,
101    ParkInv,
102    ClarkePark,
103    ClarkeParkInv,
104}
105
106impl CoordinateTransform {
107    /// Number of inputs this transform expects, in the order [`Self::call`] requires them.
108    pub fn input_count(self) -> usize {
109        match self {
110            CoordinateTransform::Clarke | CoordinateTransform::ClarkeInv => 3,
111            CoordinateTransform::Park
112            | CoordinateTransform::ParkInv
113            | CoordinateTransform::ClarkePark
114            | CoordinateTransform::ClarkeParkInv => 4,
115        }
116    }
117
118    /// The `kind=` keyword that selects this transform at the CLI level — used for diagnostics
119    /// that need to name *this specific* transform rather than the `CoordinateTransform` family
120    /// as a whole, e.g. `dae-runtime`'s own `block_kind_name`.
121    pub fn name(self) -> &'static str {
122        match self {
123            CoordinateTransform::Clarke => "clarke",
124            CoordinateTransform::ClarkeInv => "clarkeinv",
125            CoordinateTransform::Park => "park",
126            CoordinateTransform::ParkInv => "parkinv",
127            CoordinateTransform::ClarkePark => "clarkepark",
128            CoordinateTransform::ClarkeParkInv => "clarkeparkinv",
129        }
130    }
131
132    /// Conventional names for this transform's outputs, in the order [`Self::call`] returns
133    /// them — a caller wiring up a `dae-runtime` block graph names each output block after one
134    /// of these (e.g. `"pll_alpha"`, `"pll_beta"`, `"pll_zero"`).
135    pub fn output_names(self) -> [&'static str; 3] {
136        match self {
137            CoordinateTransform::Clarke => ["alpha", "beta", "zero"],
138            CoordinateTransform::ClarkeInv => ["a", "b", "c"],
139            CoordinateTransform::Park | CoordinateTransform::ClarkePark => ["d", "q", "zero"],
140            CoordinateTransform::ParkInv => ["alpha", "beta", "zero"],
141            CoordinateTransform::ClarkeParkInv => ["a", "b", "c"],
142        }
143    }
144
145    /// Evaluates this transform. `inputs.len()` must equal [`Self::input_count`] (the caller's
146    /// responsibility — a device-file-level parser is the natural place to check once at load
147    /// time, matching [`crate::waveform_arithmetic::table`]'s convention).
148    pub fn call(self, inputs: &[f64]) -> [f64; 3] {
149        match self {
150            CoordinateTransform::Clarke => clarke(inputs[0], inputs[1], inputs[2]),
151            CoordinateTransform::ClarkeInv => clarke_inv(inputs[0], inputs[1], inputs[2]),
152            CoordinateTransform::Park => park(inputs[0], inputs[1], inputs[2], inputs[3]),
153            CoordinateTransform::ParkInv => park_inv(inputs[0], inputs[1], inputs[2], inputs[3]),
154            CoordinateTransform::ClarkePark => {
155                clarke_park(inputs[0], inputs[1], inputs[2], inputs[3])
156            }
157            CoordinateTransform::ClarkeParkInv => {
158                clarke_park_inv(inputs[0], inputs[1], inputs[2], inputs[3])
159            }
160        }
161    }
162}
163
164#[cfg(test)]
165mod tests {
166    use super::*;
167
168    const TOL: f64 = 1e-9;
169
170    fn assert_close3(got: [f64; 3], want: [f64; 3]) {
171        for i in 0..3 {
172            assert!(
173                (got[i] - want[i]).abs() < TOL,
174                "component {i}: got {}, want {}",
175                got[i],
176                want[i]
177            );
178        }
179    }
180
181    /// A balanced three-phase snapshot, hand-picked so every component is easy to check by
182    /// hand: a=1, b=-0.5, c=-0.5 is `cos(0)`, `cos(-2pi/3)`, `cos(2pi/3)` scaled by amplitude 1
183    /// — i.e. the `theta=0` instant of a unit-amplitude balanced waveform.
184    const A: f64 = 1.0;
185    const B: f64 = -0.5;
186    const C: f64 = -0.5;
187
188    #[test]
189    fn clarke_matches_hand_derived_balanced_snapshot() {
190        // alpha = (2/3)(1 - 0.5*(-0.5) - 0.5*(-0.5)) = (2/3)(1.5) = 1
191        // beta = (b - c)/sqrt(3) = 0/sqrt(3) = 0
192        // zero = (1 - 0.5 - 0.5)/3 = 0
193        assert_close3(clarke(A, B, C), [1.0, 0.0, 0.0]);
194    }
195
196    #[test]
197    fn clarke_inv_is_exact_inverse_of_clarke() {
198        let [alpha, beta, zero] = clarke(A, B, C);
199        assert_close3(clarke_inv(alpha, beta, zero), [A, B, C]);
200
201        // A second, less-symmetric snapshot, to catch a round-trip bug the theta=0 case above
202        // wouldn't (its beta happens to be zero).
203        let (a, b, c) = (3.0, -1.0, 0.4);
204        let [alpha, beta, zero] = clarke(a, b, c);
205        assert_close3(clarke_inv(alpha, beta, zero), [a, b, c]);
206    }
207
208    #[test]
209    fn park_inv_is_exact_inverse_of_park() {
210        let (alpha, beta, zero, theta) = (0.7, -1.3, 0.2, 1.234);
211        let [d, q, z] = park(alpha, beta, zero, theta);
212        assert_close3(park_inv(d, q, z, theta), [alpha, beta, zero]);
213    }
214
215    /// The load-bearing property this whole module exists for: a balanced three-phase signal,
216    /// tracked with `theta` locked to its own electrical angle, transforms to a *constant*
217    /// `d`/`q` pair (amplitude on `d`, zero on `q` at this phase convention) — the reason a
218    /// three-phase `Pid` on `q` works at all: it's regulating a DC quantity, not chasing a sine
219    /// wave. Checked at several `theta` values along the rotation, not just `theta=0`.
220    #[test]
221    fn balanced_three_phase_at_its_own_angle_gives_constant_dq() {
222        let amplitude = 2.0_f64;
223        for &theta in &[0.0, 0.4, PI / 2.0, 2.1, 5.9] {
224            let a = amplitude * theta.cos();
225            let b = amplitude * (theta - 2.0 * PI / 3.0).cos();
226            let c = amplitude * (theta + 2.0 * PI / 3.0).cos();
227            let [d, q, zero] = clarke_park(a, b, c, theta);
228            assert!((d - amplitude).abs() < TOL, "theta={theta}: d={d}");
229            assert!(q.abs() < TOL, "theta={theta}: q={q}");
230            assert!(zero.abs() < TOL, "theta={theta}: zero={zero}");
231        }
232    }
233
234    /// `clarke_park`'s fused abc->dq0 must agree with composing the two separate transforms —
235    /// the upstream library this was adapted from documents having once shipped `clarke_park`
236    /// with `d`/`q` swapped relative to the two-step composition, so this equivalence is the
237    /// direct regression test for that class of bug, not just a nice-to-have cross-check.
238    #[test]
239    fn clarke_park_matches_clarke_then_park() {
240        let (a, b, c, theta) = (1.1, -0.6, -0.3, 0.77);
241        let [alpha, beta, zero] = clarke(a, b, c);
242        let composed = park(alpha, beta, zero, theta);
243        let fused = clarke_park(a, b, c, theta);
244        assert_close3(fused, composed);
245    }
246
247    #[test]
248    fn clarke_park_inv_matches_park_inv_then_clarke_inv() {
249        let (d, q, zero, theta) = (0.9, -0.2, 0.1, 2.3);
250        let [alpha, beta, z] = park_inv(d, q, zero, theta);
251        let composed = clarke_inv(alpha, beta, z);
252        let fused = clarke_park_inv(d, q, zero, theta);
253        assert_close3(fused, composed);
254    }
255
256    #[test]
257    fn clarke_park_inv_is_exact_inverse_of_clarke_park() {
258        let (a, b, c, theta) = (2.0, -1.0, -1.0, 0.5);
259        let [d, q, zero] = clarke_park(a, b, c, theta);
260        assert_close3(clarke_park_inv(d, q, zero, theta), [a, b, c]);
261    }
262
263    #[test]
264    fn anglewrap_matches_hand_derived_quadrants_and_wraps_negative_into_0_2pi() {
265        // First quadrant: alpha=1, beta=1 -> 45 degrees = pi/4.
266        assert!((anglewrap(1.0, 1.0) - PI / 4.0).abs() < TOL);
267        // Straight up: alpha=0, beta=1 -> pi/2.
268        assert!((anglewrap(0.0, 1.0) - PI / 2.0).abs() < TOL);
269        // Third quadrant would be a negative atan2 result (-3pi/4); wrapped, it's 5pi/4.
270        assert!((anglewrap(-1.0, -1.0) - 5.0 * PI / 4.0).abs() < TOL);
271        // Straight down: alpha=0, beta=-1 -> raw atan2 is -pi/2, wrapped is 3pi/2.
272        assert!((anglewrap(0.0, -1.0) - 3.0 * PI / 2.0).abs() < TOL);
273    }
274
275    #[test]
276    fn anglewrap_tracks_a_full_rotation_consistent_with_clarke() {
277        // theta swept across a full turn: clarke() of a balanced signal at that theta should
278        // recover the same theta via anglewrap (alpha/beta live on the unit circle scaled
279        // by amplitude, so their angle is theta itself).
280        let amplitude = 1.0_f64;
281        for &theta in &[0.0, 0.1, PI / 2.0, PI, 3.0 * PI / 2.0 + 0.2, 2.0 * PI - 0.1] {
282            let a = amplitude * theta.cos();
283            let b = amplitude * (theta - 2.0 * PI / 3.0).cos();
284            let c = amplitude * (theta + 2.0 * PI / 3.0).cos();
285            let [alpha, beta, _zero] = clarke(a, b, c);
286            let recovered = anglewrap(alpha, beta);
287            assert!(
288                (recovered - theta).abs() < 1e-6,
289                "theta={theta}: recovered={recovered}"
290            );
291        }
292    }
293
294    #[test]
295    fn coordinate_transform_dispatch_matches_direct_calls() {
296        let (a, b, c, theta) = (1.0, -0.5, -0.5, 0.3);
297        assert_eq!(
298            CoordinateTransform::Clarke.call(&[a, b, c]),
299            clarke(a, b, c)
300        );
301        assert_eq!(CoordinateTransform::Clarke.input_count(), 3);
302        assert_eq!(
303            CoordinateTransform::Clarke.output_names(),
304            ["alpha", "beta", "zero"]
305        );
306
307        assert_eq!(
308            CoordinateTransform::ClarkePark.call(&[a, b, c, theta]),
309            clarke_park(a, b, c, theta)
310        );
311        assert_eq!(CoordinateTransform::ClarkePark.input_count(), 4);
312        assert_eq!(
313            CoordinateTransform::ClarkePark.output_names(),
314            ["d", "q", "zero"]
315        );
316    }
317}