Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

cic sweptsine #28

Draft
wants to merge 16 commits into
base: main
Choose a base branch
from
5 changes: 5 additions & 0 deletions Cargo.toml
Original file line number Diff line number Diff line change
Expand Up @@ -16,10 +16,15 @@ num-complex = { version = "0.4.0", features = ["serde"], default-features = fals
num-traits = { version = "0.2.14", features = ["libm"], default-features = false}

[dev-dependencies]
quickcheck = "1.0.3"
quickcheck_macros = "1.0.0"
rand = "0.8"
rustfft = "6.1.0"
# futuredsp = "0.0.6"
# sdr = "0.7.0"

[features]
std = []

[profile.release]
debug = 1
2 changes: 1 addition & 1 deletion src/accu.rs
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
use num_traits::ops::wrapping::WrappingAdd;

/// Wrapping Accumulator
#[derive(Copy, Clone, Default, PartialEq, Eq, Debug)]
#[derive(Copy, Clone, Default, PartialEq, PartialOrd, Debug)]
pub struct Accu<T> {
state: T,
step: T,
Expand Down
259 changes: 259 additions & 0 deletions src/cic.rs
Original file line number Diff line number Diff line change
@@ -0,0 +1,259 @@
use core::ops::AddAssign;

use num_traits::{AsPrimitive, Num, Pow, WrappingAdd, WrappingSub};

/// Cascaded integrator comb structure
///
/// Order `N` where `N = 3` is cubic.
#[derive(Clone, Debug)]
pub struct Cic<T, const N: usize> {
/// Rate change (fast/slow - 1)
/// Interpolator: output/input - 1
/// Decimator: input/output - 1
rate: u32,
/// Up/downsampler state (count down)
index: u32,
/// Zero order hold behind comb sections.
/// Interpolator: Combined with the upsampler
/// Decimator: To support `get_decimate()`
zoh: T,
/// Comb/differentiator state
combs: [T; N],
/// Integrator state
integrators: [T; N],
}

impl<T, const N: usize> Cic<T, N>
where
T: Num + AddAssign + WrappingAdd + WrappingSub + Pow<usize, Output = T> + Copy + 'static,
u32: AsPrimitive<T>,
{
/// Create a new zero-initialized filter with the given rate change.
pub fn new(rate: u32) -> Self {
Self {
rate,
index: 0,
zoh: T::zero(),
combs: [T::zero(); N],
integrators: [T::zero(); N],
}
}

/// Filter order
///
/// * 0: zero order hold
/// * 1: linear
/// * 2: quadratic
/// * 3: cubic interpolation/decimation
///
/// etc.
pub const fn order(&self) -> usize {
N
}

/// Rate change
///
/// `fast/slow - 1`
pub const fn rate(&self) -> u32 {
self.rate
}

/// Set the rate change
///
/// `fast/slow - 1`
pub fn set_rate(&mut self, rate: u32) {
self.rate = rate;
}

/// Zero-initialize the filter state
pub fn clear(&mut self) {
*self = Self::new(self.rate);
}

/// Accepts/provides new slow-rate sample
///
/// Interpolator: accepts new input sample
/// Decimator: returns new output sample
pub const fn tick(&self) -> bool {
self.index == 0
}

/// Current interpolator output
pub fn get_interpolate(&self) -> T {
self.integrators.last().copied().unwrap_or(self.zoh)
}

/// Current decimator output
pub fn get_decimate(&self) -> T {
self.zoh
}

/// Filter gain
pub fn gain(&self) -> T {
(self.rate.as_() + T::one()).pow(N)
}

/// Right shift amount
///
/// `log2(gain())` if gain is a power of two,
/// otherwise an upper bound.
pub const fn gain_log2(&self) -> u32 {
(u32::BITS - self.rate.leading_zeros()) * N as u32
}

/// Impulse response length
pub const fn response_length(&self) -> usize {
self.rate as usize * N
}

/// Establish a settled filter state
pub fn settle_interpolate(&mut self, x: T) {
self.clear();
if let Some(c) = self.combs.first_mut() {
*c = x;
} else {
self.zoh = x;
}
let g = self.gain();
if let Some(i) = self.integrators.last_mut() {
*i = x * g;
}
}

/// Establish a settled filter state
///
/// Unimplemented!
pub fn settle_decimate(&mut self, x: T) {
self.clear();
self.zoh = x * self.gain();
unimplemented!();
}

/// Optionally ingest a new low-rate sample and
/// retrieve the next output.
///
/// A new sample must be supplied at the correct time (when [`Cic::tick()`] is true)
pub fn interpolate(&mut self, x: Option<T>) -> T {
if let Some(x) = x {
debug_assert_eq!(self.index, 0);
self.index = self.rate;
let x = self.combs.iter_mut().fold(x, |x, c| {
let y = x - *c;
*c = x;
y
});
self.zoh = x;
} else {
self.index -= 1;
}
self.integrators.iter_mut().fold(self.zoh, |x, i| {
// Overflow is not OK
*i += x;
*i
})
}

/// Ingest a new high-rate sample and optionally retrieve next output.
pub fn decimate(&mut self, x: T) -> Option<T> {
let x = self.integrators.iter_mut().fold(x, |x, i| {
// Overflow is OK if bitwidth is sufficient (input * gain)
*i = i.wrapping_add(&x);
*i
});
if let Some(index) = self.index.checked_sub(1) {
self.index = index;
None
} else {
self.index = self.rate;
let x = self.combs.iter_mut().fold(x, |x, c| {
let y = x.wrapping_sub(c);
*c = x;
y
});
self.zoh = x;
Some(self.zoh)
}
}
}

#[cfg(test)]
mod test {
use core::cmp::Ordering;

use super::*;
use quickcheck_macros::quickcheck;

#[quickcheck]
fn new(rate: u32) {
let _ = Cic::<i64, 3>::new(rate);
}

#[quickcheck]
fn identity_dec(x: Vec<i64>) {
let mut dec = Cic::<_, 3>::new(0);
for x in x {
assert_eq!(x, dec.decimate(x).unwrap());
assert_eq!(x, dec.get_decimate());
}
}

#[quickcheck]
fn identity_int(x: Vec<i64>) {
const N: usize = 3;
let mut int = Cic::<_, N>::new(0);
for x in x {
assert_eq!(x >> N, int.interpolate(Some(x >> N)));
assert_eq!(x >> N, int.get_interpolate());
}
}

#[quickcheck]
fn response_length_gain_settle(x: Vec<i32>, rate: u32) {
let mut int = Cic::<_, 3>::new(rate);
let shift = int.gain_log2();
if shift >= 32 {
return;
}
assert!(int.gain() <= 1 << shift);
for x in x {
while !int.tick() {
int.interpolate(None);
}
let y_last = int.get_interpolate();
let y_want = x as i64 * int.gain();
for i in 0..2 * int.response_length() {
let y = int.interpolate(if int.tick() { Some(x as i64) } else { None });
assert_eq!(y, int.get_interpolate());
if i < int.response_length() {
match y_want.cmp(&y_last) {
Ordering::Greater => assert!((y_last..y_want).contains(&y)),
Ordering::Less => assert!((y_want..y_last).contains(&(y - 1))),
Ordering::Equal => assert_eq!(y_want, y),
}
} else {
assert_eq!(y, y_want);
}
}
}
}

#[quickcheck]
fn settle(rate: u32, x: i32) {
let mut int = Cic::<i64, 3>::new(rate);
if int.gain_log2() >= 32 {
return;
}
int.settle_interpolate(x as _);
// let mut dec = Cic::<i64, 3>::new(rate);
// dec.settle_decimate(x as _);
for _ in 0..100 {
let y = int.interpolate(if int.tick() { Some(x as _) } else { None });
assert_eq!(y, x as i64 * int.gain());
assert_eq!(y, int.get_interpolate());
// assert_eq!(dec.get_decimate(), x as i64 * dec.gain());
// if let Some(y) = dec.decimate(x as _) {
// assert_eq!(y, x as i64 * dec.gain());
// }
}
}
}
8 changes: 4 additions & 4 deletions src/hbf.rs
Original file line number Diff line number Diff line change
Expand Up @@ -185,8 +185,8 @@ macro_rules! impl_half_i {
}
impl_half_i!(i8 i16 i32 i64 i128);

impl<'a, T: Copy + Zero + Add + Mul<Output = T> + Sum + Half, const M: usize, const N: usize> Filter
for HbfDec<'a, T, M, N>
impl<T: Copy + Zero + Add + Mul<Output = T> + Sum + Half, const M: usize, const N: usize> Filter
for HbfDec<'_, T, M, N>
{
type Item = T;

Expand Down Expand Up @@ -259,8 +259,8 @@ impl<'a, T: Copy + Zero + Add + Mul<Output = T> + Sum, const M: usize, const N:
}
}

impl<'a, T: Copy + Zero + Add + Mul<Output = T> + Sum, const M: usize, const N: usize> Filter
for HbfInt<'a, T, M, N>
impl<T: Copy + Zero + Add + Mul<Output = T> + Sum, const M: usize, const N: usize> Filter
for HbfInt<'_, T, M, N>
{
type Item = T;

Expand Down
4 changes: 4 additions & 0 deletions src/lib.rs
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,10 @@ pub use num::*;
mod dsm;
pub mod svf;
pub use dsm::*;
mod sweptsine;
pub use sweptsine::*;
mod cic;
pub use cic::*;

#[cfg(test)]
pub mod testing;
Loading
Loading