lockin: refactor Lockin

dsp/src/iir_int.rs

@@ -3,6 +3,20 @@ use serde::{Deserialize, Serialize};
 #[derive(Copy, Clone, Default, Deserialize, Serialize)]
 pub struct IIRState(pub [i32; 5]);
 
+impl IIRState {
+    #[inline(always)]
+    pub fn get_x(&self, index: usize) -> i32 {
+        // x0 is at index 0 in a biquad between updates
+        self.0[index]
+    }
+
+    #[inline(always)]
+    pub fn get_y(&self, index: usize) -> i32 {
+        // y0 is at index 2 in a biquad between updates
+        self.0[2 + index]
+    }
+}
+
 fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
     // Rounding bias, half up
     let y0 = ((y0 as i64) << shift) + (1 << (shift - 1));

dsp/src/lib.rs

@@ -118,6 +118,7 @@ where
 
 pub mod iir;
 pub mod iir_int;
+pub mod lockin;
 pub mod pll;
 pub mod reciprocal_pll;
 pub mod trig;

dsp/src/lockin.rs

@@ -0,0 +1,55 @@
+use super::{
+    iir_int,
+    trig::{atan2, cossin},
+    Complex,
+};
+use serde::{Deserialize, Serialize};
+
+#[derive(Copy, Clone, Default, Deserialize, Serialize)]
+pub struct Lockin {
+    iir: iir_int::IIR,
+    iir_state: [iir_int::IIRState; 2],
+}
+
+impl Lockin {
+    pub fn new(_corner: u8) -> Self {
+        Lockin {
+            iir: iir_int::IIR::default(), // TODO: lowpass coefficients from corner
+            iir_state: [iir_int::IIRState::default(); 2],
+        }
+    }
+
+    pub fn update(&mut self, signal: i32, phase: i32) -> Complex<i32> {
+        // Get the LO signal for demodulation.
+        let m = cossin(phase.wrapping_neg());
+
+        // Mix with the LO signal, filter with the IIR lowpass,
+        // return IQ (in-phase and quadrature) data.
+        // Note: 32x32 -> 64 bit multiplications are pretty much free.
+        Complex(
+            self.iir.update(
+                &mut self.iir_state[0],
+                ((signal as i64 * m.0 as i64) >> 32) as _,
+            ),
+            self.iir.update(
+                &mut self.iir_state[1],
+                ((signal as i64 * m.1 as i64) >> 32) as _,
+            ),
+        )
+    }
+
+    pub fn iq(&self) -> Complex<i32> {
+        Complex(self.iir_state[0].get_y(0), self.iir_state[1].get_y(0))
+    }
+
+    pub fn power(&self) -> i32 {
+        let iq = self.iq();
+        (((iq.0 as i64) * (iq.0 as i64) + (iq.1 as i64) * (iq.1 as i64)) >> 32)
+            as i32
+    }
+
+    pub fn phase(&self) -> i32 {
+        let iq = self.iq();
+        atan2(iq.1, iq.0)
+    }
+}

src/bin/lockin.rs

@@ -12,25 +12,15 @@ use rtic::cyccnt::{Instant, U32Ext};
 
 use heapless::{consts::*, String};
 
