add cossin LUT

This commit is contained in:
Matt Huszagh 2020-12-07 10:22:09 -08:00
parent a62a3bcae8
commit 4add34cf9a
4 changed files with 189 additions and 0 deletions

1
.gitignore vendored
View File

@ -1,3 +1,4 @@
/target /target
/dsp/target /dsp/target
.gdb_history .gdb_history
/dsp/src/cossin_table.txt

43
dsp/build.rs Normal file
View File

@ -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(_) => (),
}
}

View File

@ -2,9 +2,26 @@
#![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))]
pub type Complex<T> = (T, T); pub type Complex<T> = (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 iir;
pub mod lockin; pub mod lockin;
pub mod pll; pub mod pll;
pub mod trig;
pub mod unwrap; pub mod unwrap;
#[cfg(test)] #[cfg(test)]

128
dsp/src/trig.rs Normal file
View File

@ -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<i32>`
/// value.
pub fn cossin(phase: i32) -> Complex<i32> {
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<f64> = (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<f64> = (
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);
}
}