diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs index 256cde2..ff3feb5 100644 --- a/dsp/src/complex.rs +++ b/dsp/src/complex.rs @@ -21,19 +21,43 @@ impl Complex { /// 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 { /// /// ``` /// 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) diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs index 5004923..c9d3ef6 100644 --- a/dsp/src/lowpass.rs +++ b/dsp/src/lowpass.rs @@ -14,16 +14,16 @@ impl> Lowpass { /// 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); diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index 5f7451e..a043201 100644 --- a/src/bin/lockin-external.rs +++ b/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], };