rpll: refine, simplify, document and comment

This commit is contained in:
Robert Jördens 2021-01-26 18:49:31 +01:00
parent ea7b08fc64
commit 9b3a47e08b
1 changed files with 33 additions and 20 deletions

View File

@ -8,7 +8,7 @@ pub struct RPLL {
dt2: u8, // 1 << dt2 is the counter rate to update() rate ratio dt2: u8, // 1 << dt2 is the counter rate to update() rate ratio
t: i32, // current counter time t: i32, // current counter time
x: i32, // previous timestamp x: i32, // previous timestamp
ff: i32, // current frequency estimate from the frequency loop ff: i32, // current frequency estimate from frequency loop
f: i32, // current frequency estimate from both frequency and phase loop f: i32, // current frequency estimate from both frequency and phase loop
y: i32, // current phase estimate y: i32, // current phase estimate
} }
@ -23,20 +23,22 @@ impl RPLL {
/// Returns: /// Returns:
/// Initialized RPLL instance. /// Initialized RPLL instance.
pub fn new(dt2: u8, t: i32) -> RPLL { pub fn new(dt2: u8, t: i32) -> RPLL {
let mut pll = RPLL::default(); RPLL {
pll.dt2 = dt2; dt2,
pll.t = t; t,
pll ..Default::default()
}
} }
/// Advance the RPLL and optionally supply a new timestamp. /// Advance the RPLL and optionally supply a new timestamp.
/// ///
/// Args: /// Args:
/// * input: Optional new timestamp (wrapping around at the i32 boundary). /// * 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 /// * shift_frequency: Frequency lock settling time. 1 << shift_frequency is
/// frequency lock settling time in counter periods. The settling time must be larger /// frequency lock settling time in counter periods. The settling time must be larger
/// than the signal period to lock to. /// than the signal period to lock to.
/// * shift_phase: Phase lock settling time. Usually the same as /// * shift_phase: Phase lock settling time. Usually one less than
/// `shift_frequency` (see there). /// `shift_frequency` (see there).
/// ///
/// Returns: /// Returns:
@ -48,23 +50,34 @@ impl RPLL {
shift_frequency: u8, shift_frequency: u8,
shift_phase: u8, shift_phase: u8,
) -> (i32, i32) { ) -> (i32, i32) {
debug_assert!(shift_frequency > 0); debug_assert!(shift_frequency > self.dt2);
debug_assert!(shift_phase > 0); debug_assert!(shift_phase > self.dt2);
debug_assert!(32 + self.dt2 >= shift_frequency); // Advance phase
self.y = self.y.wrapping_add(self.f as i32); self.y = self.y.wrapping_add(self.f);
if let Some(x) = input { if let Some(x) = input {
self.ff = self.ff.wrapping_add((1i32 << 32 + self.dt2 - shift_frequency) // Reference period in counter cycles
- ((self.ff as i64).wrapping_mul(x.wrapping_sub(self.x) as i64) let dx = x.wrapping_sub(self.x);
+ (1i64 << shift_frequency - 1) // half-up rounding bias // Store timestamp for next time.
>> shift_frequency) as i32);
self.f = self.ff.wrapping_add(
((self.f as i64).wrapping_mul(self.t.wrapping_sub(x) as i64)
.wrapping_sub((self.y as i64) << self.dt2)
// + (1i64 << shift_phase - 1)
>> shift_phase) as i32,
);
self.x = x; 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.t = self.t.wrapping_add(1 << self.dt2);
(self.y, self.f) (self.y, self.f)
} }