Rj/misc cleanup

dsp/src/accu.rs

@@ -12,7 +12,6 @@ impl Accu {
 
 impl Iterator for Accu {
     type Item = i32;
-    #[inline]
     fn next(&mut self) -> Option<i32> {
         let s = self.state;
         self.state = self.state.wrapping_add(self.step);

dsp/src/complex.rs

@@ -14,7 +14,6 @@ impl Complex<i32> {
     /// Complex::<i32>::from_angle(1 << 30);  // pi/2
     /// Complex::<i32>::from_angle(-1 << 30);  // -pi/2
     /// ```
-    #[inline(always)]
     pub fn from_angle(angle: i32) -> Self {
         let (c, s) = cossin(angle);
         Self(c, s)

dsp/src/iir.rs

@@ -48,6 +48,15 @@ pub struct IIR {
 }
 
 impl IIR {
+    pub const fn new(gain: f32, y_min: f32, y_max: f32) -> Self {
+        Self {
+            ba: Vec5([gain, 0., 0., 0., 0.]),
+            y_offset: 0.,
+            y_min,
+            y_max,
+        }
+    }
+
     /// Configures IIR filter coefficients for proportional-integral behavior
     /// with gain limit.
     ///

dsp/src/lib.rs

@@ -3,38 +3,6 @@
 
 use core::ops::{Add, Mul, Neg};
 
-/// Bit shift, round up half.
-///
-/// # Arguments
-///
-/// `x` - Value to shift and round.
-/// `shift` - Number of bits to right shift `x`.
-///
-/// # Returns
-///
-/// Shifted and rounded value.
-#[inline(always)]
-pub fn shift_round(x: i32, shift: usize) -> i32 {
-    (x + (1 << (shift - 1))) >> shift
-}
-
-/// Integer division, round up half.
-///
-/// # Arguments
-///
-/// `dividend` - Value to divide.
-/// `divisor` - Value that divides the
-/// dividend. `dividend`+`divisor`-1 must be inside [i64::MIN,
-/// i64::MAX].
-///
-/// # Returns
-///
-/// Divided and rounded value.
-#[inline(always)]
-pub fn divide_round(dividend: i64, divisor: i64) -> i64 {
-    (dividend + (divisor - 1)) / divisor
-}
-
 fn abs<T>(x: T) -> T
 where
     T: PartialOrd + Default + Neg<Output = T>,

dsp/src/lockin.rs

@@ -14,10 +14,7 @@ impl Lockin {
     /// Create a new Lockin with given IIR coefficients.
     pub fn new(ba: Vec5) -> Self {
         Self {
-            iir: IIR {
-                ba,
-                ..Default::default()
-            },
+            iir: IIR { ba },
             state: [Vec5::default(); 2],
         }
     }

dsp/src/rpll.rs

@@ -49,7 +49,7 @@ impl RPLL {
         shift_frequency: u8,
         shift_phase: u8,
     ) -> (i32, u32) {
-        debug_assert!(shift_frequency > self.dt2);
+        debug_assert!(shift_frequency >= self.dt2);
         debug_assert!(shift_phase >= self.dt2);
         // Advance phase
         self.y = self.y.wrapping_add(self.f as i32);
@@ -66,7 +66,7 @@ impl RPLL {
             // Reference phase (1 << dt2 full turns) with gain/attenuation applied
             let p_ref = 1u32 << (32 + self.dt2 - shift_frequency);
             // Update frequency lock
-            self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig) as u32);
+            self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig));
             // Time in counter cycles between timestamp and "now"
             let dt = (x.wrapping_neg() & ((1 << self.dt2) - 1)) as u32;
             // Reference phase estimate "now"
@@ -122,6 +122,10 @@ mod test {
         }
 
         fn run(&mut self, n: usize) -> (Vec<f32>, Vec<f32>) {
+            assert!(self.period >= 1 << self.dt2);
+            assert!(self.period < 1 << self.shift_frequency);
+            assert!(self.period < 1 << self.shift_phase + 1);
+
             let mut y = Vec::<f32>::new();
             let mut f = Vec::<f32>::new();
             for _ in 0..n {
@@ -162,10 +166,6 @@ mod test {
         }
 
         fn measure(&mut self, n: usize, limits: [f32; 4]) {
-            assert!(self.period >= 1 << self.dt2);
-            assert!(self.dt2 <= self.shift_frequency);
-            assert!(self.period < 1 << self.shift_frequency);
-            assert!(self.period < 1 << self.shift_frequency + 1);
             let t_settle = (1 << self.shift_frequency - self.dt2 + 4)
                 + (1 << self.shift_phase - self.dt2 + 4);
             self.run(t_settle);
@@ -190,7 +190,7 @@ mod test {
                 print!("{:.2e} ", rel);
                 assert!(
                     rel <= 1.,
-                    "idx {}, have |{}| > want {}",
+                    "idx {}, have |{:.2e}| > limit {:.2e}",
                     i,
                     m[i],
                     limits[i]

src/bin/dual-iir.rs

@@ -17,7 +17,7 @@ use stabilizer::{hardware, server};
 use dsp::iir;
 use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
 
-const SCALE: f32 = ((1 << 15) - 1) as f32;
+const SCALE: f32 = i16::MAX as _;
 
 const TCP_RX_BUFFER_SIZE: usize = 8192;
 const TCP_TX_BUFFER_SIZE: usize = 8192;
@@ -36,7 +36,7 @@ const APP: () = {
         // Format: iir_state[ch][cascade-no][coeff]
         #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
         iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
-        #[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
+        #[init([[iir::IIR::new(1., -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2])]
         iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
     }
 

src/bin/lockin-external.rs

@@ -21,7 +21,7 @@ use hardware::{
     Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
 };
 
-const SCALE: f32 = ((1 << 15) - 1) as f32;
+const SCALE: f32 = i16::MAX as _;
 
 const TCP_RX_BUFFER_SIZE: usize = 8192;
 const TCP_TX_BUFFER_SIZE: usize = 8192;
@@ -40,7 +40,7 @@ const APP: () = {
         // Format: iir_state[ch][cascade-no][coeff]
         #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
         iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
-        #[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
+        #[init([[iir::IIR::new(1./(1 << 16) as f32, -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2])]
         iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
 
         timestamper: InputStamper,
@@ -119,49 +119,59 @@ const APP: () = {
 
         let (pll_phase, pll_frequency) = c.resources.pll.update(
             c.resources.timestamper.latest_timestamp().map(|t| t as i32),
-            23, // relative PLL frequency bandwidth: 2**-23, TODO: expose
-            22, // relative PLL phase bandwidth: 2**-22, TODO: expose
+            22, // frequency settling time (log2 counter cycles), TODO: expose
+            22, // phase settling time, TODO: expose
         );
 
         // Harmonic index of the LO: -1 to _de_modulate the fundamental (complex conjugate)
-        let harmonic: i32 = -1;
-        // Demodulation LO phase offset
-        let phase_offset: i32 = 0;
-        let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2)
-            as i32) // TODO: maybe rounding bias
+        let harmonic: i32 = -1; // TODO: expose
+                                // Demodulation LO phase offset
+        let phase_offset: i32 = 0; // TODO: expose
+
+        let sample_frequency = ((pll_frequency
+            // .wrapping_add(1 << SAMPLE_BUFFER_SIZE_LOG2 - 1)  // half-up rounding bias
+            >> SAMPLE_BUFFER_SIZE_LOG2) as i32)
             .wrapping_mul(harmonic);
         let sample_phase =
             phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
 
-        if let Some(output) = adc_samples[0]
+        let output = adc_samples[0]
             .iter()
             .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)
             })
             .last()
-        {
+            .unwrap();
+
+        // convert i/q to power/phase,
+        let power_phase = true; // TODO: expose
+
+        let mut output = if power_phase {
             // Convert from IQ to power and phase.
-            let mut power = output.abs_sqr() as _;
-            let mut phase = output.arg() 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() {
-                power = iir_ch[0][j].update(&mut iir_state[0][j], power);
-                phase = iir_ch[1][j].update(&mut iir_state[1][j], phase);
+            [output.abs_sqr() as _, output.arg() as _]
+        } else {
+            [output.0 as _, output.1 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() {
+            for k in 0..output.len() {
+                output[k] =
+                    iir_ch[k][j].update(&mut iir_state[k][j], output[k]);
             }
+        }
 
-            // Note(unsafe): range clipping to i16 is ensured by IIR filters above.
-            // Convert to DAC data.
-            for i in 0..dac_samples[0].len() {
-                unsafe {
-                    dac_samples[0][i] =
-                        (power.to_int_unchecked::<i32>() >> 16) as u16 ^ 0x8000;
-                    dac_samples[1][i] =
-                        (phase.to_int_unchecked::<i32>() >> 16) as u16 ^ 0x8000;
-                }
+        // Note(unsafe): range clipping to i16 is ensured by IIR filters above.
+        // Convert to DAC data.
+        for i in 0..dac_samples[0].len() {
+            unsafe {
+                dac_samples[0][i] =
+                    output[0].to_int_unchecked::<i16>() as u16 ^ 0x8000;
+                dac_samples[1][i] =
+                    output[1].to_int_unchecked::<i16>() as u16 ^ 0x8000;
             }
         }
     }

src/bin/lockin-internal.rs

@@ -3,13 +3,16 @@
 #![no_main]
 #![cfg_attr(feature = "nightly", feature(core_intrinsics))]
 
-// A constant sinusoid to send on the DAC output.
-const DAC_SEQUENCE: [f32; 8] =
-    [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
-
 use dsp::{iir_int, lockin::Lockin, Accu};
 use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
-use stabilizer::hardware;
+use stabilizer::{hardware, SAMPLE_BUFFER_SIZE, SAMPLE_BUFFER_SIZE_LOG2};
+
+// A constant sinusoid to send on the DAC output.
+// Full-scale gives a +/- 10V amplitude waveform. Scale it down to give +/- 1V.
+const ONE: i16 = (0.1 * u16::MAX as f32) as _;
+const SQRT2: i16 = (ONE as f32 * 0.707) as _;
+const DAC_SEQUENCE: [i16; SAMPLE_BUFFER_SIZE] =
+    [0, SQRT2, ONE, SQRT2, 0, -SQRT2, -ONE, -SQRT2]; // TODO: rotate by -2 samples to start with ONE
 
 #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
 const APP: () = {
@@ -74,45 +77,48 @@ const APP: () = {
         ];
 
         // DAC0 always generates a fixed sinusoidal output.
-        for (i, value) in DAC_SEQUENCE.iter().enumerate() {
-            // Full-scale gives a +/- 10V amplitude waveform. Scale it down to give +/- 1V.
-            let y = value * (0.1 * i16::MAX as f32);
-            // Note(unsafe): The DAC_SEQUENCE values are guaranteed to be normalized.
-            let y = unsafe { y.to_int_unchecked::<i16>() };
-
-            // Convert to DAC code
-            dac_samples[0][i] = y as u16 ^ 0x8000;
-        }
+        dac_samples[0]
+            .iter_mut()
+            .zip(DAC_SEQUENCE.iter())
+            .for_each(|(d, s)| *d = *s as u16 ^ 0x8000);
 
+        // Reference phase and frequency are known.
         let pll_phase = 0;
-        // 1/8 of the sample rate: log2(DAC_SEQUENCE.len()) == 3
-        let pll_frequency = 1i32 << (32 - 3);
+        let pll_frequency = 1i32 << (32 - SAMPLE_BUFFER_SIZE_LOG2);
 
         // Harmonic index of the LO: -1 to _de_modulate the fundamental
         let harmonic: i32 = -1;
 
         // Demodulation LO phase offset
-        let phase_offset: i32 = (0.7495 * i32::MAX as f32) as i32;
+        let phase_offset: i32 = (0.7495 * i32::MAX as f32) as i32; // TODO: adapt to sequence rotation above
         let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
         let sample_phase = phase_offset
             .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
 
-        if let Some(output) = adc_samples
+        let output = adc_samples
             .iter()
+            // Zip in the LO phase.
             .zip(Accu::new(sample_phase, sample_frequency))
-            // Convert to signed, MSB align the ADC sample.
+            // 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)
             })
+            // 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.
-            let _power = output.abs_sqr();
-            let phase = output.arg() >> 16;
+            [output.abs_sqr(), output.arg()]
+        } else {
+            [output.0, output.1]
+        };
 
-            for value in dac_samples[1].iter_mut() {
-                *value = phase as u16 ^ 0x8000;
-            }
+        for value in dac_samples[1].iter_mut() {
+            *value = (output[1] >> 16) as u16 ^ 0x8000;
         }
     }