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

rpll: extend to above-nyquist frequencies

Browse Source
This commit is contained in:
Robert Jördens 2021-01-28 22:21:42 +01:00
parent 1749d48ca3
commit 36288225b3
2 changed files with 14 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
dsp/src/rpll.rs
View File

@ -8,8 +8,8 @@ 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
ff: u32, // current frequency estimate from frequency loop
f: u32, // current frequency estimate from both frequency and phase loop
y: i32, // current phase estimate
}
@ -49,33 +49,35 @@ impl RPLL {
input: Option<i32>,
shift_frequency: u8,
shift_phase: u8,
) -> (i32, i32) {
) -> (i32, u32) {
debug_assert!(shift_frequency > self.dt2);
debug_assert!(shift_phase > self.dt2);
// Advance phase
self.y = self.y.wrapping_add(self.f);
self.y = self.y.wrapping_add(self.f as i32);
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);
let p_sig_64 = (self.ff as u64).wrapping_mul(dx as u64);
// Add half-up rounding bias and apply gain/attenuation
let p_sig = (p_sig_long.wrapping_add(1i64 << (shift_frequency - 1))
let p_sig = (p_sig_64.wrapping_add(1u64 << (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));
self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig) as u32);
// 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);
let y_ref = ((self.f >> self.dt2) as i32).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));
self.f = self
.ff
.wrapping_add((dy >> (shift_phase - self.dt2)) as u32);
}
// Advance time
self.t = self.t.wrapping_add(1 << self.dt2);

5
src/bin/lockin.rs
View File

@ -127,8 +127,9 @@ const APP: () = {
let harmonic: i32 = -1;
// Demodulation LO phase offset
let phase_offset: i32 = 0;
let sample_frequency =
(pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2).wrapping_mul(harmonic);
let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2)
as i32)
.wrapping_mul(harmonic); // TODO: maybe rounding bias
let mut sample_phase =
phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));