diff --git a/CHANGELOG.md b/CHANGELOG.md index 0df1ba7..a7a2e80 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,10 @@ See also: ## [UNRELEASED](https://github.com/quartiq/idsp/compare/v0.23.0...HEAD) - DATE +### Fixed + +* Preserve fixed-point PID integrator poles when derivative gain is limited. + ## [0.23.0](https://github.com/quartiq/idsp/compare/v0.22.1...v0.23.0) - 2026-09-25 ### Changed diff --git a/src/iir/pid.rs b/src/iir/pid.rs index 63df6a7..339854f 100644 --- a/src/iir/pid.rs +++ b/src/iir/pid.rs @@ -1,6 +1,6 @@ //! PID controller IIR filters -use core::ops::{Add, AddAssign, Div, Mul, SubAssign}; +use core::ops::{Add, Div, Mul, Sub}; use num_traits::{AsPrimitive, Float}; @@ -234,7 +234,7 @@ impl Builder { impl Build<[C; 5]> for Builder where - C: 'static + Copy + SubAssign + AddAssign, + C: 'static + Copy + Add + Sub, T: Float + AsPrimitive, { type Context = T; @@ -265,53 +265,44 @@ where // Choose relevant gains and limits and scale let mut z = period.powi(-(self.order as i32)); let mut gl = [[T::zero(); 2]; 3]; - for (gl, (i, (gain, limit))) in gl - .iter_mut() - .zip( - self.gain - .iter() - .zip(self.limit.iter()) - .enumerate() - .skip(self.order as usize), - ) - .rev() - { - gl[0] = *gain * z; + for (j, gl) in gl.iter_mut().enumerate().rev() { + let i = self.order as usize + j; + gl[0] = self.gain[i] * z; gl[1] = if i == Action::P as usize { T::one() } else { - gl[0] / *limit + gl[0] / self.limit[i] }; z = z * *period; } // Normalization let a0i = (gl[0][1] + gl[1][1] + gl[2][1]).recip(); - - // Derivative/integration kernels - let kernels = [[1, 0, 0], [1, -1, 0], [1, -2, 1]]; - - // Coefficients - let mut ba = [[T::zero().as_(); 2]; 3]; - for (gli, ki) in gl.into_iter().zip(kernels) { - // Quantize the gains and not the coefficients - let gli = gli.map(|c| (c * a0i).as_()); - for (baj, kij) in ba.iter_mut().zip(ki) { - if kij > 0 { - for _ in 0..kij { - baj[0] += gli[0]; - baj[1] -= gli[1]; - } - } else { - for _ in 0..-kij { - baj[0] -= gli[0]; - baj[1] += gli[1]; - } - } + // Quantize kernels before combining them. Negative denominator weights + // use the feedback sign and allow formats that represent -1 but not +1. + let mut gl = gl.map(|[g, l]| [(g * a0i).as_(), (-l * a0i).as_()]); + let p = Action::P as usize - self.order as usize; + let mut proportional = (-T::one()).as_(); + for (i, [_, limit]) in gl.iter().enumerate() { + if i != p { + proportional = proportional - *limit; } } - - [ba[0][0], ba[1][0], ba[2][0], ba[1][1], ba[2][1]] + // Complete normalization in coefficient arithmetic without perturbing + // zero (unlimited) or finite integral-limit kernels. + gl[p][1] = proportional; + + // Expand the quantized kernels 1, (1-z^-1), (1-z^-1)^2. + // Subtract twice rather than doubling: the doubled term may not fit. + let [[g0, _], [g1, l1], [g2, l2]] = gl; + let zero = T::zero().as_(); + [ + g0 + g1 + g2, + zero - g1 - g2 - g2, + g2, + zero - l1 - l2 - l2, + l2, + ] } } @@ -583,16 +574,75 @@ mod test { } #[test] - fn pid_i32() { + fn unlimited_integrator_with_derivative_limit() { use dsp_fixedpoint::Q32; - let b: Biquad> = pid::Builder::::default() - .gain(pid::Action::I, 1e-5) - .gain(pid::Action::P, 1e-2) - .gain(pid::Action::D, 1e0) - .limit(pid::Action::I, 1e1) - .limit(pid::Action::D, 1e-1) - .build(&1.0); - println!("{b:?}"); + for sign in [-1.0f32, 1.0] { + for period in [0.1, 1e-5] { + for d in [0.0, 1.0] { + let ba: [Q32<29>; 5] = pid::Builder::default() + .gain(pid::Action::I, sign) + .gain(pid::Action::P, sign) + .gain(pid::Action::D, sign * d) + .limit_d(sign) + .build(&period); + let ba = ba.map(|c| i64::from(c.into_bits())); + // D(z)=(1-z^-1)(1-r*z^-1): exact integrator, + // derivative pole r rounded to coefficient precision. + assert_eq!(ba[3] + ba[4], 1 << 29); + let r = -ba[4] as f64 / (1u64 << 29) as f64; + let want = f64::from(d) / f64::from(period + d); + assert!((r - want).abs() < 1e-7); + } + } + } + } + + #[test] + fn integral_limits_preserve_finite_dc_gain() { + use dsp_fixedpoint::Q32; + for (order, want) in [ + (pid::Order::P, 0.125), + (pid::Order::I, 0.25), + (pid::Order::I2, 0.5), + ] { + let ba: [Q32<29>; 5] = pid::Builder::default() + .order(order) + .gain(pid::Action::I2, 0.125f32) + .gain(pid::Action::I, 0.125) + .gain(pid::Action::P, 0.125) + .gain(pid::Action::D, 0.125) + .gain(pid::Action::D2, 0.125) + .limit_i2(0.5) + .limit_i(0.25) + .limit_d(0.5) + .limit_d2(0.5) + .build(&0.5); + let ba = ba.map(|c| f64::from(c.into_bits()) / (1u64 << 29) as f64); + let dc = (ba[0] + ba[1] + ba[2]) / (1.0 - ba[3] - ba[4]); + assert!((dc - want).abs() < 1e-7); + } + // A stable limited integrator can fit Q31, which cannot encode +1. + let ba: [Q32<31>; 5] = pid::Builder::default() + .gain(pid::Action::I, 0.125f32) + .gain(pid::Action::P, 0.125) + .limit_i(0.25) + .build(&2.0); + let ba = ba.map(|c| f64::from(c.into_bits()) / (1u64 << 31) as f64); + assert_eq!((ba[0] + ba[1]) / (1.0 - ba[3]), 0.25); + } + + #[test] + fn unlimited_double_integrator() { + use dsp_fixedpoint::Q32; + let ba: [Q32<29>; 5] = pid::Builder::default() + .order(pid::Order::I2) + .gain(pid::Action::I2, 1.0f32) + .gain(pid::Action::I, 1.0) + .gain(pid::Action::P, 1.0) + .build(&0.1); + let ba = ba.map(|c| c.into_bits()); + // D(z)=(1-z^-1)^2 preserves both poles at unity. + assert_eq!([ba[3], ba[4]], [2 << 29, -(1 << 29)]); } #[test] diff --git a/src/pll.rs b/src/pll.rs index af929e7..74661e0 100644 --- a/src/pll.rs +++ b/src/pll.rs @@ -75,6 +75,16 @@ pub struct PLLState { } impl PLLState { + /// Advance one update period at the current frequency and return phase. + /// + /// Leaves the detector, loop filter, and frequency states unchanged. + /// Use for an update without an observation; [`PLL::process`] already + /// performs this advance when an observation is supplied. + pub fn predict(&mut self) -> W { + self.y += self.frequency(); + self.y + } + /// Return the current phase estimate pub fn phase(&self) -> W { self.y @@ -88,8 +98,7 @@ impl PLLState { impl SplitProcess, W, PLLState> for PLL { fn process(&self, state: &mut PLLState, x: W) -> W { - // advance output phase, oscillator DC pole - state.y += state.frequency(); + state.predict(); // phase error let z0 = state.clamp.process(x + state.y).0 >> 1; // nyquist zero @@ -114,6 +123,27 @@ mod tests { use super::*; use core::num::Wrapping as W; + #[test] + fn prediction_wraps_without_correcting() { + let mut state = PLLState { + y: W(i32::MAX - 7), + f: W((16i64 << 32) + 123), + f0: 999, + z0: 7, + y0: 9, + ..Default::default() + }; + let unchanged = |s: &PLLState| (s.clamp.x0, s.clamp.clamp, s.z0, s.y0, s.f0, s.f); + let before = unchanged(&state); + let mut expected = state.y; + for _ in 0..4 { + expected += W(16); + assert_eq!(state.predict(), expected); + assert_eq!(state.phase(), expected); + assert_eq!(unchanged(&state), before); + } + } + #[test] fn converge_pll() { let p = PLL::from_bandwidth(5e-2, 4.0); diff --git a/src/sweptsine.rs b/src/sweptsine.rs index cbbc493..3613a51 100644 --- a/src/sweptsine.rs +++ b/src/sweptsine.rs @@ -76,28 +76,43 @@ impl Sweep { self.state as f64 / (Q as f64 * self.rate as f64) } - /// Evaluate integrated sweep at a given time + /// Analytical exponential phase in turns at time `t` in samples. + /// + /// Ignoring recurrence quantization, sample `n` from [`AccuOsc::new`] + /// has phase `continuous(n + 1) - cycles()`: accumulation starts at zero + /// and each increment is added before emitting a sample. #[inline] pub fn continuous(&self, t: f64) -> f64 { self.cycles() * Real::exp(self.rate() * t) } - /// Inverse filter + /// Analytical positive-frequency inverse for the sine from [`AccuOsc::new`]. + /// + /// `f` is in cycles/sample and sample zero is the first emitted sample. + /// Uses the initial sweep state and includes its initial-phase and + /// one-sample advance corrections. Requires positive rate, state, and `f`. + /// Finite windowing and recurrence/trig quantization are not modeled; + /// long sweeps can lose precision in the analytical f32 phase. /// /// * Stimulus `x(t)` (`AccuOsc::new(Sweep::fit(...)).collect()` plus windowing) /// * Response `y(t)` /// * Response FFT `Y(f)` /// * Stimulus inverse filter `X'(f)` - /// * Transfer function `H(f) = X'(f) Y(f)' + /// * Transfer function `H(f) = X'(f) Y(f)` /// * Impulse response `h(t)` /// * Windowing each response using `delay()` /// * Order responses `H_n(f)` pub fn inverse_filter(&self, mut f: f32) -> Complex { let rate = Real::ln_1p(self.rate as f32 / Q); + // S/R is C in Q32 turns: narrow before converting to preserve frac(C). + let phase = (self.state / i64::from(self.rate)) as i32; + let phase = phase as f32 / Q - f + 0.125; f /= rate; let amp = 2.0 * rate * f.sqrt(); let inv_cycles = Q * self.rate as f32 / self.state as f32; - let turns = 0.125 - f * (1.0 - Real::ln(f * inv_cycles)); + // Reduce before adding bounded offsets and converting turns to radians. + // This preserves the offsets, not bits already lost in the chirp phase. + let turns = Real::fract(-f * (1.0 - Real::ln(f * inv_cycles))) + phase; let (im, re) = Real::sin_cos(f32::TAU() * turns); Complex::new(amp * re, amp * im) } @@ -107,6 +122,10 @@ impl Sweep { /// * stop: maximum stop frequency in units of sample rate (e.g. Nyquist, 0.5) /// * harmonics: number of harmonics to sweep /// * cycles: number of cycles (phase wraps) per harmonic (>= 1) + /// + /// Integer cycles remove the constant phase offset modulo one turn. + /// Rates with nearly integer `octave()` also align octave boundaries to + /// samples; neither choice eliminates recurrence rounding. pub fn fit(stop: f32, harmonics: f32, cycles: f32) -> Result { if !(0.0..=0.5).contains(&stop) { return Err(SweepError::Stop); @@ -206,6 +225,48 @@ mod test { } } + #[test] + fn emitted_phase_and_inverse() { + for cycles in [3.0, 3.25] { + let sweep = Sweep::fit(0.25, 100.0, cycles).unwrap(); + let phase = |t: f64| sweep.continuous(t + 1.0) - sweep.cycles(); + let mut osc = AccuOsc::new(sweep.clone()); + for n in 0..512 { + osc.next().unwrap(); + let error = osc.state().0 as f64 / (1u128 << 64) as f64 - phase(n as f64); + assert!((error - error.round()).abs() < 1e-8); + } + for f in [0.01_f32, 0.05, 0.2] { + // Stationary phase: phi'(t) = f. The positive-frequency + // sine spectrum has phase 2*pi*(phi(t) - f*t) - pi/4. + let frequency = f as f64; + let t = (frequency / sweep.state()).ln() / sweep.rate() - 1.0; + let angle = f64::TAU() * (frequency * t - phase(t)) + f64::FRAC_PI_4(); + let (im, re) = angle.sin_cos(); + let amplitude = 2.0 * (sweep.rate() * frequency).sqrt(); + let inverse = sweep.inverse_filter(f).0.map(|v| v as f64 / amplitude); + assert!(allclose(&inverse, &[re, im], 0.0, 2e-3)); + } + } + } + + #[test] + fn inverse_large_cycle_offset() { + // These states round to the same f32, but differ by a quarter cycle. + let sweep = Sweep::new(1, 1 << 60); + let shifted = Sweep::new(1, sweep.state + (1 << 30)); + let f = 1.0 / (1u32 << 30) as f32; + let a = sweep.inverse_filter(f); + let b = shifted.inverse_filter(f); + let scale = a.norm_sqr().sqrt(); + assert!(allclose( + &b.0.map(|v| v / scale), + &[-a.im() / scale, a.re() / scale], + 0.0, + 1e-6, + )); + } + #[test] fn invalid_fit() { for cycles in [0.0, 0.5, f32::NAN, f32::INFINITY] {