From 7b9fc3b2b33ca5dbfef2d684dfb5d43e5066e9d6 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Tue, 26 Jan 2021 18:49:58 +0100
Subject: [PATCH] iir_int: move lowpass coefficient calculation to iirstate

---
 dsp/src/iir_int.rs | 31 +++++++++++++++++++++++++++++++
 dsp/src/lockin.rs  | 26 ++------------------------
 src/bin/lockin.rs  |  6 +++---
 3 files changed, 36 insertions(+), 27 deletions(-)

diff --git a/dsp/src/iir_int.rs b/dsp/src/iir_int.rs
index ea78dd6..a27af60 100644
--- a/dsp/src/iir_int.rs
+++ b/dsp/src/iir_int.rs
@@ -1,8 +1,39 @@
+use super::cossin;
 use serde::{Deserialize, Serialize};
 
+/// Generic vector for integer IIR filter.
+/// This struct is used to hold the x/y input/output data vector or the b/a coefficient
+/// vector.
 #[derive(Copy, Clone, Default, Deserialize, Serialize)]
 pub struct IIRState(pub [i32; 5]);
 
+impl IIRState {
+    /// Lowpass biquad filter using cutoff and sampling frequencies.  Taken from:
+    /// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html
+    ///
+    /// # Args
+    /// * `f` - Corner frequency, or 3dB cutoff frequency (in units of sample rate).
+    /// * `q` - Quality factor (1/sqrt(2) for critical).
+    /// * `k` - DC gain.
+    ///
+    /// # Returns
+    /// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 is set to -1.
+    pub fn lowpass(f: f64, q: f64, k: f64) -> IIRState {
+        let scale = (1i64 << 32) as f64;
+        let fcossin = cossin((f * scale) as u32 as i32);
+        let fcos = fcossin.0 as f64 / scale;
+        let fsin = fcossin.1 as f64 / scale;
+        let alpha = fsin / (2. * q);
+        // IIR uses Q2.30 fixed point
+        let a0 = (1. + alpha) / (1 << IIR::SHIFT) as f64;
+        let b0 = (k / 2. * (1. - fcos) / a0) as _;
+        let a1 = (2. * fcos / a0) as _;
+        let a2 = ((alpha - 1.) / a0) as _;
+
+        IIRState([b0, 2 * b0, b0, a1, a2])
+    }
+}
+
 fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
     // Rounding bias, half up
     let y0 = ((y0 as i64) << shift) + (1 << (shift - 1));
diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs
index b542bd4..2af38f9 100644
--- a/dsp/src/lockin.rs
+++ b/dsp/src/lockin.rs
@@ -41,7 +41,7 @@ impl Lockin {
 mod test {
     use crate::{
         atan2,
-        iir_int::{IIRState, IIR},
+        iir_int::IIRState,
         lockin::Lockin,
         rpll::RPLL,
         testing::{isclose, max_error},
@@ -189,28 +189,6 @@ mod test {
         None
     }
 
-    /// Lowpass biquad filter using cutoff and sampling frequencies.  Taken from:
-    /// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html
-    ///
-    /// # Args
-    /// * `fc` - Corner frequency, or 3dB cutoff frequency (in units of sample rate).
-    /// * `q` - Quality factor (1/sqrt(2) for critical).
-    /// * `k` - DC gain.
-    ///
-    /// # Returns
-    /// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 is set to -1.
-    fn lowpass_iir_coefficients(fc: f64, q: f64, k: f64) -> IIRState {
-        let f = 2. * PI * fc;
-        let a = f.sin() / (2. * q);
-        // IIR uses Q2.30 fixed point
-        let a0 = (1. + a) / (1 << IIR::SHIFT) as f64;
-        let b0 = (k / 2. * (1. - f.cos()) / a0).round() as _;
-        let a1 = (2. * f.cos() / a0).round() as _;
-        let a2 = ((a - 1.) / a0).round() as _;
-
-        IIRState([b0, 2 * b0, b0, a1, a2])
-    }
-
     /// Compute the maximum effect of input noise on the lock-in magnitude computation.
     ///
     /// The maximum effect of noise on the magnitude computation is given by:
@@ -415,7 +393,7 @@ mod test {
             harmonic,
             (demodulation_phase_offset / (2. * PI) * (1i64 << 32) as f64)
                 .round() as i32,
-            &lowpass_iir_coefficients(
+            &IIRState::lowpass(
                 corner_frequency,
                 1. / 2f64.sqrt(), // critical q
                 2.,
diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index e1fbb6d..7066ad8 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -56,7 +56,7 @@ const APP: () = {
         let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2, 0);
 
         let lockin = Lockin::new(
-            &iir_int::IIRState::default(), // TODO: lowpass, expose
+            &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose
         );
 
         // Enable ADC/DAC events
@@ -119,8 +119,8 @@ const APP: () = {
 
         let (pll_phase, pll_frequency) = c.resources.pll.update(
             c.resources.timestamper.latest_timestamp().map(|t| t as i32),
-            21, // relative PLL frequency bandwidth: 2**-21, TODO: expose
-            21, // relative PLL phase bandwidth: 2**-21, TODO: expose
+            22, // relative PLL frequency bandwidth: 2**-22, TODO: expose
+            22, // relative PLL phase bandwidth: 2**-22, TODO: expose
         );
 
         // Harmonic index of the LO: -1 to _de_modulate the fundamental