diff --git a/CHANGELOG.md b/CHANGELOG.md index 5635d7b..d61023d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,9 +11,13 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 * `iir::Filter`: use non-inverse Q * `iir::Pid` renamed to `PidBuilder` -* `iir::PidBuilder` feedback order is not guessed but explicit * `iir::Pid` added with miniconf support +* `iir::PidBuilder` feedback order is not guessed but explicit + +### Added + * `iir::BiquadRepr` enum to convert from different representations to quantized biquad +* `cordic` `i32` reference CORDIC implementation, all modes ## [0.17.0](https://github.com/quartiq/idsp/compare/v0.16.0..v0.17.0) - 2025-01-20 diff --git a/Cargo.toml b/Cargo.toml index da1df73..2556b2c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -2,7 +2,7 @@ name = "idsp" version = "0.17.0" resolver = "2" -edition = "2021" +edition = "2024" authors = ["Robert Jördens "] license = "MIT OR Apache-2.0" description = "DSP algorithms for embedded, mostly integer math" @@ -29,6 +29,7 @@ rand = "0.9.1" rustfft = "6.1.0" # futuredsp = "0.0.6" # sdr = "0.7.0" +log = "0.4" [features] std = [] diff --git a/build.rs b/build.rs index bd51ff7..cf1fe0c 100644 --- a/build.rs +++ b/build.rs @@ -37,10 +37,62 @@ fn write_cossin_table() { write!(file, " {},", cos + (sin << 16)).unwrap(); } writeln!(file, "\n];").unwrap(); +} - println!("cargo:rerun-if-changed=build.rs"); +fn write_cordic_tables() { + const DEPTH: i32 = 30; + + let out_dir = env::var_os("OUT_DIR").unwrap(); + let dest_path = Path::new(&out_dir).join("cordic_tables.rs"); + let mut file = File::create(dest_path).unwrap(); + + const Q31: f64 = (1i64 << 31) as _; + writeln!( + file, + "/// Gain of cordic in circular mode.\npub const CORDIC_CIRCULAR_GAIN: f64 = {};", + (0..DEPTH).fold(1.0, |f, i| f * (1.0 + 0.25f64.powi(i)).sqrt()) + ) + .unwrap(); + writeln!( + file, + "pub(crate) const CORDIC_CIRCULAR: [i32; {DEPTH}] = {:?};", + (0..DEPTH) + .map(|i| (0.5f64.powi(i).atan() / PI * Q31).round() as i64 as _) + .collect::>() + ) + .unwrap(); + + let mut f = 1.0f64; + let mut k = 4; + for i in 1..DEPTH { + let r = if i == k { + k = 3 * i + 1; + 2 + } else { + 1 + }; + for _ in 0..r { + f *= (1.0 - 0.25f64.powi(i)).sqrt(); + } + } + writeln!( + file, + "/// Gain of cordic in hyperbolic mode.\npub const CORDIC_HYPERBOLIC_GAIN: f64 = {};", + f + ) + .unwrap(); + writeln!( + file, + "pub(crate) const CORDIC_HYPERBOLIC: [i32; {DEPTH}] = {:?};", + (0..DEPTH) + .map(|i| (0.5f64.powi(i + 1).atanh() * Q31).round() as i64 as _) + .collect::>() + ) + .unwrap(); } fn main() { write_cossin_table(); + write_cordic_tables(); + println!("cargo:rerun-if-changed=build.rs"); } diff --git a/src/cordic.rs b/src/cordic.rs new file mode 100644 index 0000000..0b0864e --- /dev/null +++ b/src/cordic.rs @@ -0,0 +1,287 @@ +// https://www.st.com/resource/en/design_tip/dt0085-coordinate-rotation-digital-computer-algorithm-cordic-to-compute-trigonometric-and-hyperbolic-functions-stmicroelectronics.pdf + +include!(concat!(env!("OUT_DIR"), "/cordic_tables.rs")); + +const ROTATE: bool = false; +const DEROTATE: bool = true; +const CIRCULAR: u8 = 0; +const HYPERBOLIC: u8 = 1; +const LINEAR: u8 = 2; + +/// Generic CORDIC +#[inline] +fn cordic( + mut x: i32, + mut y: i32, + mut z: i32, + iter: Option, +) -> (i32, i32) { + // Microrotation table + let a = match COORD { + CIRCULAR => &CORDIC_CIRCULAR, + _ => &CORDIC_HYPERBOLIC, + }; + // MSB + let left = if VECTORING { + x < 0 + } else { + z.wrapping_sub(i32::MIN >> 1) < 0 + }; + if left { + x = -x; + y = -y; + z = z.wrapping_sub(i32::MIN); + } + // Hyperbolic repetition marker + let mut k = 4; + for (mut i, &(mut a)) in a[..iter.unwrap_or(a.len())].iter().enumerate() { + // For linear mode the microrotations are computed, not looked up + if COORD == LINEAR { + a = (i32::MIN as u32 >> i) as _; + } + // Hyperbolic starts at i = 1 + if COORD == HYPERBOLIC { + i += 1; + } + // Hyperbolic repeats some rotations for convergence + let repeat = if COORD == HYPERBOLIC && i == k { + k = 3 * i + 1; + 2 + } else { + 1 + }; + for _ in 0..repeat { + // "sigma" + let lower = if VECTORING { y <= 0 } else { z >= 0 }; + let (dx, dy) = (y >> i, x >> i); + if lower { + if COORD == CIRCULAR { + x -= dx; + } else if COORD == HYPERBOLIC { + x += dx; + } + y += dy; + z = z.wrapping_sub(a); + } else { + if COORD == CIRCULAR { + x += dx; + } else if COORD == HYPERBOLIC { + x -= dx; + } + y -= dy; + z = z.wrapping_add(a); + } + } + } + (x, if VECTORING { z } else { y }) +} + +/// Returns `F*(x*cos(z*PI) - y*sin(z*PI)), F*(x*sin(z*PI) + y*cos(z*PI))` +pub fn cos_sin(x: i32, y: i32, z: i32) -> (i32, i32) { + cordic::(x, y, z, None) +} + +/// Returns `F*sqrt(x**2 + y**2), z + atan2(y, x)/PI` +pub fn sqrt_atan2(x: i32, y: i32, z: i32) -> (i32, i32) { + cordic::(x, y, z, None) +} + +/// Returns `x, y + x*z` +pub fn mul(x: i32, y: i32, z: i32) -> i32 { + cordic::(x, y, z, None).1 +} + +/// Returns `x, z + y/x` +pub fn div(x: i32, y: i32, z: i32) -> i32 { + cordic::(x, y, z, None).1 +} + +/// Returns `G*(x*cosh(z) + y*sinh(z)), G*(x*sinh(z) + y*cosh(z))` +pub fn cosh_sinh(x: i32, y: i32, z: i32) -> (i32, i32) { + cordic::(x, y, z, None) +} + +/// Returns `G*sqrt(x**2 - y**2), z + atanh2(y, x)` +pub fn sqrt_atanh2(x: i32, y: i32, z: i32) -> (i32, i32) { + cordic::(x, y, z, None) +} + +#[cfg(test)] +mod test { + use core::f64::consts::PI; + use log; + use quickcheck_macros::quickcheck; + use rand::{prelude::*, rngs::StdRng}; + + use super::*; + + const Q31: f64 = (1i64 << 31) as _; + + fn f2i(x: f64) -> i32 { + (x * Q31).round() as i64 as _ + } + fn i2f(x: i32) -> f64 { + (x as f64 / Q31) as _ + } + + const F: f64 = CORDIC_CIRCULAR_GAIN.recip(); + const G: f64 = CORDIC_HYPERBOLIC_GAIN.recip(); + + fn cos_sin_err(x: f64, y: f64, z: f64) -> f64 { + let xy = cos_sin(f2i(x * F), f2i(y * F), f2i(z)); + let xy = (i2f(xy.0), i2f(xy.1)); + let (s, c) = (z * PI).sin_cos(); + let xy0 = (c * x - s * y, s * x + c * y); + let (dx, dy) = (xy.0 - xy0.0, xy.1 - xy0.1); + let dr = (dx.powi(2) + dy.powi(2)).sqrt(); + let da = i2f(f2i(xy.1.atan2(xy.0) / PI - y.atan2(x) / PI - z)); + log::debug!("{dx:.10},{dy:.10} ~ {dr:.10}@{da:.10}"); + dr * Q31 + } + + fn sqrt_atan2_err(x: f64, y: f64) -> f64 { + let (r, z) = sqrt_atan2(f2i(x * F), f2i(y * F), 0); + let (r, z) = (i2f(r), i2f(z)); + let r0 = (x.powi(2) + y.powi(2)).sqrt(); + let z0 = y.atan2(x) / PI; + let da = i2f(f2i(z - z0)); + let dr = ((r - r0).powi(2) + ((da * PI).sin() * r0).powi(2)).sqrt(); + log::debug!("{dr:.10}@{da:.10}"); + dr * Q31 + } + + fn sqrt_atanh2_err(x: f64, y: f64) -> f64 { + let (r, z) = sqrt_atanh2(f2i(x * G), f2i(y * G), 0); + let (r, z) = (i2f(r), i2f(z)); + let r0 = (x.powi(2) - y.powi(2)).sqrt(); + let z0 = (y / x).atanh(); + let da = i2f(f2i(z - z0)); + let dr = (r - r0).abs(); + log::debug!("{dr:.10}@{da:.10}"); + dr * Q31 + } + + #[test] + fn basic_rot() { + assert!((CORDIC_CIRCULAR_GAIN - 1.64676025812107).abs() < 1e-14,); + + cos_sin_err(0.50, 0.2, 0.123); + cos_sin_err(0.01, 0.0, -0.35); + cos_sin_err(0.605, 0.0, 0.35); + cos_sin_err(-0.3, 0.4, 0.55); + cos_sin_err(-0.3, -0.4, -0.55); + cos_sin_err(-0.3, -0.4, 0.8); + cos_sin_err(-0.3, -0.4, -0.8); + } + + fn test_values(n: usize) -> Vec { + let mut rng = StdRng::seed_from_u64(42); + let mut n: Vec<_> = core::iter::from_fn(|| Some(rng.random())).take(n).collect(); + n.extend([ + 0, + 1, + -1, + 0xf, + -0xf, + 0x55555555, + -0x55555555, + 0x5aaaaaaa, + -0x5aaaaaaa, + 0x7fffffff, + -0x7fffffff, + 1 << 29, + -1 << 29, + 1 << 30, + -1 << 30, + i32::MIN, + i32::MAX, + ]); + n + } + + #[test] + fn meanmax_rot() { + let n = test_values(50); + let mut mean = 0.0; + let mut max: f64 = 0.0; + for x in n.iter() { + for y in n.iter() { + for z in n.iter() { + let (x, y) = (i2f(*x), i2f(*y)); + if 1.0 - x.powi(2) - y.powi(2) <= 1e-9 { + continue; + } + let dr = cos_sin_err(x, y, i2f(*z)); + mean += dr; + max = max.max(dr); + } + } + } + mean /= n.len().pow(3) as f64; + log::info!("{mean} {max}"); + assert!(mean < 5.0); + assert!(max < 24.0); + } + + #[test] + fn meanmax_vect() { + let n = test_values(300); + let mut mean = 0.0; + let mut max: f64 = 0.0; + for x in n.iter() { + for y in n.iter() { + let (x, y) = (i2f(*x), i2f(*y)); + if 1.0 - x.powi(2) - y.powi(2) <= 1e-9 { + continue; + } + let dr = sqrt_atan2_err(x, y); + mean += dr; + max = max.max(dr); + } + } + mean /= n.len().pow(2) as f64; + log::info!("{mean} {max}"); + assert!(mean < 8.0); + assert!(max < 30.0); + } + + #[quickcheck] + fn check_rot(x: i32, y: i32, z: i32) -> bool { + let (x, y) = (i2f(x), i2f(y)); + if CORDIC_CIRCULAR_GAIN.powi(2) * (x.powi(2) + y.powi(2)) >= 1.0 { + return true; + } + cos_sin_err(x, y, i2f(z)) <= 22.0 + } + + #[quickcheck] + fn check_vect(x: i32, y: i32) -> bool { + let (x, y) = (i2f(x), i2f(y)); + if CORDIC_CIRCULAR_GAIN.powi(2) * (x.powi(2) + y.powi(2)) >= 1.0 { + return true; + } + sqrt_atan2_err(x, y) <= 29.0 + } + + #[quickcheck] + fn check_hyp_vect(x: i32, y: i32) -> bool { + let (x, y) = (i2f(x), i2f(y)); + if CORDIC_HYPERBOLIC_GAIN.powi(2) * (x.powi(2) - y.powi(2)) >= 1.0 { + return true; + } + if y.abs() > x.abs() || (y / x).atanh() >= 1.1 { + return true; + } + + sqrt_atanh2_err(x, y); + true + } + + #[test] + fn test_atanh() { + sqrt_atanh2_err(0.8, 0.1); + sqrt_atanh2_err(0.8, 0.1); + sqrt_atanh2_err(0.8, 0.1); + sqrt_atanh2_err(0.99, 0.0); + } +} diff --git a/src/hbf.rs b/src/hbf.rs index ecb8118..3df462c 100644 --- a/src/hbf.rs +++ b/src/hbf.rs @@ -678,7 +678,7 @@ impl Filter for HbfIntCascade { #[cfg(test)] mod test { use super::*; - use rustfft::{num_complex::Complex, FftPlanner}; + use rustfft::{FftPlanner, num_complex::Complex}; #[test] fn test() { diff --git a/src/iir/pid.rs b/src/iir/pid.rs index 519e7d9..06a090a 100644 --- a/src/iir/pid.rs +++ b/src/iir/pid.rs @@ -2,7 +2,7 @@ use miniconf::{Leaf, Tree}; use num_traits::{AsPrimitive, Float}; use serde::{Deserialize, Serialize}; -use crate::{iir::Biquad, Coefficient}; +use crate::{Coefficient, iir::Biquad}; /// Feedback term order #[derive(Clone, Debug, Copy, Serialize, Deserialize, Default, PartialEq, PartialOrd)] diff --git a/src/iir/repr.rs b/src/iir/repr.rs index 4d42c73..67e1f3e 100644 --- a/src/iir/repr.rs +++ b/src/iir/repr.rs @@ -3,8 +3,8 @@ use num_traits::{AsPrimitive, Float, FloatConst}; use serde::{Deserialize, Serialize}; use crate::{ - iir::{Biquad, Pid, Shape}, Coefficient, + iir::{Biquad, Pid, Shape}, }; /// Floating point BA coefficients before quantization diff --git a/src/lib.rs b/src/lib.rs index ce97191..79b9067 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -36,6 +36,8 @@ mod sweptsine; pub use sweptsine::*; mod cic; pub use cic::*; +mod cordic; +pub use cordic::*; #[cfg(test)] pub mod testing; diff --git a/src/rpll.rs b/src/rpll.rs index da7f0dd..7203bbc 100644 --- a/src/rpll.rs +++ b/src/rpll.rs @@ -140,7 +140,7 @@ mod test { assert!(self.time - self.next_noisy < 1 << self.rpll.dt2); self.next = self.next.wrapping_add(self.period); let timestamp = self.next_noisy; - let p_noise = self.rng.gen_range(-self.noise..=self.noise); + let p_noise = self.rng.random_range(-self.noise..=self.noise); self.next_noisy = self.next.wrapping_add(p_noise); Some(timestamp) } else { diff --git a/src/sweptsine.rs b/src/sweptsine.rs index d2aaee6..30d630e 100644 --- a/src/sweptsine.rs +++ b/src/sweptsine.rs @@ -1,7 +1,7 @@ use core::iter::FusedIterator; -use crate::{cossin::cossin, Complex}; -use num_traits::{real::Real, FloatConst}; +use crate::{Complex, cossin::cossin}; +use num_traits::{FloatConst, real::Real}; const Q: f32 = (1i64 << 32) as f32; @@ -190,8 +190,10 @@ mod test { assert_eq!(sweep.state(), sweep.continuous(0.0) * sweep.rate()); // Start/stop as desired assert!((stop * 0.99..=1.01 * stop).contains(&(sweep.state() as f32 * harmonics))); - assert!((stop * 0.99..=1.01 * stop) - .contains(&((sweep.continuous(len as _) * sweep.rate()) as f32))); + assert!( + (stop * 0.99..=1.01 * stop) + .contains(&((sweep.continuous(len as _) * sweep.rate()) as f32)) + ); // Zero crossings and wraps // Note inclusion of 0 for h in 0..harmonics as i32 { diff --git a/src/unwrap.rs b/src/unwrap.rs index e1f66a3..fea040b 100644 --- a/src/unwrap.rs +++ b/src/unwrap.rs @@ -3,10 +3,10 @@ use core::{ ops::{BitAnd, Shr}, }; use num_traits::{ + Signed, cast::AsPrimitive, identities::Zero, ops::wrapping::{WrappingAdd, WrappingSub}, - Signed, }; use serde::{Deserialize, Serialize};