rpll: refine

This commit is contained in:
Robert Jördens 2021-01-26 14:40:44 +01:00
parent 16009c3b7e
commit ea7b08fc64
3 changed files with 63 additions and 29 deletions

View File

@ -405,8 +405,7 @@ mod test {
"Too many timestamps per batch. Each batch can have at most 1 timestamp." "Too many timestamps per batch. Each batch can have at most 1 timestamp."
); );
let adc_sample_ticks_log2 = let adc_sample_ticks_log2 = (internal_frequency).log2().round() as u8;
(internal_frequency).log2().round() as u8;
assert!( assert!(
adc_sample_ticks_log2 + sample_buffer_size_log2 <= 32, 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." "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."
@ -422,8 +421,10 @@ mod test {
2., 2.,
), // DC gain to get to full scale with the image filtered out ), // DC gain to get to full scale with the image filtered out
); );
let mut timestamp_handler = let mut timestamp_handler = RPLL::new(
RPLL::new((adc_sample_ticks_log2 + sample_buffer_size_log2) as u8); (adc_sample_ticks_log2 + sample_buffer_size_log2) as u8,
0,
);
let mut timestamp_start: u64 = 0; let mut timestamp_start: u64 = 0;
let time_constant: f64 = 1. / (2. * PI * corner_frequency); let time_constant: f64 = 1. / (2. * PI * corner_frequency);
@ -484,7 +485,11 @@ mod test {
timestamp_start += batch_sample_count; timestamp_start += batch_sample_count;
let (demodulation_initial_phase, demodulation_frequency) = let (demodulation_initial_phase, demodulation_frequency) =
timestamp_handler.update(timestamp.map(|t| t as i32), pll_shift_frequency, pll_shift_phase); timestamp_handler.update(
timestamp.map(|t| t as i32),
pll_shift_frequency,
pll_shift_phase,
);
let output = lockin.update( let output = lockin.update(
adc_signal, adc_signal,
demodulation_initial_phase as i32, demodulation_initial_phase as i32,

View File

@ -1,20 +1,47 @@
/// 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)] #[derive(Copy, Clone, Default)]
pub struct RPLL { pub struct RPLL {
dt2: u8, dt2: u8, // 1 << dt2 is the counter rate to update() rate ratio
t: i32, t: i32, // current counter time
f2: i64, x: i32, // previous timestamp
y1: i32, ff: i32, // current frequency estimate from the frequency loop
xj1: i32, f: i32, // current frequency estimate from both frequency and phase loop
f1: i32, y: i32, // current phase estimate
} }
impl RPLL { impl RPLL {
pub fn new(dt2: u8) -> 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 {
let mut pll = RPLL::default(); let mut pll = RPLL::default();
pll.dt2 = dt2; pll.dt2 = dt2;
pll.t = t;
pll pll
} }
/// Advance the RPLL and optionally supply a new timestamp.
///
/// Args:
/// * input: Optional new timestamp (wrapping around at the i32 boundary).
/// * 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 the same as
/// `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( pub fn update(
&mut self, &mut self,
input: Option<i32>, input: Option<i32>,
@ -24,19 +51,21 @@ impl RPLL {
debug_assert!(shift_frequency > 0); debug_assert!(shift_frequency > 0);
debug_assert!(shift_phase > 0); debug_assert!(shift_phase > 0);
debug_assert!(32 + self.dt2 >= shift_frequency); debug_assert!(32 + self.dt2 >= shift_frequency);
self.y1 = self.y1.wrapping_add(self.f1); self.y = self.y.wrapping_add(self.f as i32);
if let Some(xj) = input { if let Some(x) = input {
self.f2 = self.f2.wrapping_add((1i64 << 32 + self.dt2 - shift_frequency) self.ff = self.ff.wrapping_add((1i32 << 32 + self.dt2 - shift_frequency)
- (self.f2.wrapping_mul(xj.wrapping_sub(self.xj1) as i64) - ((self.ff as i64).wrapping_mul(x.wrapping_sub(self.x) as i64)
+ (1i64 << shift_frequency - 1) + (1i64 << shift_frequency - 1) // half-up rounding bias
>> shift_frequency)); >> shift_frequency) as i32);
self.f1 = (self.f2 as i32) self.f = self.ff.wrapping_add(
.wrapping_add((self.f2.wrapping_mul(self.t.wrapping_sub(xj) as i64) ((self.f as i64).wrapping_mul(self.t.wrapping_sub(x) as i64)
- ((self.y1 as i64) << self.dt2) .wrapping_sub((self.y as i64) << self.dt2)
+ (1i64 << shift_phase - 1) // + (1i64 << shift_phase - 1)
>> shift_phase) as i32); >> shift_phase) as i32,
);
self.x = x;
} }
self.t = self.t.wrapping_add(1 << self.dt2); self.t = self.t.wrapping_add(1 << self.dt2);
(self.y1, self.f1) (self.y, self.f)
} }
} }

View File

@ -53,8 +53,7 @@ const APP: () = {
// Configure the microcontroller // Configure the microcontroller
let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
let pll = let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2, 0);
RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2);
let lockin = Lockin::new( let lockin = Lockin::new(
&iir_int::IIRState::default(), // TODO: lowpass, expose &iir_int::IIRState::default(), // TODO: lowpass, expose
@ -128,9 +127,10 @@ const APP: () = {
let harmonic: i32 = -1; let harmonic: i32 = -1;
// Demodulation LO phase offset // Demodulation LO phase offset
let phase_offset: i32 = 0; let phase_offset: i32 = 0;
let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic); let sample_frequency =
let mut sample_phase = phase_offset (pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2).wrapping_mul(harmonic);
.wrapping_add((pll_phase as i32).wrapping_mul(harmonic)); let mut sample_phase =
phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
for i in 0..adc_samples[0].len() { for i in 0..adc_samples[0].len() {
// Convert to signed, MSB align the ADC sample. // Convert to signed, MSB align the ADC sample.