95 lines
3.7 KiB
Rust
95 lines
3.7 KiB
Rust
/// 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.
|
|
///
|
|
/// Returns:
|
|
/// Initialized RPLL instance.
|
|
pub fn new(dt2: u8) -> RPLL {
|
|
RPLL {
|
|
dt2,
|
|
..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 mut dt = self.t.wrapping_sub(x);
|
|
|
|
if dt < 0 {
|
|
// Timestamp is in the future
|
|
// Advance phase and time until we are just past the most recent
|
|
// timestamp.
|
|
let mut dt_ceil = dt >> self.dt2;
|
|
self.y = self.y.wrapping_sub(self.f.wrapping_mul(dt_ceil));
|
|
dt_ceil <<= self.dt2;
|
|
self.t = self.t.wrapping_sub(dt_ceil);
|
|
dt = dt.wrapping_sub(dt_ceil);
|
|
}
|
|
|
|
// 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)
|
|
}
|
|
}
|