-use stabilizer::{hardware, server};
+use stabilizer::{
-
+    hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2,
-use dsp::{
-    iir, iir_int,
-    reciprocal_pll::TimestampHandler,
-    trig::{atan2, cossin},
-    Complex,
 };
+
+use dsp::{iir, lockin::Lockin, reciprocal_pll::TimestampHandler};
 use hardware::{
     Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
 };
 
-// Frequency scaling factor for lock-in harmonic demodulation.
-const HARMONIC: u32 = 1;
-// Phase offset applied to the lock-in demodulation signal.
-const PHASE_OFFSET: u32 = 0;
-const ADC_SAMPLE_TICKS_LOG2: u16 = 8;
-const SAMPLE_BUFFER_SIZE_LOG2: usize = 3;
-
 const SCALE: f32 = ((1 << 15) - 1) as f32;
 
 const TCP_RX_BUFFER_SIZE: usize = 8192;
@@ -54,9 +44,8 @@ const APP: () = {
         iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
 
         timestamper: InputStamper,
-        timestamp_handler: TimestampHandler,
+        pll: TimestampHandler,
-        iir_lockin: iir_int::IIR,
+        lockin: Lockin,
-        iir_state_lockin: [iir_int::IIRState; 2],
     }
 
     #[init]
@@ -64,14 +53,16 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
-        let timestamp_handler = TimestampHandler::new(
+        let pll = TimestampHandler::new(
-            4,
+            4, // relative PLL frequency bandwidth: 2**-4, TODO: expose
-            3,
+            3, // relative PLL phase bandwidth: 2**-3, TODO: expose
             ADC_SAMPLE_TICKS_LOG2 as usize,
             SAMPLE_BUFFER_SIZE_LOG2,
         );
-        let iir_lockin = iir_int::IIR::default();
+
-        let iir_state_lockin = [iir_int::IIRState::default(); 2];
+        let lockin = Lockin::new(
+            10, // relative Locking lowpass filter bandwidth, TODO: expose
+        );
 
         // Enable ADC/DAC events
         stabilizer.adcs.0.start();
@@ -82,6 +73,9 @@ const APP: () = {
         // Start sampling ADCs.
         stabilizer.adc_dac_timer.start();
 
+        // Start recording digital input timestamps.
+        stabilizer.timestamp_timer.start();
+
         init::LateResources {
             afes: stabilizer.afes,
             adcs: stabilizer.adcs,
@@ -89,9 +83,8 @@ const APP: () = {
             net_interface: stabilizer.net.interface,
             timestamper: stabilizer.timestamper,
 
-            timestamp_handler,
+            pll,
-            iir_lockin,
+            lockin,
-            iir_state_lockin,
         }
     }
 
@@ -111,7 +104,9 @@ const APP: () = {
     ///
     /// Because the ADC and DAC operate at the same rate, these two constraints actually implement
     /// the same time bounds, meeting one also means the other is also met.
-    #[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch, iir_lockin, iir_state_lockin, timestamp_handler, timestamper], priority=2)]
+    ///
+    /// TODO: document lockin
+    #[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch, lockin, timestamper, pll], priority=2)]
     fn process(c: process::Context) {
         let adc_samples = [
             c.resources.adcs.0.acquire_buffer(),
@@ -123,49 +118,47 @@ const APP: () = {
             c.resources.dacs.1.acquire_buffer(),
         ];
 
-        let iir_lockin = c.resources.iir_lockin;
-        let iir_state_lockin = c.resources.iir_state_lockin;
         let iir_ch = c.resources.iir_ch;
         let iir_state = c.resources.iir_state;
+        let lockin = c.resources.lockin;
 
         let (pll_phase, pll_frequency) = c
             .resources
-            .timestamp_handler
+            .pll
             .update(c.resources.timestamper.latest_timestamp());
-        let frequency = pll_frequency.wrapping_mul(HARMONIC);
+
-        let mut phase =
+        // Harmonic index to demodulate
-            PHASE_OFFSET.wrapping_add(pll_phase.wrapping_mul(HARMONIC));
+        let harmonic: i32 = -1;
+        // Demodulation LO phase offset
+        let phase_offset: i32 = 0;
+        let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
+        let mut sample_phase = phase_offset
+            .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
 
         for i in 0..adc_samples[0].len() {
-            let m = cossin((phase as i32).wrapping_neg());
+            // Convert to signed, MSB align the ADC sample.
-            phase = phase.wrapping_add(frequency);
+            let input = (adc_samples[0][i] as i16 as i32) << 16;
+            // Obtain demodulated, filtered IQ sample.
+            lockin.update(input, sample_phase);
+            // Advance the sample phase.
+            sample_phase = sample_phase.wrapping_add(sample_frequency);
 
-            let signal = (adc_samples[0][i] as i16 as i32) << 16;
+            // Convert from IQ to power and phase.
-            let signal = Complex(
+            let mut power = lockin.power() as _;
-                iir_lockin.update(
+            let mut phase = lockin.phase() as _;
-                    &mut iir_state_lockin[0],
-                    ((signal as i64 * m.0 as i64) >> 32) as _,
-                ),
-                iir_lockin.update(
-                    &mut iir_state_lockin[1],
-                    ((signal as i64 * m.1 as i64) >> 16) as _,
-                ),
-            );
-
-            let mut magnitude =
-                (signal.0 * signal.0 + signal.1 * signal.1) as _;
-            let mut phase = atan2(signal.1, signal.0) as _;
 
+            // Filter power and phase through IIR filters.
+            // Note: Normalization to be done in filters. Phase will wrap happily.
             for j in 0..iir_state[0].len() {
-                magnitude =
+                power = iir_ch[0][j].update(&mut iir_state[0][j], power);
-                    iir_ch[0][j].update(&mut iir_state[0][j], magnitude);
                 phase = iir_ch[1][j].update(&mut iir_state[1][j], phase);
             }
 
             // Note(unsafe): range clipping to i16 is ensured by IIR filters above.
+            // Convert to DAC data.
             unsafe {
                 dac_samples[0][i] =
-                    magnitude.to_int_unchecked::<i16>() as u16 ^ 0x8000;
+                    power.to_int_unchecked::<i16>() as u16 ^ 0x8000;
                 dac_samples[1][i] =
                     phase.to_int_unchecked::<i16>() as u16 ^ 0x8000;
             }

src/lib.rs

@@ -9,7 +9,9 @@ pub mod server;
 // The number of ticks in the ADC sampling timer. The timer runs at 100MHz, so the step size is
 // equal to 10ns per tick.
 // Currently, the sample rate is equal to: Fsample = 100/256 MHz = 390.625 KHz
-const ADC_SAMPLE_TICKS: u16 = 256;
+pub const ADC_SAMPLE_TICKS_LOG2: u16 = 8;
+pub const ADC_SAMPLE_TICKS: u16 = 1 << ADC_SAMPLE_TICKS_LOG2;
 
 // The desired ADC sample processing buffer size.
-const SAMPLE_BUFFER_SIZE: usize = 8;
+pub const SAMPLE_BUFFER_SIZE_LOG2: usize = 3;
+pub const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2;