pounder_test/dsp/src/cossin.rs

154 lines
4.9 KiB
Rust
Raw Normal View History

use core::f64::consts::PI;
2020-12-08 02:22:09 +08:00
2020-12-11 00:17:09 +08:00
include!(concat!(env!("OUT_DIR"), "/cossin_table.rs"));
2020-12-08 02:22:09 +08:00
/// 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)
2020-12-08 02:22:09 +08:00
///
/// # Arguments
/// * `phase` - 32-bit phase.
2020-12-08 02:22:09 +08:00
///
/// # Returns
2021-02-01 19:07:03 +08:00
/// The cos and sin values of the provided phase as a `(i32, i32)`
2021-03-01 20:29:59 +08:00
/// tuple. With a 7-bit deep LUT there is 9e-6 max and 4e-6 RMS error
/// in each quadrature over 20 bit phase.
2021-02-01 19:07:03 +08:00
pub fn cossin(phase: i32) -> (i32, i32) {
// Phase bits excluding the three highest MSB
const OCTANT_BITS: usize = 32 - 3;
// 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;
let mut phase = phase;
if octant & 1 != 0 {
2020-12-08 02:22:09 +08:00
// phase = pi/4 - phase
phase = !phase;
2020-12-08 02:22:09 +08:00
}
// Mask off octant bits. This leaves the angle in the range [0, pi/4).
phase &= (1 << OCTANT_BITS) - 1;
// 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;
let lookup = COSSIN[(phase >> ALIGN_MSB) as usize];
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.
2021-03-01 20:29:59 +08:00
phase -= 1 << (ALIGN_MSB - 1);
// Cancel the -1 bias that was conditionally introduced above.
// This lowers the DC spur from 2e-8 to 2e-10 magnitude.
// phase += (octant & 1) as i32;
// Fixed point pi/4.
2021-03-01 20:29:59 +08:00
const PI4: i32 = (PI / 4. * (1 << 16) as f64) as _;
// No rounding bias necessary here since we keep enough low bits.
let dphi = (phase * PI4) >> 16;
// 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 & 0xffff) as i32 + (1 << 16);
let mut sin = (lookup >> 16) as i32;
let dcos = (sin * dphi) >> COSSIN_DEPTH;
let dsin = (cos * dphi) >> (COSSIN_DEPTH + 1);
cos = (cos << (ALIGN_MSB - 1)) - dcos;
sin = (sin << ALIGN_MSB) + dsin;
// Unmap using octant bits.
if octant & 2 != 0 {
core::mem::swap(&mut sin, &mut cos);
2020-12-08 02:22:09 +08:00
}
if octant & 4 != 0 {
cos = -cos;
2020-12-08 02:22:09 +08:00
}
if octant & 8 != 0 {
sin = -sin;
2020-12-08 02:22:09 +08:00
}
2021-02-01 19:07:03 +08:00
(cos, sin)
2020-12-08 02:22:09 +08:00
}
#[cfg(test)]
mod tests {
use super::*;
use core::f64::consts::PI;
2020-12-08 02:22:09 +08:00
#[test]
fn cossin_error_max_rms_all_phase() {
// Constant amplitude error due to LUT data range.
2021-02-27 22:37:15 +08:00
const AMPLITUDE: f64 = (1i64 << 31) as f64 - 0.85 * (1i64 << 15) as f64;
2021-01-18 05:19:14 +08:00
const MAX_PHASE: f64 = (1i64 << 32) as _;
2021-02-18 20:17:24 +08:00
let mut rms_err = (0f64, 0f64);
let mut sum_err = (0f64, 0f64);
let mut max_err = (0f64, 0f64);
let mut sum = (0f64, 0f64);
let mut demod = (0f64, 0f64);
2020-12-13 19:36:23 +08:00
// use std::{fs::File, io::{BufWriter, prelude::*}, path::Path};
// let mut file = BufWriter::new(File::create(Path::new("data.bin")).unwrap());
2021-01-21 23:12:59 +08:00
// log2 of the number of phase values to check
const PHASE_DEPTH: usize = 20;
for phase in 0..(1 << PHASE_DEPTH) {
let phase = (phase << (32 - PHASE_DEPTH)) as i32;
let have = cossin(phase);
2020-12-13 19:36:23 +08:00
// 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);
2020-12-08 02:22:09 +08:00
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());
2020-12-08 02:22:09 +08:00
}
2021-03-01 20:29:59 +08:00
rms_err.0 /= (1 << PHASE_DEPTH) as f64;
rms_err.1 /= (1 << PHASE_DEPTH) as f64;
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);
2021-03-01 20:29:59 +08:00
assert!(sum.1.abs() < 3e-8);
assert!(demod.0.abs() < 4e-10);
2021-03-01 20:29:59 +08:00
assert!(demod.1.abs() < 1e-8);
assert!(sum_err.0.abs() < 4e-10);
assert!(sum_err.1.abs() < 4e-10);
2021-03-01 20:29:59 +08:00
assert!(rms_err.0.sqrt() < 4e-6);
assert!(rms_err.1.sqrt() < 4e-6);
2021-03-01 20:29:59 +08:00
assert!(max_err.0 < 1e-5);
assert!(max_err.1 < 1e-5);
2020-12-08 02:22:09 +08:00
}
}