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: 0,
80            a1: 0,
81            a2: 0,
82            state: [0; 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(2 * self.kd);
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 = [0; 3];
102    }
103
104    pub fn process(&mut self, in_val: q31) -> q31 {
105        let acc = (self.state[2] as i64)
106            + ((self.a0 as i64 * in_val as i64) >> 31)
107            + ((self.a1 as i64 * self.state[0] as i64) >> 31)
108            + ((self.a2 as i64 * self.state[1] as i64) >> 31);
109        let out = acc.clamp(i32::MIN as i64, i32::MAX as i64) as q31;
110        self.state[1] = self.state[0];
111        self.state[0] = in_val;
112        self.state[2] = out;
113        out
114    }
115}
116
117pub fn pid_q31(instance: &mut PidInstanceQ31, in_val: q31) -> q31 {
118    instance.process(in_val)
119}
120
121// --- PID Controller (Q15) ---
122
123#[derive(Debug, Clone, PartialEq, Default)]
124pub struct PidInstanceQ15 {
125    pub a0: q15,
126    pub a1: q15,
127    pub a2: q15,
128    pub state: [q15; 3],
129    pub kp: q15,
130    pub ki: q15,
131    pub kd: q15,
132}
133
134impl PidInstanceQ15 {
135    pub fn new(kp: q15, ki: q15, kd: q15) -> Self {
136        let mut pid = Self {
137            a0: 0,
138            a1: 0,
139            a2: 0,
140            state: [0; 3],
141            kp,
142            ki,
143            kd,
144        };
145        pid.init(1);
146        pid
147    }
148
149    pub fn init(&mut self, reset_state_flag: i32) {
150        self.a0 = self.kp.saturating_add(self.ki).saturating_add(self.kd);
151        self.a1 = (-self.kp).saturating_sub(2 * self.kd);
152        self.a2 = self.kd;
153        if reset_state_flag != 0 {
154            self.reset();
155        }
156    }
157
158    pub fn reset(&mut self) {
159        self.state = [0; 3];
160    }
161
162    pub fn process(&mut self, in_val: q15) -> q15 {
163        let acc = (self.state[2] as i32)
164            + ((self.a0 as i32 * in_val as i32) >> 15)
165            + ((self.a1 as i32 * self.state[0] as i32) >> 15)
166            + ((self.a2 as i32 * self.state[1] as i32) >> 15);
167        let out = acc.clamp(i16::MIN as i32, i16::MAX as i32) as q15;
168        self.state[1] = self.state[0];
169        self.state[0] = in_val;
170        self.state[2] = out;
171        out
172    }
173}
174
175pub fn pid_q15(instance: &mut PidInstanceQ15, in_val: q15) -> q15 {
176    instance.process(in_val)
177}
178
179// --- Clarke Transform ---
180
181/// Forward Clarke transform for f32: 3-phase (ia, ib) -> 2-phase (alpha, beta).
182pub fn clarke_f32(ia: f32, ib: f32, p_alpha: &mut f32, p_beta: &mut f32) {
183    *p_alpha = ia;
184    let inv_sqrt_3 = 0.57735026919f32; // 1 / sqrt(3)
185    *p_beta = (ia + 2.0 * ib) * inv_sqrt_3;
186}
187
188/// Inverse Clarke transform for f32: 2-phase (alpha, beta) -> 3-phase (ia, ib).
189pub fn inv_clarke_f32(alpha: f32, beta: f32, p_ia: &mut f32, p_ib: &mut f32) {
190    *p_ia = alpha;
191    let sqrt_3_div_2 = 0.86602540378f32; // sqrt(3) / 2
192    *p_ib = -0.5 * alpha + sqrt_3_div_2 * beta;
193}
194
195// --- Park Transform ---
196
197/// Forward Park transform for f32: 2-phase stationary (alpha, beta) + angle theta (rad) -> 2-phase rotating (d, q).
198pub fn park_f32(alpha: f32, beta: f32, theta: f32, p_d: &mut f32, p_q: &mut f32) {
199    let cos_t = theta.cos();
200    let sin_t = theta.sin();
201    *p_d = alpha * cos_t + beta * sin_t;
202    *p_q = -alpha * sin_t + beta * cos_t;
203}
204
205/// Inverse Park transform for f32: 2-phase rotating (d, q) + angle theta (rad) -> 2-phase stationary (alpha, beta).
206pub fn inv_park_f32(d: f32, q: f32, theta: f32, p_alpha: &mut f32, p_beta: &mut f32) {
207    let cos_t = theta.cos();
208    let sin_t = theta.sin();
209    *p_alpha = d * cos_t - q * sin_t;
210    *p_beta = d * sin_t + q * cos_t;
211}
embedded_dsp/controller.rs

embedded_dsp/
controller.rs
