diff --git a/dsp/src/rpll.rs b/dsp/src/rpll.rs index 2bfd29c..e3b3894 100644 --- a/dsp/src/rpll.rs +++ b/dsp/src/rpll.rs @@ -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, 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); diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs index 7066ad8..cbdda51 100644 --- a/src/bin/lockin.rs +++ b/src/bin/lockin.rs @@ -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));