189: Feature/unwrap r=jordens a=jordens



Co-authored-by: Robert Jördens <rj@quartiq.de>
This commit is contained in:
bors[bot] 2020-12-05 10:44:55 +00:00 committed by GitHub
commit 9b6f9c5744
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 98 additions and 5 deletions

View File

@ -5,6 +5,7 @@ pub type Complex<T> = (T, T);
pub mod iir; pub mod iir;
pub mod lockin; pub mod lockin;
pub mod pll; pub mod pll;
pub mod unwrap;
#[cfg(test)] #[cfg(test)]
mod testing; mod testing;

View File

@ -29,7 +29,7 @@ use serde::{Deserialize, Serialize};
/// The extension to I^3,I^2,I behavior to track chirps phase-accurately or to i64 data to /// The extension to I^3,I^2,I behavior to track chirps phase-accurately or to i64 data to
/// increase resolution for extremely narrowband applications is obvious. /// increase resolution for extremely narrowband applications is obvious.
#[derive(Copy, Clone, Default, Deserialize, Serialize)] #[derive(Copy, Clone, Default, Deserialize, Serialize)]
pub struct PLLState { pub struct PLL {
// last input phase // last input phase
x: i32, x: i32,
// filtered frequency // filtered frequency
@ -38,7 +38,7 @@ pub struct PLLState {
y: i32, y: i32,
} }
impl PLLState { impl PLL {
/// Update the PLL with a new phase sample. /// Update the PLL with a new phase sample.
/// ///
/// Args: /// Args:
@ -49,7 +49,7 @@ impl PLLState {
/// Returns: /// Returns:
/// A tuple of instantaneous phase and frequency (the current phase increment). /// A tuple of instantaneous phase and frequency (the current phase increment).
pub fn update(&mut self, x: i32, shift: u8) -> (i32, i32) { pub fn update(&mut self, x: i32, shift: u8) -> (i32, i32) {
debug_assert!(shift >= 1 && shift <= 31); debug_assert!((1..=30).contains(&shift));
let bias = 1i32 << shift; let bias = 1i32 << shift;
let e = x.wrapping_sub(self.f); let e = x.wrapping_sub(self.f);
self.f = self.f.wrapping_add( self.f = self.f.wrapping_add(
@ -57,7 +57,7 @@ impl PLLState {
); );
self.x = x; self.x = x;
let f = self.f.wrapping_add( let f = self.f.wrapping_add(
bias.wrapping_add(e).wrapping_sub(self.y) >> shift - 1, bias.wrapping_add(e).wrapping_sub(self.y) >> (shift - 1),
); );
self.y = self.y.wrapping_add(f); self.y = self.y.wrapping_add(f);
(self.y, f) (self.y, f)
@ -69,9 +69,28 @@ mod tests {
use super::*; use super::*;
#[test] #[test]
fn mini() { fn mini() {
let mut p = PLLState::default(); let mut p = PLL::default();
let (y, f) = p.update(0x10000, 10); let (y, f) = p.update(0x10000, 10);
assert_eq!(y, 0xc2); assert_eq!(y, 0xc2);
assert_eq!(f, y); assert_eq!(f, y);
} }
#[test]
fn converge() {
let mut p = PLL::default();
let f0 = 0x71f63049_i32;
let shift = 10;
let n = 31 << shift + 2;
let mut x = 0i32;
for i in 0..n {
x = x.wrapping_add(f0);
let (y, f) = p.update(x, shift);
if i > n / 4 {
assert_eq!(f.wrapping_sub(f0).abs() <= 1, true);
}
if i > n / 2 {
assert_eq!(y.wrapping_sub(x).abs() < 1 << 18, true);
}
}
}
} }

73
dsp/src/unwrap.rs Normal file
View File

@ -0,0 +1,73 @@
use serde::{Deserialize, Serialize};
/// Get phase wrap from x to y.
///
/// Phases are modulo integer overflow.
///
/// Args:
/// * `x`: Old phase sample
/// * `y`: New phase sample
///
/// Returns:
/// A tuple containg the (wrapped) phase difference and
/// one times the direction of the wrap.
pub fn get_wrap(x: i32, y: i32) -> (i32, i8) {
let delta = y.wrapping_sub(x);
let wrap = (delta >= 0) as i8 - (y >= x) as i8;
(delta, wrap)
}
/// Phase unwrapper.
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
pub struct Unwrapper {
// last input
x: i32,
// last wraps
v: i32,
}
impl Unwrapper {
/// Unwrap a new sample from a phase sequence and update the
/// unwrapper state.
///
/// Args:
/// * `x`: New phase sample
///
/// Returns:
/// A tuple containing the (wrapped) phase difference
/// and the signed number of phase wraps corresponding to the new sample.
pub fn update(&mut self, x: i32) -> (i32, i32) {
let (dx, v) = get_wrap(self.x, x);
self.x = x;
self.v = self.v.wrapping_add(v as i32);
(dx, self.v)
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn mini() {
for (x0, x1, v) in [
(0i32, 0i32, 0i8),
(1, 1, 0),
(-1, -1, 0),
(1, -1, 0),
(-1, 1, 0),
(0, 0x7fff_ffff, 0),
(-1, 0x7fff_ffff, -1),
(0, -0x8000_0000, 0),
(1, -0x8000_0000, 1),
(-0x6000_0000, 0x6000_0000, -1),
(0x6000_0000, -0x6000_0000, 1),
(0x6000_0000, -0x6000_0000, 1),
(0x6000_0000, -0x6000_0000, 1),
]
.iter()
{
let (_dx, w) = get_wrap(*x0, *x1);
assert_eq!(*v, w, " = get_wrap({:#x}, {:#x})", *x0, *x1);
}
}
}