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}