diff --git a/CHANGELOG.md b/CHANGELOG.md index 9672ff7..e693928 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,10 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 * `Dsm`: Delta sigma modulator/noise shaper in MASH-(1)^K architecture. +### Changed + +* `Pid`: Fixed period/rate mixup. + ## [0.14.1](https://github.com/quartiq/idsp/compare/v0.14.0..v0.14.1) - 2024-01-15 * Fixed changelog diff --git a/src/iir/pid.rs b/src/iir/pid.rs index 388ec09..0066673 100644 --- a/src/iir/pid.rs +++ b/src/iir/pid.rs @@ -85,6 +85,8 @@ impl Pid { /// Gains are accurate in the low frequency limit. Towards Nyquist, the /// frequency response is warped. /// + /// Note that limit signs and gain signs should match. + /// /// ``` /// # use idsp::iir::*; /// let tau = 1e-3; @@ -97,7 +99,7 @@ impl Pid { /// .into(); /// let x0 = 5.0; /// let y0 = i.update(&mut [0.0; 4], x0); - /// assert!((y0 / (x0 * ki / tau) - 1.0).abs() < 2.0 * f32::EPSILON); + /// assert!((y0 / (x0 * tau * ki) - 1.0).abs() < 2.0 * f32::EPSILON); /// ``` /// /// # Arguments @@ -113,6 +115,9 @@ impl Pid { /// Gain limit units are `output/input`. See also [`Pid::gain()`]. /// Multiple gains and limits may interact and lead to peaking. /// + /// Note that limit signs and gain signs should match and that the + /// default limits are positive infinity. + /// /// ``` /// # use idsp::iir::*; /// let ki_limit = 1e3; @@ -175,7 +180,8 @@ impl Pid { } // Scale gains, compute limits - let mut zi = self.period.powi(low as i32 - KP as i32); + let mut zi = self.period.powi(KP as i32 - low as i32); + let p = self.period.recip(); let mut gl = [[T::zero(); 2]; 3]; for (gli, (i, (ggi, lli))) in gl.iter_mut().zip( self.gains @@ -186,7 +192,7 @@ impl Pid { ) { gli[0] = *ggi * zi; gli[1] = if i == KP { T::one() } else { gli[0] / *lli }; - zi = zi * self.period; + zi = zi * p; } let a0i = T::one() / (gl[0][1] + gl[1][1] + gl[2][1]); @@ -271,7 +277,7 @@ mod test { let mut xy = [0.0; 4]; for i in 1..10 { let y_have = b.update(&mut xy, 1.0); - let y_want = (i as f32) * (ki / tau); + let y_want = (i as f32) * tau * ki; assert!( (y_have / y_want - 1.0).abs() < 3.0 * f32::EPSILON, "{i}: have {y_have} != {y_want}"