From 1824ac1b10d0baf7eca2d97129dcef697e675b75 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 3 Dec 2021 11:51:30 +0100 Subject: [PATCH 1/5] unchecked math on nightly --- .github/workflows/ci.yml | 3 +++ Cargo.toml | 3 +++ src/lib.rs | 1 + src/lockin.rs | 2 +- src/lowpass.rs | 10 ++++++---- src/pll.rs | 23 ++++++++++++----------- src/rpll.rs | 29 ++++++++++++++++------------- src/tools.rs | 20 ++++++++++++++++++++ 8 files changed, 62 insertions(+), 29 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 5d759ee..4eb7b27 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -34,6 +34,9 @@ jobs: matrix: toolchain: [stable, beta, nightly] features: [''] + include: + - toolchain: nightly + features: ['nightly'] steps: - uses: actions/checkout@v2 - uses: actions-rs/toolchain@v1 diff --git a/Cargo.toml b/Cargo.toml index 010894d..d9ad939 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -16,6 +16,9 @@ num-complex = { version = "0.4.0", features = ["serde"], default-features = fals miniconf = { version = "0.2.0", optional = false } num-traits = { version = "0.2.14", features = ["libm"], default-features = false} +[features] +nightly = [] + [dev-dependencies] easybench = "1.0" rand = "0.8" diff --git a/src/lib.rs b/src/lib.rs index 581f585..12f6231 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -1,3 +1,4 @@ +#![cfg_attr(feature = "nightly", feature(unchecked_math))] #![cfg_attr(not(test), no_std)] mod tools; diff --git a/src/lockin.rs b/src/lockin.rs index a9f5e84..07e191f 100644 --- a/src/lockin.rs +++ b/src/lockin.rs @@ -7,7 +7,7 @@ pub struct Lockin { impl Lockin { /// Update the lockin with a sample taken at a given phase. - pub fn update(&mut self, sample: i32, phase: i32, k: u8) -> Complex { + pub fn update(&mut self, sample: i32, phase: i32, k: i32) -> Complex { // Get the LO signal for demodulation and mix the sample; let mix = Complex::from_angle(phase).mul_scaled(sample); diff --git a/src/lowpass.rs b/src/lowpass.rs index 7573e28..ae82db1 100644 --- a/src/lowpass.rs +++ b/src/lowpass.rs @@ -1,8 +1,10 @@ +use crate::tools::{unchecked_shl, unchecked_shr}; + /// Arbitrary order, high dynamic range, wide coefficient range, /// lowpass filter implementation. DC gain is 1. /// /// Type argument N is the filter order. -#[derive(Clone)] +#[derive(Copy, Clone)] pub struct Lowpass { // IIR state storage y: [i32; N], @@ -23,17 +25,17 @@ impl Lowpass { /// /// # Return /// Filtered output y. - pub fn update(&mut self, x: i32, k: u8) -> i32 { + pub fn update(&mut self, x: i32, k: i32) -> i32 { debug_assert!(k & 31 == k); // This is an unrolled and optimized first-order IIR loop // that works for all possible time constants. // Note T-DF-I and the zeros at Nyquist. let mut x = x; for y in self.y.iter_mut() { - let dy = x.saturating_sub(*y) >> k; + let dy = unchecked_shr(x.saturating_sub(*y), k); *y += dy; x = *y - (dy >> 1); } - x.saturating_add((self.y.len() as i32) << (k - 1).max(0)) + x.saturating_add(unchecked_shl(self.y.len() as i32, (k - 1).max(0))) } } diff --git a/src/pll.rs b/src/pll.rs index 3697102..d1e24af 100644 --- a/src/pll.rs +++ b/src/pll.rs @@ -1,3 +1,4 @@ +use crate::tools::{unchecked_shl, unchecked_shr}; use serde::{Deserialize, Serialize}; /// Type-II, sampled phase, discrete time PLL @@ -53,24 +54,24 @@ impl PLL { /// /// Returns: /// A tuple of instantaneous phase and frequency (the current phase increment). - pub fn update(&mut self, x: Option, shift_frequency: u8, shift_phase: u8) -> (i32, i32) { + pub fn update(&mut self, x: Option, shift_frequency: i32, shift_phase: i32) -> (i32, i32) { debug_assert!((1..=30).contains(&shift_frequency)); debug_assert!((1..=30).contains(&shift_phase)); let f = if let Some(x) = x { let e = x.wrapping_sub(self.f); - self.f = self.f.wrapping_add( - (1i32 << (shift_frequency - 1)) + self.f = self.f.wrapping_add(unchecked_shr( + unchecked_shl(1i32, shift_frequency - 1) .wrapping_add(e) - .wrapping_sub(self.x) - >> shift_frequency, - ); + .wrapping_sub(self.x), + shift_frequency, + )); self.x = x; - self.f.wrapping_add( - (1i32 << (shift_phase - 1)) + self.f.wrapping_add(unchecked_shr( + unchecked_shl(1i32, shift_phase - 1) .wrapping_add(e) - .wrapping_sub(self.y) - >> shift_phase, - ) + .wrapping_sub(self.y), + shift_phase, + )) } else { self.x = self.x.wrapping_add(self.f); self.f diff --git a/src/rpll.rs b/src/rpll.rs index 5671e30..37dc35f 100644 --- a/src/rpll.rs +++ b/src/rpll.rs @@ -1,3 +1,6 @@ +use crate::tools::{unchecked_shl, unchecked_shr}; +use serde::{Deserialize, Serialize}; + /// Reciprocal PLL. /// /// Consumes noisy, quantized timestamps of a reference signal and reconstructs @@ -5,13 +8,13 @@ /// 1 << 32 of) that reference. /// In other words, `update()` rate ralative to reference frequency, /// `u32::MAX` corresponding to both being equal. -#[derive(Copy, Clone, Default)] +#[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct RPLL { - dt2: u8, // 1 << dt2 is the counter rate to update() rate ratio - x: i32, // previous timestamp - ff: u32, // current frequency estimate from frequency loop - f: u32, // current frequency estimate from both frequency and phase loop - y: i32, // current phase estimate + dt2: i32, // 1 << dt2 is the counter rate to update() rate ratio + x: i32, // previous timestamp + ff: u32, // current frequency estimate from frequency loop + f: u32, // current frequency estimate from both frequency and phase loop + y: i32, // current phase estimate } impl RPLL { @@ -22,7 +25,7 @@ impl RPLL { /// /// Returns: /// Initialized RPLL instance. - pub fn new(dt2: u8) -> Self { + pub fn new(dt2: i32) -> Self { Self { dt2, ..Default::default() @@ -46,8 +49,8 @@ impl RPLL { pub fn update( &mut self, input: Option, - shift_frequency: u8, - shift_phase: u8, + shift_frequency: i32, + shift_phase: i32, ) -> (i32, u32) { debug_assert!(shift_frequency >= self.dt2); debug_assert!(shift_phase >= self.dt2); @@ -68,11 +71,11 @@ impl RPLL { // 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 = (x.wrapping_neg() & ((1 << self.dt2) - 1)) as u32; + let dt = (x.wrapping_neg() & (unchecked_shl(1, self.dt2) - 1)) as u32; // Reference phase estimate "now" let y_ref = (self.f >> self.dt2).wrapping_mul(dt) as i32; // Phase error with gain - let dy = y_ref.wrapping_sub(self.y) >> (shift_phase - self.dt2); + let dy = unchecked_shr(y_ref.wrapping_sub(self.y), shift_phase - self.dt2); // Current frequency estimate from frequency lock and phase error self.f = self.ff.wrapping_add(dy as u32); } @@ -94,8 +97,8 @@ mod test { struct Harness { rpll: RPLL, - shift_frequency: u8, - shift_phase: u8, + shift_frequency: i32, + shift_phase: i32, noise: i32, period: i32, next: i32, diff --git a/src/tools.rs b/src/tools.rs index 5844670..d4ca84f 100644 --- a/src/tools.rs +++ b/src/tools.rs @@ -51,3 +51,23 @@ pub fn macc_i32(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 { .fold(y0, |y, xa| y + xa); (y >> shift) as i32 } + +#[cfg(feature = "nightly")] +pub fn unchecked_shr(x: i32, k: i32) -> i32 { + unsafe { x.unchecked_shr(k) } +} + +#[cfg(not(feature = "nightly"))] +pub fn unchecked_shr(x: i32, k: i32) -> i32 { + x >> k +} + +#[cfg(feature = "nightly")] +pub fn unchecked_shl(x: i32, k: i32) -> i32 { + unsafe { x.unchecked_shl(k) } +} + +#[cfg(not(feature = "nightly"))] +pub fn unchecked_shl(x: i32, k: i32) -> i32 { + x << k +} From 38488e3c3257e1f04e186aa3af8fbc29fe927e69 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 3 Dec 2021 12:39:54 +0100 Subject: [PATCH 2/5] fix ci --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4eb7b27..5a06c07 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -36,7 +36,7 @@ jobs: features: [''] include: - toolchain: nightly - features: ['nightly'] + features: 'nightly' steps: - uses: actions/checkout@v2 - uses: actions-rs/toolchain@v1 From 83f80edd32dee7fd1042af3280567456356b2cda Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 3 Dec 2021 12:43:28 +0100 Subject: [PATCH 3/5] unchecked math shifts with u32 --- src/lockin.rs | 2 +- src/lowpass.rs | 2 +- src/pll.rs | 2 +- src/rpll.rs | 12 ++++++------ src/tools.rs | 12 ++++++------ 5 files changed, 15 insertions(+), 15 deletions(-) diff --git a/src/lockin.rs b/src/lockin.rs index 07e191f..88da53d 100644 --- a/src/lockin.rs +++ b/src/lockin.rs @@ -7,7 +7,7 @@ pub struct Lockin { impl Lockin { /// Update the lockin with a sample taken at a given phase. - pub fn update(&mut self, sample: i32, phase: i32, k: i32) -> Complex { + pub fn update(&mut self, sample: i32, phase: i32, k: u32) -> Complex { // Get the LO signal for demodulation and mix the sample; let mix = Complex::from_angle(phase).mul_scaled(sample); diff --git a/src/lowpass.rs b/src/lowpass.rs index ae82db1..ccecd32 100644 --- a/src/lowpass.rs +++ b/src/lowpass.rs @@ -25,7 +25,7 @@ impl Lowpass { /// /// # Return /// Filtered output y. - pub fn update(&mut self, x: i32, k: i32) -> i32 { + pub fn update(&mut self, x: i32, k: u32) -> i32 { debug_assert!(k & 31 == k); // This is an unrolled and optimized first-order IIR loop // that works for all possible time constants. diff --git a/src/pll.rs b/src/pll.rs index d1e24af..10c1c66 100644 --- a/src/pll.rs +++ b/src/pll.rs @@ -54,7 +54,7 @@ impl PLL { /// /// Returns: /// A tuple of instantaneous phase and frequency (the current phase increment). - pub fn update(&mut self, x: Option, shift_frequency: i32, shift_phase: i32) -> (i32, i32) { + pub fn update(&mut self, x: Option, shift_frequency: u32, shift_phase: u32) -> (i32, i32) { debug_assert!((1..=30).contains(&shift_frequency)); debug_assert!((1..=30).contains(&shift_phase)); let f = if let Some(x) = x { diff --git a/src/rpll.rs b/src/rpll.rs index 37dc35f..9444b25 100644 --- a/src/rpll.rs +++ b/src/rpll.rs @@ -10,7 +10,7 @@ use serde::{Deserialize, Serialize}; /// `u32::MAX` corresponding to both being equal. #[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct RPLL { - dt2: i32, // 1 << dt2 is the counter rate to update() rate ratio + dt2: u32, // 1 << dt2 is the counter rate to update() rate ratio x: i32, // previous timestamp ff: u32, // current frequency estimate from frequency loop f: u32, // current frequency estimate from both frequency and phase loop @@ -25,7 +25,7 @@ impl RPLL { /// /// Returns: /// Initialized RPLL instance. - pub fn new(dt2: i32) -> Self { + pub fn new(dt2: u32) -> Self { Self { dt2, ..Default::default() @@ -49,8 +49,8 @@ impl RPLL { pub fn update( &mut self, input: Option, - shift_frequency: i32, - shift_phase: i32, + shift_frequency: u32, + shift_phase: u32, ) -> (i32, u32) { debug_assert!(shift_frequency >= self.dt2); debug_assert!(shift_phase >= self.dt2); @@ -97,8 +97,8 @@ mod test { struct Harness { rpll: RPLL, - shift_frequency: i32, - shift_phase: i32, + shift_frequency: u32, + shift_phase: u32, noise: i32, period: i32, next: i32, diff --git a/src/tools.rs b/src/tools.rs index d4ca84f..16eae5a 100644 --- a/src/tools.rs +++ b/src/tools.rs @@ -53,21 +53,21 @@ pub fn macc_i32(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 { } #[cfg(feature = "nightly")] -pub fn unchecked_shr(x: i32, k: i32) -> i32 { - unsafe { x.unchecked_shr(k) } +pub fn unchecked_shr(x: i32, k: u32) -> i32 { + unsafe { x.unchecked_shr(k as _) } } #[cfg(not(feature = "nightly"))] -pub fn unchecked_shr(x: i32, k: i32) -> i32 { +pub fn unchecked_shr(x: i32, k: u32) -> i32 { x >> k } #[cfg(feature = "nightly")] -pub fn unchecked_shl(x: i32, k: i32) -> i32 { - unsafe { x.unchecked_shl(k) } +pub fn unchecked_shl(x: i32, k: u32) -> i32 { + unsafe { x.unchecked_shl(k as _) } } #[cfg(not(feature = "nightly"))] -pub fn unchecked_shl(x: i32, k: i32) -> i32 { +pub fn unchecked_shl(x: i32, k: u32) -> i32 { x << k } From efb08d56fd0dcd5d20f046cbdc157d1731319a07 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 3 Dec 2021 12:46:36 +0100 Subject: [PATCH 4/5] ci: override toolchain --- .github/workflows/ci.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 5a06c07..b0757cd 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -42,6 +42,7 @@ jobs: - uses: actions-rs/toolchain@v1 with: toolchain: ${{ matrix.toolchain }} + override: true - uses: Swatinem/rust-cache@v1 - name: cargo build uses: actions-rs/cargo@v1 From 307782af112c46a69255c1db24c499fb2ae48a2f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Mon, 24 Jan 2022 14:05:57 +0100 Subject: [PATCH 5/5] fmt --- src/pll.rs | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/pll.rs b/src/pll.rs index ae767e6..23cb5be 100644 --- a/src/pll.rs +++ b/src/pll.rs @@ -63,19 +63,21 @@ impl PLL { if let Some(x) = x { let df = unchecked_shr( unchecked_shl(1, shift_frequency - 1) - .wrapping_add(x) - .wrapping_sub(self.x) - .wrapping_sub(self.f), - shift_frequency); + .wrapping_add(x) + .wrapping_sub(self.x) + .wrapping_sub(self.f), + shift_frequency, + ); self.x = x; self.f = self.f.wrapping_add(df); let f = self.f.wrapping_sub(df >> 1); self.y = self.y.wrapping_add(f); let dy = unchecked_shr( unchecked_shl(1, shift_phase - 1) - .wrapping_add(x) - .wrapping_sub(self.y), - shift_phase); + .wrapping_add(x) + .wrapping_sub(self.y), + shift_phase, + ); self.y = self.y.wrapping_add(dy); let y = self.y.wrapping_sub(dy >> 1); (y, f.wrapping_add(dy))