From 4add34cf9ab6f64cf865420dc8a7dfe396238075 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Mon, 7 Dec 2020 10:22:09 -0800 Subject: [PATCH] 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); + } +}