Skip to main content

continuous_blocks/
discrete_pid.rs

1//! A discrete-time PID (P/I/PI/PD/PID), realized directly from its own block diagram (three
2//! parallel branches, summed) rather than by combining into one z-domain transfer function and
3//! converting back to state-space — see `book/dev-guide/src/discrete-time-blocks.md`'s own
4//! Category 3 for why, and for the source-manual correction (a docling reading-order swap, not
5//! assumed) the integral/derivative formulas below depend on.
6//!
7//! The derivative action's own internal low-pass filter is **always** realized with the
8//! Forward Euler integrator formula, regardless of what [`DiscreteIntegrationMethod`] the
9//! integral action uses — not an arbitrary choice: Forward Euler is the only one of the three
10//! methods with no direct feedthrough, and this filter's own topology (its integrator's output
11//! feeds back into the very sum that computes the integrator's next input) would be an
12//! unresolvable same-step algebraic loop under Backward Euler or Trapezoidal, both of which
13//! *do* have feedthrough.
14
15/// How the integral action's own discrete integrator advances — see each variant's own
16/// recursion in [`DiscretePid::step`]. Verified directly against the source manual's own PDF
17/// (not the initial, swapped automated conversion — see this module's own doc comment).
18#[derive(Debug, Clone, Copy, PartialEq, Eq)]
19pub enum DiscreteIntegrationMethod {
20    /// `y[k] = y[k-1] + T*e[k-1]` — no direct feedthrough (`y[k]` never references `e[k]`).
21    ForwardEuler,
22    /// `y[k] = y[k-1] + T*e[k]` — direct feedthrough.
23    BackwardEuler,
24    /// `y[k] = y[k-1] + (T/2)*(e[k-1]+e[k])` — direct feedthrough.
25    Trapezoidal,
26}
27
28impl DiscreteIntegrationMethod {
29    pub fn from_name(name: &str) -> Option<Self> {
30        use DiscreteIntegrationMethod::*;
31        Some(match name {
32            "forward" => ForwardEuler,
33            "backward" => BackwardEuler,
34            "trapezoidal" => Trapezoidal,
35            _ => return None,
36        })
37    }
38
39    pub fn name(self) -> &'static str {
40        use DiscreteIntegrationMethod::*;
41        match self {
42            ForwardEuler => "forward",
43            BackwardEuler => "backward",
44            Trapezoidal => "trapezoidal",
45        }
46    }
47}
48
49/// A discrete PID's own configuration — `kp`/`ki`/`kd` the usual gains, `n` the derivative
50/// filter coefficient (this codebase's own existing name for the concept the source manual
51/// calls `Kf` — same idea, kept consistent with the continuous [`crate::Pid`]'s own field name
52/// rather than importing a second spelling for it), `period` the sample period `T` baked in at
53/// construction (from the declaring block's own `SampleTimeSpec::Periodic`, not a separate
54/// netlist field — the discretized recursion's own coefficients need the *same* period the
55/// block is scheduled at, so there is exactly one place to declare it).
56#[derive(Debug, Clone, Copy, PartialEq)]
57pub struct DiscretePid {
58    pub kp: f64,
59    pub ki: f64,
60    pub kd: f64,
61    pub n: f64,
62    pub period: f64,
63    pub method: DiscreteIntegrationMethod,
64}
65
66/// [`DiscretePid`]'s own persisted state — two small scalars per branch that needs memory (the
67/// proportional action is stateless). `int_prev_e` is only ever read/written when `method` is
68/// `ForwardEuler`/`Trapezoidal` (both need a remembered previous error); `BackwardEuler` never
69/// touches it, but it's still always present in this struct rather than modeled as an
70/// `Option`/per-method enum, since a fixed-shape state is simpler for `dae-runtime` to hold
71/// regardless of which method a given instance happens to use.
72#[derive(Debug, Clone, Copy, PartialEq, Default)]
73pub struct DiscretePidState {
74    /// The integral action's own accumulated value (`Ki`-scaled contribution to `u` is
75    /// `ki * int_state` after each step's own update — see [`DiscretePid::step`]).
76    pub int_state: f64,
77    /// The integral action's own remembered previous error — only meaningful for
78    /// `ForwardEuler`/`Trapezoidal`.
79    pub int_prev_e: f64,
80    /// The derivative filter's own accumulated value.
81    pub filt_state: f64,
82    /// The derivative filter's own remembered previous input (`v[k-1]`) — always meaningful,
83    /// since the filter is always Forward Euler.
84    pub filt_prev_v: f64,
85}
86
87impl DiscretePid {
88    /// One step: given this step's own error `e`, advances `state` in place and returns the
89    /// **unsaturated** controller output `u[k] = Kp*e[k] + Ki*int_output[k] + Kd*v[k]` — the
90    /// caller (`dae-runtime`) applies anti-windup clamping on top of this, the same
91    /// tentative-step/reject logic the continuous `Pid`'s own arm already uses (this method
92    /// itself has no notion of saturation limits at all).
93    pub fn step(&self, state: &mut DiscretePidState, e: f64) -> f64 {
94        // Integral action: a plain discrete integrator on the raw error, using whichever method
95        // this instance was configured with -- the corrected formulas from
96        // `discrete-time-blocks.md`'s own Category 1 survey.
97        let int_output = match self.method {
98            DiscreteIntegrationMethod::ForwardEuler => {
99                let y = state.int_state + self.period * state.int_prev_e;
100                state.int_state = y;
101                state.int_prev_e = e;
102                y
103            }
104            DiscreteIntegrationMethod::BackwardEuler => {
105                let y = state.int_state + self.period * e;
106                state.int_state = y;
107                y
108            }
109            DiscreteIntegrationMethod::Trapezoidal => {
110                let y = state.int_state + (self.period / 2.0) * (state.int_prev_e + e);
111                state.int_state = y;
112                state.int_prev_e = e;
113                y
114            }
115        };
116
117        // Derivative action: always Forward Euler (see this module's own doc comment for why).
118        // `filt_state` is read *before* being updated -- no feedthrough, so `v` can be computed
119        // from this step's own `e` with no algebraic loop.
120        let v = self.n * (self.kd * e - state.filt_state);
121        let filt_next = state.filt_state + self.period * state.filt_prev_v;
122        state.filt_prev_v = v;
123        state.filt_state = filt_next;
124
125        self.kp * e + self.ki * int_output + v
126    }
127}
128
129#[cfg(test)]
130mod tests {
131    use super::*;
132
133    #[test]
134    fn pure_p_is_stateless_and_matches_kp_times_error_every_step() {
135        let pid = DiscretePid {
136            kp: 3.0,
137            ki: 0.0,
138            kd: 0.0,
139            n: 1.0,
140            period: 0.01,
141            method: DiscreteIntegrationMethod::ForwardEuler,
142        };
143        let mut state = DiscretePidState::default();
144        assert_eq!(pid.step(&mut state, 2.0), 6.0);
145        assert_eq!(pid.step(&mut state, -1.0), -3.0); // no memory of the previous call
146    }
147
148    #[test]
149    fn forward_euler_integral_matches_the_hand_derived_no_feedthrough_recursion() {
150        // y[k] = y[k-1] + T*e[k-1] -- e held at 2.0 throughout, T=0.1, Ki=5.
151        let pid = DiscretePid {
152            kp: 0.0,
153            ki: 5.0,
154            kd: 0.0,
155            n: 1.0,
156            period: 0.1,
157            method: DiscreteIntegrationMethod::ForwardEuler,
158        };
159        let mut state = DiscretePidState::default();
160        // Step 1: int_state=0 + 0.1*int_prev_e(=0, default) = 0 -> u = 5*0 = 0.
161        assert_eq!(pid.step(&mut state, 2.0), 0.0);
162        // Step 2: int_state=0 + 0.1*2.0 (prev e) = 0.2 -> u = 5*0.2 = 1.0.
163        assert_eq!(pid.step(&mut state, 2.0), 1.0);
164        // Step 3: int_state=0.2 + 0.1*2.0 = 0.4 -> u = 5*0.4 = 2.0.
165        assert_eq!(pid.step(&mut state, 2.0), 2.0);
166    }
167
168    #[test]
169    fn backward_euler_integral_has_feedthrough_unlike_forward_euler() {
170        // y[k] = y[k-1] + T*e[k] -- same e/T/Ki as above, but should react on the *first* call
171        // (feedthrough), unlike Forward Euler's zero first output above.
172        let pid = DiscretePid {
173            kp: 0.0,
174            ki: 5.0,
175            kd: 0.0,
176            n: 1.0,
177            period: 0.1,
178            method: DiscreteIntegrationMethod::BackwardEuler,
179        };
180        let mut state = DiscretePidState::default();
181        // Step 1: int_state=0 + 0.1*2.0 = 0.2 -> u = 5*0.2 = 1.0 (nonzero on the first call).
182        assert_eq!(pid.step(&mut state, 2.0), 1.0);
183        assert_eq!(pid.step(&mut state, 2.0), 2.0);
184    }
185
186    #[test]
187    fn trapezoidal_integral_averages_current_and_previous_error() {
188        let pid = DiscretePid {
189            kp: 0.0,
190            ki: 1.0,
191            kd: 0.0,
192            n: 1.0,
193            period: 1.0,
194            method: DiscreteIntegrationMethod::Trapezoidal,
195        };
196        let mut state = DiscretePidState::default();
197        // Step 1: y = 0 + (1/2)*(0 (prev, default) + 2.0) = 1.0.
198        assert_eq!(pid.step(&mut state, 2.0), 1.0);
199        // Step 2: y = 1.0 + (1/2)*(2.0 + 4.0) = 1.0 + 3.0 = 4.0.
200        assert_eq!(pid.step(&mut state, 4.0), 4.0);
201    }
202
203    #[test]
204    fn pure_d_settles_to_zero_for_a_constant_error_a_real_derivative_property() {
205        // A real derivative action's output must decay to zero when its own input stops
206        // changing -- checked here numerically (many steps at constant e), not just that the
207        // recursion runs.
208        let pid = DiscretePid {
209            kp: 0.0,
210            ki: 0.0,
211            kd: 2.0,
212            n: 50.0, // a fast filter pole so this settles within a modest number of steps
213            period: 0.001,
214            method: DiscreteIntegrationMethod::ForwardEuler,
215        };
216        let mut state = DiscretePidState::default();
217        let mut last = f64::MAX;
218        for _ in 0..2000 {
219            last = pid.step(&mut state, 3.0); // constant error
220        }
221        assert!(
222            last.abs() < 1e-6,
223            "expected settled derivative ~0, got {last}"
224        );
225    }
226
227    #[test]
228    fn from_name_matches_conventional_names_and_rejects_unknown() {
229        assert_eq!(
230            DiscreteIntegrationMethod::from_name("backward"),
231            Some(DiscreteIntegrationMethod::BackwardEuler)
232        );
233        assert_eq!(DiscreteIntegrationMethod::from_name("nope"), None);
234    }
235}