From d1f41b3ad5518b6a211c2e4dd8592e4b6798436d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Robert=20J=C3=B6rdens?= <rj@quartiq.de>
Date: Tue, 26 Jan 2021 19:19:03 +0100
Subject: [PATCH] int_iir: use taylor for lowpass

---
 dsp/src/iir_int.rs | 24 +++++++++++++++++++-----
 1 file changed, 19 insertions(+), 5 deletions(-)

diff --git a/dsp/src/iir_int.rs b/dsp/src/iir_int.rs
index a27af60..e731484 100644
--- a/dsp/src/iir_int.rs
+++ b/dsp/src/iir_int.rs
@@ -1,4 +1,4 @@
-use super::cossin;
+use core::f64::consts::PI;
 use serde::{Deserialize, Serialize};
 
 /// Generic vector for integer IIR filter.
@@ -13,16 +13,19 @@ impl IIRState {
     ///
     /// # Args
     /// * `f` - Corner frequency, or 3dB cutoff frequency (in units of sample rate).
+    ///         This is only accurate for low corner frequencies less than ~0.01.
     /// * `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.
+    ///
+    /// # Note
     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;
+        // 3rd order Taylor approximation of sin and cos.
+        let f = f * 2. * PI;
+        let fsin = f - f * f * f / 6.;
+        let fcos = 1. - f * f / 2.;
         let alpha = fsin / (2. * q);
         // IIR uses Q2.30 fixed point
         let a0 = (1. + alpha) / (1 << IIR::SHIFT) as f64;
@@ -88,3 +91,14 @@ impl IIR {
         y0
     }
 }
+
+#[cfg(test)]
+mod test {
+    use super::IIRState;
+
+    #[test]
+    fn lowpass_gen() {
+        let ba = IIRState::lowpass(1e-3, 1. / 2f64.sqrt(), 2.);
+        println!("{:?}", ba.0);
+    }
+}