diff --git a/dsp/src/iir_int.rs b/dsp/src/iir_int.rs index ea78dd6..64ab175 100644 --- a/dsp/src/iir_int.rs +++ b/dsp/src/iir_int.rs @@ -1,8 +1,40 @@ +use core::f32::consts::PI; use serde::{Deserialize, Serialize}; +/// Generic vector for integer IIR filter. +/// This struct is used to hold the x/y input/output data vector or the b/a coefficient +/// vector. #[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct IIRState(pub [i32; 5]); +impl IIRState { + /// Lowpass biquad filter using cutoff and sampling frequencies. Taken from: + /// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html + /// + /// # Args + /// * `f` - Corner frequency, or 3dB cutoff frequency (in units of sample rate). + /// This is only accurate for low corner frequencies less than ~0.01. + /// * `q` - Quality factor (1/sqrt(2) for critical). + /// * `k` - DC gain. + /// + /// # Returns + /// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 is set to -1. + pub fn lowpass(f: f32, q: f32, k: f32) -> IIRState { + // 3rd order Taylor approximation of sin and cos. + let f = f * 2. * PI; + let fsin = f - f * f * f / 6.; + let fcos = 1. - f * f / 2.; + let alpha = fsin / (2. * q); + // IIR uses Q2.30 fixed point + let a0 = (1. + alpha) / (1 << IIR::SHIFT) as f32; + let b0 = (k / 2. * (1. - fcos) / a0) as _; + let a1 = (2. * fcos / a0) as _; + let a2 = ((alpha - 1.) / a0) as _; + + IIRState([b0, 2 * b0, b0, a1, a2]) + } +} + fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 { // Rounding bias, half up let y0 = ((y0 as i64) << shift) + (1 << (shift - 1)); @@ -57,3 +89,14 @@ impl IIR { y0 } } + +#[cfg(test)] +mod test { + use super::IIRState; + + #[test] + fn lowpass_gen() { + let ba = IIRState::lowpass(1e-3, 1. / 2f32.sqrt(), 2.); + println!("{:?}", ba.0); + } +} diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 56a3004..191054a 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -119,7 +119,7 @@ pub mod iir; pub mod iir_int; pub mod lockin; pub mod pll; -pub mod reciprocal_pll; +pub mod rpll; pub mod unwrap; pub use atan2::atan2; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 72f505c..aa397bb 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -41,9 +41,9 @@ impl Lockin { mod test { use crate::{ atan2, - iir_int::{IIRState, IIR}, + iir_int::IIRState, lockin::Lockin, - reciprocal_pll::TimestampHandler, + rpll::RPLL, testing::{isclose, max_error}, Complex, }; @@ -157,911 +157,4 @@ mod test { .sum::() .max(1. / ADC_SCALE / 2.) // 1/2 LSB from quantization } - - /// Reference clock timestamp values in one ADC batch period starting at `timestamp_start`. The - /// number of timestamps in a batch can be 0 or 1, so this returns an Option containing a timestamp - /// only if one occurred during the batch. - /// - /// # Args - /// * `reference_period` - External reference signal period in units of the internal clock period. - /// * `timestamp_start` - Start time in terms of the internal clock count. This is the start time of - /// the current processing sequence. - /// * `timestamp_stop` - Stop time in terms of the internal clock count. - /// - /// # Returns - /// An Option, containing a timestamp if one occurred during the current batch period. - fn adc_batch_timestamps( - reference_period: f64, - timestamp_start: u64, - timestamp_stop: u64, - ) -> Option { - let start_count = timestamp_start as f64 % reference_period; - - let timestamp = (reference_period - start_count) % reference_period; - - if timestamp < (timestamp_stop - timestamp_start) as f64 { - return Some( - ((timestamp_start + timestamp.round() as u64) % (1u64 << 32)) - as u32, - ); - } - - None - } - - /// Lowpass biquad filter using cutoff and sampling frequencies. Taken from: - /// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html - /// - /// # Args - /// * `fc` - Corner frequency, or 3dB cutoff frequency (in units of sample rate). - /// * `q` - Quality factor (1/sqrt(2) for critical). - /// * `k` - DC gain. - /// - /// # Returns - /// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 is set to -1. - fn lowpass_iir_coefficients(fc: f64, q: f64, k: f64) -> IIRState { - let f = 2. * PI * fc; - let a = f.sin() / (2. * q); - // IIR uses Q2.30 fixed point - let a0 = (1. + a) / (1 << IIR::SHIFT) as f64; - let b0 = (k / 2. * (1. - f.cos()) / a0).round() as _; - let a1 = (2. * f.cos() / a0).round() as _; - let a2 = ((a - 1.) / a0).round() as _; - - IIRState([b0, 2 * b0, b0, a1, a2]) - } - - /// Compute the maximum effect of input noise on the lock-in magnitude computation. - /// - /// The maximum effect of noise on the magnitude computation is given by: - /// - /// | sqrt((I+n*sin(x))**2 + (Q+n*cos(x))**2) - sqrt(I**2 + Q**2) | - /// - /// * I is the in-phase component of the portion of the input signal with the same frequency as the - /// demodulation signal. - /// * Q is the quadrature component. - /// * n is the total noise amplitude (from all contributions, after attenuation from filtering). - /// * x is the phase of the demodulation signal. - /// - /// We need to find the demodulation phase (x) that maximizes this expression. We can ignore the - /// absolute value operation by also considering the expression minimum. The locations of the - /// minimum and maximum can be computed analytically by finding the value of x when the derivative - /// of this expression with respect to x is 0. When we solve this equation, we find: - /// - /// x = atan(I/Q) - /// - /// It's worth noting that this solution is technically only valid when cos(x)!=0 (i.e., - /// x!=pi/2,-pi/2). However, this is not a problem because we only get these values when Q=0. Rust - /// correctly computes atan(inf)=pi/2, which is precisely what we want because x=pi/2 maximizes - /// sin(x) and therefore also the noise effect. - /// - /// The other maximum or minimum is pi radians away from this value. - /// - /// # Args - /// * `total_noise_amplitude` - Combined amplitude of all noise sources sampled by the ADC. - /// * `in_phase_actual` - Value of the in-phase component if no noise were present at the ADC input. - /// * `quadrature_actual` - Value of the quadrature component if no noise were present at the ADC - /// input. - /// * `desired_input_amplitude` - Amplitude of the desired input signal. That is, the input signal - /// component with the same frequency as the demodulation signal. - /// - /// # Returns - /// Approximation of the maximum effect on the magnitude computation due to noise sources at the ADC - /// input. - fn magnitude_noise( - total_noise_amplitude: f64, - in_phase_actual: f64, - quadrature_actual: f64, - desired_input_amplitude: f64, - ) -> f64 { - // See function documentation for explanation. - let noise = |in_phase_delta: f64, quadrature_delta: f64| -> f64 { - (((in_phase_actual + in_phase_delta).powf(2.) - + (quadrature_actual + quadrature_delta).powf(2.)) - .sqrt() - - desired_input_amplitude) - .abs() - }; - - let phase = (in_phase_actual / quadrature_actual).atan(); - let max_noise_1 = noise( - total_noise_amplitude * phase.sin(), - total_noise_amplitude * phase.cos(), - ); - let max_noise_2 = noise( - total_noise_amplitude * (phase + PI).sin(), - total_noise_amplitude * (phase + PI).cos(), - ); - - max_noise_1.max(max_noise_2) - } - - /// Compute the maximum phase deviation from the correct value due to the input noise sources. - /// - /// The maximum effect of noise on the phase computation is given by: - /// - /// | atan2(Q+n*cos(x), I+n*sin(x)) - atan2(Q, I) | - /// - /// See `magnitude_noise` for an explanation of the terms in this mathematical expression. - /// - /// This expression is harder to compute analytically than the expression in `magnitude_noise`. We - /// could compute it numerically, but that's expensive. However, we can use heuristics to try to - /// guess the values of x that will maximize the noise effect. Intuitively, the difference will be - /// largest when the Y-argument of the atan2 function (Q+n*cos(x)) is pushed in the opposite - /// direction of the noise effect on the X-argument (i.e., cos(x) and sin(x) have different - /// signs). We can use: - /// - /// * sin(x)=+-1 (+- denotes plus or minus), cos(x)=0, - /// * sin(x)=0, cos(x)=+-1, and - /// * the value of x that maximizes |sin(x)-cos(x)| (when sin(x)=1/sqrt(2) and cos(x)=-1/sqrt(2), or - /// when the signs are flipped) - /// - /// The first choice addresses cases in which |I|>>|Q|, the second choice addresses cases in which - /// |Q|>>|I|, and the third choice addresses cases in which |I|~|Q|. We can test all of these cases - /// as an approximation for the real maximum. - /// - /// # Args - /// * `total_noise_amplitude` - Total amplitude of all input noise sources. - /// * `in_phase_actual` - Value of the in-phase component if no noise were present at the input. - /// * `quadrature_actual` - Value of the quadrature component if no noise were present at the input. - /// - /// # Returns - /// Approximation of the maximum effect on the phase computation due to noise sources at the ADC - /// input. - fn phase_noise( - total_noise_amplitude: f64, - in_phase_actual: f64, - quadrature_actual: f64, - ) -> f64 { - // See function documentation for explanation. - let noise = |in_phase_delta: f64, quadrature_delta: f64| -> f64 { - ((quadrature_actual + quadrature_delta) - .atan2(in_phase_actual + in_phase_delta) - - quadrature_actual.atan2(in_phase_actual)) - .abs() - }; - - let mut max_noise: f64 = 0.; - for (in_phase_delta, quadrature_delta) in [ - ( - total_noise_amplitude / 2_f64.sqrt(), - total_noise_amplitude / -2_f64.sqrt(), - ), - ( - total_noise_amplitude / -2_f64.sqrt(), - total_noise_amplitude / 2_f64.sqrt(), - ), - (total_noise_amplitude, 0.), - (-total_noise_amplitude, 0.), - (0., total_noise_amplitude), - (0., -total_noise_amplitude), - ] - .iter() - { - max_noise = - max_noise.max(noise(*in_phase_delta, *quadrature_delta)); - } - - max_noise - } - - /// Lowpass filter test for in-phase/quadrature and magnitude/phase computations. - /// - /// This attempts to "intelligently" model acceptable tolerance ranges for the measured in-phase, - /// quadrature, magnitude and phase results of lock-in processing for a typical low-pass filter - /// application. So, instead of testing whether the lock-in processing extracts the true magnitude - /// and phase (or in-phase and quadrature components) of the input signal, it attempts to calculate - /// what the lock-in processing should compute given any set of input noise sources. For example, if - /// a noise source of sufficient strength differs in frequency by 1kHz from the reference frequency - /// and the filter cutoff frequency is also 1kHz, testing if the lock-in amplifier extracts the - /// amplitude and phase of the input signal whose frequency is equal to the demodulation frequency - /// is doomed to failure. Instead, this function tests whether the lock-in correctly adheres to its - /// actual transfer function, whether or not it was given reasonable inputs. The logic for computing - /// acceptable tolerance ranges is performed in `sampled_noise_amplitude`, `magnitude_noise`, and - /// `phase_noise`. - /// - /// # Args - /// * `internal_frequency` - Internal clock frequency (Hz). The internal clock increments timestamp - /// counter values used to record the edges of the external reference. - /// * `reference_frequency` - External reference frequency (in Hz). - /// * `demodulation_phase_offset` - Phase offset applied to the in-phase and quadrature demodulation - /// signals. - /// * `harmonic` - Scaling factor for the demodulation frequency. E.g., 2 would demodulate with the - /// first harmonic of the reference frequency. - /// * `sample_buffer_size_log2` - The base-2 logarithm of the number of samples in a processing - /// batch. - /// * `pll_shift_frequency` - See `pll::update()`. - /// * `pll_shift_phase` - See `pll::update()`. - /// * `corner_frequency` - Lowpass filter 3dB cutoff frequency. - /// * `desired_input` - `Tone` giving the frequency, amplitude and phase of the desired result. - /// * `noise_inputs` - Vector of `Tone` for any noise inputs on top of `desired_input`. - /// * `time_constant_factor` - Number of time constants after which the output is considered valid. - /// * `tolerance` - Acceptable relative tolerance for the magnitude and angle outputs. This is added - /// to fixed tolerance values computed inside this function. The outputs must remain within this - /// tolerance between `time_constant_factor` and `time_constant_factor+1` time constants. - fn lowpass_test( - internal_frequency: f64, - reference_frequency: f64, - demodulation_phase_offset: f64, - harmonic: i32, - sample_buffer_size_log2: usize, - pll_shift_frequency: u8, - pll_shift_phase: u8, - corner_frequency: f64, - desired_input: Tone, - tones: &mut Vec, - time_constant_factor: f64, - tolerance: f64, - ) { - assert!( - isclose((internal_frequency).log2(), (internal_frequency).log2().round(), 0., 1e-5), - "The number of internal clock cycles in one ADC sampling period must be a power-of-two." - ); - - assert!( - internal_frequency / reference_frequency - >= internal_frequency - * (1 << sample_buffer_size_log2) as f64, - "Too many timestamps per batch. Each batch can have at most 1 timestamp." - ); - - let adc_sample_ticks_log2 = - (internal_frequency).log2().round() as usize; - assert!( - adc_sample_ticks_log2 + sample_buffer_size_log2 <= 32, - "The base-2 log of the number of ADC ticks in a sampling period plus the base-2 log of the sample buffer size must be less than 32." - ); - - let mut lockin = PllLockin::new( - harmonic, - (demodulation_phase_offset / (2. * PI) * (1i64 << 32) as f64) - .round() as i32, - &lowpass_iir_coefficients( - corner_frequency, - 1. / 2f64.sqrt(), // critical q - 2., - ), // DC gain to get to full scale with the image filtered out - ); - let mut timestamp_handler = TimestampHandler::new( - pll_shift_frequency, - pll_shift_phase, - adc_sample_ticks_log2, - sample_buffer_size_log2, - ); - - let mut timestamp_start: u64 = 0; - let time_constant: f64 = 1. / (2. * PI * corner_frequency); - // Account for the pll settling time (see its documentation). - let pll_time_constant_samples = - (1 << pll_shift_phase.max(pll_shift_frequency)) as usize; - let low_pass_time_constant_samples = - (time_constant_factor * time_constant - / (1 << sample_buffer_size_log2) as f64) as usize; - let samples = - pll_time_constant_samples + low_pass_time_constant_samples; - // Ensure the result remains within tolerance for 1 time constant after `time_constant_factor` - // time constants. - let extra_samples = time_constant as usize; - let batch_sample_count = - 1_u64 << (adc_sample_ticks_log2 + sample_buffer_size_log2); - - let effective_phase_offset = - desired_input.phase - demodulation_phase_offset; - let in_phase_actual = - linear(desired_input.amplitude_dbfs) * effective_phase_offset.cos(); - let quadrature_actual = - linear(desired_input.amplitude_dbfs) * effective_phase_offset.sin(); - - let total_noise_amplitude = sampled_noise_amplitude( - tones, - reference_frequency * harmonic as f64, - corner_frequency, - ); - // Add some fixed error to account for errors introduced by the PLL, our custom trig functions - // and integer division. It's a bit difficult to be precise about this. I've added a 1% - // (relative to full scale) error. - let total_magnitude_noise = magnitude_noise( - total_noise_amplitude, - in_phase_actual, - quadrature_actual, - linear(desired_input.amplitude_dbfs), - ) + 1e-2; - let total_phase_noise = phase_noise( - total_noise_amplitude, - in_phase_actual, - quadrature_actual, - ) + 1e-2 * 2. * PI; - - tones.push(desired_input); - - for n in 0..(samples + extra_samples) { - let adc_signal = sample_tones( - &tones, - timestamp_start as f64 / internal_frequency, - 1 << sample_buffer_size_log2, - ); - let timestamp = adc_batch_timestamps( - internal_frequency / reference_frequency, - timestamp_start, - timestamp_start + batch_sample_count - 1, - ); - timestamp_start += batch_sample_count; - - let (demodulation_initial_phase, demodulation_frequency) = - timestamp_handler.update(timestamp); - let output = lockin.update( - adc_signal, - demodulation_initial_phase as i32, - demodulation_frequency as i32, - ); - let magnitude = (((output.0 as i64) * (output.0 as i64) - + (output.1 as i64) * (output.1 as i64)) - >> 32) as i32; - let phase = atan2(output.1, output.0); - - // Ensure stable within tolerance for 1 time constant after `time_constant_factor`. - if n >= samples { - // We want our full-scale magnitude to be 1. Our fixed-point numbers treated as integers - // set the full-scale magnitude to 1<<60. So, we must divide by this number. However, - // we've already divided by 1<<32 in the magnitude computation to keep our values within - // the i32 limits, so we just need to divide by an additional 1<<28. - let amplitude_normalized = - (magnitude as f64 / (1_u64 << 28) as f64).sqrt(); - assert!( - isclose(linear(desired_input.amplitude_dbfs), amplitude_normalized, tolerance, total_magnitude_noise), - "magnitude actual: {:.4} ({:.2} dBFS), magnitude computed: {:.4} ({:.2} dBFS), tolerance: {:.4}", - linear(desired_input.amplitude_dbfs), - desired_input.amplitude_dbfs, - amplitude_normalized, - 20.*amplitude_normalized.log10(), - max_error(linear(desired_input.amplitude_dbfs), amplitude_normalized, tolerance, total_magnitude_noise), - ); - let phase_normalized = - phase as f64 / (1_u64 << 32) as f64 * (2. * PI); - assert!( - isclose( - effective_phase_offset, - phase_normalized, - tolerance, - total_phase_noise - ), - "phase actual: {:.4}, phase computed: {:.4}, tolerance: {:.4}", - effective_phase_offset, - phase_normalized, - max_error( - effective_phase_offset, - phase_normalized, - tolerance, - total_phase_noise - ), - ); - - let in_phase_normalized = output.0 as f64 / (1 << 30) as f64; - let quadrature_normalized = output.1 as f64 / (1 << 30) as f64; - - assert!( - isclose( - in_phase_actual, - in_phase_normalized, - total_noise_amplitude, - tolerance - ), - "in-phase actual: {:.4}, in-phase computed: {:.3}, tolerance: {:.4}", - in_phase_actual, - in_phase_normalized, - max_error( - in_phase_actual, - in_phase_normalized, - total_noise_amplitude, - tolerance - ), - ); - assert!( - isclose( - quadrature_actual, - quadrature_normalized, - total_noise_amplitude, - tolerance - ), - "quadrature actual: {:.4}, quadrature computed: {:.4}, tolerance: {:.4}", - quadrature_actual, - quadrature_normalized, - max_error( - quadrature_actual, - quadrature_normalized, - total_noise_amplitude, - tolerance - ), - ); - } - } - } - - #[test] - fn lowpass() { - let internal_frequency: f64 = 64.; - let signal_frequency: f64 = 64e-3; - let harmonic: i32 = 1; - let sample_buffer_size_log2: usize = 2; - let pll_shift_frequency: u8 = 3; - let pll_shift_phase: u8 = 2; - let corner_frequency: f64 = 1e-3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 6.; - let tolerance: f64 = 1e-2; - - lowpass_test( - internal_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - sample_buffer_size_log2, - pll_shift_frequency, - pll_shift_phase, - corner_frequency, - Tone { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase: 0., - }, - &mut vec![ - Tone { - frequency: 1.1 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - Tone { - frequency: 0.9 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - ], - time_constant_factor, - tolerance, - ); - } - - #[test] - fn lowpass_demodulation_phase_offset_pi_2() { - let internal_frequency: f64 = 64.; - let signal_frequency: f64 = 64e-3; - let harmonic: i32 = 1; - let sample_buffer_size_log2: usize = 2; - let pll_shift_frequency: u8 = 3; - let pll_shift_phase: u8 = 2; - let corner_frequency: f64 = 1e-3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = PI / 2.; - let time_constant_factor: f64 = 6.; - let tolerance: f64 = 1e-2; - - lowpass_test( - internal_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - sample_buffer_size_log2, - pll_shift_frequency, - pll_shift_phase, - corner_frequency, - Tone { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase: 0., - }, - &mut vec![ - Tone { - frequency: 1.1 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - Tone { - frequency: 0.9 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - ], - time_constant_factor, - tolerance, - ); - } - - #[test] - fn lowpass_phase_offset_pi_2() { - let internal_frequency: f64 = 64.; - let signal_frequency: f64 = 64e-3; - let harmonic: i32 = 1; - let sample_buffer_size_log2: usize = 2; - let pll_shift_frequency: u8 = 3; - let pll_shift_phase: u8 = 2; - let corner_frequency: f64 = 1e-3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 6.; - let tolerance: f64 = 1e-2; - - lowpass_test( - internal_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - sample_buffer_size_log2, - pll_shift_frequency, - pll_shift_phase, - corner_frequency, - Tone { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase: PI / 2., - }, - &mut vec![ - Tone { - frequency: 1.1 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - Tone { - frequency: 0.9 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - ], - time_constant_factor, - tolerance, - ); - } - - #[test] - fn lowpass_fundamental_71e_3_phase_offset_pi_4() { - let internal_frequency: f64 = 64.; - let signal_frequency: f64 = 71e-3; - let harmonic: i32 = 1; - let sample_buffer_size_log2: usize = 2; - let pll_shift_frequency: u8 = 3; - let pll_shift_phase: u8 = 2; - let corner_frequency: f64 = 0.6e-3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f64 = 1e-2; - - lowpass_test( - internal_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - sample_buffer_size_log2, - pll_shift_frequency, - pll_shift_phase, - corner_frequency, - Tone { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase: PI / 4., - }, - &mut vec![ - Tone { - frequency: 1.1 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - Tone { - frequency: 0.9 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - ], - time_constant_factor, - tolerance, - ); - } - - #[test] - fn lowpass_first_harmonic() { - let internal_frequency: f64 = 64.; - let signal_frequency: f64 = 50e-3; - let harmonic: i32 = 2; - let sample_buffer_size_log2: usize = 2; - let pll_shift_frequency: u8 = 2; - let pll_shift_phase: u8 = 1; - let corner_frequency: f64 = 1e-3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f64 = 1e-2; - - lowpass_test( - internal_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - sample_buffer_size_log2, - pll_shift_frequency, - pll_shift_phase, - corner_frequency, - Tone { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase: 0., - }, - &mut vec![ - Tone { - frequency: 1.2 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - Tone { - frequency: 0.8 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - ], - time_constant_factor, - tolerance, - ); - } - - #[test] - fn lowpass_second_harmonic() { - let internal_frequency: f64 = 64.; - let signal_frequency: f64 = 50e-3; - let harmonic: i32 = 3; - let sample_buffer_size_log2: usize = 2; - let pll_shift_frequency: u8 = 2; - let pll_shift_phase: u8 = 1; - let corner_frequency: f64 = 1e-3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f64 = 1e-2; - - lowpass_test( - internal_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - sample_buffer_size_log2, - pll_shift_frequency, - pll_shift_phase, - corner_frequency, - Tone { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase: 0., - }, - &mut vec![ - Tone { - frequency: 1.2 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - Tone { - frequency: 0.8 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - ], - time_constant_factor, - tolerance, - ); - } - - #[test] - fn lowpass_third_harmonic() { - let internal_frequency: f64 = 64.; - let signal_frequency: f64 = 50e-3; - let harmonic: i32 = 4; - let sample_buffer_size_log2: usize = 2; - let pll_shift_frequency: u8 = 2; - let pll_shift_phase: u8 = 1; - let corner_frequency: f64 = 1e-3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f64 = 1e-2; - - lowpass_test( - internal_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - sample_buffer_size_log2, - pll_shift_frequency, - pll_shift_phase, - corner_frequency, - Tone { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase: 0., - }, - &mut vec![ - Tone { - frequency: 1.2 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - Tone { - frequency: 0.8 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - ], - time_constant_factor, - tolerance, - ); - } - - #[test] - fn lowpass_first_harmonic_phase_shift() { - let internal_frequency: f64 = 64.; - let signal_frequency: f64 = 50e-3; - let harmonic: i32 = 2; - let sample_buffer_size_log2: usize = 2; - let pll_shift_frequency: u8 = 2; - let pll_shift_phase: u8 = 1; - let corner_frequency: f64 = 1e-3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f64 = 1e-2; - - lowpass_test( - internal_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - sample_buffer_size_log2, - pll_shift_frequency, - pll_shift_phase, - corner_frequency, - Tone { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase: PI / 4., - }, - &mut vec![ - Tone { - frequency: 1.2 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - Tone { - frequency: 0.8 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - ], - time_constant_factor, - tolerance, - ); - } - - #[test] - fn lowpass_adc_frequency_1e6() { - let internal_frequency: f64 = 32.; - let signal_frequency: f64 = 100e-3; - let harmonic: i32 = 1; - let sample_buffer_size_log2: usize = 2; - let pll_shift_frequency: u8 = 2; - let pll_shift_phase: u8 = 1; - let corner_frequency: f64 = 1e-3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f64 = 1e-2; - - lowpass_test( - internal_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - sample_buffer_size_log2, - pll_shift_frequency, - pll_shift_phase, - corner_frequency, - Tone { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase: 0., - }, - &mut vec![ - Tone { - frequency: 1.2 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - Tone { - frequency: 0.8 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - ], - time_constant_factor, - tolerance, - ); - } - - #[test] - fn lowpass_internal_frequency_125e6() { - let internal_frequency: f64 = 64.; - let signal_frequency: f64 = 100e-3; - let harmonic: i32 = 1; - let sample_buffer_size_log2: usize = 2; - let pll_shift_frequency: u8 = 2; - let pll_shift_phase: u8 = 1; - let corner_frequency: f64 = 1e-3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f64 = 1e-2; - - lowpass_test( - internal_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - sample_buffer_size_log2, - pll_shift_frequency, - pll_shift_phase, - corner_frequency, - Tone { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase: 0., - }, - &mut vec![ - Tone { - frequency: 1.2 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - Tone { - frequency: 0.8 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }, - ], - time_constant_factor, - tolerance, - ); - } - - #[test] - fn lowpass_low_signal_frequency() { - let internal_frequency: f64 = 64.; - let signal_frequency: f64 = 10e-3; - let harmonic: i32 = 1; - let sample_buffer_size_log2: usize = 2; - let pll_shift_frequency: u8 = 2; - let pll_shift_phase: u8 = 1; - let corner_frequency: f64 = 1e-3; - let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; - let demodulation_phase_offset: f64 = 0.; - let time_constant_factor: f64 = 5.; - let tolerance: f64 = 1e-1; - - lowpass_test( - internal_frequency, - signal_frequency, - demodulation_phase_offset, - harmonic, - sample_buffer_size_log2, - pll_shift_frequency, - pll_shift_phase, - corner_frequency, - Tone { - frequency: demodulation_frequency, - amplitude_dbfs: -30., - phase: 0., - }, - &mut vec![Tone { - frequency: 1.1 * demodulation_frequency, - amplitude_dbfs: -20., - phase: 0., - }], - time_constant_factor, - tolerance, - ); - } } diff --git a/dsp/src/reciprocal_pll.rs b/dsp/src/reciprocal_pll.rs deleted file mode 100644 index d06abbb..0000000 --- a/dsp/src/reciprocal_pll.rs +++ /dev/null @@ -1,100 +0,0 @@ -use super::{divide_round, pll::PLL}; - -/// Processes external timestamps to produce the frequency and initial phase of the demodulation -/// signal. -pub struct TimestampHandler { - pll: PLL, - pll_shift_frequency: u8, - pll_shift_phase: u8, - // Index of the current ADC batch. - batch_index: u32, - // Most recent phase and frequency values of the external reference. - reference_phase: i64, - reference_frequency: i64, - adc_sample_ticks_log2: usize, - sample_buffer_size_log2: usize, -} - -impl TimestampHandler { - /// Construct a new `TimestampHandler` instance. - /// - /// # Args - /// * `pll_shift_frequency` - See `PLL::update()`. - /// * `pll_shift_phase` - See `PLL::update()`. - /// * `adc_sample_ticks_log2` - Number of ticks in one ADC sampling timer period. - /// * `sample_buffer_size_log2` - Number of ADC samples in one processing batch. - /// - /// # Returns - /// New `TimestampHandler` instance. - pub fn new( - pll_shift_frequency: u8, - pll_shift_phase: u8, - adc_sample_ticks_log2: usize, - sample_buffer_size_log2: usize, - ) -> Self { - TimestampHandler { - pll: PLL::default(), - pll_shift_frequency, - pll_shift_phase, - batch_index: 0, - reference_phase: 0, - reference_frequency: 0, - adc_sample_ticks_log2, - sample_buffer_size_log2, - } - } - - /// Compute the initial phase and frequency of the demodulation signal. - /// - /// # Args - /// * `timestamp` - Counter value corresponding to an external reference edge. - /// - /// # Returns - /// Tuple consisting of the initial phase value and frequency of the demodulation signal. - pub fn update(&mut self, timestamp: Option) -> (u32, u32) { - if let Some(t) = timestamp { - let (phase, frequency) = self.pll.update( - t as i32, - self.pll_shift_frequency, - self.pll_shift_phase, - ); - self.reference_phase = phase as u32 as i64; - self.reference_frequency = frequency as u32 as i64; - } - - let demodulation_frequency: u32; - let demodulation_initial_phase: u32; - - if self.reference_frequency == 0 { - demodulation_frequency = u32::MAX; - demodulation_initial_phase = u32::MAX; - } else { - demodulation_frequency = divide_round( - 1 << (32 + self.adc_sample_ticks_log2), - self.reference_frequency, - ) as u32; - demodulation_initial_phase = divide_round( - (((self.batch_index as i64) - << (self.adc_sample_ticks_log2 - + self.sample_buffer_size_log2)) - - self.reference_phase) - << 32, - self.reference_frequency, - ) as u32; - } - - if self.batch_index - < (1 << (32 - - self.adc_sample_ticks_log2 - - self.sample_buffer_size_log2)) - - 1 - { - self.batch_index += 1; - } else { - self.batch_index = 0; - self.reference_phase -= 1 << 32; - } - - (demodulation_initial_phase, demodulation_frequency) - } -} diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs new file mode 100644 index 0000000..2bfd29c --- /dev/null +++ b/dsp/src/rpll.rs @@ -0,0 +1,84 @@ +/// Reciprocal PLL. +/// +/// Consumes noisy, quantized timestamps of a reference signal and reconstructs +/// the phase and frequency of the update() invocations with respect to (and in units of +/// 1 << 32 of) that reference. +#[derive(Copy, Clone, Default)] +pub struct RPLL { + dt2: u8, // 1 << dt2 is the counter rate to update() rate ratio + t: i32, // current counter time + x: i32, // previous timestamp + ff: i32, // current frequency estimate from frequency loop + f: i32, // current frequency estimate from both frequency and phase loop + y: i32, // current phase estimate +} + +impl RPLL { + /// Create a new RPLL instance. + /// + /// Args: + /// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio. + /// * t: Counter time. Counter value at the first update() call. Typically 0. + /// + /// Returns: + /// Initialized RPLL instance. + pub fn new(dt2: u8, t: i32) -> RPLL { + RPLL { + dt2, + t, + ..Default::default() + } + } + + /// Advance the RPLL and optionally supply a new timestamp. + /// + /// Args: + /// * input: Optional new timestamp (wrapping around at the i32 boundary). + /// There can be at most one timestamp per `update()` cycle (1 << dt2 counter cycles). + /// * shift_frequency: Frequency lock settling time. 1 << shift_frequency is + /// frequency lock settling time in counter periods. The settling time must be larger + /// than the signal period to lock to. + /// * shift_phase: Phase lock settling time. Usually one less than + /// `shift_frequency` (see there). + /// + /// Returns: + /// A tuple containing the current phase (wrapping at the i32 boundary, pi) and + /// frequency (wrapping at the i32 boundary, Nyquist) estimate. + pub fn update( + &mut self, + input: Option, + shift_frequency: u8, + shift_phase: u8, + ) -> (i32, i32) { + debug_assert!(shift_frequency > self.dt2); + debug_assert!(shift_phase > self.dt2); + // Advance phase + self.y = self.y.wrapping_add(self.f); + if let Some(x) = input { + // Reference period in counter cycles + let dx = x.wrapping_sub(self.x); + // Store timestamp for next time. + self.x = x; + // Phase using the current frequency estimate + let p_sig_long = (self.ff as i64).wrapping_mul(dx as i64); + // Add half-up rounding bias and apply gain/attenuation + let p_sig = (p_sig_long.wrapping_add(1i64 << (shift_frequency - 1)) + >> shift_frequency) as i32; + // Reference phase (1 << dt2 full turns) with gain/attenuation applied + let p_ref = 1i32 << (32 + self.dt2 - shift_frequency); + // Update frequency lock + self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig)); + // Time in counter cycles between timestamp and "now" + let dt = self.t.wrapping_sub(x); + // Reference phase estimate "now" + let y_ref = (self.f >> self.dt2).wrapping_mul(dt); + // Phase error + let dy = y_ref.wrapping_sub(self.y); + // Current frequency estimate from frequency lock and phase error + self.f = self.ff.wrapping_add(dy >> (shift_phase - self.dt2)); + } + // Advance time + self.t = self.t.wrapping_add(1 << self.dt2); + (self.y, self.f) + } +} diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 304c35e..7066ad8 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -16,7 +16,7 @@ use stabilizer::{ hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2, }; -use dsp::{iir, iir_int, lockin::Lockin, reciprocal_pll::TimestampHandler}; +use dsp::{iir, iir_int, lockin::Lockin, rpll::RPLL}; use hardware::{ Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1, }; @@ -44,7 +44,7 @@ const APP: () = { iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], timestamper: InputStamper, - pll: TimestampHandler, + pll: RPLL, lockin: Lockin, } @@ -53,15 +53,10 @@ const APP: () = { // Configure the microcontroller let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); - let pll = TimestampHandler::new( - 4, // relative PLL frequency bandwidth: 2**-4, TODO: expose - 3, // relative PLL phase bandwidth: 2**-3, TODO: expose - ADC_SAMPLE_TICKS_LOG2 as usize, - SAMPLE_BUFFER_SIZE_LOG2, - ); + let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2, 0); let lockin = Lockin::new( - &iir_int::IIRState::default(), // TODO: lowpass, expose + &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose ); // Enable ADC/DAC events @@ -122,18 +117,20 @@ const APP: () = { let iir_state = c.resources.iir_state; let lockin = c.resources.lockin; - let (pll_phase, pll_frequency) = c - .resources - .pll - .update(c.resources.timestamper.latest_timestamp()); + let (pll_phase, pll_frequency) = c.resources.pll.update( + c.resources.timestamper.latest_timestamp().map(|t| t as i32), + 22, // relative PLL frequency bandwidth: 2**-22, TODO: expose + 22, // relative PLL phase bandwidth: 2**-22, TODO: expose + ); // Harmonic index of the LO: -1 to _de_modulate the fundamental 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)); + let sample_frequency = + (pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2).wrapping_mul(harmonic); + let mut sample_phase = + phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic)); for i in 0..adc_samples[0].len() { // Convert to signed, MSB align the ADC sample. diff --git a/src/lib.rs b/src/lib.rs index 254e626..218e3cf 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -9,9 +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 -pub const ADC_SAMPLE_TICKS_LOG2: u16 = 8; +pub const ADC_SAMPLE_TICKS_LOG2: u8 = 8; pub const ADC_SAMPLE_TICKS: u16 = 1 << ADC_SAMPLE_TICKS_LOG2; // The desired ADC sample processing buffer size. -pub const SAMPLE_BUFFER_SIZE_LOG2: usize = 3; +pub const SAMPLE_BUFFER_SIZE_LOG2: u8 = 3; pub const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2;