diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs index 256cde2..6353f9a 100644 --- a/dsp/src/complex.rs +++ b/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(pub T, pub T); +impl Complex { + pub fn map(&self, func: F) -> Self + where + F: Fn(T) -> T, + { + Complex(func(self.0), func(self.1)) + } +} + impl Complex { /// Return a Complex on the unit circle given an angle. /// @@ -21,19 +32,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,14 +79,51 @@ 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) } } + +impl Mul for Complex { + 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 for Complex { + 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 for Complex { + 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, + ) + } +} diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 75c7b97..b2f38c1 100644 --- a/dsp/src/lockin.rs +++ b/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; 2], - k: u8, + state: [Lowpass; 2], } 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 { + /// The lowpass has a gain of `1 << k`. + pub fn update(&mut self, sample: i16, phase: i32, k: u8) -> Complex { // 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] - .update(((sample as i64 * lo.0 as i64) >> 32) as _, self.k), - self.state[1] - .update(((sample as i64 * lo.1 as i64) >> 32) as _, self.k), + self.state[0].update(mix.0, k), + self.state[1].update(mix.1, k), ) } } diff --git a/dsp/src/lowpass.rs b/dsp/src/lowpass.rs index fb2eedf..91fae2f 100644 --- a/dsp/src/lowpass.rs +++ b/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> { // IIR state storage - xy: GenericArray, + y: GenericArray, } impl> Lowpass { /// Update the filter with a new sample. /// /// # Args - /// * `x`: Input data - /// * `k`: Log2 time constant, 1..32 + /// * `x`: Input data, needs `k` bits headroom. + /// * `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. - let mut x0 = x; - let mut x1 = self.xy[0]; - self.xy[0] = x0; - for y1 in self.xy[1..].iter_mut() { - x0 = *y1 - + (((x0 >> 2) + (x1 >> 2) - (*y1 >> 1) + (1 << (k - 1))) >> k); - x1 = *y1; - *y1 = x0; + // Note DF-II and the zeros at Nyquist. + let mut x = x << k; + for y in self.y.iter_mut() { + let dy = (x - *y + (1 << (k - 1))) >> k; + *y += dy; + x = *y - (dy >> 1); } - x0 + x } } diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index fad27ec..c0e6cae 100644 --- a/src/bin/lockin-external.rs +++ b/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()], - "frequency_discriminator" => [pll_frequency as i32, output.arg()], + "power_phase" => [(output.log2() << 24) as _, output.arg()], + "frequency_discriminator" => [pll_frequency as _, output.arg()], _ => [output.0, output.1], }; diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs index 287815a..cfa3115 100644 --- a/src/bin/lockin-internal.rs +++ b/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; } }