kai
/
pounder_test
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Merge pull request #241 from quartiq/rpll 

Reciprocal PLL
This commit is contained in:
Robert Jördens 2021-01-26 19:37:36 +01:00 committed by GitHub
parent 659a6879f7 7c1fa9695a
commit c769bdbd4c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 145 additions and 1028 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
dsp/src/iir_int.rs
View File

@ -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);
}
}

2
dsp/src/lib.rs
View File

@ -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;

911
dsp/src/lockin.rs
View File

@ -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::<f64>()
.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<u32> {
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<Tone>,
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,
);
}
}

100
dsp/src/reciprocal_pll.rs
View File

@ -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, 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)
}
}

84
dsp/src/rpll.rs Normal file
View File

@ -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<i32>,
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)
}
}

29
src/bin/lockin.rs
View File

@ -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.

4
src/lib.rs
View File

@ -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;