embedded_dsp/
controller.rs

1//! Controller functions (PID motor control, Clarke transform, Park transform, Inverse Clarke, Inverse Park).
2
3#[allow(unused_imports)]
4use crate::math::FloatMath;
5use crate::types::*;
6
7// --- PID Controller (f32) ---
8
9/// Instance structure for the floating-point PID Control.
10#[derive(Debug, Clone, PartialEq, Default)]
11pub struct PidInstanceF32 {
12    pub a0: f32,
13    pub a1: f32,
14    pub a2: f32,
15    pub state: [f32; 3],
16    pub kp: f32,
17    pub ki: f32,
18    pub kd: f32,
19}
20
21impl PidInstanceF32 {
22    pub fn new(kp: f32, ki: f32, kd: f32) -> Self {
23        let mut pid = Self {
24            a0: 0.0,
25            a1: 0.0,
26            a2: 0.0,
27            state: [0.0; 3],
28            kp,
29            ki,
30            kd,
31        };
32        pid.init(1);
33        pid
34    }
35
36    pub fn init(&mut self, reset_state_flag: i32) {
37        self.a0 = self.kp + self.ki + self.kd;
38        self.a1 = -self.kp - 2.0 * self.kd;
39        self.a2 = self.kd;
40        if reset_state_flag != 0 {
41            self.reset();
42        }
43    }
44
45    pub fn reset(&mut self) {
46        self.state = [0.0; 3];
47    }
48
49    pub fn process(&mut self, in_val: f32) -> f32 {
50        let out =
51            self.state[2] + self.a0 * in_val + self.a1 * self.state[0] + self.a2 * self.state[1];
52        self.state[1] = self.state[0];
53        self.state[0] = in_val;
54        self.state[2] = out;
55        out
56    }
57}
58
59pub fn pid_f32(instance: &mut PidInstanceF32, in_val: f32) -> f32 {
60    instance.process(in_val)
61}
62
63// --- PID Controller (Q31) ---
64
65#[derive(Debug, Clone, PartialEq, Default)]
66pub struct PidInstanceQ31 {
67    pub a0: q31,
68    pub a1: q31,
69    pub a2: q31,
70    pub state: [q31; 3],
71    pub kp: q31,
72    pub ki: q31,
73    pub kd: q31,
74}
75
76impl PidInstanceQ31 {
77    pub fn new(kp: q31, ki: q31, kd: q31) -> Self {
78        let mut pid = Self {
79            a0: q31::ZERO,
80            a1: q31::ZERO,
81            a2: q31::ZERO,
82            state: [q31::ZERO; 3],
83            kp,
84            ki,
85            kd,
86        };
87        pid.init(1);
88        pid
89    }
90
91    pub fn init(&mut self, reset_state_flag: i32) {
92        self.a0 = self.kp.saturating_add(self.ki).saturating_add(self.kd);
93        self.a1 = (-self.kp).saturating_sub(self.kd.wrapping_mul_int(2));
94        self.a2 = self.kd;
95        if reset_state_flag != 0 {
96            self.reset();
97        }
98    }
99
100    pub fn reset(&mut self) {
101        self.state = [q31::ZERO; 3];
102    }
103
104    /// Each MAC term wraps individually (`i32`-wide, not the wider
105    /// intermediate a naive i64 accumulator would allow), and only the final
106    /// sum saturates. At the extreme edge where a coefficient and its paired
107    /// value are both exactly Q31 `MIN` (`-1.0`), the term wraps to `MIN`
108    /// instead of the mathematically exact `+2^31`, which can flip that
109    /// term's sign in the final sum. This only affects that single boundary
110    /// input combination.
111    pub fn process(&mut self, in_val: q31) -> q31 {
112        let t0 = self.a0.wrapping_mul(in_val).to_bits();
113        let t1 = self.a1.wrapping_mul(self.state[0]).to_bits();
114        let t2 = self.a2.wrapping_mul(self.state[1]).to_bits();
115        let acc = (self.state[2].to_bits() as i64) + (t0 as i64) + (t1 as i64) + (t2 as i64);
116        let out = q31::from_bits(acc.clamp(i32::MIN as i64, i32::MAX as i64) as i32);
117        self.state[1] = self.state[0];
118        self.state[0] = in_val;
119        self.state[2] = out;
120        out
121    }
122}
123
124pub fn pid_q31(instance: &mut PidInstanceQ31, in_val: q31) -> q31 {
125    instance.process(in_val)
126}
127
128// --- PID Controller (Q15) ---
129
130#[derive(Debug, Clone, PartialEq, Default)]
131pub struct PidInstanceQ15 {
132    pub a0: q15,
133    pub a1: q15,
134    pub a2: q15,
135    pub state: [q15; 3],
136    pub kp: q15,
137    pub ki: q15,
138    pub kd: q15,
139}
140
141impl PidInstanceQ15 {
142    pub fn new(kp: q15, ki: q15, kd: q15) -> Self {
143        let mut pid = Self {
144            a0: q15::ZERO,
145            a1: q15::ZERO,
146            a2: q15::ZERO,
147            state: [q15::ZERO; 3],
148            kp,
149            ki,
150            kd,
151        };
152        pid.init(1);
153        pid
154    }
155
156    pub fn init(&mut self, reset_state_flag: i32) {
157        self.a0 = self.kp.saturating_add(self.ki).saturating_add(self.kd);
158        self.a1 = (-self.kp).saturating_sub(self.kd.wrapping_mul_int(2));
159        self.a2 = self.kd;
160        if reset_state_flag != 0 {
161            self.reset();
162        }
163    }
164
165    pub fn reset(&mut self) {
166        self.state = [q15::ZERO; 3];
167    }
168
169    /// Same per-term wrapping caveat as [`PidInstanceQ31::process`]: at the
170    /// extreme edge where a coefficient and its paired value are both
171    /// exactly Q15 `MIN` (`-1.0`), that term wraps to `MIN` instead of the
172    /// mathematically exact `+2^15`.
173    pub fn process(&mut self, in_val: q15) -> q15 {
174        let t0 = self.a0.wrapping_mul(in_val).to_bits();
175        let t1 = self.a1.wrapping_mul(self.state[0]).to_bits();
176        let t2 = self.a2.wrapping_mul(self.state[1]).to_bits();
177        let acc = (self.state[2].to_bits() as i32) + (t0 as i32) + (t1 as i32) + (t2 as i32);
178        let out = q15::from_bits(acc.clamp(i16::MIN as i32, i16::MAX as i32) as i16);
179        self.state[1] = self.state[0];
180        self.state[0] = in_val;
181        self.state[2] = out;
182        out
183    }
184}
185
186pub fn pid_q15(instance: &mut PidInstanceQ15, in_val: q15) -> q15 {
187    instance.process(in_val)
188}
189
190// --- Clarke Transform ---
191
192/// Forward Clarke transform for f32: 3-phase (ia, ib) -> 2-phase (alpha, beta).
193pub fn clarke_f32(ia: f32, ib: f32, p_alpha: &mut f32, p_beta: &mut f32) {
194    *p_alpha = ia;
195    let inv_sqrt_3 = 0.57735026919f32; // 1 / sqrt(3)
196    *p_beta = (ia + 2.0 * ib) * inv_sqrt_3;
197}
198
199/// Inverse Clarke transform for f32: 2-phase (alpha, beta) -> 3-phase (ia, ib).
200pub fn inv_clarke_f32(alpha: f32, beta: f32, p_ia: &mut f32, p_ib: &mut f32) {
201    *p_ia = alpha;
202    let sqrt_3_div_2 = 0.86602540378f32; // sqrt(3) / 2
203    *p_ib = -0.5 * alpha + sqrt_3_div_2 * beta;
204}
205
206// --- Park Transform ---
207
208/// Forward Park transform for f32: 2-phase stationary (alpha, beta) + angle theta (rad) -> 2-phase rotating (d, q).
209pub fn park_f32(alpha: f32, beta: f32, theta: f32, p_d: &mut f32, p_q: &mut f32) {
210    let cos_t = theta.cos();
211    let sin_t = theta.sin();
212    *p_d = alpha * cos_t + beta * sin_t;
213    *p_q = -alpha * sin_t + beta * cos_t;
214}
215
216/// Inverse Park transform for f32: 2-phase rotating (d, q) + angle theta (rad) -> 2-phase stationary (alpha, beta).
217pub fn inv_park_f32(d: f32, q: f32, theta: f32, p_alpha: &mut f32, p_beta: &mut f32) {
218    let cos_t = theta.cos();
219    let sin_t = theta.sin();
220    *p_alpha = d * cos_t - q * sin_t;
221    *p_beta = d * sin_t + q * cos_t;
222}
223
224const INV_SQRT3_Q15: i32 = 18919; // 1/√3 in Q15
225const SQRT3_2_Q15: i32 = 28378; // √3/2 in Q15
226
227#[inline]
228fn sat_q15_i32(v: i32) -> q15 {
229    q15::from_bits(v.clamp(i16::MIN as i32, i16::MAX as i32) as i16)
230}
231
232/// Forward Clarke transform in Q15.
233pub fn clarke_q15(ia: q15, ib: q15, p_alpha: &mut q15, p_beta: &mut q15) {
234    *p_alpha = ia;
235    let acc = (ia.to_bits() as i32 + 2 * ib.to_bits() as i32) * INV_SQRT3_Q15;
236    *p_beta = sat_q15_i32(acc >> 15);
237}
238
239/// Inverse Clarke transform in Q15.
240pub fn inv_clarke_q15(alpha: q15, beta: q15, p_ia: &mut q15, p_ib: &mut q15) {
241    *p_ia = alpha;
242    let acc = -((alpha.to_bits() as i32) << 14) + SQRT3_2_Q15 * beta.to_bits() as i32;
243    *p_ib = sat_q15_i32(acc >> 15);
244}
245
246/// Forward Park transform in Q15. `sin_t` / `cos_t` are Q15 sine/cosine of θ
247/// (CMSIS-style; e.g. take `sin_cos_q31` and shift `>> 16`).
248pub fn park_q15(alpha: q15, beta: q15, sin_t: q15, cos_t: q15, p_d: &mut q15, p_q: &mut q15) {
249    let (alpha, beta, sin_t, cos_t) = (
250        alpha.to_bits() as i32,
251        beta.to_bits() as i32,
252        sin_t.to_bits() as i32,
253        cos_t.to_bits() as i32,
254    );
255    let d = (alpha * cos_t + beta * sin_t) >> 15;
256    let q = (-alpha * sin_t + beta * cos_t) >> 15;
257    *p_d = sat_q15_i32(d);
258    *p_q = sat_q15_i32(q);
259}
260
261/// Inverse Park transform in Q15. `sin_t` / `cos_t` are Q15 sine/cosine of θ.
262pub fn inv_park_q15(d: q15, q: q15, sin_t: q15, cos_t: q15, p_alpha: &mut q15, p_beta: &mut q15) {
263    let (d, q, sin_t, cos_t) = (
264        d.to_bits() as i32,
265        q.to_bits() as i32,
266        sin_t.to_bits() as i32,
267        cos_t.to_bits() as i32,
268    );
269    let alpha = (d * cos_t - q * sin_t) >> 15;
270    let beta = (d * sin_t + q * cos_t) >> 15;
271    *p_alpha = sat_q15_i32(alpha);
272    *p_beta = sat_q15_i32(beta);
273}
embedded_dsp/controller.rs

embedded_dsp/
controller.rs
