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}