rpll: refine, simplify, document and comment
This commit is contained in:
parent
ea7b08fc64
commit
9b3a47e08b
@ -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)
|
||||||
}
|
}
|
||||||
|
Loading…
Reference in New Issue
Block a user