continuous_blocks/pid.rs
1use crate::state_space::StateSpace;
2use crate::transfer_function::TransferFunction;
3
4/// <!-- doc-ref: pid -->
5/// Parallel PID with a filtered derivative term, using the standard `P, I, D, N` (filter
6/// coefficient) parameterization common to block-diagram continuous-time controller blocks:
7///
8/// $$C(s) = K_p + \frac{K_i}{s} + \frac{K_d N s}{s + N}$$
9///
10/// The pure-derivative term `Kd*s` alone is not realizable as a proper (causal) transfer
11/// function, hence the filter pole at `-N` — a standard, universally-used approximation (a
12/// larger `N` tracks the ideal derivative more closely but stiffens the resulting ODE).
13///
14/// This struct is the compiled gain set only — see `general_mna::block_graph::BlockKind::Pid`
15/// for the netlist-facing component reference (parameters, errors, netlist form, example) and
16/// the anti-windup behavior, which lives at the block-graph evaluation layer, not here.
17#[derive(Debug, Clone, Copy, PartialEq)]
18pub struct Pid {
19 /// Proportional gain.
20 pub kp: f64,
21 /// Integral gain.
22 pub ki: f64,
23 /// Derivative gain.
24 pub kd: f64,
25 /// Derivative filter coefficient; the filter pole sits at `-n`. Must be `> 0` — see
26 /// [`Pid::new`].
27 pub n: f64,
28}
29
30#[derive(Debug, Clone, Copy, PartialEq, Eq)]
31pub enum PidError {
32 /// `n <= 0` puts the derivative filter's pole at `-n`, i.e. at the origin or in the
33 /// right half-plane — a non-causal or outright unstable filter, not the standard,
34 /// universally-used approximation this parameterization exists to give.
35 NonPositiveFilterCoefficient,
36}
37
38impl Pid {
39 pub fn new(kp: f64, ki: f64, kd: f64, n: f64) -> Result<Self, PidError> {
40 if n <= 0.0 {
41 return Err(PidError::NonPositiveFilterCoefficient);
42 }
43 Ok(Pid { kp, ki, kd, n })
44 }
45
46 /// Puts `C(s)` over the common denominator `s*(s+N)`:
47 ///
48 /// ```text
49 /// C(s) = [(Kp+Kd*N)*s^2 + (Kp*N+Ki)*s + Ki*N] / [s^2 + N*s]
50 /// ```
51 ///
52 /// Derived by hand before implementation (see `docs/architecture.md`'s Milestone 5 note):
53 /// with `Kd=0` this numerator factors exactly as `(s+N)(Kp*s+Ki)`, so the filter pole/zero
54 /// cancel and the closed-loop step response of the resulting state-space realization
55 /// reduces to the ideal PI response `y(t) = Kp + Ki*t` for *every* `t`, not just
56 /// asymptotically — `tests/step_responses.rs` checks exactly this.
57 pub fn to_transfer_function(&self) -> TransferFunction {
58 let num = vec![
59 self.kp + self.kd * self.n,
60 self.kp * self.n + self.ki,
61 self.ki * self.n,
62 ];
63 let den = vec![1.0, self.n, 0.0];
64 TransferFunction::new(num, den).expect("PID transfer function is always proper")
65 }
66
67 pub fn to_state_space(&self) -> StateSpace {
68 self.to_transfer_function().to_state_space()
69 }
70}
71
72#[cfg(test)]
73mod tests {
74 use super::*;
75
76 #[test]
77 fn kd_zero_cancels_to_pure_pi_numerator() {
78 // With Kd=0, numerator should equal (s+N)(Kp*s+Ki) expanded, which is what
79 // to_transfer_function already computes directly -- this test instead cross-checks it
80 // against the expanded-by-hand form for a concrete (Kp, Ki, N), as an independent
81 // arithmetic check of the formula itself (not just that the code returns its own
82 // formula back).
83 let pid = Pid::new(2.0, 3.0, 0.0, 10.0).unwrap();
84 let tf = pid.to_transfer_function();
85 // (s+10)(2s+3) = 2s^2 + 3s + 20s + 30 = 2s^2 + 23s + 30
86 assert_eq!(tf.num, vec![2.0, 23.0, 30.0]);
87 assert_eq!(tf.den, vec![1.0, 10.0, 0.0]);
88 }
89
90 #[test]
91 fn non_positive_filter_coefficient_is_rejected_not_silently_unstable() {
92 assert_eq!(
93 Pid::new(2.0, 3.0, 0.5, 0.0),
94 Err(PidError::NonPositiveFilterCoefficient)
95 );
96 assert_eq!(
97 Pid::new(2.0, 3.0, 0.5, -10.0),
98 Err(PidError::NonPositiveFilterCoefficient)
99 );
100 }
101}