diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 5d759ee..b0757cd 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -34,11 +34,15 @@ jobs: matrix: toolchain: [stable, beta, nightly] features: [''] + include: + - toolchain: nightly + features: 'nightly' steps: - uses: actions/checkout@v2 - uses: actions-rs/toolchain@v1 with: toolchain: ${{ matrix.toolchain }} + override: true - uses: Swatinem/rust-cache@v1 - name: cargo build uses: actions-rs/cargo@v1 diff --git a/Cargo.toml b/Cargo.toml index 400c5c6..0f06b87 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -16,6 +16,9 @@ num-complex = { version = "0.4.0", features = ["serde"], default-features = fals miniconf = "0.3" 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/lowpass.rs b/src/lowpass.rs index a30a54d..57cef63 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], @@ -30,11 +32,11 @@ impl Lowpass { // 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((N as i32) << (k - 1).max(0)) + x.saturating_add(unchecked_shl(N as i32, (k - 1).max(0))) } /// Return the current filter output diff --git a/src/pll.rs b/src/pll.rs index 357e332..23cb5be 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 @@ -60,19 +61,23 @@ impl PLL { debug_assert!((1..=30).contains(&shift_frequency)); debug_assert!((1..=30).contains(&shift_phase)); if let Some(x) = x { - let df = (1i32 << (shift_frequency - 1)) - .wrapping_add(x) - .wrapping_sub(self.x) - .wrapping_sub(self.f) - >> shift_frequency; + let df = unchecked_shr( + unchecked_shl(1, shift_frequency - 1) + .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 = (1i32 << (shift_phase - 1)) - .wrapping_add(x) - .wrapping_sub(self.y) - >> shift_phase; + let dy = unchecked_shr( + unchecked_shl(1, shift_phase - 1) + .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)) diff --git a/src/rpll.rs b/src/rpll.rs index e307d96..87cef23 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,7 +8,7 @@ /// 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: u32, // 1 << dt2 is the counter rate to update() rate ratio x: i32, // previous timestamp @@ -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); } diff --git a/src/tools.rs b/src/tools.rs index 5844670..16eae5a 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: u32) -> i32 { + unsafe { x.unchecked_shr(k as _) } +} + +#[cfg(not(feature = "nightly"))] +pub fn unchecked_shr(x: i32, k: u32) -> i32 { + x >> k +} + +#[cfg(feature = "nightly")] +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: u32) -> i32 { + x << k +}