cu_pid/
lib.rs

1#![cfg_attr(not(feature = "std"), no_std)]
2
3#[cfg(not(feature = "std"))]
4extern crate alloc;
5
6use bincode::de::Decoder;
7use bincode::enc::Encoder;
8use bincode::error::{DecodeError, EncodeError};
9use bincode::{Decode, Encode};
10use core::marker::PhantomData;
11use cu29::prelude::*;
12use cu29::reflect::{Reflect, ReflectTypePath};
13use serde::{Deserialize, Serialize};
14
15#[cfg(not(feature = "std"))]
16use alloc::format;
17
18/// Output of the PID controller.
19#[derive(Debug, Default, Clone, Encode, Decode, Serialize, Deserialize, Reflect)]
20pub struct PIDControlOutputPayload {
21    /// Proportional term
22    pub p: f32,
23    /// Integral term
24    pub i: f32,
25    /// Derivative term
26    pub d: f32,
27    /// Final output
28    pub output: f32,
29}
30
31/// This is the underlying standard PID controller.
32#[derive(Reflect)]
33pub struct PIDController {
34    // Configuration
35    kp: f32,
36    ki: f32,
37    kd: f32,
38    setpoint: f32,
39    p_limit: f32,
40    i_limit: f32,
41    d_limit: f32,
42    output_limit: f32,
43    sampling: CuDuration,
44    // Internal state
45    integral: f32,
46    last_error: f32,
47    elapsed: CuDuration,
48    last_output: PIDControlOutputPayload,
49}
50
51impl PIDController {
52    #[allow(clippy::too_many_arguments)]
53    pub fn new(
54        kp: f32,
55        ki: f32,
56        kd: f32,
57        setpoint: f32,
58        p_limit: f32,
59        i_limit: f32,
60        d_limit: f32,
61        output_limit: f32,
62        sampling: CuDuration, // to avoid oversampling and get a bunch of zeros.
63    ) -> Self {
64        PIDController {
65            kp,
66            ki,
67            kd,
68            setpoint,
69            integral: 0.0,
70            last_error: 0.0,
71            p_limit,
72            i_limit,
73            d_limit,
74            output_limit,
75            elapsed: CuDuration::default(),
76            sampling,
77            last_output: PIDControlOutputPayload::default(),
78        }
79    }
80
81    pub fn reset(&mut self) {
82        self.integral = 0.0f32;
83        self.last_error = 0.0f32;
84    }
85
86    pub fn reset_integral(&mut self) {
87        self.integral = 0.0f32;
88    }
89
90    pub fn init_measurement(&mut self, measurement: f32) {
91        self.last_error = self.setpoint - measurement;
92        self.elapsed = self.sampling; // force the computation on the first next_control_output
93    }
94
95    pub fn next_control_output(
96        &mut self,
97        measurement: f32,
98        dt: CuDuration,
99    ) -> PIDControlOutputPayload {
100        self.elapsed += dt;
101
102        if self.elapsed < self.sampling {
103            // if we bang too fast the PID controller, just keep on giving the same answer
104            return self.last_output.clone();
105        }
106
107        let error = self.setpoint - measurement;
108        let CuDuration(elapsed) = self.elapsed;
109        let dt = elapsed as f32 / 1_000_000f32; // the unit is kind of arbitrary.
110        if dt == 0.0 {
111            return self.last_output.clone();
112        }
113
114        // Proportional term
115        let p_unbounded = self.kp * error;
116        let p = p_unbounded.clamp(-self.p_limit, self.p_limit);
117
118        // Integral term (accumulated over time)
119        self.integral += error * dt;
120        let i_unbounded = self.ki * self.integral;
121        let i = i_unbounded.clamp(-self.i_limit, self.i_limit);
122
123        // Derivative term (rate of change)
124        let derivative = (error - self.last_error) / dt;
125        let d_unbounded = self.kd * derivative;
126        let d = d_unbounded.clamp(-self.d_limit, self.d_limit);
127
128        // Update last error for next calculation
129        self.last_error = error;
130
131        // Final output: sum of P, I, D with output limit
132        let output_unbounded = p + i + d;
133        let output = output_unbounded.clamp(-self.output_limit, self.output_limit);
134
135        let output = PIDControlOutputPayload { p, i, d, output };
136
137        self.last_output = output.clone();
138        self.elapsed = CuDuration::default();
139        output
140    }
141}
142
143impl Freezable for PIDController {
144    fn freeze<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
145        Encode::encode(&self.integral, encoder)?;
146        Encode::encode(&self.last_error, encoder)?;
147        Encode::encode(&self.elapsed, encoder)?;
148        Encode::encode(&self.last_output, encoder)?;
149        Ok(())
150    }
151
152    fn thaw<D: Decoder>(&mut self, decoder: &mut D) -> Result<(), DecodeError> {
153        self.integral = Decode::decode(decoder)?;
154        self.last_error = Decode::decode(decoder)?;
155        self.elapsed = Decode::decode(decoder)?;
156        self.last_output = Decode::decode(decoder)?;
157        Ok(())
158    }
159}
160
161/// This is the Copper task encapsulating the PID controller.
162#[derive(Reflect)]
163pub struct GenericPIDTask<I>
164where
165    f32: for<'a> From<&'a I>,
166{
167    #[reflect(ignore)]
168    _marker: PhantomData<fn() -> I>,
169    pid: PIDController,
170    first_run: bool,
171    last_tov: CuTime,
172    setpoint: f32,
173    cutoff: f32,
174}
175
176impl<I> CuTask for GenericPIDTask<I>
177where
178    f32: for<'a> From<&'a I>,
179    I: CuMsgPayload + ReflectTypePath + 'static,
180{
181    type Resources<'r> = ();
182    type Input<'m> = input_msg!(I);
183    type Output<'m> = output_msg!(PIDControlOutputPayload);
184
185    fn new(config: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self>
186    where
187        Self: Sized,
188    {
189        match config {
190            Some(config) => {
191                debug!("PIDTask config loaded");
192                let setpoint: f32 = config
193                    .get::<f64>("setpoint")?
194                    .ok_or("'setpoint' not found in config")?
195                    as f32;
196
197                let cutoff: f32 = config.get::<f64>("cutoff")?.ok_or(
198                    "'cutoff' not found in config, please set an operating +/- limit on the input.",
199                )? as f32;
200
201                // p is mandatory
202                let kp = match config.get::<f64>("kp")? {
203                    Some(kp) => Ok(kp as f32),
204                    None => Err(CuError::from(
205                        "'kp' not found in the config. We need at least 'kp' to make the PID algorithm work.",
206                    )),
207                }?;
208
209                let p_limit = getcfg(config, "pl", 2.0f32)?;
210                let ki = getcfg(config, "ki", 0.0f32)?;
211                let i_limit = getcfg(config, "il", 1.0f32)?;
212                let kd = getcfg(config, "kd", 0.0f32)?;
213                let d_limit = getcfg(config, "dl", 2.0f32)?;
214                let output_limit = getcfg(config, "ol", 1.0f32)?;
215
216                let sampling = if let Some(value) = config.get::<u32>("sampling_ms")? {
217                    CuDuration::from(value as u64 * 1_000_000u64)
218                } else {
219                    CuDuration::default()
220                };
221
222                let pid: PIDController = PIDController::new(
223                    kp,
224                    ki,
225                    kd,
226                    setpoint,
227                    p_limit,
228                    i_limit,
229                    d_limit,
230                    output_limit,
231                    sampling,
232                );
233
234                Ok(Self {
235                    _marker: PhantomData,
236                    pid,
237                    first_run: true,
238                    last_tov: CuTime::default(),
239                    setpoint,
240                    cutoff,
241                })
242            }
243            None => Err(CuError::from("PIDTask needs a config.")),
244        }
245    }
246
247    fn process(
248        &mut self,
249        _ctx: &CuContext,
250        input: &Self::Input<'_>,
251        output: &mut Self::Output<'_>,
252    ) -> CuResult<()> {
253        output.tov = input.tov;
254        match input.payload() {
255            Some(payload) => {
256                let tov = match input.tov {
257                    Tov::Time(single) => single,
258                    _ => return Err("Unexpected variant for a TOV of PID".into()),
259                };
260
261                let measure: f32 = payload.into();
262
263                if self.first_run {
264                    self.first_run = false;
265                    self.last_tov = tov;
266                    self.pid.init_measurement(measure);
267                    output.clear_payload();
268                    return Ok(());
269                }
270                let dt = tov - self.last_tov;
271                self.last_tov = tov;
272
273                // update the status of the pid.
274                let state = self.pid.next_control_output(measure, dt);
275                // But safety check if the input is within operational margins and cut power if it is not.
276                let upper_limit = self.setpoint + self.cutoff;
277                let lower_limit = self.setpoint - self.cutoff;
278                if measure > upper_limit {
279                    return Err(format!("{} > {} (cutoff)", measure, upper_limit).into());
280                }
281                if measure < lower_limit {
282                    return Err(format!("{} < {} (cutoff)", measure, lower_limit).into());
283                }
284                output.metadata.set_status(format!(
285                    "{:>5.2} {:>5.2} {:>5.2} {:>5.2}",
286                    &state.output, &state.p, &state.i, &state.d
287                ));
288                output.set_payload(state);
289            }
290            None => output.clear_payload(),
291        };
292        Ok(())
293    }
294
295    fn stop(&mut self, _ctx: &CuContext) -> CuResult<()> {
296        self.pid.reset();
297        self.first_run = true;
298        Ok(())
299    }
300}
301
302/// Store/Restore the internal state of the PID controller.
303impl<I> Freezable for GenericPIDTask<I>
304where
305    f32: for<'a> From<&'a I>,
306{
307    fn freeze<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
308        self.pid.freeze(encoder)?;
309        Encode::encode(&self.first_run, encoder)?;
310        Encode::encode(&self.last_tov, encoder)?;
311        Ok(())
312    }
313
314    fn thaw<D: Decoder>(&mut self, decoder: &mut D) -> Result<(), DecodeError> {
315        self.pid.thaw(decoder)?;
316        self.first_run = Decode::decode(decoder)?;
317        self.last_tov = Decode::decode(decoder)?;
318        Ok(())
319    }
320}
321
322// Small helper befause we do this again and again
323fn getcfg(config: &ComponentConfig, key: &str, default: f32) -> Result<f32, ConfigError> {
324    Ok(config
325        .get::<f64>(key)?
326        .map(|value| value as f32)
327        .unwrap_or(default))
328}
329
330#[cfg(test)]
331mod tests {
332    use super::*;
333    use bincode::config::standard;
334    use bincode::de::DecoderImpl;
335    use bincode::de::read::SliceReader;
336    use bincode::encode_to_vec;
337
338    #[derive(Clone, Copy)]
339    struct TestInput;
340
341    impl From<&TestInput> for f32 {
342        fn from(_: &TestInput) -> Self {
343            0.0
344        }
345    }
346
347    fn sample_task() -> GenericPIDTask<TestInput> {
348        GenericPIDTask {
349            _marker: PhantomData,
350            pid: PIDController {
351                kp: 1.0,
352                ki: 2.0,
353                kd: 3.0,
354                setpoint: 4.0,
355                p_limit: 5.0,
356                i_limit: 6.0,
357                d_limit: 7.0,
358                output_limit: 8.0,
359                sampling: CuDuration::from(9),
360                integral: 10.0,
361                last_error: 11.0,
362                elapsed: CuDuration::from(12),
363                last_output: PIDControlOutputPayload {
364                    p: 13.0,
365                    i: 14.0,
366                    d: 15.0,
367                    output: 16.0,
368                },
369            },
370            first_run: false,
371            last_tov: CuTime::from(17_u64),
372            setpoint: 18.0,
373            cutoff: 19.0,
374        }
375    }
376
377    #[test]
378    fn freeze_thaw_restores_pid_timekeeping_state() {
379        let original = sample_task();
380        let bytes =
381            encode_to_vec(BincodeAdapter(&original), standard()).expect("encode pid task state");
382
383        let mut restored = sample_task();
384        restored.pid.integral = -1.0;
385        restored.pid.last_error = -2.0;
386        restored.pid.elapsed = CuDuration::from(999);
387        restored.pid.last_output = PIDControlOutputPayload {
388            p: -3.0,
389            i: -4.0,
390            d: -5.0,
391            output: -6.0,
392        };
393        restored.first_run = true;
394        restored.last_tov = CuTime::from(1_000_u64);
395
396        let reader = SliceReader::new(&bytes);
397        let mut decoder = DecoderImpl::new(reader, standard(), ());
398        restored.thaw(&mut decoder).expect("thaw pid task state");
399
400        assert_eq!(restored.pid.integral, original.pid.integral);
401        assert_eq!(restored.pid.last_error, original.pid.last_error);
402        assert_eq!(restored.pid.elapsed, original.pid.elapsed);
403        assert_eq!(restored.pid.last_output.p, original.pid.last_output.p);
404        assert_eq!(restored.pid.last_output.i, original.pid.last_output.i);
405        assert_eq!(restored.pid.last_output.d, original.pid.last_output.d);
406        assert_eq!(
407            restored.pid.last_output.output,
408            original.pid.last_output.output
409        );
410        assert_eq!(restored.first_run, original.first_run);
411        assert_eq!(restored.last_tov, original.last_tov);
412    }
413}
cu_pid/lib.rs

cu_pid/
lib.rs
