Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
144 changes: 97 additions & 47 deletions src/iir/pid.rs
Original file line number Diff line number Diff line change
@@ -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};

Expand Down Expand Up @@ -234,7 +234,7 @@ impl<T: Float> Builder<T> {

impl<T, C> Build<[C; 5]> for Builder<T>
where
C: 'static + Copy + SubAssign + AddAssign,
C: 'static + Copy + Add<Output = C> + Sub<Output = C>,
T: Float + AsPrimitive<C>,
{
type Context = T;
Expand Down Expand Up @@ -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,
]
}
}

Expand Down Expand Up @@ -583,16 +574,75 @@ mod test {
}

#[test]
fn pid_i32() {
fn unlimited_integrator_with_derivative_limit() {
use dsp_fixedpoint::Q32;
let b: Biquad<Q32<29>> = pid::Builder::<f32>::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]
Expand Down
34 changes: 32 additions & 2 deletions src/pll.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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<i32> {
self.y += self.frequency();
self.y
}

/// Return the current phase estimate
pub fn phase(&self) -> W<i32> {
self.y
Expand All @@ -88,8 +98,7 @@ impl PLLState {

impl SplitProcess<W<i32>, W<i32>, PLLState> for PLL {
fn process(&self, state: &mut PLLState, x: W<i32>) -> W<i32> {
// 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
Expand All @@ -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);
Expand Down
69 changes: 65 additions & 4 deletions src/sweptsine.rs
Original file line number Diff line number Diff line change
Expand Up @@ -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<f32> {
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)
}
Expand All @@ -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<Self, SweepError> {
if !(0.0..=0.5).contains(&stop) {
return Err(SweepError::Stop);
Expand Down Expand Up @@ -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] {
Expand Down
Loading