Merge pull request #267 from quartiq/rj/lowpass2

lowpass2

dsp/src/complex.rs

@@ -1,8 +1,19 @@
+use core::ops::Mul;
+
 use super::{atan2, cossin};
 
 #[derive(Copy, Clone, Default, PartialEq, Debug)]
 pub struct Complex<T>(pub T, pub T);
 
+impl<T: Copy> Complex<T> {
+    pub fn map<F>(&self, func: F) -> Self
+    where
+        F: Fn(T) -> T,
+    {
+        Complex(func(self.0), func(self.1))
+    }
+}
+
 impl Complex<i32> {
     /// Return a Complex on the unit circle given an angle.
     ///
@@ -21,19 +32,43 @@ impl Complex<i32> {
 
     /// Return the absolute square (the squared magnitude).
     ///
-    /// Note: Normalization is `1 << 31`, i.e. Q0.31.
+    /// Note: Normalization is `1 << 32`, i.e. U0.32.
+    ///
+    /// Note(panic): This will panic for `Complex(i32::MIN, i32::MIN)`
     ///
     /// Example:
     ///
     /// ```
     /// use dsp::Complex;
-    /// assert_eq!(Complex(i32::MAX, 0).abs_sqr(), i32::MAX - 1);
+    /// assert_eq!(Complex(i32::MIN, 0).abs_sqr(), 1 << 31);
-    /// assert_eq!(Complex(i32::MIN + 1, 0).abs_sqr(), i32::MAX - 1);
+    /// assert_eq!(Complex(i32::MAX, i32::MAX).abs_sqr(), u32::MAX - 3);
     /// ```
-    pub fn abs_sqr(&self) -> i32 {
+    pub fn abs_sqr(&self) -> u32 {
         (((self.0 as i64) * (self.0 as i64)
             + (self.1 as i64) * (self.1 as i64))
-            >> 31) as i32
+            >> 31) as u32
+    }
+
+    /// log2(power) re full scale approximation
+    ///
+    /// TODO: scale up, interpolate
+    ///
+    /// Panic:
+    /// This will panic for `Complex(i32::MIN, i32::MIN)`
+    ///
+    /// Example:
+    ///
+    /// ```
+    /// use dsp::Complex;
+    /// assert_eq!(Complex(i32::MAX, i32::MAX).log2(), -1);
+    /// assert_eq!(Complex(i32::MAX, 0).log2(), -2);
+    /// assert_eq!(Complex(1, 0).log2(), -63);
+    /// assert_eq!(Complex(0, 0).log2(), -64);
+    /// ```
+    pub fn log2(&self) -> i32 {
+        let a = (self.0 as i64) * (self.0 as i64)
+            + (self.1 as i64) * (self.1 as i64);
+        -(a.leading_zeros() as i32)
     }
 
     /// Return the angle.
@@ -44,14 +79,51 @@ impl Complex<i32> {
     ///
     /// ```
     /// use dsp::Complex;
-    /// assert_eq!(Complex(i32::MAX, 0).arg(), 0);
+    /// assert_eq!(Complex(1, 0).arg(), 0);
     /// assert_eq!(Complex(-i32::MAX, 1).arg(), i32::MAX);
     /// assert_eq!(Complex(-i32::MAX, -1).arg(), -i32::MAX);
-    /// assert_eq!(Complex(0, -i32::MAX).arg(), -i32::MAX >> 1);
+    /// assert_eq!(Complex(0, -1).arg(), -i32::MAX >> 1);
-    /// assert_eq!(Complex(0, i32::MAX).arg(), (i32::MAX >> 1) + 1);
+    /// assert_eq!(Complex(0, 1).arg(), (i32::MAX >> 1) + 1);
-    /// assert_eq!(Complex(i32::MAX, i32::MAX).arg(), (i32::MAX >> 2) + 1);
+    /// assert_eq!(Complex(1, 1).arg(), (i32::MAX >> 2) + 1);
     /// ```
     pub fn arg(&self) -> i32 {
         atan2(self.1, self.0)
     }
 }
+
+impl Mul for Complex<i32> {
+    type Output = Self;
+
+    fn mul(self, other: Self) -> Self {
+        let a = self.0 as i64;
+        let b = self.1 as i64;
+        let c = other.0 as i64;
+        let d = other.1 as i64;
+        Complex(
+            ((a * c - b * d + (1 << 31)) >> 32) as i32,
+            ((b * c + a * d + (1 << 31)) >> 32) as i32,
+        )
+    }
+}
+
+impl Mul<i32> for Complex<i32> {
+    type Output = Self;
+
+    fn mul(self, other: i32) -> Self {
+        Complex(
+            ((other as i64 * self.0 as i64 + (1 << 31)) >> 32) as i32,
+            ((other as i64 * self.1 as i64 + (1 << 31)) >> 32) as i32,
+        )
+    }
+}
+
+impl Mul<i16> for Complex<i32> {
+    type Output = Self;
+
+    fn mul(self, other: i16) -> Self {
+        Complex(
+            (other as i32 * (self.0 >> 16) + (1 << 15)) >> 16,
+            (other as i32 * (self.1 >> 16) + (1 << 15)) >> 16,
+        )
+    }
+}

dsp/src/lockin.rs

@@ -1,35 +1,34 @@
 use super::{lowpass::Lowpass, Complex};
-use generic_array::typenum::U3;
+use generic_array::typenum::U2;
 
 #[derive(Clone, Default)]
 pub struct Lockin {
-    state: [Lowpass<U3>; 2],
+    state: [Lowpass<U2>; 2],
-    k: u8,
 }
 
 impl Lockin {
     /// Create a new Lockin with given IIR coefficients.
-    pub fn new(k: u8) -> Self {
+    pub fn new() -> Self {
         let lp = Lowpass::default();
         Self {
             state: [lp.clone(), lp],
-            k,
         }
     }
 
     /// Update the lockin with a sample taken at a given phase.
-    pub fn update(&mut self, sample: i32, phase: i32) -> Complex<i32> {
+    /// The lowpass has a gain of `1 << k`.
+    pub fn update(&mut self, sample: i16, phase: i32, k: u8) -> Complex<i32> {
         // Get the LO signal for demodulation.
         let lo = Complex::from_angle(phase);
 
-        // Mix with the LO signal, filter with the IIR lowpass,
+        // Mix with the LO signal
+        let mix = lo * sample;
+
+        // Filter with the IIR lowpass,
         // return IQ (in-phase and quadrature) data.
-        // Note: 32x32 -> 64 bit multiplications are pretty much free.
         Complex(
-            self.state[0]
+            self.state[0].update(mix.0, k),
-                .update(((sample as i64 * lo.0 as i64) >> 32) as _, self.k),
+            self.state[1].update(mix.1, k),
-            self.state[1]
-                .update(((sample as i64 * lo.1 as i64) >> 32) as _, self.k),
         )
     }
 }

dsp/src/lowpass.rs

@@ -1,38 +1,35 @@
 use generic_array::{ArrayLength, GenericArray};
 
 /// Arbitrary order, high dynamic range, wide coefficient range,
-/// lowpass filter implementation.
+/// lowpass filter implementation. DC gain is 1.
 ///
-/// Type argument N is the filter order + 1.
+/// Type argument N is the filter order.
 #[derive(Clone, Default)]
 pub struct Lowpass<N: ArrayLength<i32>> {
     // IIR state storage
-    xy: GenericArray<i32, N>,
+    y: GenericArray<i32, N>,
 }
 
 impl<N: ArrayLength<i32>> Lowpass<N> {
     /// Update the filter with a new sample.
     ///
     /// # Args
-    /// * `x`: Input data
+    /// * `x`: Input data, needs `k` bits headroom.
-    /// * `k`: Log2 time constant, 1..32
+    /// * `k`: Log2 time constant, 0..31.
     ///
     /// # Return
-    /// Filtered output y
+    /// Filtered output y, with gain of `1 << k`.
     pub fn update(&mut self, x: i32, k: u8) -> i32 {
-        debug_assert!((1..32).contains(&k));
+        debug_assert!(k & 31 == k);
         // This is an unrolled and optimized first-order IIR loop
         // that works for all possible time constants.
-        // Note the zero(s) at Nyquist.
+        // Note DF-II and the zeros at Nyquist.
-        let mut x0 = x;
+        let mut x = x << k;
-        let mut x1 = self.xy[0];
+        for y in self.y.iter_mut() {
-        self.xy[0] = x0;
+            let dy = (x - *y + (1 << (k - 1))) >> k;
-        for y1 in self.xy[1..].iter_mut() {
+            *y += dy;
-            x0 = *y1
+            x = *y - (dy >> 1);
-                + (((x0 >> 2) + (x1 >> 2) - (*y1 >> 1) + (1 << (k - 1))) >> k);
-            x1 = *y1;
-            *y1 = x0;
         }
-        x0
+        x
     }
 }

src/bin/lockin-external.rs

@@ -34,7 +34,7 @@ const APP: () = {
                 + design_parameters::SAMPLE_BUFFER_SIZE_LOG2,
         );
 
-        let lockin = Lockin::new(10);
+        let lockin = Lockin::new();
 
         // Enable ADC/DAC events
         stabilizer.adcs.0.start();
@@ -102,6 +102,9 @@ const APP: () = {
         // Demodulation LO phase offset
         let phase_offset: i32 = 0; // TODO: expose
 
+        // Log2 lowpass time constant
+        let time_constant: u8 = 6; // TODO: expose
+
         let sample_frequency = ((pll_frequency
             // .wrapping_add(1 << design_parameters::SAMPLE_BUFFER_SIZE_LOG2 - 1)  // half-up rounding bias
             >> design_parameters::SAMPLE_BUFFER_SIZE_LOG2)
@@ -115,7 +118,7 @@ const APP: () = {
             .zip(Accu::new(sample_phase, sample_frequency))
             // Convert to signed, MSB align the ADC sample.
             .map(|(&sample, phase)| {
-                lockin.update((sample as i16 as i32) << 16, phase)
+                lockin.update(sample as i16, phase, time_constant)
             })
             .last()
             .unwrap();
@@ -123,8 +126,8 @@ const APP: () = {
         let conf = "frequency_discriminator";
         let output = match conf {
             // Convert from IQ to power and phase.
-            "power_phase" => [output.abs_sqr(), output.arg()],
+            "power_phase" => [(output.log2() << 24) as _, output.arg()],
-            "frequency_discriminator" => [pll_frequency as i32, output.arg()],
+            "frequency_discriminator" => [pll_frequency as _, output.arg()],
             _ => [output.0, output.1],
         };
 

src/bin/lockin-internal.rs

@@ -28,7 +28,7 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
-        let lockin = Lockin::new(10); // TODO: expose
+        let lockin = Lockin::new();
 
         // Enable ADC/DAC events
         stabilizer.adcs.1.start();
@@ -85,10 +85,14 @@ const APP: () = {
             1i32 << (32 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2);
 
         // Harmonic index of the LO: -1 to _de_modulate the fundamental
-        let harmonic: i32 = -1;
+        let harmonic: i32 = -1; // TODO: expose
 
         // Demodulation LO phase offset
-        let phase_offset: i32 = (0.25 * i32::MAX as f32) as i32;
+        let phase_offset: i32 = (0.25 * i32::MAX as f32) as i32; // TODO: expose
+
+        // Log2 lowpass time constant.
+        let time_constant: u8 = 8;
+
         let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
         let sample_phase = phase_offset
             .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
@@ -99,24 +103,14 @@ const APP: () = {
             .zip(Accu::new(sample_phase, sample_frequency))
             // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter)
             .map(|(&sample, phase)| {
-                lockin.update((sample as i16 as i32) << 16, phase)
+                lockin.update(sample as i16, phase, time_constant)
             })
             // Decimate
             .last()
             .unwrap();
 
-        // convert i/q to power/phase,
-        let power_phase = true; // TODO: expose
-
-        let output = if power_phase {
-            // Convert from IQ to power and phase.
-            [output.abs_sqr(), output.arg()]
-        } else {
-            [output.0, output.1]
-        };
-
         for value in dac_samples[1].iter_mut() {
-            *value = (output[1] >> 16) as u16 ^ 0x8000;
+            *value = (output.arg() >> 16) as u16 ^ 0x8000;
         }
     }