diff --git a/dsp/src/accu.rs b/dsp/src/accu.rs index 99369b2..954c35f 100644 --- a/dsp/src/accu.rs +++ b/dsp/src/accu.rs @@ -12,7 +12,6 @@ impl Accu { impl Iterator for Accu { type Item = i32; - #[inline] fn next(&mut self) -> Option { let s = self.state; self.state = self.state.wrapping_add(self.step); diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs index 9829312..256cde2 100644 --- a/dsp/src/complex.rs +++ b/dsp/src/complex.rs @@ -14,7 +14,6 @@ impl Complex { /// Complex::::from_angle(1 << 30); // pi/2 /// Complex::::from_angle(-1 << 30); // -pi/2 /// ``` - #[inline(always)] pub fn from_angle(angle: i32) -> Self { let (c, s) = cossin(angle); Self(c, s) diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index 8d0dbe4..4206c2b 100644 --- a/dsp/src/iir.rs +++ b/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. /// diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 74ebdbb..f42e7c7 100644 --- a/dsp/src/lib.rs +++ b/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(x: T) -> T where T: PartialOrd + Default + Neg, diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index fcbb4c9..ea80836 100644 --- a/dsp/src/lockin.rs +++ b/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], } } diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index 79a2b30..d4bd86f 100644 --- a/dsp/src/rpll.rs +++ b/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, Vec) { + 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::::new(); let mut f = Vec::::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] diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs index ef79998..e28465b 100644 --- a/src/bin/dual-iir.rs +++ b/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], } diff --git a/src/bin/lockin-external.rs b/src/bin/lockin-external.rs index 3668b60..f3717bb 100644 --- a/src/bin/lockin-external.rs +++ b/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,21 +119,23 @@ 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. @@ -141,27 +143,35 @@ const APP: () = { 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 _; + [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() { - power = iir_ch[0][j].update(&mut iir_state[0][j], power); - phase = iir_ch[1][j].update(&mut iir_state[1][j], phase); + // 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::() >> 16) as u16 ^ 0x8000; - dac_samples[1][i] = - (phase.to_int_unchecked::() >> 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::() as u16 ^ 0x8000; + dac_samples[1][i] = + output[1].to_int_unchecked::() as u16 ^ 0x8000; } } } diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs index e4a2521..7a83a87 100644 --- a/src/bin/lockin-internal.rs +++ b/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::() }; - - // 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() - { - // Convert from IQ to power and phase. - let _power = output.abs_sqr(); - let phase = output.arg() >> 16; + .unwrap(); - for value in dac_samples[1].iter_mut() { - *value = phase as u16 ^ 0x8000; - } + // 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; } }