complex: log2, update bins

dsp/src/complex.rs

@@ -21,19 +21,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 + 1, 0).abs_sqr(), i32::MAX - 1);
+    /// assert_eq!(Complex(i32::MIN, 0).abs_sqr(), 1 << 31);
+    /// 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,12 +68,12 @@ 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, i32::MAX).arg(), (i32::MAX >> 1) + 1);
-    /// assert_eq!(Complex(i32::MAX, i32::MAX).arg(), (i32::MAX >> 2) + 1);
+    /// assert_eq!(Complex(0, -1).arg(), -i32::MAX >> 1);
+    /// assert_eq!(Complex(0, 1).arg(), (i32::MAX >> 1) + 1);
+    /// assert_eq!(Complex(1, 1).arg(), (i32::MAX >> 2) + 1);
     /// ```
     pub fn arg(&self) -> i32 {
         atan2(self.1, self.0)

dsp/src/lowpass.rs

@@ -14,16 +14,16 @@ impl<N: ArrayLength<i32>> Lowpass<N> {
     /// Update the filter with a new sample.
     ///
     /// # Args
-    /// * `x`: Input data, needs k bits headroom
+    /// * `x`: Input data
     /// * `k`: Log2 time constant, 0..31
     ///
     /// # Return
-    /// Filtered output y
+    /// Filtered output y, needs `k` bits headroom
     pub fn update(&mut self, x: i32, k: u8) -> i32 {
         debug_assert!(k & 31 == k);
         // This is an unrolled and optimized first-order IIR loop
         // that works for all possible time constants.
-        // Note DF-II and the zero(s) at Nyquist.
+        // Note DF-II and the zeros at Nyquist.
         let mut x = x;
         for y in self.y.iter_mut() {
             let dy = x - (*y >> k);

src/bin/lockin-external.rs

@@ -126,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()],
-            "frequency_discriminator" => [pll_frequency as i32, output.arg()],
+            "power_phase" => [(output.log2() << 10) as _, output.arg()],
+            "frequency_discriminator" => [pll_frequency as _, output.arg()],
             _ => [output.0 << 16, output.1 << 16],
         };