init

.gitignore

@@ -0,0 +1,2 @@
+/target
+Cargo.lock

Cargo.toml

@@ -0,0 +1,28 @@
+[package]
+name = "idsp"
+version = "0.1.0"
+resolver = "2"
+authors = ["Robert Jördens <[email protected]>"]
+edition = "2018"
+license = "MIT OR Apache-2.0"
+description = "DSP algorithms for embedded, mostly integer math"
+homepage = "https://github.com/quartiq/idsp"
+repository = "https://github.com/quartiq/idsp.git"
+documentation = "https://docs.rs/idsp"
+
+[dependencies]
+serde = { version = "1.0", features = ["derive"], default-features = false }
+num-complex = { version = "0.4.0", features = ["serde"], default-features = false }
+miniconf = { version = "0.1", optional = false }
+
+[dev-dependencies]
+easybench = "1.0"
+rand = "0.8"
+ndarray = "0.15"
+
+[[bench]]
+name = "micro"
+harness = false
+
+[features]
+nightly = []

README.md

@@ -0,0 +1,16 @@
+# Embedded DSP algorithms
+
+[![GitHub release](https://img.shields.io/github/v/release/quartiq/idsp?include_prereleases)](https://github.com/quartiq/idsp/releases)
+[![Documentation](https://img.shields.io/badge/docs-online-success)](https://docs.rs/idsp)
+[![QUARTIQ Matrix Chat](https://img.shields.io/matrix/quartiq:matrix.org)](https://matrix.to/#/#quartiq:matrix.org)
+[![Continuous Integration](https://github.com/quartiq/idsp/actions/workflows/ci.yml/badge.svg)](https://github.com/quartiq/idsp/actions/workflows/ci.yml)
+
+This crate contains some tuned DSP algorithms for general and especially embedded use.
+Many of the algorithms are implemented on integer datatypes for several reasons that become important in certain cases:
+
+* Speed: even with a hard FP unit integer operations are faster.
+* Accuracy: single precision FP has a 24 bit mantissa, `i32` has full 32 bit.
+* No rounding errors.
+* Natural wrap around at the integer overflow: critical for phase/frequency applications (`cossin`, `Unwrapper`, `atan2`, `PLL`, `RPLL` etc.)
+
+The prime user for these algorithms is [Stabilizer](https://github.com/quartiq/stabilizer).

benches/micro.rs

@@ -0,0 +1,93 @@
+use core::f32::consts::PI;
+
+use easybench::bench_env;
+
+use idsp::{atan2, cossin, iir, iir_int, Lowpass, PLL, RPLL};
+
+fn atan2_bench() {
+    let xi = (10 << 16) as i32;
+    let xf = xi as f32 / i32::MAX as f32;
+
+    let yi = (-26_328 << 16) as i32;
+    let yf = yi as f32 / i32::MAX as f32;
+
+    println!(
+        "atan2(yi, xi): {}",
+        bench_env((yi, xi), |(yi, xi)| atan2(*yi, *xi))
+    );
+    println!(
+        "yf.atan2(xf): {}",
+        bench_env((yf, xf), |(yf, xf)| yf.atan2(*xf))
+    );
+}
+
+fn cossin_bench() {
+    let zi = -0x7304_2531_i32;
+    let zf = zi as f32 / i32::MAX as f32 * PI;
+    println!("cossin(zi): {}", bench_env(zi, |zi| cossin(*zi)));
+    println!("zf.sin_cos(): {}", bench_env(zf, |zf| zf.sin_cos()));
+}
+
+fn rpll_bench() {
+    let mut dut = RPLL::new(8);
+    println!(
+        "RPLL::update(Some(t), 21, 20): {}",
+        bench_env(Some(0x241), |x| dut.update(*x, 21, 20))
+    );
+    println!(
+        "RPLL::update(Some(t), sf, sp): {}",
+        bench_env((Some(0x241), 21, 20), |(x, p, q)| dut.update(*x, *p, *q))
+    );
+}
+
+fn pll_bench() {
+    let mut dut = PLL::default();
+    println!(
+        "PLL::update(t, 12, 12): {}",
+        bench_env(Some(0x241), |x| dut.update(*x, 12, 12))
+    );
+    println!(
+        "PLL::update(t, sf, sp): {}",
+        bench_env((Some(0x241), 21, 20), |(x, p, q)| dut.update(*x, *p, *q))
+    );
+}
+
+fn iir_int_bench() {
+    let dut = iir_int::IIR::default();
+    let mut xy = iir_int::Vec5::default();
+    println!(
+        "int_iir::IIR::update(s, x): {}",
+        bench_env(0x2832, |x| dut.update(&mut xy, *x))
+    );
+}
+
+fn iir_bench() {
+    let dut = iir::IIR::default();
+    let mut xy = iir::Vec5::default();
+    println!(
+        "int::IIR::update(s, x): {}",
+        bench_env(0.32241, |x| dut.update(&mut xy, *x, true))
+    );
+}
+
+fn lowpass_bench() {
+    let mut dut = Lowpass::<4>::default();
+    println!(
+        "Lowpass::<4>::update(x, k): {}",
+        bench_env((0x32421, 14), |(x, k)| dut.update(*x, *k))
+    );
+    println!(
+        "Lowpass::<4>::update(x, 14): {}",
+        bench_env(0x32421, |x| dut.update(*x, 14))
+    );
+}
+
+fn main() {
+    atan2_bench();
+    cossin_bench();
+    rpll_bench();
+    pll_bench();
+    iir_int_bench();
+    iir_bench();
+    lowpass_bench();
+}

build.rs

@@ -0,0 +1,48 @@
+use std::env;
+use std::f64::consts::PI;
+use std::fs::File;
+use std::io::prelude::*;
+use std::path::Path;
+
+fn write_cossin_table() {
+    const DEPTH: usize = 7;
+
+    let out_dir = env::var_os("OUT_DIR").unwrap();
+    let dest_path = Path::new(&out_dir).join("cossin_table.rs");
+    let mut file = File::create(dest_path).unwrap();
+
+    writeln!(file, "pub(crate) const COSSIN_DEPTH: usize = {};", DEPTH)
+        .unwrap();
+    write!(
+        file,
+        "pub(crate) const COSSIN: [u32; 1 << COSSIN_DEPTH] = ["
+    )
+    .unwrap();
+
+    // Treat sin and cos as unsigned values since the sign will always be
+    // positive in the range [0, pi/4).
+    // No headroom for interpolation rounding error (this is needed for
+    // DEPTH = 6 for example).
+    const AMPLITUDE: f64 = u16::MAX as f64;
+
+    for i in 0..(1 << DEPTH) {
+        if i % 4 == 0 {
+            write!(file, "\n   ").unwrap();
+        }
+        // Use midpoint samples to save one entry in the LUT
+        let (sin, cos) =
+            (PI / 4. * ((i as f64 + 0.5) / (1 << DEPTH) as f64)).sin_cos();
+        // Add one bit accuracy to cos due to 0.5 < cos(z) <= 1 for |z| < pi/4
+        // The -1 LSB is cancelled when unscaling with the biased half amplitude
+        let cos = ((cos * 2. - 1.) * AMPLITUDE - 1.).round() as u32;
+        let sin = (sin * AMPLITUDE).round() as u32;
+        write!(file, " {},", cos + (sin << 16)).unwrap();
+    }
+    writeln!(file, "\n];").unwrap();
+
+    println!("cargo:rerun-if-changed=build.rs");
+}
+
+fn main() {
+    write_cossin_table();
+}

src/accu.rs

@@ -0,0 +1,20 @@
+#[derive(Copy, Clone, Default, PartialEq, Debug)]
+pub struct Accu {
+    state: i32,
+    step: i32,
+}
+
+impl Accu {
+    pub fn new(state: i32, step: i32) -> Self {
+        Self { state, step }
+    }
+}
+
+impl Iterator for Accu {
+    type Item = i32;
+    fn next(&mut self) -> Option<i32> {
+        let s = self.state;
+        self.state = self.state.wrapping_add(self.step);
+        Some(s)
+    }
+}

src/atan2.rs

@@ -0,0 +1,137 @@
+/// 2-argument arctangent function.
+///
+/// This implementation uses all integer arithmetic for fast
+/// computation. It is designed to have high accuracy near the axes
+/// and lower away from the axes. It is additionally designed so that
+/// the error changes slowly with respect to the angle.
+///
+/// # Arguments
+///
+/// * `y` - Y-axis component.
+/// * `x` - X-axis component.
+///
+/// # Returns
+///
+/// The angle between the x-axis and the ray to the point (x,y). The
+/// result range is from i32::MIN to i32::MAX, where i32::MIN
+/// represents -pi and, equivalently, +pi. i32::MAX represents one
+/// count less than +pi.
+pub fn atan2(y: i32, x: i32) -> i32 {
+    let sign = (x < 0, y < 0);
+
+    let mut y = y.wrapping_abs() as u32;
+    let mut x = x.wrapping_abs() as u32;
+
+    let y_greater = y > x;
+    if y_greater {
+        core::mem::swap(&mut y, &mut x);
+    }
+
+    let z = (16 - y.leading_zeros() as i32).max(0);
+
+    x >>= z;
+    if x == 0 {
+        return 0;
+    }
+    y >>= z;
+    let r = (y << 16) / x;
+    debug_assert!(r <= 1 << 16);
+
+    // Uses the general procedure described in the following
+    // Mathematics stack exchange answer:
+    //
+    // https://math.stackexchange.com/a/1105038/583981
+    //
+    // The atan approximation method has been modified to be cheaper
+    // to compute and to be more compatible with integer
+    // arithmetic. The approximation technique used here is
+    //
+    // pi / 4 * r + C * r * (1 - abs(r))
+    //
+    // which is taken from Rajan 2006: Efficient Approximations for
+    // the Arctangent Function.
+    //
+    // The least mean squared error solution is C = 0.279 (no the 0.285 that
+    // Rajan uses). K = C*4/pi.
+    // Q5 for K provides sufficient correction accuracy while preserving
+    // as much smoothness of the quadratic correction as possible.
+    const FP_K: usize = 5;
+    const K: u32 = (0.35489 * (1 << FP_K) as f64) as u32;
+    // debug_assert!(K == 11);
+
+    // `r` is unsigned Q16.16 and <= 1
+    // `angle` is signed Q1.31 with 1 << 31 == +- pi
+    // Since K < 0.5 and r*(1 - r) <= 0.25 the correction product can use
+    // 4 bits for K, and 15 bits for r and 1-r to remain within the u32 range.
+    let mut angle = ((r << 13)
+        + ((K * (r >> 1) * ((1 << 15) - (r >> 1))) >> (FP_K + 1)))
+        as i32;
+
+    if y_greater {
+        angle = (1 << 30) - angle;
+    }
+
+    if sign.0 {
+        angle = i32::MAX - angle;
+    }
+
+    if sign.1 {
+        angle = angle.wrapping_neg();
+        // Negation ends up in slightly faster assembly
+        // angle = !angle;
+    }
+
+    angle
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use core::f64::consts::PI;
+
+    fn angle_to_axis(angle: f64) -> f64 {
+        let angle = angle % (PI / 2.);
+        (PI / 2. - angle).min(angle)
+    }
+
+    #[test]
+    fn atan2_absolute_error() {
+        const N: usize = 321;
+        let mut test_vals = [0i32; N + 4];
+        let scale = (1i64 << 31) as f64;
+        for i in 0..N {
+            test_vals[i] = (scale * (-1. + 2. * i as f64 / N as f64)) as i32;
+        }
+
+        assert!(test_vals.contains(&i32::MIN));
+        test_vals[N] = i32::MAX;
+        test_vals[N + 1] = 0;
+        test_vals[N + 2] = -1;
+        test_vals[N + 3] = 1;
+
+        let mut rms_err = 0f64;
+        let mut abs_err = 0f64;
+        let mut rel_err = 0f64;
+
+        for &x in test_vals.iter() {
+            for &y in test_vals.iter() {
+                let want = (y as f64 / scale).atan2(x as f64 / scale);
+                let have = atan2(y, x) as f64 * PI / scale;
+
+                let err = (have - want).abs();
+                abs_err = abs_err.max(err);
+                rms_err += err * err;
+                if err > 3e-5 {
+                    rel_err = rel_err.max(err / angle_to_axis(want));
+                }
+            }
+        }
+        rms_err = rms_err.sqrt() / test_vals.len() as f64;
+        println!("max abs err: {:.2e}", abs_err);
+        println!("rms abs err: {:.2e}", rms_err);
+        println!("max rel err: {:.2e}", rel_err);
+        assert!(abs_err < 5e-3);
+        assert!(rms_err < 3e-3);
+        assert!(rel_err < 0.6);
+    }
+}