From 4add34cf9ab6f64cf865420dc8a7dfe396238075 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Mon, 7 Dec 2020 10:22:09 -0800 Subject: [PATCH 01/48] add cossin LUT --- .gitignore | 1 + dsp/build.rs | 43 ++++++++++++++++ dsp/src/lib.rs | 17 +++++++ dsp/src/trig.rs | 128 ++++++++++++++++++++++++++++++++++++++++++++++++ 4 files changed, 189 insertions(+) create mode 100644 dsp/build.rs create mode 100644 dsp/src/trig.rs diff --git a/.gitignore b/.gitignore index 68b644a..241d97c 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ /target /dsp/target .gdb_history +/dsp/src/cossin_table.txt diff --git a/dsp/build.rs b/dsp/build.rs new file mode 100644 index 0000000..2e6fbfe --- /dev/null +++ b/dsp/build.rs @@ -0,0 +1,43 @@ +use std::fs::File; +use std::path::Path; +use std::io::prelude::*; +use std::f64::consts::PI; + +const TABLE_DEPTH: usize = 8; +const TABLE_SIZE: usize = 1 << TABLE_DEPTH; +// Treat sin and cos as unsigned values since the sign will always be +// positive in the range [0, pi/4). +const SINCOS_MAX: f64 = u16::MAX as f64; + +fn main() { + let path = Path::new("src").join("cossin_table.txt"); + let display = path.display(); + + let mut file = match File::create(&path) { + Err(why) => panic!("failed to write to {}: {}", display, why), + Ok(file) => file, + }; + + match file.write_all("[\n".as_bytes()) { + Err(why) => panic!("failed to write to {}: {}", display, why), + Ok(_) => (), + } + + let phase_delta = PI / 4. / TABLE_SIZE as f64; + let phase_offset = phase_delta / 2.; + for i in 0..TABLE_SIZE { + let phase = phase_offset + phase_delta * (i as f64); + let cos = ((phase.cos() - 0.5) * 2. * SINCOS_MAX).round() as u16; + let sin = (phase.sin() * SINCOS_MAX).round() as u16; + let s = format!(" ({}, {}),\n", cos, sin); + match file.write_all(s.as_bytes()) { + Err(why) => panic!("failed to write to {}: {}", display, why), + Ok(_) => (), + } + } + + match file.write_all("]\n".as_bytes()) { + Err(why) => panic!("failed to write to {}: {}", display, why), + Ok(_) => (), + } +} diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 10cfcaa..b7bfeeb 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -2,9 +2,26 @@ #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] pub type Complex = (T, T); + +/// Round up half. +/// +/// # Arguments +/// +/// `x` - Value to shift and round. +/// `shift` - Number of bits to right shift `x`. +/// +/// # Returns +/// +/// Shifted and rounded value. +pub fn shift_round(x: i32, shift: i32) -> i32 { + (x + (1 << (shift - 1))) >> shift +} + + pub mod iir; pub mod lockin; pub mod pll; +pub mod trig; pub mod unwrap; #[cfg(test)] diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs new file mode 100644 index 0000000..e8f170d --- /dev/null +++ b/dsp/src/trig.rs @@ -0,0 +1,128 @@ +use core::mem::swap; +use super::{Complex, shift_round}; + +const PHASE_BITS: i32 = 20; +const LUT_DEPTH: i32 = 8; +const LUT_SIZE: usize = 1 << LUT_DEPTH as usize; +const OCTANT_BITS: i32 = 3; +const INTERPOLATION_BITS: i32 = PHASE_BITS - LUT_DEPTH - OCTANT_BITS; +static COSSIN_TABLE: [(u16, u16); LUT_SIZE] = include!("cossin_table.txt"); + +// Approximate pi/4 with an integer multiplier and right bit +// shift. The numerator is designed to saturate the i32 range. +const PI_4_NUMERATOR: i32 = 50; +const PI_4_RIGHT_SHIFT: i32 = 6; + +/// Compute the cosine and sine of an angle. +/// +/// # Arguments +/// +/// `phase` - 20-bit fixed-point phase value. +/// +/// # Returns +/// +/// The cos and sin values of the provided phase as a `Complex` +/// value. +pub fn cossin(phase: i32) -> Complex { + let mut phase = phase; + let octant = ( + (phase & (1 << (PHASE_BITS - 1))) >> (PHASE_BITS - 1), + (phase & (1 << (PHASE_BITS - 2))) >> (PHASE_BITS - 2), + (phase & (1 << (PHASE_BITS - 3))) >> (PHASE_BITS - 3), + ); + + // Mask off octant bits. This leaves the angle in the range [0, + // pi/4). + phase &= (1 << (PHASE_BITS - OCTANT_BITS)) - 1; + + if octant.2 == 1 { + // phase = pi/4 - phase + phase = (1 << (INTERPOLATION_BITS + LUT_DEPTH)) - 1 - phase; + } + + let interpolation: i32 = phase & ((1 << INTERPOLATION_BITS) - 1); + + phase >>= INTERPOLATION_BITS; + + let (mut cos, mut sin) = { + let lookup = COSSIN_TABLE[phase as usize]; + ( + // 1/2 < cos(0<=x<=pi/4) <= 1. So, to spread out the cos + // values and use the space more efficiently, we can + // subtract 1/2 and multiply by 2. Therefore, we add 1 + // back in here. The sin values must be multiplied by 2 to + // have the same scale as the cos values. + lookup.0 as i32 + u16::MAX as i32, + (lookup.1 as i32) << 1, + ) + }; + + // The phase values used for the LUT are adjusted up by half the + // phase step. The interpolation must accurately reflect this. So, + // an interpolation phase offset less than half the maximum + // involves a negative phase offset. The rest us a non-negative + // phase offset. + let interpolation_factor = + (interpolation - (1 << (INTERPOLATION_BITS - 1))) * PI_4_NUMERATOR; + let dsin = shift_round( + cos * interpolation_factor, + LUT_DEPTH + INTERPOLATION_BITS + PI_4_RIGHT_SHIFT, + ); + let dcos = shift_round( + -sin * interpolation_factor, + LUT_DEPTH + INTERPOLATION_BITS + PI_4_RIGHT_SHIFT, + ); + + cos += dcos; + sin += dsin; + + if octant.1 ^ octant.2 == 1 { + swap(&mut sin, &mut cos); + } + + if octant.0 ^ octant.1 == 1 { + cos *= -1; + } + + if octant.0 == 1 { + sin *= -1; + } + + (cos, sin) +} + +#[cfg(test)] +mod tests { + use super::*; + use core::f64::consts::PI; + + #[test] + fn error_max_rms_all_phase() { + let max_amplitude: f64 = ((1 << 15) - 1) as f64; + let mut rms_err: Complex = (0., 0.); + + for i in 0..(1 << PHASE_BITS) { + let phase = i as i32; + let radian_phase: f64 = + 2. * PI * (phase as f64 + 0.5) / ((1 << PHASE_BITS) as f64); + + let actual: Complex = ( + max_amplitude * radian_phase.cos(), + max_amplitude * radian_phase.sin(), + ); + let computed = cossin(phase); + + let err = ( + computed.0 as f64 / 4. - actual.0, + computed.1 as f64 / 4. - actual.1, + ); + rms_err.0 += err.0 * err.0 / (1 << PHASE_BITS) as f64; + rms_err.1 += err.1 * err.1 / (1 << PHASE_BITS) as f64; + + assert!(err.0.abs() < 0.89); + assert!(err.1.abs() < 0.89); + } + assert!(rms_err.0.sqrt() < 0.41); + assert!(rms_err.1.sqrt() < 0.41); + } +} From a82b0f3e908a53d1f53794368fa8ea9e16f5e2bb Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 9 Dec 2020 15:53:56 -0800 Subject: [PATCH 02/48] trig: fix formatting --- dsp/build.rs | 6 +++--- dsp/src/lib.rs | 1 - dsp/src/trig.rs | 2 +- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/dsp/build.rs b/dsp/build.rs index 2e6fbfe..1b60363 100644 --- a/dsp/build.rs +++ b/dsp/build.rs @@ -1,7 +1,7 @@ -use std::fs::File; -use std::path::Path; -use std::io::prelude::*; use std::f64::consts::PI; +use std::fs::File; +use std::io::prelude::*; +use std::path::Path; const TABLE_DEPTH: usize = 8; const TABLE_SIZE: usize = 1 << TABLE_DEPTH; diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index b7bfeeb..798b0fc 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -17,7 +17,6 @@ pub fn shift_round(x: i32, shift: i32) -> i32 { (x + (1 << (shift - 1))) >> shift } - pub mod iir; pub mod lockin; pub mod pll; diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index e8f170d..3c7eb2b 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -1,5 +1,5 @@ +use super::{shift_round, Complex}; use core::mem::swap; -use super::{Complex, shift_round}; const PHASE_BITS: i32 = 20; const LUT_DEPTH: i32 = 8; From afef1b0c26ea31693908f0a27a6908fecad4f846 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 10 Dec 2020 04:04:37 +0000 Subject: [PATCH 03/48] build(deps): bump paste from 1.0.3 to 1.0.4 Bumps [paste](https://github.com/dtolnay/paste) from 1.0.3 to 1.0.4. - [Release notes](https://github.com/dtolnay/paste/releases) - [Commits](https://github.com/dtolnay/paste/compare/1.0.3...1.0.4) Signed-off-by: dependabot[bot] --- Cargo.lock | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 9cf3a58..d9cb7d0 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -360,9 +360,9 @@ dependencies = [ [[package]] name = "paste" -version = "1.0.3" +version = "1.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7151b083b0664ed58ed669fcdd92f01c3d2fdbf10af4931a301474950b52bfa9" +checksum = "c5d65c4d95931acda4498f675e332fcbdc9a06705cd07086c510e9b6009cd1c1" [[package]] name = "proc-macro2" From 77cb0bbad0743868a80ab79c6d47eb34bff80053 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 10 Dec 2020 16:56:13 +0100 Subject: [PATCH 04/48] cossin: refactor and tweak * shrink the LUT by another bit * correctly use the octant bit to offset the dphi to LUT entry midpoint * add more diagnistics to the unittest and rewrite it in relative units * MSB-align phase and output to match the PLL data, dynamic range and remove the need for roudning bias. * clean up the build.rs table generator a bit --- .gitignore | 2 +- dsp/build.rs | 68 +++++++++-------- dsp/src/lib.rs | 4 +- dsp/src/trig.rs | 192 ++++++++++++++++++++++++++---------------------- 4 files changed, 142 insertions(+), 124 deletions(-) diff --git a/.gitignore b/.gitignore index 241d97c..1a29db4 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ /target /dsp/target .gdb_history -/dsp/src/cossin_table.txt +/dsp/src/cossin_table.rs diff --git a/dsp/build.rs b/dsp/build.rs index 1b60363..07a4c2f 100644 --- a/dsp/build.rs +++ b/dsp/build.rs @@ -3,41 +3,39 @@ use std::fs::File; use std::io::prelude::*; use std::path::Path; -const TABLE_DEPTH: usize = 8; -const TABLE_SIZE: usize = 1 << TABLE_DEPTH; -// Treat sin and cos as unsigned values since the sign will always be -// positive in the range [0, pi/4). -const SINCOS_MAX: f64 = u16::MAX as f64; +fn write_cossin_table() { + const DEPTH: usize = 7; + + let mut file = + File::create(Path::new("src").join("cossin_table.rs")).unwrap(); + writeln!(file, "pub(crate) const COSSIN_DEPTH: usize = {};", DEPTH) + .unwrap(); + write!( + file, + "pub(crate) const COSSIN: [(u16, u16); 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) { + // use midpoint samples to save one entry in the LUT + let phase = (PI / 4. / (1 << DEPTH) as f64) * (i as f64 + 0.5); + // add one bit accuracy to cos due to 0.5 < cos(z) <= 1 for |z| < pi/4 + let cos = ((phase.cos() - 0.5) * 2. * AMPLITUDE).round() as u16; + let sin = (phase.sin() * AMPLITUDE).round() as u16; + if i % 4 == 0 { + write!(file, "\n ").unwrap(); + } + write!(file, " ({}, {}),", cos, sin).unwrap(); + } + writeln!(file, "\n];").unwrap(); +} fn main() { - let path = Path::new("src").join("cossin_table.txt"); - let display = path.display(); - - let mut file = match File::create(&path) { - Err(why) => panic!("failed to write to {}: {}", display, why), - Ok(file) => file, - }; - - match file.write_all("[\n".as_bytes()) { - Err(why) => panic!("failed to write to {}: {}", display, why), - Ok(_) => (), - } - - let phase_delta = PI / 4. / TABLE_SIZE as f64; - let phase_offset = phase_delta / 2.; - for i in 0..TABLE_SIZE { - let phase = phase_offset + phase_delta * (i as f64); - let cos = ((phase.cos() - 0.5) * 2. * SINCOS_MAX).round() as u16; - let sin = (phase.sin() * SINCOS_MAX).round() as u16; - let s = format!(" ({}, {}),\n", cos, sin); - match file.write_all(s.as_bytes()) { - Err(why) => panic!("failed to write to {}: {}", display, why), - Ok(_) => (), - } - } - - match file.write_all("]\n".as_bytes()) { - Err(why) => panic!("failed to write to {}: {}", display, why), - Ok(_) => (), - } + write_cossin_table(); } diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 798b0fc..98bd661 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -13,10 +13,12 @@ pub type Complex = (T, T); /// # Returns /// /// Shifted and rounded value. -pub fn shift_round(x: i32, shift: i32) -> i32 { +#[inline(always)] +pub fn shift_round(x: i32, shift: usize) -> i32 { (x + (1 << (shift - 1))) >> shift } +mod cossin_table; pub mod iir; pub mod lockin; pub mod pll; diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index 3c7eb2b..cdded60 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -1,90 +1,72 @@ -use super::{shift_round, Complex}; -use core::mem::swap; - -const PHASE_BITS: i32 = 20; -const LUT_DEPTH: i32 = 8; -const LUT_SIZE: usize = 1 << LUT_DEPTH as usize; -const OCTANT_BITS: i32 = 3; -const INTERPOLATION_BITS: i32 = PHASE_BITS - LUT_DEPTH - OCTANT_BITS; -static COSSIN_TABLE: [(u16, u16); LUT_SIZE] = include!("cossin_table.txt"); - -// Approximate pi/4 with an integer multiplier and right bit -// shift. The numerator is designed to saturate the i32 range. -const PI_4_NUMERATOR: i32 = 50; -const PI_4_RIGHT_SHIFT: i32 = 6; +use super::{ + cossin_table::{COSSIN, COSSIN_DEPTH}, + Complex, +}; +use core::f64::consts::PI; /// Compute the cosine and sine of an angle. +/// This is ported from the MiSoC cossin core. +/// (https://github.com/m-labs/misoc/blob/master/misoc/cores/cossin.py) /// /// # Arguments -/// -/// `phase` - 20-bit fixed-point phase value. +/// * `phase` - 32-bit phase. /// /// # Returns -/// /// The cos and sin values of the provided phase as a `Complex` -/// value. +/// value. With a 7-bit deep LUT there is 1e-5 max and 6e-8 RMS error +/// in each quadrature over 20 bit phase. pub fn cossin(phase: i32) -> Complex { - let mut phase = phase; - let octant = ( - (phase & (1 << (PHASE_BITS - 1))) >> (PHASE_BITS - 1), - (phase & (1 << (PHASE_BITS - 2))) >> (PHASE_BITS - 2), - (phase & (1 << (PHASE_BITS - 3))) >> (PHASE_BITS - 3), - ); + // Phase bits excluding the three highes MSB + const OCTANT_BITS: usize = 32 - 3; - // Mask off octant bits. This leaves the angle in the range [0, - // pi/4). - phase &= (1 << (PHASE_BITS - OCTANT_BITS)) - 1; + // This is a slightly more compact way to compute the four flags for + // octant mapping/unmapping used below. + let mut octant = (phase as u32) >> OCTANT_BITS; + octant ^= octant << 1; - if octant.2 == 1 { + // Mask off octant bits. This leaves the angle in the range [0, pi/4). + let mut phase = phase & ((1 << OCTANT_BITS) - 1); + + if octant & 1 != 0 { // phase = pi/4 - phase - phase = (1 << (INTERPOLATION_BITS + LUT_DEPTH)) - 1 - phase; + phase = (1 << OCTANT_BITS) - 1 - phase; } - let interpolation: i32 = phase & ((1 << INTERPOLATION_BITS) - 1); + let lookup = COSSIN[(phase >> (OCTANT_BITS - COSSIN_DEPTH)) as usize]; + // 1/2 < cos(0 <= x <= pi/4) <= 1: Shift the cos + // values and scale the sine values as encoded in the LUT. + let mut cos = lookup.0 as i32 + u16::MAX as i32; + let mut sin = (lookup.1 as i32) << 1; - phase >>= INTERPOLATION_BITS; + // 16 + 1 bits for cos/sin and 15 for dphi to saturate the i32 range. + const ALIGN_MSB: usize = 32 - 16 - 1; + phase >>= OCTANT_BITS - COSSIN_DEPTH - ALIGN_MSB; + phase &= (1 << ALIGN_MSB) - 1; + // The phase values used for the LUT are at midpoint for the truncated phase. + // Interpolate relative to the LUT entry midpoint. + phase -= (1 << (ALIGN_MSB - 1)) - (octant & 1) as i32; + // Fixed point pi/4. + const PI4: i32 = (PI / 4. * (1 << (32 - ALIGN_MSB)) as f64) as i32; + // No rounding bias necessary here since we keep enough low bits. + let dphi = (phase * PI4) >> (32 - ALIGN_MSB); - let (mut cos, mut sin) = { - let lookup = COSSIN_TABLE[phase as usize]; - ( - // 1/2 < cos(0<=x<=pi/4) <= 1. So, to spread out the cos - // values and use the space more efficiently, we can - // subtract 1/2 and multiply by 2. Therefore, we add 1 - // back in here. The sin values must be multiplied by 2 to - // have the same scale as the cos values. - lookup.0 as i32 + u16::MAX as i32, - (lookup.1 as i32) << 1, - ) - }; + // Make room for the sign bit. + let dcos = (sin * dphi) >> (COSSIN_DEPTH + 1); + let dsin = (cos * dphi) >> (COSSIN_DEPTH + 1); - // The phase values used for the LUT are adjusted up by half the - // phase step. The interpolation must accurately reflect this. So, - // an interpolation phase offset less than half the maximum - // involves a negative phase offset. The rest us a non-negative - // phase offset. - let interpolation_factor = - (interpolation - (1 << (INTERPOLATION_BITS - 1))) * PI_4_NUMERATOR; - let dsin = shift_round( - cos * interpolation_factor, - LUT_DEPTH + INTERPOLATION_BITS + PI_4_RIGHT_SHIFT, - ); - let dcos = shift_round( - -sin * interpolation_factor, - LUT_DEPTH + INTERPOLATION_BITS + PI_4_RIGHT_SHIFT, - ); + cos = (cos << (ALIGN_MSB - 1)) - dcos; + sin = (sin << (ALIGN_MSB - 1)) + dsin; - cos += dcos; - sin += dsin; - - if octant.1 ^ octant.2 == 1 { - swap(&mut sin, &mut cos); + // Unmap using octant bits. + if octant & 2 != 0 { + core::mem::swap(&mut sin, &mut cos); } - if octant.0 ^ octant.1 == 1 { + if octant & 4 != 0 { cos *= -1; } - if octant.0 == 1 { + if octant & 8 != 0 { sin *= -1; } @@ -94,35 +76,71 @@ pub fn cossin(phase: i32) -> Complex { #[cfg(test)] mod tests { use super::*; - use core::f64::consts::PI; - #[test] fn error_max_rms_all_phase() { - let max_amplitude: f64 = ((1 << 15) - 1) as f64; + // Constant amplitude error due to LUT data range. + const AMPLITUDE: f64 = ((1i64 << 31) - (1i64 << 15)) as f64; + const MAX_PHASE: f64 = (1i64 << 32) as f64; let mut rms_err: Complex = (0., 0.); + let mut sum_err: Complex = (0., 0.); + let mut max_err: Complex = (0., 0.); + let mut sum: Complex = (0., 0.); + let mut demod: Complex = (0., 0.); - for i in 0..(1 << PHASE_BITS) { - let phase = i as i32; - let radian_phase: f64 = - 2. * PI * (phase as f64 + 0.5) / ((1 << PHASE_BITS) as f64); + // use std::{fs::File, io::prelude::*, path::Path}; + // let mut file = File::create(Path::new("data.csv")).unwrap(); - let actual: Complex = ( - max_amplitude * radian_phase.cos(), - max_amplitude * radian_phase.sin(), - ); - let computed = cossin(phase); + const PHASE_DEPTH: usize = 20; - let err = ( - computed.0 as f64 / 4. - actual.0, - computed.1 as f64 / 4. - actual.1, - ); - rms_err.0 += err.0 * err.0 / (1 << PHASE_BITS) as f64; - rms_err.1 += err.1 * err.1 / (1 << PHASE_BITS) as f64; + for phase in 0..(1 << PHASE_DEPTH) { + let phase = (phase << (32 - PHASE_DEPTH)) as i32; + let have = cossin(phase); + // writeln!(file, " {},{}", have.0, have.1).unwrap(); - assert!(err.0.abs() < 0.89); - assert!(err.1.abs() < 0.89); + let have = (have.0 as f64 / AMPLITUDE, have.1 as f64 / AMPLITUDE); + + let radian_phase = 2. * PI * phase as f64 / MAX_PHASE; + let want = (radian_phase.cos(), radian_phase.sin()); + + sum.0 += have.0; + sum.1 += have.1; + + demod.0 += have.0 * want.0 - have.1 * want.1; + demod.1 += have.1 * want.0 + have.0 * want.1; + + let err = (have.0 - want.0, have.1 - want.1); + + sum_err.0 += err.0; + sum_err.1 += err.1; + + rms_err.0 += err.0 * err.0; + rms_err.1 += err.1 * err.1; + + max_err.0 = max_err.0.max(err.0.abs()); + max_err.1 = max_err.1.max(err.1.abs()); } - assert!(rms_err.0.sqrt() < 0.41); - assert!(rms_err.1.sqrt() < 0.41); + rms_err.0 /= MAX_PHASE; + rms_err.1 /= MAX_PHASE; + + println!("sum: {:.2e} {:.2e}", sum.0, sum.1); + println!("demod: {:.2e} {:.2e}", demod.0, demod.1); + println!("sum_err: {:.2e} {:.2e}", sum_err.0, sum_err.1); + println!("rms: {:.2e} {:.2e}", rms_err.0.sqrt(), rms_err.1.sqrt()); + println!("max: {:.2e} {:.2e}", max_err.0, max_err.1); + + assert!(sum.0.abs() < 4e-10); + assert!(sum.1.abs() < 4e-10); + + assert!(demod.0.abs() < 4e-10); + assert!(demod.1.abs() < 4e-10); + + assert!(sum_err.0.abs() < 4e-10); + assert!(sum_err.1.abs() < 4e-10); + + assert!(rms_err.0.sqrt() < 6e-8); + assert!(rms_err.1.sqrt() < 6e-8); + + assert!(max_err.0 < 1.1e-5); + assert!(max_err.1 < 1.1e-5); } } From 7fa4b76e4d05a3a8d0474196248808006cb17e5a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 10 Dec 2020 17:17:09 +0100 Subject: [PATCH 05/48] cossin_table: fix build script usage --- dsp/build.rs | 7 +++++-- dsp/src/lib.rs | 1 - dsp/src/trig.rs | 7 +++---- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/dsp/build.rs b/dsp/build.rs index 07a4c2f..e8c9f99 100644 --- a/dsp/build.rs +++ b/dsp/build.rs @@ -1,3 +1,4 @@ +use std::env; use std::f64::consts::PI; use std::fs::File; use std::io::prelude::*; @@ -6,8 +7,10 @@ use std::path::Path; fn write_cossin_table() { const DEPTH: usize = 7; - let mut file = - File::create(Path::new("src").join("cossin_table.rs")).unwrap(); + 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!( diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 98bd661..6dd20f7 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -18,7 +18,6 @@ pub fn shift_round(x: i32, shift: usize) -> i32 { (x + (1 << (shift - 1))) >> shift } -mod cossin_table; pub mod iir; pub mod lockin; pub mod pll; diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index cdded60..ecef4d4 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -1,9 +1,8 @@ -use super::{ - cossin_table::{COSSIN, COSSIN_DEPTH}, - Complex, -}; +use super::Complex; use core::f64::consts::PI; +include!(concat!(env!("OUT_DIR"), "/cossin_table.rs")); + /// Compute the cosine and sine of an angle. /// This is ported from the MiSoC cossin core. /// (https://github.com/m-labs/misoc/blob/master/misoc/cores/cossin.py) From b9751ae1d0e969c65c8872e0631746daabb9f5d6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 10 Dec 2020 17:17:59 +0100 Subject: [PATCH 06/48] dsp/build.rs: add cargo tag --- dsp/build.rs | 2 ++ 1 file changed, 2 insertions(+) diff --git a/dsp/build.rs b/dsp/build.rs index e8c9f99..ca43736 100644 --- a/dsp/build.rs +++ b/dsp/build.rs @@ -37,6 +37,8 @@ fn write_cossin_table() { write!(file, " ({}, {}),", cos, sin).unwrap(); } writeln!(file, "\n];").unwrap(); + + println!("cargo:rerun-if-changed=build.rs"); } fn main() { From 72b7e72040067f06ae53acff52d6482bfec461a7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 10 Dec 2020 17:49:46 +0100 Subject: [PATCH 07/48] gitignore: no need to ignore table, is in OUT_DIR now --- .gitignore | 1 - 1 file changed, 1 deletion(-) diff --git a/.gitignore b/.gitignore index 1a29db4..68b644a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,3 @@ /target /dsp/target .gdb_history -/dsp/src/cossin_table.rs From de304c503b6990b9fa17b7f596b6081794efb78f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 10 Dec 2020 17:53:45 +0100 Subject: [PATCH 08/48] cargo: go back to target-cpu=cortex-m4 Appears to give better code. Test by matthusagh. --- .cargo/config | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.cargo/config b/.cargo/config index ea1d6c0..deffdfc 100644 --- a/.cargo/config +++ b/.cargo/config @@ -5,7 +5,7 @@ rustflags = [ # The target (below) defaults to cortex-m4 # There currently are two different options to go beyond that: # 1. cortex-m7 has the right flags and instructions (FPU) but no instruction schedule yet - "-C", "target-cpu=cortex-m7", +# "-C", "target-cpu=cortex-m7", # 2. cortex-m4 with the additional fpv5 instructions and a potentially # better-than-nothing instruction schedule "-C", "target-feature=+fp-armv8d16", From a85738a651a8158fbeeccd721420c689819b82fa Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 11 Dec 2020 15:19:13 +0100 Subject: [PATCH 09/48] dsp: add host benchmark --- .github/workflows/ci.yml | 31 ++- .gitignore | 2 - Cargo.lock | 490 ++++++++++++++++++++++++++++++++++++++- Cargo.toml | 3 + dsp/Cargo.lock | 70 ------ dsp/Cargo.toml | 7 + dsp/benches/cossin.rs | 11 + 7 files changed, 523 insertions(+), 91 deletions(-) delete mode 100644 dsp/Cargo.lock create mode 100644 dsp/benches/cossin.rs diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 6c7b0e9..932774a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -18,25 +18,16 @@ jobs: with: profile: minimal toolchain: nightly + target: thumbv7em-none-eabihf override: true - components: rustfmt + components: + - rustfmt + - clippy - name: cargo fmt --check uses: actions-rs/cargo@v1 with: command: fmt args: --all -- --check - - clippy: - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v2 - - uses: actions-rs/toolchain@v1 - with: - profile: minimal - toolchain: nightly - target: thumbv7em-none-eabihf - override: true - components: clippy - uses: actions-rs/clippy-check@v1 continue-on-error: true with: @@ -71,11 +62,6 @@ jobs: with: command: build args: --release - - name: cargo build release+semihosting - uses: actions-rs/cargo@v1 - with: - command: build - args: --release --features semihosting test: runs-on: ubuntu-latest @@ -95,6 +81,15 @@ jobs: with: command: test args: --package dsp --target=x86_64-unknown-linux-gnu + - name: cargo bench + uses: actions-rs/cargo@v1 + with: + command: bench + args: --package dsp --target=x86_64-unknown-linux-gnu + - uses: actions/upload-artifact@v1 + with: + name: stabilizer_${{ github.sha }}_criterion + path: ./target/criterion # Tell bors about it # https://github.com/rtic-rs/cortex-m-rtic/blob/8a4f9c6b8ae91bebeea0791680f89375a78bffc6/.github/workflows/build.yml#L566-L603 diff --git a/.gitignore b/.gitignore index 68b644a..ea8c4bf 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1 @@ /target -/dsp/target -.gdb_history diff --git a/Cargo.lock b/Cargo.lock index d9cb7d0..e35ab18 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -40,6 +40,17 @@ dependencies = [ "embedded-hal", ] +[[package]] +name = "atty" +version = "0.2.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d9b39be18770d11421cdb1b9947a45dd3f37e93092cbf377614828a319d5fee8" +dependencies = [ + "hermit-abi", + "libc", + "winapi", +] + [[package]] name = "autocfg" version = "1.0.1" @@ -85,6 +96,24 @@ version = "0.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c147d86912d04bef727828fda769a76ca81629a46d8ba311a8d58a26aa91473d" +[[package]] +name = "bstr" +version = "0.2.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "473fc6b38233f9af7baa94fb5852dca389e3d95b8e21c8e3719301462c5d9faf" +dependencies = [ + "lazy_static", + "memchr", + "regex-automata", + "serde", +] + +[[package]] +name = "bumpalo" +version = "3.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2e8c087f005730276d1096a652e92a8bacee2e2472bcc9715a74d2bec38b5820" + [[package]] name = "byteorder" version = "1.3.4" @@ -106,6 +135,29 @@ version = "0.1.10" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4785bdd1c96b2a846b2bd7cc02e86b6b3dbf14e7e53446c4f54c92a361040822" +[[package]] +name = "cfg-if" +version = "1.0.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd" + +[[package]] +name = "clap" +version = "2.33.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "37e58ac78573c40708d45522f0d80fa2f01cc4f9b4e2bf749807255454312002" +dependencies = [ + "bitflags", + "textwrap", + "unicode-width", +] + +[[package]] +name = "const_fn" +version = "0.4.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cd51eab21ab4fd6a3bf889e2d0958c0a6e3a61ad04260325e919e652a2a62826" + [[package]] name = "cortex-m" version = "0.6.4" @@ -185,14 +237,125 @@ dependencies = [ "cortex-m", ] +[[package]] +name = "criterion" +version = "0.3.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "70daa7ceec6cf143990669a04c7df13391d55fb27bd4079d252fca774ba244d8" +dependencies = [ + "atty", + "cast", + "clap", + "criterion-plot", + "csv", + "itertools", + "lazy_static", + "num-traits", + "oorandom", + "plotters", + "rayon", + "regex", + "serde", + "serde_cbor", + "serde_derive", + "serde_json", + "tinytemplate", + "walkdir", +] + +[[package]] +name = "criterion-plot" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e022feadec601fba1649cfa83586381a4ad31c6bf3a9ab7d408118b05dd9889d" +dependencies = [ + "cast", + "itertools", +] + +[[package]] +name = "crossbeam-channel" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dca26ee1f8d361640700bde38b2c37d8c22b3ce2d360e1fc1c74ea4b0aa7d775" +dependencies = [ + "cfg-if 1.0.0", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-deque" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "94af6efb46fef72616855b036a624cf27ba656ffc9be1b9a3c931cfc7749a9a9" +dependencies = [ + "cfg-if 1.0.0", + "crossbeam-epoch", + "crossbeam-utils", +] + +[[package]] +name = "crossbeam-epoch" +version = "0.9.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1aaa739f95311c2c7887a76863f500026092fb1dce0161dab577e559ef3569d" +dependencies = [ + "cfg-if 1.0.0", + "const_fn", + "crossbeam-utils", + "lazy_static", + "memoffset", + "scopeguard", +] + +[[package]] +name = "crossbeam-utils" +version = "0.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "02d96d1e189ef58269ebe5b97953da3274d83a93af647c2ddd6f9dab28cedb8d" +dependencies = [ + "autocfg", + "cfg-if 1.0.0", + "lazy_static", +] + +[[package]] +name = "csv" +version = "1.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f9d58633299b24b515ac72a3f869f8b91306a3cec616a602843a383acd6f9e97" +dependencies = [ + "bstr", + "csv-core", + "itoa", + "ryu", + "serde", +] + +[[package]] +name = "csv-core" +version = "0.1.10" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2b2466559f260f48ad25fe6317b3c8dac77b5bdb5763ac7d9d6103530663bc90" +dependencies = [ + "memchr", +] + [[package]] name = "dsp" version = "0.1.0" dependencies = [ + "criterion", "libm", "serde", ] +[[package]] +name = "either" +version = "1.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e78d4f1cc4ae33bbfc157ed5d5a5ef3bc29227303d595861deb238fcec4e9457" + [[package]] name = "embedded-dma" version = "0.1.2" @@ -260,6 +423,12 @@ dependencies = [ "version_check", ] +[[package]] +name = "half" +version = "1.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d36fab90f82edc3c747f9d438e06cf0a491055896f2a279638bb5beed6c40177" + [[package]] name = "hash32" version = "0.1.1" @@ -288,6 +457,15 @@ dependencies = [ "stable_deref_trait", ] +[[package]] +name = "hermit-abi" +version = "0.1.17" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5aca5565f760fb5b220e499d72710ed156fdb74e631659e99377d9ebfbd13ae8" +dependencies = [ + "libc", +] + [[package]] name = "indexmap" version = "1.6.0" @@ -298,6 +476,42 @@ dependencies = [ "hashbrown", ] +[[package]] +name = "itertools" +version = "0.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "284f18f85651fe11e8a991b2adb42cb078325c996ed026d994719efcfca1d54b" +dependencies = [ + "either", +] + +[[package]] +name = "itoa" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "dc6f3ad7b9d11a0c00842ff8de1b60ee58661048eb8049ed33c73594f359d7e6" + +[[package]] +name = "js-sys" +version = "0.3.46" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cf3d7383929f7c9c7c2d0fa596f325832df98c3704f2c60553080f7127a58175" +dependencies = [ + "wasm-bindgen", +] + +[[package]] +name = "lazy_static" +version = "1.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" + +[[package]] +name = "libc" +version = "0.2.81" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1482821306169ec4d07f6aca392a4681f66c75c9918aa49641a2595db64053cb" + [[package]] name = "libm" version = "0.2.1" @@ -310,7 +524,7 @@ version = "0.4.11" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "4fabed175da42fed1fa0746b0ea71f412aa9d35e76e95e59b192c64b9dc2bf8b" dependencies = [ - "cfg-if", + "cfg-if 0.1.10", ] [[package]] @@ -327,6 +541,21 @@ dependencies = [ "embedded-hal", ] +[[package]] +name = "memchr" +version = "2.3.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ee1c47aaa256ecabcaea351eae4a9b01ef39ed810004e298d2511ed284b1525" + +[[package]] +name = "memoffset" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "157b4208e3059a8f9e78d559edc658e13df41410cb3ae03979c83130067fdd87" +dependencies = [ + "autocfg", +] + [[package]] name = "nb" version = "0.1.3" @@ -342,6 +571,31 @@ version = "1.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "546c37ac5d9e56f55e73b677106873d9d9f5190605e41a856503623648488cae" +[[package]] +name = "num-traits" +version = "0.2.14" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9a64b1ec5cda2586e284722486d802acf1f7dbdc623e2bfc57e65ca1cd099290" +dependencies = [ + "autocfg", +] + +[[package]] +name = "num_cpus" +version = "1.13.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "05499f3756671c15885fee9034446956fff3f243d6077b91e5767df161f766b3" +dependencies = [ + "hermit-abi", + "libc", +] + +[[package]] +name = "oorandom" +version = "11.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0ab1bc2a289d34bd04a330323ac98a1b4bc82c9d9fcb1e66b63caa84da26b575" + [[package]] name = "panic-halt" version = "0.2.0" @@ -364,6 +618,18 @@ version = "1.0.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "c5d65c4d95931acda4498f675e332fcbdc9a06705cd07086c510e9b6009cd1c1" +[[package]] +name = "plotters" +version = "0.2.15" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0d1685fbe7beba33de0330629da9d955ac75bd54f33d7b79f9a895590124f6bb" +dependencies = [ + "js-sys", + "num-traits", + "wasm-bindgen", + "web-sys", +] + [[package]] name = "proc-macro2" version = "1.0.24" @@ -388,6 +654,55 @@ version = "0.2.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e2a38df5b15c8d5c7e8654189744d8e396bddc18ad48041a500ce52d6948941f" +[[package]] +name = "rayon" +version = "1.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "8b0d8e0819fadc20c74ea8373106ead0600e3a67ef1fe8da56e39b9ae7275674" +dependencies = [ + "autocfg", + "crossbeam-deque", + "either", + "rayon-core", +] + +[[package]] +name = "rayon-core" +version = "1.9.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9ab346ac5921dc62ffa9f89b7a773907511cdfa5490c572ae9be1be33e8afa4a" +dependencies = [ + "crossbeam-channel", + "crossbeam-deque", + "crossbeam-utils", + "lazy_static", + "num_cpus", +] + +[[package]] +name = "regex" +version = "1.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "38cf2c13ed4745de91a5eb834e11c00bcc3709e773173b2ce4c56c9fbde04b9c" +dependencies = [ + "regex-syntax", +] + +[[package]] +name = "regex-automata" +version = "0.1.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae1ded71d66a4a97f5e961fd0cb25a5f366a42a41570d16a763a69c092c26ae4" +dependencies = [ + "byteorder", +] + +[[package]] +name = "regex-syntax" +version = "0.6.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3b181ba2dcf07aaccad5448e8ead58db5b742cf85dfe035e2227f137a539a189" + [[package]] name = "rtic-core" version = "0.3.1" @@ -414,6 +729,27 @@ dependencies = [ "semver", ] +[[package]] +name = "ryu" +version = "1.0.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "71d301d4193d031abdd79ff7e3dd721168a9572ef3fe51a1517aba235bd8f86e" + +[[package]] +name = "same-file" +version = "1.0.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "93fc1dc3aaa9bfed95e02e6eadabb4baf7e3078b0bd1b4d7b6b0b68378900502" +dependencies = [ + "winapi-util", +] + +[[package]] +name = "scopeguard" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d29ab0c6d3fc0ee92fe66e2d99f700eab17a8d57d1c1d3b748380fb20baa78cd" + [[package]] name = "semver" version = "0.9.0" @@ -448,6 +784,16 @@ dependencies = [ "serde", ] +[[package]] +name = "serde_cbor" +version = "0.11.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e18acfa2f90e8b735b2836ab8d538de304cbb6729a7360729ea5a895d15a622" +dependencies = [ + "half", + "serde", +] + [[package]] name = "serde_derive" version = "1.0.118" @@ -459,6 +805,17 @@ dependencies = [ "syn", ] +[[package]] +name = "serde_json" +version = "1.0.60" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1500e84d27fe482ed1dc791a56eddc2f230046a040fa908c08bda1d9fb615779" +dependencies = [ + "itoa", + "ryu", + "serde", +] + [[package]] name = "smoltcp" version = "0.6.0" @@ -543,12 +900,37 @@ dependencies = [ "unicode-xid", ] +[[package]] +name = "textwrap" +version = "0.11.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d326610f408c7a4eb6f51c37c330e496b08506c9457c9d34287ecc38809fb060" +dependencies = [ + "unicode-width", +] + +[[package]] +name = "tinytemplate" +version = "1.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6d3dc76004a03cec1c5932bca4cdc2e39aaa798e3f82363dd94f9adf6098c12f" +dependencies = [ + "serde", + "serde_json", +] + [[package]] name = "typenum" version = "1.12.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "373c8a200f9e67a0c95e62a4f52fbf80c23b4381c05a17845531982fa99e6b33" +[[package]] +name = "unicode-width" +version = "0.1.8" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "9337591893a19b88d8d87f2cec1e73fad5cdfd10e5a6f349f498ad6ea2ffb1e3" + [[package]] name = "unicode-xid" version = "0.2.1" @@ -581,3 +963,109 @@ checksum = "0d67cb4616d99b940db1d6bd28844ff97108b498a6ca850e5b6191a532063286" dependencies = [ "vcell", ] + +[[package]] +name = "walkdir" +version = "2.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "777182bc735b6424e1a57516d35ed72cb8019d85c8c9bf536dccb3445c1a2f7d" +dependencies = [ + "same-file", + "winapi", + "winapi-util", +] + +[[package]] +name = "wasm-bindgen" +version = "0.2.69" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "3cd364751395ca0f68cafb17666eee36b63077fb5ecd972bbcd74c90c4bf736e" +dependencies = [ + "cfg-if 1.0.0", + "wasm-bindgen-macro", +] + +[[package]] +name = "wasm-bindgen-backend" +version = "0.2.69" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1114f89ab1f4106e5b55e688b828c0ab0ea593a1ea7c094b141b14cbaaec2d62" +dependencies = [ + "bumpalo", + "lazy_static", + "log", + "proc-macro2", + "quote", + "syn", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-macro" +version = "0.2.69" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7a6ac8995ead1f084a8dea1e65f194d0973800c7f571f6edd70adf06ecf77084" +dependencies = [ + "quote", + "wasm-bindgen-macro-support", +] + +[[package]] +name = "wasm-bindgen-macro-support" +version = "0.2.69" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b5a48c72f299d80557c7c62e37e7225369ecc0c963964059509fbafe917c7549" +dependencies = [ + "proc-macro2", + "quote", + "syn", + "wasm-bindgen-backend", + "wasm-bindgen-shared", +] + +[[package]] +name = "wasm-bindgen-shared" +version = "0.2.69" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7e7811dd7f9398f14cc76efd356f98f03aa30419dea46aa810d71e819fc97158" + +[[package]] +name = "web-sys" +version = "0.3.46" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "222b1ef9334f92a21d3fb53dc3fd80f30836959a90f9274a626d7e06315ba3c3" +dependencies = [ + "js-sys", + "wasm-bindgen", +] + +[[package]] +name = "winapi" +version = "0.3.9" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5c839a674fcd7a98952e593242ea400abe93992746761e38641405d28b00f419" +dependencies = [ + "winapi-i686-pc-windows-gnu", + "winapi-x86_64-pc-windows-gnu", +] + +[[package]] +name = "winapi-i686-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ac3b87c63620426dd9b991e5ce0329eff545bccbbb34f3be09ff6fb6ab51b7b6" + +[[package]] +name = "winapi-util" +version = "0.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "70ec6ce85bb158151cae5e5c87f95a8e97d2c0c4b001223f33a334e3ce5de178" +dependencies = [ + "winapi", +] + +[[package]] +name = "winapi-x86_64-pc-windows-gnu" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "712e227841d057c1ee1cd2fb22fa7e5a5461ae8e48fa2ca79ec42cfc1931183f" diff --git a/Cargo.toml b/Cargo.toml index 7217589..9dfa28a 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -24,6 +24,9 @@ maintenance = { status = "experimental" } features = [] default-target = "thumbv7em-none-eabihf" +[workspace] +members = ["ad9959", "dsp"] + [dependencies] cortex-m = { version = "0.6", features = ["const-fn"] } cortex-m-rt = { version = "0.6", features = ["device"] } diff --git a/dsp/Cargo.lock b/dsp/Cargo.lock deleted file mode 100644 index cda08c3..0000000 --- a/dsp/Cargo.lock +++ /dev/null @@ -1,70 +0,0 @@ -# This file is automatically @generated by Cargo. -# It is not intended for manual editing. -[[package]] -name = "dsp" -version = "0.1.0" -dependencies = [ - "libm", - "serde", -] - -[[package]] -name = "libm" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c7d73b3f436185384286bd8098d17ec07c9a7d2388a6599f824d8502b529702a" - -[[package]] -name = "proc-macro2" -version = "1.0.24" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1e0704ee1a7e00d7bb417d0770ea303c1bccbabf0ef1667dae92b5967f5f8a71" -dependencies = [ - "unicode-xid", -] - -[[package]] -name = "quote" -version = "1.0.7" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "aa563d17ecb180e500da1cfd2b028310ac758de548efdd203e18f283af693f37" -dependencies = [ - "proc-macro2", -] - -[[package]] -name = "serde" -version = "1.0.117" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b88fa983de7720629c9387e9f517353ed404164b1e482c970a90c1a4aaf7dc1a" -dependencies = [ - "serde_derive", -] - -[[package]] -name = "serde_derive" -version = "1.0.117" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cbd1ae72adb44aab48f325a02444a5fc079349a8d804c1fc922aed3f7454c74e" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - -[[package]] -name = "syn" -version = "1.0.50" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "443b4178719c5a851e1bde36ce12da21d74a0e60b4d982ec3385a933c812f0f6" -dependencies = [ - "proc-macro2", - "quote", - "unicode-xid", -] - -[[package]] -name = "unicode-xid" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f7fe0bb3479651439c9112f72b6c505038574c9fbb575ed1bf3b797fa39dd564" diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml index 2d0c15c..8313a49 100644 --- a/dsp/Cargo.toml +++ b/dsp/Cargo.toml @@ -8,5 +8,12 @@ edition = "2018" libm = "0.2.1" serde = { version = "1.0", features = ["derive"], default-features = false } +[dev-dependencies] +criterion = "0.3" + +[[bench]] +name = "cossin" +harness = false + [features] nightly = [] diff --git a/dsp/benches/cossin.rs b/dsp/benches/cossin.rs new file mode 100644 index 0000000..8168843 --- /dev/null +++ b/dsp/benches/cossin.rs @@ -0,0 +1,11 @@ +use criterion::{black_box, criterion_group, criterion_main, Criterion}; +use dsp::trig::cossin; + +fn cossin_bench(c: &mut Criterion) { + c.bench_function("cossin(0)", |b| { + b.iter(|| cossin(black_box(0))) + }); +} + +criterion_group!(benches, cossin_bench); +criterion_main!(benches); From 4fbd729cb41903fa3e9c29a5c9991276faab9db6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 11 Dec 2020 17:04:34 +0100 Subject: [PATCH 10/48] gha: fix toolchain components --- .github/workflows/ci.yml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 932774a..73781e5 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -20,9 +20,7 @@ jobs: toolchain: nightly target: thumbv7em-none-eabihf override: true - components: - - rustfmt - - clippy + components: rustfmt, clippy - name: cargo fmt --check uses: actions-rs/cargo@v1 with: From a709ab171e37a29d34d57935a7ddce2eefba7266 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 11 Dec 2020 17:06:07 +0100 Subject: [PATCH 11/48] gha: fix bors --- .github/workflows/ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 73781e5..5028da0 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -96,8 +96,8 @@ jobs: if: github.event_name == 'push' && success() needs: - style - - clippy - compile + - test runs-on: ubuntu-latest steps: - name: Mark the job as a success @@ -107,8 +107,8 @@ jobs: if: github.event_name == 'push' && !success() needs: - style - - clippy - compile + - test runs-on: ubuntu-latest steps: - name: Mark the job as a failure From 5cd93d3318c7524b76c8b1b30b2a1c9d85d31e2e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 11 Dec 2020 17:08:16 +0100 Subject: [PATCH 12/48] fmt --- dsp/benches/cossin.rs | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/dsp/benches/cossin.rs b/dsp/benches/cossin.rs index 8168843..f082c7b 100644 --- a/dsp/benches/cossin.rs +++ b/dsp/benches/cossin.rs @@ -2,9 +2,7 @@ use criterion::{black_box, criterion_group, criterion_main, Criterion}; use dsp::trig::cossin; fn cossin_bench(c: &mut Criterion) { - c.bench_function("cossin(0)", |b| { - b.iter(|| cossin(black_box(0))) - }); + c.bench_function("cossin(0)", |b| b.iter(|| cossin(black_box(0)))); } criterion_group!(benches, cossin_bench); From d4fceea5d186db34075286447448deba9f10f987 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 11 Dec 2020 17:26:50 +0100 Subject: [PATCH 13/48] cossin: bench against (i32 as f32).sin_cos() --- dsp/benches/cossin.rs | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/dsp/benches/cossin.rs b/dsp/benches/cossin.rs index f082c7b..cbd0499 100644 --- a/dsp/benches/cossin.rs +++ b/dsp/benches/cossin.rs @@ -2,7 +2,11 @@ use criterion::{black_box, criterion_group, criterion_main, Criterion}; use dsp::trig::cossin; fn cossin_bench(c: &mut Criterion) { - c.bench_function("cossin(0)", |b| b.iter(|| cossin(black_box(0)))); + let z = -0x7304_2531_i32; + c.bench_function("cossin(z)", |b| b.iter(|| cossin(black_box(z)))); + c.bench_function("(z as f32).sin_cos()", |b| { + b.iter(|| (black_box(z) as f32).sin_cos()) + }); } criterion_group!(benches, cossin_bench); From a70110d8cc4d7218ea1a3533ec86e14bd3f81357 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 11 Dec 2020 17:27:02 +0100 Subject: [PATCH 14/48] gha: fix artifact --- .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 5028da0..d20cd26 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -87,7 +87,7 @@ jobs: - uses: actions/upload-artifact@v1 with: name: stabilizer_${{ github.sha }}_criterion - path: ./target/criterion + path: ./dsp/target/criterion # Tell bors about it # https://github.com/rtic-rs/cortex-m-rtic/blob/8a4f9c6b8ae91bebeea0791680f89375a78bffc6/.github/workflows/build.yml#L566-L603 From 028ff3847d72f66dc33cdb3d59e2e9f321c2b07d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 11 Dec 2020 17:36:10 +0100 Subject: [PATCH 15/48] upload artifacts --- .github/workflows/ci.yml | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d20cd26..ee6aa09 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -46,6 +46,7 @@ jobs: toolchain: ${{ matrix.toolchain }} target: thumbv7em-none-eabihf override: true + components: llvm-tools-preview - name: cargo check uses: actions-rs/cargo@v1 with: @@ -60,6 +61,22 @@ jobs: with: command: build args: --release + - name: cargo-binutils + uses: actions-rs/cargo@v1 + with: + command: install + args: cargo-binutils + - name: cargo objcopy + uses: actions-rs/cargo@v1 + with: + command: objcopy + args: --release -- -O binary stabilizer.bin + - uses: actions/upload-artifact@v1 + with: + name: stabilizer_${{ github.sha }}_bin + path: | + ./target/thumbv7em-none-eabihf/release/stabilizer + ./stabilizer.bin test: runs-on: ubuntu-latest From 3b7d90fb45c1aa0b9644e91fbd836ed18ff58172 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 11 Dec 2020 18:07:15 +0100 Subject: [PATCH 16/48] gha: artifact tweak --- .github/workflows/ci.yml | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index ee6aa09..65c73b2 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -66,17 +66,22 @@ jobs: with: command: install args: cargo-binutils + - name: cargo size + uses: actions-rs/cargo@v1 + with: + command: size + args: --release - name: cargo objcopy uses: actions-rs/cargo@v1 with: command: objcopy - args: --release -- -O binary stabilizer.bin + args: --release --verbose -- -O binary stabilizer.bin - uses: actions/upload-artifact@v1 with: name: stabilizer_${{ github.sha }}_bin path: | - ./target/thumbv7em-none-eabihf/release/stabilizer - ./stabilizer.bin + target/*/release/stabilizer + stabilizer.bin test: runs-on: ubuntu-latest @@ -104,7 +109,7 @@ jobs: - uses: actions/upload-artifact@v1 with: name: stabilizer_${{ github.sha }}_criterion - path: ./dsp/target/criterion + path: dsp/target/criterion # Tell bors about it # https://github.com/rtic-rs/cortex-m-rtic/blob/8a4f9c6b8ae91bebeea0791680f89375a78bffc6/.github/workflows/build.yml#L566-L603 From 193b8e222886732571d0befa25609257c1fd5534 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 11 Dec 2020 18:22:16 +0100 Subject: [PATCH 17/48] gha: upload-artifacts@v2 --- .github/workflows/ci.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 65c73b2..1744abe 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -75,13 +75,13 @@ jobs: uses: actions-rs/cargo@v1 with: command: objcopy - args: --release --verbose -- -O binary stabilizer.bin - - uses: actions/upload-artifact@v1 + args: --release --verbose -- -O binary stabilizer-release.bin + - uses: actions/upload-artifact@v2 with: name: stabilizer_${{ github.sha }}_bin path: | - target/*/release/stabilizer - stabilizer.bin + target/*/*/stabilizer + stabilizer-release.bin test: runs-on: ubuntu-latest From f8b121600e77e09d3e46b613813c0d5dbf283287 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 11 Dec 2020 18:34:42 +0100 Subject: [PATCH 18/48] gha: add rust to artifact name --- .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 1744abe..aaa8a8f 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -78,7 +78,7 @@ jobs: args: --release --verbose -- -O binary stabilizer-release.bin - uses: actions/upload-artifact@v2 with: - name: stabilizer_${{ github.sha }}_bin + name: stabilizer_${{ github.sha }}_rust-${{ matrix.toolchain }} path: | target/*/*/stabilizer stabilizer-release.bin From d271dccaba801819a4eae91aaa2e369c5b7535bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 11 Dec 2020 19:08:11 +0100 Subject: [PATCH 19/48] cossin bench: be fair to glibc --- dsp/benches/cossin.rs | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/dsp/benches/cossin.rs b/dsp/benches/cossin.rs index cbd0499..4e23774 100644 --- a/dsp/benches/cossin.rs +++ b/dsp/benches/cossin.rs @@ -1,12 +1,12 @@ +use core::f32::consts::PI; use criterion::{black_box, criterion_group, criterion_main, Criterion}; use dsp::trig::cossin; fn cossin_bench(c: &mut Criterion) { - let z = -0x7304_2531_i32; - c.bench_function("cossin(z)", |b| b.iter(|| cossin(black_box(z)))); - c.bench_function("(z as f32).sin_cos()", |b| { - b.iter(|| (black_box(z) as f32).sin_cos()) - }); + let zi = -0x7304_2531_i32; + let zf = zi as f32 / i32::MAX as f32 * PI; + c.bench_function("cossin(zi)", |b| b.iter(|| cossin(black_box(zi)))); + c.bench_function("zf.sin_cos()", |b| b.iter(|| black_box(zf).sin_cos())); } criterion_group!(benches, cossin_bench); From 107a4ac96fea1477a85840f0e43db881789216a3 Mon Sep 17 00:00:00 2001 From: Sebastien Bourdeauducq Date: Sat, 12 Dec 2020 16:33:37 +0800 Subject: [PATCH 20/48] update cargosha256 --- cargosha256.nix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cargosha256.nix b/cargosha256.nix index 4da95a5..92745cf 100644 --- a/cargosha256.nix +++ b/cargosha256.nix @@ -1 +1 @@ -"05b1xcr9jachnih0d6i63cfjcb88xrddmr2kf4h3vfwpjf8y9w10" +"0mrnm74wd5c1cl3av8iqndg6xrm07vs862882m59pjnrgy3z2zqj" From 4fc1f4397eb4b7c1275e8674995685d439ac05a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sun, 13 Dec 2020 12:36:09 +0100 Subject: [PATCH 21/48] gha: upload only relevant --- .github/workflows/ci.yml | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index aaa8a8f..d0ea705 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -77,10 +77,11 @@ jobs: command: objcopy args: --release --verbose -- -O binary stabilizer-release.bin - uses: actions/upload-artifact@v2 + if: ${{ matrix.toolchain == 'stable' }} with: - name: stabilizer_${{ github.sha }}_rust-${{ matrix.toolchain }} + name: stabilizer_${{ github.sha }} path: | - target/*/*/stabilizer + target/*/release/stabilizer stabilizer-release.bin test: @@ -106,10 +107,6 @@ jobs: with: command: bench args: --package dsp --target=x86_64-unknown-linux-gnu - - uses: actions/upload-artifact@v1 - with: - name: stabilizer_${{ github.sha }}_criterion - path: dsp/target/criterion # Tell bors about it # https://github.com/rtic-rs/cortex-m-rtic/blob/8a4f9c6b8ae91bebeea0791680f89375a78bffc6/.github/workflows/build.yml#L566-L603 From 75c4120258966d3a22d21157659697c0fa66d2e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sun, 13 Dec 2020 12:36:23 +0100 Subject: [PATCH 22/48] cossin: buffer test data output --- dsp/src/trig.rs | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index ecef4d4..5a99232 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -86,15 +86,16 @@ mod tests { let mut sum: Complex = (0., 0.); let mut demod: Complex = (0., 0.); - // use std::{fs::File, io::prelude::*, path::Path}; - // let mut file = File::create(Path::new("data.csv")).unwrap(); + // use std::{fs::File, io::{BufWriter, prelude::*}, path::Path}; + // let mut file = BufWriter::new(File::create(Path::new("data.bin")).unwrap()); const PHASE_DEPTH: usize = 20; for phase in 0..(1 << PHASE_DEPTH) { let phase = (phase << (32 - PHASE_DEPTH)) as i32; let have = cossin(phase); - // writeln!(file, " {},{}", have.0, have.1).unwrap(); + // file.write(&have.0.to_le_bytes()).unwrap(); + // file.write(&have.1.to_le_bytes()).unwrap(); let have = (have.0 as f64 / AMPLITUDE, have.1 as f64 / AMPLITUDE); From 469c89ea701359205c41718d7693847c8b668e97 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sun, 13 Dec 2020 22:24:40 +0100 Subject: [PATCH 23/48] pll: refine gains --- dsp/src/pll.rs | 52 +++++++++++++++++++++++++++++++++----------------- 1 file changed, 34 insertions(+), 18 deletions(-) diff --git a/dsp/src/pll.rs b/dsp/src/pll.rs index 5ee054d..74377f3 100644 --- a/dsp/src/pll.rs +++ b/dsp/src/pll.rs @@ -10,12 +10,14 @@ use serde::{Deserialize, Serialize}; /// stable for any gain (1 <= shift <= 30). It has a single parameter that determines the loop /// bandwidth in octave steps. The gain can be changed freely between updates. /// -/// The frequency and phase settling time constants for an (any) frequency jump are `1 << shift` +/// The frequency and phase settling time constants for a frequency/phase jump are `1 << shift` /// update cycles. The loop bandwidth is about `1/(2*pi*(1 << shift))` in units of the sample rate. +/// While the phase is being settled within one turn, there is a typically very small frequency +/// overshoot. /// /// All math is naturally wrapping 32 bit integer. Phase and frequency are understood modulo that /// overflow in the first Nyquist zone. Expressing the IIR equations in other ways (e.g. single -/// (T)-DF-{I,II} biquad/IIR) would break on overflow. +/// (T)-DF-{I,II} biquad/IIR) would break on overflow (i.e. every cycle). /// /// There are no floating point rounding errors here. But there is integer quantization/truncation /// error of the `shift` lowest bits leading to a phase offset for very low gains. Truncation @@ -23,8 +25,8 @@ use serde::{Deserialize, Serialize}; /// efficiently by dithering. /// /// This PLL does not unwrap phase slips during lock acquisition. This can and should be -/// implemented elsewhere by (down) scaling and then unwrapping the input phase and (up) scaling -/// and wrapping output phase and frequency. This affects dynamic range accordingly. +/// implemented elsewhere by unwrapping and scaling the input phase and un-scaling +/// and wrapping output phase and frequency. This affects dynamic range, gain, and noise accordingly. /// /// The extension to I^3,I^2,I behavior to track chirps phase-accurately or to i64 data to /// increase resolution for extremely narrowband applications is obvious. @@ -39,25 +41,39 @@ pub struct PLL { } impl PLL { - /// Update the PLL with a new phase sample. + /// Update the PLL with a new phase sample. This needs to be called (sampled) periodically. + /// The signal's phase/frequency is reconstructed relative to the sampling period. /// /// Args: /// * `input`: New input phase sample. - /// * `shift`: Error scaling. The frequency gain per update is `1/(1 << shift)`. The phase gain - /// is always twice the frequency gain. + /// * `shift_frequency`: Frequency error scaling. The frequency gain per update is + /// `1/(1 << shift_frequency)`. + /// * `shift_phase`: Phase error scaling. The phase gain is `1/(1 << shift_phase)` + /// per update. A good value is typically `shift_frequency - 1`. /// /// Returns: /// A tuple of instantaneous phase and frequency (the current phase increment). - pub fn update(&mut self, x: i32, shift: u8) -> (i32, i32) { - debug_assert!((1..=30).contains(&shift)); - let bias = 1i32 << shift; + pub fn update( + &mut self, + x: i32, + shift_frequency: u8, + shift_phase: u8, + ) -> (i32, i32) { + debug_assert!((1..=30).contains(&shift_frequency)); + debug_assert!((1..=30).contains(&shift_phase)); let e = x.wrapping_sub(self.f); self.f = self.f.wrapping_add( - (bias >> 1).wrapping_add(e).wrapping_sub(self.x) >> shift, + (1i32 << (shift_frequency - 1)) + .wrapping_add(e) + .wrapping_sub(self.x) + >> shift_frequency, ); self.x = x; let f = self.f.wrapping_add( - bias.wrapping_add(e).wrapping_sub(self.y) >> (shift - 1), + (1i32 << (shift_phase - 1)) + .wrapping_add(e) + .wrapping_sub(self.y) + >> shift_phase, ); self.y = self.y.wrapping_add(f); (self.y, f) @@ -70,8 +86,8 @@ mod tests { #[test] fn mini() { let mut p = PLL::default(); - let (y, f) = p.update(0x10000, 10); - assert_eq!(y, 0xc2); + let (y, f) = p.update(0x10000, 8, 4); + assert_eq!(y, 0x1100); assert_eq!(f, y); } @@ -79,17 +95,17 @@ mod tests { fn converge() { let mut p = PLL::default(); let f0 = 0x71f63049_i32; - let shift = 10; - let n = 31 << shift + 2; + let shift = (10, 9); + let n = 31 << shift.0 + 2; let mut x = 0i32; for i in 0..n { x = x.wrapping_add(f0); - let (y, f) = p.update(x, shift); + let (y, f) = p.update(x, shift.0, shift.1); if i > n / 4 { assert_eq!(f.wrapping_sub(f0).abs() <= 1, true); } if i > n / 2 { - // The remaining error is removed by dithering. + // The remaining error would be removed by dithering. assert_eq!(y.wrapping_sub(x).abs() < 1 << 18, true); } } From e89db65722672bbbbc9178b5ba1214126854f17a Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 16 Dec 2020 15:25:31 -0800 Subject: [PATCH 24/48] rename trig.rs -> cossin.rs --- dsp/src/{trig.rs => cossin.rs} | 0 dsp/src/lib.rs | 3 ++- 2 files changed, 2 insertions(+), 1 deletion(-) rename dsp/src/{trig.rs => cossin.rs} (100%) diff --git a/dsp/src/trig.rs b/dsp/src/cossin.rs similarity index 100% rename from dsp/src/trig.rs rename to dsp/src/cossin.rs diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 6dd20f7..ef0c131 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -18,10 +18,11 @@ pub fn shift_round(x: i32, shift: usize) -> i32 { (x + (1 << (shift - 1))) >> shift } +pub mod atan2; +pub mod cossin; pub mod iir; pub mod lockin; pub mod pll; -pub mod trig; pub mod unwrap; #[cfg(test)] From 17f9f0750eee1ec0fff98d46720e8a45b8702148 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 16 Dec 2020 16:01:50 -0800 Subject: [PATCH 25/48] dsp: move abs to lib.rs --- dsp/src/iir.rs | 12 +----------- dsp/src/lib.rs | 13 +++++++++++++ 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index c6f2100..48c92e9 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -2,23 +2,13 @@ use core::ops::{Add, Mul, Neg}; use serde::{Deserialize, Serialize}; use core::f32; +use super::abs; // These are implemented here because core::f32 doesn't have them (yet). // They are naive and don't handle inf/nan. // `compiler-intrinsics`/llvm should have better (robust, universal, and // faster) implementations. -fn abs(x: T) -> T -where - T: PartialOrd + Default + Neg, -{ - if x >= T::default() { - x - } else { - -x - } -} - fn copysign(x: T, y: T) -> T where T: PartialOrd + Default + Neg, diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index ef0c131..2fbd121 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -1,6 +1,8 @@ #![cfg_attr(not(test), no_std)] #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] +use core::ops::Neg; + pub type Complex = (T, T); /// Round up half. @@ -18,6 +20,17 @@ pub fn shift_round(x: i32, shift: usize) -> i32 { (x + (1 << (shift - 1))) >> shift } +fn abs(x: T) -> T +where + T: PartialOrd + Default + Neg, +{ + if x >= T::default() { + x + } else { + -x + } +} + pub mod atan2; pub mod cossin; pub mod iir; From 6d651da758f44d0a8b6702cb4e364da3fad276fa Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 16 Dec 2020 16:02:17 -0800 Subject: [PATCH 26/48] dsp: add f64 isclose testing function --- dsp/src/testing.rs | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/dsp/src/testing.rs b/dsp/src/testing.rs index 1a8e109..098ec87 100644 --- a/dsp/src/testing.rs +++ b/dsp/src/testing.rs @@ -1,6 +1,10 @@ use super::Complex; -pub fn isclose(a: f32, b: f32, rtol: f32, atol: f32) -> bool { +pub fn isclose(a: f64, b: f64, rtol: f64, atol: f64) -> bool { + (a - b).abs() <= a.abs().max(b.abs()) * rtol + atol +} + +pub fn isclosef(a: f32, b: f32, rtol: f32, atol: f32) -> bool { (a - b).abs() <= a.abs().max(b.abs()) * rtol + atol } @@ -10,7 +14,7 @@ pub fn complex_isclose( rtol: f32, atol: f32, ) -> bool { - isclose(a.0, b.0, rtol, atol) && isclose(a.1, b.1, rtol, atol) + isclosef(a.0, b.0, rtol, atol) && isclosef(a.1, b.1, rtol, atol) } pub fn complex_allclose( From 5d055b01a03f31b0950ce5e36c214a1fc6289a96 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 16 Dec 2020 16:02:42 -0800 Subject: [PATCH 27/48] dsp: add atan2 --- dsp/src/atan2.rs | 126 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 126 insertions(+) create mode 100644 dsp/src/atan2.rs diff --git a/dsp/src/atan2.rs b/dsp/src/atan2.rs new file mode 100644 index 0000000..a5f4d3e --- /dev/null +++ b/dsp/src/atan2.rs @@ -0,0 +1,126 @@ +use super::{abs, shift_round}; + +/// 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 +/// corresponds to an angle of -pi and i32::MAX corresponds to an +/// angle of +pi. +pub fn atan2(y: i32, x: i32) -> i32 { + let y = y >> 16; + let x = x >> 16; + + let ux = abs::(x); + let uy = abs::(y); + + // 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 * x + 0.285 * x * (1 - abs(x)) + // + // which is taken from Rajan 2006: Efficient Approximations for + // the Arctangent Function. + let (min, max) = if ux < uy { (ux, uy) } else { (uy, ux) }; + + if max == 0 { + return 0; + } + + let ratio = (min << 15) / max; + + let mut angle = { + // pi/4, referenced to i16::MAX + const PI_4_FACTOR: i32 = 25735; + // 0.285, referenced to i16::MAX + const FACTOR_0285: i32 = 9339; + // 1/pi, referenced to u16::MAX + const PI_INVERTED_FACTOR: i32 = 20861; + + let r1 = shift_round(ratio * PI_4_FACTOR, 15); + let r2 = shift_round( + (shift_round(ratio * FACTOR_0285, 15)) * (i16::MAX as i32 - ratio), + 15, + ); + (r1 + r2) * PI_INVERTED_FACTOR + }; + + if uy > ux { + angle = (i32::MAX >> 1) - angle; + } + + if x < 0 { + angle = i32::MAX - angle; + } + + if y < 0 { + angle *= -1; + } + + angle +} + +#[cfg(test)] +mod tests { + use super::*; + use core::f64::consts::PI; + use crate::testing::isclose; + + fn angle_to_axis(angle: f64) -> f64 { + let angle = angle % (PI / 2.); + (PI / 2. - angle).min(angle) + } + + #[test] + fn absolute_error() { + const NUM_VALS: usize = 1_001; + let mut test_vals: [f64; NUM_VALS] = [0.; NUM_VALS]; + let val_bounds: (f64, f64) = (-1., 1.); + let val_delta: f64 = + (val_bounds.1 - val_bounds.0) / (NUM_VALS - 1) as f64; + for i in 0..NUM_VALS { + test_vals[i] = val_bounds.0 + i as f64 * val_delta; + } + + for &x in test_vals.iter() { + for &y in test_vals.iter() { + let atol: f64 = 4e-5; + let rtol: f64 = 0.127; + let actual = (y.atan2(x) as f64 * i16::MAX as f64).round() + / i16::MAX as f64; + let tol = atol + rtol * angle_to_axis(actual).abs(); + let computed = (atan2( + ((y * i16::MAX as f64) as i32) << 16, + ((x * i16::MAX as f64) as i32) << 16, + ) >> 16) as f64 + / i16::MAX as f64 + * PI; + + if !isclose(computed, actual, 0., tol) { + println!("(x, y) : {}, {}", x, y); + println!("actual : {}", actual); + println!("computed : {}", computed); + println!("tolerance: {}\n", tol); + assert!(false); + } + } + } + } +} From e257545321cb27fd808c35f5c7d8ddedca632fbb Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 16 Dec 2020 16:14:11 -0800 Subject: [PATCH 28/48] fix formatting --- dsp/src/atan2.rs | 2 +- dsp/src/iir.rs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/dsp/src/atan2.rs b/dsp/src/atan2.rs index a5f4d3e..2643d19 100644 --- a/dsp/src/atan2.rs +++ b/dsp/src/atan2.rs @@ -80,8 +80,8 @@ pub fn atan2(y: i32, x: i32) -> i32 { #[cfg(test)] mod tests { use super::*; - use core::f64::consts::PI; use crate::testing::isclose; + use core::f64::consts::PI; fn angle_to_axis(angle: f64) -> f64 { let angle = angle % (PI / 2.); diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index 48c92e9..5ff0970 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -1,8 +1,8 @@ use core::ops::{Add, Mul, Neg}; use serde::{Deserialize, Serialize}; -use core::f32; use super::abs; +use core::f32; // These are implemented here because core::f32 doesn't have them (yet). // They are naive and don't handle inf/nan. From 7c4f6082068d8ae3bda2896f29b00c7b4ab50ee4 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 16 Dec 2020 16:26:44 -0800 Subject: [PATCH 29/48] move cossin and atan2 into the same trig file --- dsp/benches/cossin.rs | 2 +- dsp/src/atan2.rs | 126 --------------------------------- dsp/src/lib.rs | 3 +- dsp/src/{cossin.rs => trig.rs} | 123 +++++++++++++++++++++++++++++++- 4 files changed, 124 insertions(+), 130 deletions(-) delete mode 100644 dsp/src/atan2.rs rename dsp/src/{cossin.rs => trig.rs} (57%) diff --git a/dsp/benches/cossin.rs b/dsp/benches/cossin.rs index 4e23774..9f88e1b 100644 --- a/dsp/benches/cossin.rs +++ b/dsp/benches/cossin.rs @@ -1,6 +1,6 @@ use core::f32::consts::PI; use criterion::{black_box, criterion_group, criterion_main, Criterion}; -use dsp::trig::cossin; +use dsp::cossin::cossin; fn cossin_bench(c: &mut Criterion) { let zi = -0x7304_2531_i32; diff --git a/dsp/src/atan2.rs b/dsp/src/atan2.rs deleted file mode 100644 index 2643d19..0000000 --- a/dsp/src/atan2.rs +++ /dev/null @@ -1,126 +0,0 @@ -use super::{abs, shift_round}; - -/// 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 -/// corresponds to an angle of -pi and i32::MAX corresponds to an -/// angle of +pi. -pub fn atan2(y: i32, x: i32) -> i32 { - let y = y >> 16; - let x = x >> 16; - - let ux = abs::(x); - let uy = abs::(y); - - // 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 * x + 0.285 * x * (1 - abs(x)) - // - // which is taken from Rajan 2006: Efficient Approximations for - // the Arctangent Function. - let (min, max) = if ux < uy { (ux, uy) } else { (uy, ux) }; - - if max == 0 { - return 0; - } - - let ratio = (min << 15) / max; - - let mut angle = { - // pi/4, referenced to i16::MAX - const PI_4_FACTOR: i32 = 25735; - // 0.285, referenced to i16::MAX - const FACTOR_0285: i32 = 9339; - // 1/pi, referenced to u16::MAX - const PI_INVERTED_FACTOR: i32 = 20861; - - let r1 = shift_round(ratio * PI_4_FACTOR, 15); - let r2 = shift_round( - (shift_round(ratio * FACTOR_0285, 15)) * (i16::MAX as i32 - ratio), - 15, - ); - (r1 + r2) * PI_INVERTED_FACTOR - }; - - if uy > ux { - angle = (i32::MAX >> 1) - angle; - } - - if x < 0 { - angle = i32::MAX - angle; - } - - if y < 0 { - angle *= -1; - } - - angle -} - -#[cfg(test)] -mod tests { - use super::*; - use crate::testing::isclose; - use core::f64::consts::PI; - - fn angle_to_axis(angle: f64) -> f64 { - let angle = angle % (PI / 2.); - (PI / 2. - angle).min(angle) - } - - #[test] - fn absolute_error() { - const NUM_VALS: usize = 1_001; - let mut test_vals: [f64; NUM_VALS] = [0.; NUM_VALS]; - let val_bounds: (f64, f64) = (-1., 1.); - let val_delta: f64 = - (val_bounds.1 - val_bounds.0) / (NUM_VALS - 1) as f64; - for i in 0..NUM_VALS { - test_vals[i] = val_bounds.0 + i as f64 * val_delta; - } - - for &x in test_vals.iter() { - for &y in test_vals.iter() { - let atol: f64 = 4e-5; - let rtol: f64 = 0.127; - let actual = (y.atan2(x) as f64 * i16::MAX as f64).round() - / i16::MAX as f64; - let tol = atol + rtol * angle_to_axis(actual).abs(); - let computed = (atan2( - ((y * i16::MAX as f64) as i32) << 16, - ((x * i16::MAX as f64) as i32) << 16, - ) >> 16) as f64 - / i16::MAX as f64 - * PI; - - if !isclose(computed, actual, 0., tol) { - println!("(x, y) : {}, {}", x, y); - println!("actual : {}", actual); - println!("computed : {}", computed); - println!("tolerance: {}\n", tol); - assert!(false); - } - } - } - } -} diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 2fbd121..90f62f6 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -31,11 +31,10 @@ where } } -pub mod atan2; -pub mod cossin; pub mod iir; pub mod lockin; pub mod pll; +pub mod trig; pub mod unwrap; #[cfg(test)] diff --git a/dsp/src/cossin.rs b/dsp/src/trig.rs similarity index 57% rename from dsp/src/cossin.rs rename to dsp/src/trig.rs index 5a99232..72435e0 100644 --- a/dsp/src/cossin.rs +++ b/dsp/src/trig.rs @@ -1,8 +1,85 @@ -use super::Complex; +use super::{abs, shift_round, Complex}; use core::f64::consts::PI; include!(concat!(env!("OUT_DIR"), "/cossin_table.rs")); +/// 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 +/// corresponds to an angle of -pi and i32::MAX corresponds to an +/// angle of +pi. +pub fn atan2(y: i32, x: i32) -> i32 { + let y = y >> 16; + let x = x >> 16; + + let ux = abs::(x); + let uy = abs::(y); + + // 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 * x + 0.285 * x * (1 - abs(x)) + // + // which is taken from Rajan 2006: Efficient Approximations for + // the Arctangent Function. + let (min, max) = if ux < uy { (ux, uy) } else { (uy, ux) }; + + if max == 0 { + return 0; + } + + let ratio = (min << 15) / max; + + let mut angle = { + // pi/4, referenced to i16::MAX + const PI_4_FACTOR: i32 = 25735; + // 0.285, referenced to i16::MAX + const FACTOR_0285: i32 = 9339; + // 1/pi, referenced to u16::MAX + const PI_INVERTED_FACTOR: i32 = 20861; + + let r1 = shift_round(ratio * PI_4_FACTOR, 15); + let r2 = shift_round( + (shift_round(ratio * FACTOR_0285, 15)) * (i16::MAX as i32 - ratio), + 15, + ); + (r1 + r2) * PI_INVERTED_FACTOR + }; + + if uy > ux { + angle = (i32::MAX >> 1) - angle; + } + + if x < 0 { + angle = i32::MAX - angle; + } + + if y < 0 { + angle *= -1; + } + + angle +} + /// Compute the cosine and sine of an angle. /// This is ported from the MiSoC cossin core. /// (https://github.com/m-labs/misoc/blob/master/misoc/cores/cossin.py) @@ -75,6 +152,14 @@ pub fn cossin(phase: i32) -> Complex { #[cfg(test)] mod tests { use super::*; + use crate::testing::isclose; + use core::f64::consts::PI; + + fn angle_to_axis(angle: f64) -> f64 { + let angle = angle % (PI / 2.); + (PI / 2. - angle).min(angle) + } + #[test] fn error_max_rms_all_phase() { // Constant amplitude error due to LUT data range. @@ -143,4 +228,40 @@ mod tests { assert!(max_err.0 < 1.1e-5); assert!(max_err.1 < 1.1e-5); } + + #[test] + fn absolute_error() { + const NUM_VALS: usize = 1_001; + let mut test_vals: [f64; NUM_VALS] = [0.; NUM_VALS]; + let val_bounds: (f64, f64) = (-1., 1.); + let val_delta: f64 = + (val_bounds.1 - val_bounds.0) / (NUM_VALS - 1) as f64; + for i in 0..NUM_VALS { + test_vals[i] = val_bounds.0 + i as f64 * val_delta; + } + + for &x in test_vals.iter() { + for &y in test_vals.iter() { + let atol: f64 = 4e-5; + let rtol: f64 = 0.127; + let actual = (y.atan2(x) as f64 * i16::MAX as f64).round() + / i16::MAX as f64; + let tol = atol + rtol * angle_to_axis(actual).abs(); + let computed = (atan2( + ((y * i16::MAX as f64) as i32) << 16, + ((x * i16::MAX as f64) as i32) << 16, + ) >> 16) as f64 + / i16::MAX as f64 + * PI; + + if !isclose(computed, actual, 0., tol) { + println!("(x, y) : {}, {}", x, y); + println!("actual : {}", actual); + println!("computed : {}", computed); + println!("tolerance: {}\n", tol); + assert!(false); + } + } + } + } } From 85ae70fe6205ce25e6b80840c92aaf1e4221d2a5 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 16 Dec 2020 16:28:49 -0800 Subject: [PATCH 30/48] rename trig tests to delineate between cossin and atan2 --- dsp/src/trig.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index 72435e0..4dc26be 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -161,7 +161,7 @@ mod tests { } #[test] - fn error_max_rms_all_phase() { + fn cossin_error_max_rms_all_phase() { // Constant amplitude error due to LUT data range. const AMPLITUDE: f64 = ((1i64 << 31) - (1i64 << 15)) as f64; const MAX_PHASE: f64 = (1i64 << 32) as f64; @@ -230,7 +230,7 @@ mod tests { } #[test] - fn absolute_error() { + fn atan2_absolute_error() { const NUM_VALS: usize = 1_001; let mut test_vals: [f64; NUM_VALS] = [0.; NUM_VALS]; let val_bounds: (f64, f64) = (-1., 1.); From 2ddaab8fae34b4f01d2f2029eb18f3676e41ab56 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 16 Dec 2020 16:57:18 -0800 Subject: [PATCH 31/48] dsp: fix bench import path --- dsp/benches/cossin.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dsp/benches/cossin.rs b/dsp/benches/cossin.rs index 9f88e1b..4e23774 100644 --- a/dsp/benches/cossin.rs +++ b/dsp/benches/cossin.rs @@ -1,6 +1,6 @@ use core::f32::consts::PI; use criterion::{black_box, criterion_group, criterion_main, Criterion}; -use dsp::cossin::cossin; +use dsp::trig::cossin; fn cossin_bench(c: &mut Criterion) { let zi = -0x7304_2531_i32; From d9d500743f41aa150c055263aedfdba36c4ffbd0 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 17 Dec 2020 08:02:54 -0800 Subject: [PATCH 32/48] simplify atan initial angle expression --- dsp/src/trig.rs | 18 ++++++------------ 1 file changed, 6 insertions(+), 12 deletions(-) diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index 4dc26be..e306356 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -50,19 +50,13 @@ pub fn atan2(y: i32, x: i32) -> i32 { let ratio = (min << 15) / max; let mut angle = { - // pi/4, referenced to i16::MAX - const PI_4_FACTOR: i32 = 25735; - // 0.285, referenced to i16::MAX - const FACTOR_0285: i32 = 9339; - // 1/pi, referenced to u16::MAX - const PI_INVERTED_FACTOR: i32 = 20861; + const K1: i32 = + ((1_f64 / 4_f64 + 0.285_f64 / PI) * (1 << 16) as f64) as i32; + const K2: i32 = ((0.285_f64 / PI) * (1 << 16) as f64) as i32; - let r1 = shift_round(ratio * PI_4_FACTOR, 15); - let r2 = shift_round( - (shift_round(ratio * FACTOR_0285, 15)) * (i16::MAX as i32 - ratio), - 15, - ); - (r1 + r2) * PI_INVERTED_FACTOR + let ratio_squared = shift_round(ratio * ratio, 15); + + ratio * K1 - K2 * ratio_squared }; if uy > ux { From d7111a3aa811deed34f01c0682ee6fada8978c61 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 17 Dec 2020 08:04:53 -0800 Subject: [PATCH 33/48] dsp/trig: let compiler infer type parameter in atan2 abs call --- dsp/src/trig.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index e306356..51e8b2e 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -25,8 +25,8 @@ pub fn atan2(y: i32, x: i32) -> i32 { let y = y >> 16; let x = x >> 16; - let ux = abs::(x); - let uy = abs::(y); + let ux = abs(x); + let uy = abs(y); // Uses the general procedure described in the following // Mathematics stack exchange answer: From 5717991ada1847549b3154a20803f21c5688c3e5 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 17 Dec 2020 09:31:18 -0800 Subject: [PATCH 34/48] atan2: result range is from i32::MIN+1 to i32::MAX --- dsp/src/trig.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index 51e8b2e..57e4fec 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -18,7 +18,7 @@ include!(concat!(env!("OUT_DIR"), "/cossin_table.rs")); /// # 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 +/// result range is from i32::MIN+1 to i32::MAX, where i32::MIN+1 /// corresponds to an angle of -pi and i32::MAX corresponds to an /// angle of +pi. pub fn atan2(y: i32, x: i32) -> i32 { From cb38c3e3bd3f28f32bfba3ed2abf048772af45a9 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 17 Dec 2020 09:31:38 -0800 Subject: [PATCH 35/48] atan2: clarify sharing bits between atan argument and constant factors --- dsp/src/trig.rs | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index 57e4fec..d25d50b 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -47,16 +47,21 @@ pub fn atan2(y: i32, x: i32) -> i32 { return 0; } - let ratio = (min << 15) / max; + // We need to share the 31 available non-sign bits between the + // atan argument and constant factors used in the atan + // approximation. Sharing the bits roughly equally between them + // gives good accuracy. + const ATAN_ARGUMENT_BITS: usize = 15; + let ratio = (min << ATAN_ARGUMENT_BITS) / max; let mut angle = { - const K1: i32 = - ((1_f64 / 4_f64 + 0.285_f64 / PI) * (1 << 16) as f64) as i32; - const K2: i32 = ((0.285_f64 / PI) * (1 << 16) as f64) as i32; + const K1: i32 = ((1. / 4. + 0.285 / PI) + * (1 << (31 - ATAN_ARGUMENT_BITS)) as f64) + as i32; + const K2: i32 = + ((0.285 / PI) * (1 << (31 - ATAN_ARGUMENT_BITS)) as f64) as i32; - let ratio_squared = shift_round(ratio * ratio, 15); - - ratio * K1 - K2 * ratio_squared + ratio * K1 - K2 * shift_round(ratio * ratio, ATAN_ARGUMENT_BITS) }; if uy > ux { From 1f28949bc5d859144a6fc96cdf28cd6959dd2e29 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 17 Dec 2020 09:47:39 -0800 Subject: [PATCH 36/48] atan2: store sign bits and greater of |x| and |y| --- dsp/src/trig.rs | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index d25d50b..53aecfa 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -1,4 +1,4 @@ -use super::{abs, shift_round, Complex}; +use super::{shift_round, Complex}; use core::f64::consts::PI; include!(concat!(env!("OUT_DIR"), "/cossin_table.rs")); @@ -22,11 +22,18 @@ include!(concat!(env!("OUT_DIR"), "/cossin_table.rs")); /// corresponds to an angle of -pi and i32::MAX corresponds to an /// angle of +pi. pub fn atan2(y: i32, x: i32) -> i32 { - let y = y >> 16; - let x = x >> 16; + let mut y = y >> 16; + let mut x = x >> 16; - let ux = abs(x); - let uy = abs(y); + let sign = ((y >> 14) & 2) | ((x >> 15) & 1); + if sign & 1 == 1 { + x *= -1; + } + if sign & 2 == 2 { + y *= -1; + } + + let y_greater = y > x; // Uses the general procedure described in the following // Mathematics stack exchange answer: @@ -41,7 +48,7 @@ pub fn atan2(y: i32, x: i32) -> i32 { // // which is taken from Rajan 2006: Efficient Approximations for // the Arctangent Function. - let (min, max) = if ux < uy { (ux, uy) } else { (uy, ux) }; + let (min, max) = if y_greater { (x, y) } else { (y, x) }; if max == 0 { return 0; @@ -64,15 +71,15 @@ pub fn atan2(y: i32, x: i32) -> i32 { ratio * K1 - K2 * shift_round(ratio * ratio, ATAN_ARGUMENT_BITS) }; - if uy > ux { + if y_greater { angle = (i32::MAX >> 1) - angle; } - if x < 0 { + if sign & 1 == 1 { angle = i32::MAX - angle; } - if y < 0 { + if sign & 2 == 2 { angle *= -1; } From 56641d5838cba59fa55d82af2e0d495185b227cc Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 17 Dec 2020 10:02:35 -0800 Subject: [PATCH 37/48] atan2: specify why we cannot use more than 15 bits for the atan argument --- dsp/src/trig.rs | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index 53aecfa..6d0acff 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -57,7 +57,9 @@ pub fn atan2(y: i32, x: i32) -> i32 { // We need to share the 31 available non-sign bits between the // atan argument and constant factors used in the atan // approximation. Sharing the bits roughly equally between them - // gives good accuracy. + // gives good accuracy. Additionally, we cannot increase the + // number of atan argument bits beyond 15 because we must square + // it. const ATAN_ARGUMENT_BITS: usize = 15; let ratio = (min << ATAN_ARGUMENT_BITS) / max; From 09a744f59c5771fa044acd1694809e4d055ef157 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 17 Dec 2020 10:03:16 -0800 Subject: [PATCH 38/48] dsp: move iir generic math functions to top-level module scope --- dsp/src/iir.rs | 69 +------------------------------------------------- dsp/src/lib.rs | 68 ++++++++++++++++++++++++++++++++++++++++++++++++- 2 files changed, 68 insertions(+), 69 deletions(-) diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index 5ff0970..dbec8d8 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -1,75 +1,8 @@ -use core::ops::{Add, Mul, Neg}; use serde::{Deserialize, Serialize}; -use super::abs; +use super::{abs, copysign, macc, max, min}; use core::f32; -// These are implemented here because core::f32 doesn't have them (yet). -// They are naive and don't handle inf/nan. -// `compiler-intrinsics`/llvm should have better (robust, universal, and -// faster) implementations. - -fn copysign(x: T, y: T) -> T -where - T: PartialOrd + Default + Neg, -{ - if (x >= T::default() && y >= T::default()) - || (x <= T::default() && y <= T::default()) - { - x - } else { - -x - } -} - -#[cfg(not(feature = "nightly"))] -fn max(x: T, y: T) -> T -where - T: PartialOrd, -{ - if x > y { - x - } else { - y - } -} - -#[cfg(not(feature = "nightly"))] -fn min(x: T, y: T) -> T -where - T: PartialOrd, -{ - if x < y { - x - } else { - y - } -} - -#[cfg(feature = "nightly")] -fn max(x: f32, y: f32) -> f32 { - core::intrinsics::maxnumf32(x, y) -} - -#[cfg(feature = "nightly")] -fn min(x: f32, y: f32) -> f32 { - core::intrinsics::minnumf32(x, y) -} - -// Multiply-accumulate vectors `x` and `a`. -// -// A.k.a. dot product. -// Rust/LLVM optimize this nicely. -fn macc(y0: T, x: &[T], a: &[T]) -> T -where - T: Add + Mul + Copy, -{ - x.iter() - .zip(a) - .map(|(x, a)| *x * *a) - .fold(y0, |y, xa| y + xa) -} - /// IIR state and coefficients type. /// /// To represent the IIR state (input and output memory) during the filter update diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 90f62f6..67b1882 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -1,7 +1,7 @@ #![cfg_attr(not(test), no_std)] #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] -use core::ops::Neg; +use core::ops::{Add, Mul, Neg}; pub type Complex = (T, T); @@ -31,6 +31,72 @@ where } } +// These are implemented here because core::f32 doesn't have them (yet). +// They are naive and don't handle inf/nan. +// `compiler-intrinsics`/llvm should have better (robust, universal, and +// faster) implementations. + +fn copysign(x: T, y: T) -> T +where + T: PartialOrd + Default + Neg, +{ + if (x >= T::default() && y >= T::default()) + || (x <= T::default() && y <= T::default()) + { + x + } else { + -x + } +} + +#[cfg(not(feature = "nightly"))] +fn max(x: T, y: T) -> T +where + T: PartialOrd, +{ + if x > y { + x + } else { + y + } +} + +#[cfg(not(feature = "nightly"))] +fn min(x: T, y: T) -> T +where + T: PartialOrd, +{ + if x < y { + x + } else { + y + } +} + +#[cfg(feature = "nightly")] +fn max(x: f32, y: f32) -> f32 { + core::intrinsics::maxnumf32(x, y) +} + +#[cfg(feature = "nightly")] +fn min(x: f32, y: f32) -> f32 { + core::intrinsics::minnumf32(x, y) +} + +// Multiply-accumulate vectors `x` and `a`. +// +// A.k.a. dot product. +// Rust/LLVM optimize this nicely. +fn macc(y0: T, x: &[T], a: &[T]) -> T +where + T: Add + Mul + Copy, +{ + x.iter() + .zip(a) + .map(|(x, a)| *x * *a) + .fold(y0, |y, xa| y + xa) +} + pub mod iir; pub mod lockin; pub mod pll; From 6ffc42021edbcf34e4eabd2a3213b8d191e22e5e Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 17 Dec 2020 10:09:12 -0800 Subject: [PATCH 39/48] move atan2 test before cossin test to mimic function order --- dsp/src/trig.rs | 72 ++++++++++++++++++++++++------------------------- 1 file changed, 36 insertions(+), 36 deletions(-) diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index 6d0acff..ccf9292 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -168,6 +168,42 @@ mod tests { (PI / 2. - angle).min(angle) } + #[test] + fn atan2_absolute_error() { + const NUM_VALS: usize = 1_001; + let mut test_vals: [f64; NUM_VALS] = [0.; NUM_VALS]; + let val_bounds: (f64, f64) = (-1., 1.); + let val_delta: f64 = + (val_bounds.1 - val_bounds.0) / (NUM_VALS - 1) as f64; + for i in 0..NUM_VALS { + test_vals[i] = val_bounds.0 + i as f64 * val_delta; + } + + for &x in test_vals.iter() { + for &y in test_vals.iter() { + let atol: f64 = 4e-5; + let rtol: f64 = 0.127; + let actual = (y.atan2(x) as f64 * i16::MAX as f64).round() + / i16::MAX as f64; + let tol = atol + rtol * angle_to_axis(actual).abs(); + let computed = (atan2( + ((y * i16::MAX as f64) as i32) << 16, + ((x * i16::MAX as f64) as i32) << 16, + ) >> 16) as f64 + / i16::MAX as f64 + * PI; + + if !isclose(computed, actual, 0., tol) { + println!("(x, y) : {}, {}", x, y); + println!("actual : {}", actual); + println!("computed : {}", computed); + println!("tolerance: {}\n", tol); + assert!(false); + } + } + } + } + #[test] fn cossin_error_max_rms_all_phase() { // Constant amplitude error due to LUT data range. @@ -236,40 +272,4 @@ mod tests { assert!(max_err.0 < 1.1e-5); assert!(max_err.1 < 1.1e-5); } - - #[test] - fn atan2_absolute_error() { - const NUM_VALS: usize = 1_001; - let mut test_vals: [f64; NUM_VALS] = [0.; NUM_VALS]; - let val_bounds: (f64, f64) = (-1., 1.); - let val_delta: f64 = - (val_bounds.1 - val_bounds.0) / (NUM_VALS - 1) as f64; - for i in 0..NUM_VALS { - test_vals[i] = val_bounds.0 + i as f64 * val_delta; - } - - for &x in test_vals.iter() { - for &y in test_vals.iter() { - let atol: f64 = 4e-5; - let rtol: f64 = 0.127; - let actual = (y.atan2(x) as f64 * i16::MAX as f64).round() - / i16::MAX as f64; - let tol = atol + rtol * angle_to_axis(actual).abs(); - let computed = (atan2( - ((y * i16::MAX as f64) as i32) << 16, - ((x * i16::MAX as f64) as i32) << 16, - ) >> 16) as f64 - / i16::MAX as f64 - * PI; - - if !isclose(computed, actual, 0., tol) { - println!("(x, y) : {}, {}", x, y); - println!("actual : {}", actual); - println!("computed : {}", computed); - println!("tolerance: {}\n", tol); - assert!(false); - } - } - } - } } From 9c5e68ceea82c8f9096473cfb74d0ef930d34843 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 17 Dec 2020 11:34:39 -0800 Subject: [PATCH 40/48] atan2: test min and max angle inputs --- dsp/src/trig.rs | 27 +++++++++++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index ccf9292..5d73846 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -179,10 +179,10 @@ mod tests { test_vals[i] = val_bounds.0 + i as f64 * val_delta; } + let atol: f64 = 4e-5; + let rtol: f64 = 0.127; for &x in test_vals.iter() { for &y in test_vals.iter() { - let atol: f64 = 4e-5; - let rtol: f64 = 0.127; let actual = (y.atan2(x) as f64 * i16::MAX as f64).round() / i16::MAX as f64; let tol = atol + rtol * angle_to_axis(actual).abs(); @@ -202,6 +202,29 @@ mod tests { } } } + + // test min and max explicitly + for (x, y) in [ + ((i16::MIN as i32 + 1) << 16, -(1 << 16) as i32), + ((i16::MIN as i32 + 1) << 16, (1 << 16) as i32), + ] + .iter() + { + let yf = *y as f64 / ((i16::MAX as i32) << 16) as f64; + let xf = *x as f64 / ((i16::MAX as i32) << 16) as f64; + let actual = + (yf.atan2(xf) * i16::MAX as f64).round() / i16::MAX as f64; + let computed = (atan2(*y, *x) >> 16) as f64 / i16::MAX as f64 * PI; + let tol = atol + rtol * angle_to_axis(actual).abs(); + + if !isclose(computed, actual, 0., tol) { + println!("(x, y) : {}, {}", *x, *y); + println!("actual : {}", actual); + println!("computed : {}", computed); + println!("tolerance: {}\n", tol); + assert!(false); + } + } } #[test] From 17cf71f22bc3978cb50f21c50367f108aa8f9ee9 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 17 Dec 2020 11:39:32 -0800 Subject: [PATCH 41/48] atan2: replace min, max with x, y --- dsp/src/trig.rs | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index 5d73846..13ce844 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -48,9 +48,11 @@ pub fn atan2(y: i32, x: i32) -> i32 { // // which is taken from Rajan 2006: Efficient Approximations for // the Arctangent Function. - let (min, max) = if y_greater { (x, y) } else { (y, x) }; + if y_greater { + core::mem::swap(&mut x, &mut y); + } - if max == 0 { + if x == 0 { return 0; } @@ -61,7 +63,7 @@ pub fn atan2(y: i32, x: i32) -> i32 { // number of atan argument bits beyond 15 because we must square // it. const ATAN_ARGUMENT_BITS: usize = 15; - let ratio = (min << ATAN_ARGUMENT_BITS) / max; + let ratio = (y << ATAN_ARGUMENT_BITS) / x; let mut angle = { const K1: i32 = ((1. / 4. + 0.285 / PI) From 3125365a1580731d104d74f3023f7c8fb5e1ff9e Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 17 Dec 2020 14:01:57 -0800 Subject: [PATCH 42/48] add atan2 host benchmark --- dsp/Cargo.toml | 2 +- dsp/benches/cossin.rs | 13 ------------- dsp/benches/trig.rs | 28 ++++++++++++++++++++++++++++ 3 files changed, 29 insertions(+), 14 deletions(-) delete mode 100644 dsp/benches/cossin.rs create mode 100644 dsp/benches/trig.rs diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml index 8313a49..548e64f 100644 --- a/dsp/Cargo.toml +++ b/dsp/Cargo.toml @@ -12,7 +12,7 @@ serde = { version = "1.0", features = ["derive"], default-features = false } criterion = "0.3" [[bench]] -name = "cossin" +name = "trig" harness = false [features] diff --git a/dsp/benches/cossin.rs b/dsp/benches/cossin.rs deleted file mode 100644 index 4e23774..0000000 --- a/dsp/benches/cossin.rs +++ /dev/null @@ -1,13 +0,0 @@ -use core::f32::consts::PI; -use criterion::{black_box, criterion_group, criterion_main, Criterion}; -use dsp::trig::cossin; - -fn cossin_bench(c: &mut Criterion) { - let zi = -0x7304_2531_i32; - let zf = zi as f32 / i32::MAX as f32 * PI; - c.bench_function("cossin(zi)", |b| b.iter(|| cossin(black_box(zi)))); - c.bench_function("zf.sin_cos()", |b| b.iter(|| black_box(zf).sin_cos())); -} - -criterion_group!(benches, cossin_bench); -criterion_main!(benches); diff --git a/dsp/benches/trig.rs b/dsp/benches/trig.rs new file mode 100644 index 0000000..19b6cce --- /dev/null +++ b/dsp/benches/trig.rs @@ -0,0 +1,28 @@ +use core::f32::consts::PI; +use criterion::{black_box, criterion_group, criterion_main, Criterion}; +use dsp::trig::{atan2, cossin}; + +fn atan2_bench(c: &mut Criterion) { + 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; + + c.bench_function("atan2(y, x)", |b| { + b.iter(|| atan2(black_box(yi), black_box(xi))) + }); + c.bench_function("y.atan2(x)", |b| { + b.iter(|| black_box(yf).atan2(black_box(xf))) + }); +} + +fn cossin_bench(c: &mut Criterion) { + let zi = -0x7304_2531_i32; + let zf = zi as f32 / i32::MAX as f32 * PI; + c.bench_function("cossin(zi)", |b| b.iter(|| cossin(black_box(zi)))); + c.bench_function("zf.sin_cos()", |b| b.iter(|| black_box(zf).sin_cos())); +} + +criterion_group!(benches, atan2_bench, cossin_bench); +criterion_main!(benches); From 7e794373f45c942cd3f108b32afe743cf52cf777 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Thu, 17 Dec 2020 14:21:39 -0800 Subject: [PATCH 43/48] atan2: fix output range description --- dsp/src/trig.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index 13ce844..9873d7e 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -18,9 +18,9 @@ include!(concat!(env!("OUT_DIR"), "/cossin_table.rs")); /// # Returns /// /// The angle between the x-axis and the ray to the point (x,y). The -/// result range is from i32::MIN+1 to i32::MAX, where i32::MIN+1 -/// corresponds to an angle of -pi and i32::MAX corresponds to an -/// angle of +pi. +/// 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 mut y = y >> 16; let mut x = x >> 16; From 12d5945d811062cfa06b83cc4d51b8962d0646a3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 18 Dec 2020 15:46:21 +0100 Subject: [PATCH 44/48] dsp/testing: simplify --- dsp/src/testing.rs | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/dsp/src/testing.rs b/dsp/src/testing.rs index 098ec87..4a14f22 100644 --- a/dsp/src/testing.rs +++ b/dsp/src/testing.rs @@ -1,3 +1,4 @@ +#![allow(dead_code)] use super::Complex; pub fn isclose(a: f64, b: f64, rtol: f64, atol: f64) -> bool { @@ -23,9 +24,7 @@ pub fn complex_allclose( rtol: f32, atol: f32, ) -> bool { - let mut result: bool = true; - a.iter().zip(b.iter()).for_each(|(i, j)| { - result &= complex_isclose(*i, *j, rtol, atol); - }); - result + a.iter() + .zip(b) + .all(|(&i, &j)| complex_isclose(i, j, rtol, atol)) } From 8d9af70c19d2606a99b68a70cb03b2834f152f3f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sun, 20 Dec 2020 20:35:26 +0100 Subject: [PATCH 45/48] trig/atan2: refine * use dynamic scaling of the inputs to get accurate ratios (effectively floating point) to maintain accuracy for small arguments * this also allows shifting later and keep more bits * use u32 ratio to keep one more bit * merge the corner case unittests into the big test value list * print rms, absolute and axis-relative angle * simplify the correction expression to get rid of one multiplication * use 5 bit for the correction constant and 15 bits for r * least squares optimal correction constant, this lowers the max error below 5e-5 --- dsp/src/trig.rs | 154 +++++++++++++++++++++--------------------------- 1 file changed, 66 insertions(+), 88 deletions(-) diff --git a/dsp/src/trig.rs b/dsp/src/trig.rs index 9873d7e..3f96609 100644 --- a/dsp/src/trig.rs +++ b/dsp/src/trig.rs @@ -1,4 +1,4 @@ -use super::{shift_round, Complex}; +use super::Complex; use core::f64::consts::PI; include!(concat!(env!("OUT_DIR"), "/cossin_table.rs")); @@ -22,18 +22,25 @@ include!(concat!(env!("OUT_DIR"), "/cossin_table.rs")); /// represents -pi and, equivalently, +pi. i32::MAX represents one /// count less than +pi. pub fn atan2(y: i32, x: i32) -> i32 { - let mut y = y >> 16; - let mut x = x >> 16; + let sign = (x < 0, y < 0); - let sign = ((y >> 14) & 2) | ((x >> 15) & 1); - if sign & 1 == 1 { - x *= -1; - } - if sign & 2 == 2 { - y *= -1; - } + 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: @@ -44,47 +51,37 @@ pub fn atan2(y: i32, x: i32) -> i32 { // to compute and to be more compatible with integer // arithmetic. The approximation technique used here is // - // pi / 4 * x + 0.285 * x * (1 - abs(x)) + // pi / 4 * r + C * r * (1 - abs(r)) // // which is taken from Rajan 2006: Efficient Approximations for // the Arctangent Function. - if y_greater { - core::mem::swap(&mut x, &mut y); - } + // + // 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); - if x == 0 { - return 0; - } - - // We need to share the 31 available non-sign bits between the - // atan argument and constant factors used in the atan - // approximation. Sharing the bits roughly equally between them - // gives good accuracy. Additionally, we cannot increase the - // number of atan argument bits beyond 15 because we must square - // it. - const ATAN_ARGUMENT_BITS: usize = 15; - let ratio = (y << ATAN_ARGUMENT_BITS) / x; - - let mut angle = { - const K1: i32 = ((1. / 4. + 0.285 / PI) - * (1 << (31 - ATAN_ARGUMENT_BITS)) as f64) - as i32; - const K2: i32 = - ((0.285 / PI) * (1 << (31 - ATAN_ARGUMENT_BITS)) as f64) as i32; - - ratio * K1 - K2 * shift_round(ratio * ratio, ATAN_ARGUMENT_BITS) - }; + // `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 = (i32::MAX >> 1) - angle; + angle = (1 << 30) - angle; } - if sign & 1 == 1 { + if sign.0 { angle = i32::MAX - angle; } - if sign & 2 == 2 { - angle *= -1; + if sign.1 { + angle = angle.wrapping_neg(); } angle @@ -162,7 +159,6 @@ pub fn cossin(phase: i32) -> Complex { #[cfg(test)] mod tests { use super::*; - use crate::testing::isclose; use core::f64::consts::PI; fn angle_to_axis(angle: f64) -> f64 { @@ -172,61 +168,43 @@ mod tests { #[test] fn atan2_absolute_error() { - const NUM_VALS: usize = 1_001; - let mut test_vals: [f64; NUM_VALS] = [0.; NUM_VALS]; - let val_bounds: (f64, f64) = (-1., 1.); - let val_delta: f64 = - (val_bounds.1 - val_bounds.0) / (NUM_VALS - 1) as f64; - for i in 0..NUM_VALS { - test_vals[i] = val_bounds.0 + i as f64 * val_delta; + 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; } - let atol: f64 = 4e-5; - let rtol: f64 = 0.127; + 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 actual = (y.atan2(x) as f64 * i16::MAX as f64).round() - / i16::MAX as f64; - let tol = atol + rtol * angle_to_axis(actual).abs(); - let computed = (atan2( - ((y * i16::MAX as f64) as i32) << 16, - ((x * i16::MAX as f64) as i32) << 16, - ) >> 16) as f64 - / i16::MAX as f64 - * PI; + let want = (y as f64 / scale).atan2(x as f64 / scale); + let have = atan2(y, x) as f64 * PI / scale; - if !isclose(computed, actual, 0., tol) { - println!("(x, y) : {}, {}", x, y); - println!("actual : {}", actual); - println!("computed : {}", computed); - println!("tolerance: {}\n", tol); - assert!(false); + 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)); } } } - - // test min and max explicitly - for (x, y) in [ - ((i16::MIN as i32 + 1) << 16, -(1 << 16) as i32), - ((i16::MIN as i32 + 1) << 16, (1 << 16) as i32), - ] - .iter() - { - let yf = *y as f64 / ((i16::MAX as i32) << 16) as f64; - let xf = *x as f64 / ((i16::MAX as i32) << 16) as f64; - let actual = - (yf.atan2(xf) * i16::MAX as f64).round() / i16::MAX as f64; - let computed = (atan2(*y, *x) >> 16) as f64 / i16::MAX as f64 * PI; - let tol = atol + rtol * angle_to_axis(actual).abs(); - - if !isclose(computed, actual, 0., tol) { - println!("(x, y) : {}, {}", *x, *y); - println!("actual : {}", actual); - println!("computed : {}", computed); - println!("tolerance: {}\n", tol); - assert!(false); - } - } + 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); } #[test] From cc42c0c477c03293994692ff8f072c643b1f2f11 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 22 Dec 2020 16:49:12 +0100 Subject: [PATCH 46/48] iir_int: add optimized integer iir implementation --- dsp/src/iir_int.rs | 58 ++++++++++++++++++++++++++++++++++++++++++++++ dsp/src/lib.rs | 1 + 2 files changed, 59 insertions(+) create mode 100644 dsp/src/iir_int.rs diff --git a/dsp/src/iir_int.rs b/dsp/src/iir_int.rs new file mode 100644 index 0000000..1a4a6a9 --- /dev/null +++ b/dsp/src/iir_int.rs @@ -0,0 +1,58 @@ +use serde::{Deserialize, Serialize}; + +pub type IIRState = [i32; 5]; + +fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 { + // Rounding bias, half up + let y0 = ((y0 as i64) << shift) + (1 << (shift - 1)); + let y = x + .iter() + .zip(a) + .map(|(x, a)| *x as i64 * *a as i64) + .fold(y0, |y, xa| y + xa); + (y >> shift) as i32 +} + +/// Integer biquad IIR +/// +/// See `dsp::iir::IIR` for general implementation details. +/// Offset and limiting disabled to suit lowpass applications. +/// Coefficient scaling fixed and optimized. +#[derive(Copy, Clone, Deserialize, Serialize)] +pub struct IIR { + pub ba: IIRState, + // pub y_offset: i32, + // pub y_min: i32, + // pub y_max: i32, +} + +impl IIR { + /// Coefficient fixed point: signed Q2.30. + /// Tailored to low-passes PI, II etc. + const SHIFT: u32 = 30; + + /// Feed a new input value into the filter, update the filter state, and + /// return the new output. Only the state `xy` is modified. + /// + /// # Arguments + /// * `xy` - Current filter state. + /// * `x0` - New input. + pub fn update(&self, xy: &mut IIRState, x0: i32) -> i32 { + let n = self.ba.len(); + debug_assert!(xy.len() == n); + // `xy` contains x0 x1 y0 y1 y2 + // Increment time x1 x2 y1 y2 y3 + // Shift x1 x1 x2 y1 y2 + // This unrolls better than xy.rotate_right(1) + xy.copy_within(0..n - 1, 1); + // Store x0 x0 x1 x2 y1 y2 + xy[0] = x0; + // Compute y0 by multiply-accumulate + let y0 = macc(0, xy, &self.ba, IIR::SHIFT); + // Limit y0 + // let y0 = y0.max(self.y_min).min(self.y_max); + // Store y0 x0 x1 y0 y1 y2 + xy[n / 2] = y0; + y0 + } +} diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 67b1882..fb189fa 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -98,6 +98,7 @@ where } pub mod iir; +pub mod iir_int; pub mod lockin; pub mod pll; pub mod trig; From 13543ce048c9b0c8f63bfc3a15a9f7e48bc9c24a Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Mon, 4 Jan 2021 11:14:27 -0800 Subject: [PATCH 47/48] pll update input is named "x" not "input" --- dsp/src/pll.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dsp/src/pll.rs b/dsp/src/pll.rs index 74377f3..8df750f 100644 --- a/dsp/src/pll.rs +++ b/dsp/src/pll.rs @@ -45,7 +45,7 @@ impl PLL { /// The signal's phase/frequency is reconstructed relative to the sampling period. /// /// Args: - /// * `input`: New input phase sample. + /// * `x`: New input phase sample. /// * `shift_frequency`: Frequency error scaling. The frequency gain per update is /// `1/(1 << shift_frequency)`. /// * `shift_phase`: Phase error scaling. The phase gain is `1/(1 << shift_phase)` From a3cd17fd70031549fcf891208c4bd29914b72421 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Mon, 4 Jan 2021 16:37:46 -0800 Subject: [PATCH 48/48] pin clippy to stable --- .github/workflows/ci.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d0ea705..e8e0e4e 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -29,6 +29,7 @@ jobs: - uses: actions-rs/clippy-check@v1 continue-on-error: true with: + toolchain: stable token: ${{ secrets.GITHUB_TOKEN }} compile: