kai/pounder_test
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

rafactor complex, cossin, atan2

Browse Source
This commit is contained in:
Robert Jördens 2021-01-21 16:12:59 +01:00
parent cb280c3303
commit 0cd2140668
7 changed files with 175 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

135
dsp/src/atan2.rs Normal file
View File

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

17
dsp/src/complex.rs Normal file
View File

@ -0,0 +1,17 @@
use super::atan2;
use serde::{Deserialize, Serialize};
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
pub struct Complex<T>(pub T, pub T);
impl Complex<i32> {
pub fn power(&self) -> i32 {
(((self.0 as i64) * (self.0 as i64)
+ (self.1 as i64) * (self.1 as i64))
>> 32) as i32
}
pub fn phase(&self) -> i32 {
atan2(self.1, self.0)
}
}

141
dsp/src/trig.rs → dsp/src/cossin.rs
View File

@ -3,90 +3,6 @@ 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
/// represents -pi and, equivalently, +pi. i32::MAX represents one
/// count less than +pi.
pub fn atan2(y: i32, x: i32) -> i32 {
let sign = (x < 0, y < 0);
let mut y = y.wrapping_abs() as u32;
let mut x = x.wrapping_abs() as u32;
let y_greater = y > x;
if y_greater {
core::mem::swap(&mut y, &mut x);
}
let z = (16 - y.leading_zeros() as i32).max(0);
x >>= z;
if x == 0 {
return 0;
}
y >>= z;
let r = (y << 16) / x;
debug_assert!(r <= 1 << 16);
// Uses the general procedure described in the following
// Mathematics stack exchange answer:
//
// https://math.stackexchange.com/a/1105038/583981
//
// The atan approximation method has been modified to be cheaper
// to compute and to be more compatible with integer
// arithmetic. The approximation technique used here is
//
// pi / 4 * r + C * r * (1 - abs(r))
//
// which is taken from Rajan 2006: Efficient Approximations for
// the Arctangent Function.
//
// The least mean squared error solution is C = 0.279 (no the 0.285 that
// Rajan uses). K = C*4/pi.
// Q5 for K provides sufficient correction accuracy while preserving
// as much smoothness of the quadratic correction as possible.
const FP_K: usize = 5;
const K: u32 = (0.35489 * (1 << FP_K) as f64) as u32;
// debug_assert!(K == 11);
// `r` is unsigned Q16.16 and <= 1
// `angle` is signed Q1.31 with 1 << 31 == +- pi
// Since K < 0.5 and r*(1 - r) <= 0.25 the correction product can use
// 4 bits for K, and 15 bits for r and 1-r to remain within the u32 range.
let mut angle = ((r << 13)
+ ((K * (r >> 1) * ((1 << 15) - (r >> 1))) >> (FP_K + 1)))
as i32;
if y_greater {
angle = (1 << 30) - angle;
}
if sign.0 {
angle = i32::MAX - angle;
}
if sign.1 {
angle = angle.wrapping_neg();
}
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)
@ -161,66 +77,21 @@ mod tests {
use super::*;
use core::f64::consts::PI;
fn angle_to_axis(angle: f64) -> f64 {
let angle = angle % (PI / 2.);
(PI / 2. - angle).min(angle)
}
#[test]
fn atan2_absolute_error() {
const N: usize = 321;
let mut test_vals = [0i32; N + 4];
let scale = (1i64 << 31) as f64;
for i in 0..N {
test_vals[i] = (scale * (-1. + 2. * i as f64 / N as f64)) as i32;
}
assert!(test_vals.contains(&i32::MIN));
test_vals[N] = i32::MAX;
test_vals[N + 1] = 0;
test_vals[N + 2] = -1;
test_vals[N + 3] = 1;
let mut rms_err = 0f64;
let mut abs_err = 0f64;
let mut rel_err = 0f64;
for &x in test_vals.iter() {
for &y in test_vals.iter() {
let want = (y as f64 / scale).atan2(x as f64 / scale);
let have = atan2(y, x) as f64 * PI / scale;
let err = (have - want).abs();
abs_err = abs_err.max(err);
rms_err += err * err;
if err > 3e-5 {
rel_err = rel_err.max(err / angle_to_axis(want));
}
}
}
rms_err = rms_err.sqrt() / test_vals.len() as f64;
println!("max abs err: {:.2e}", abs_err);
println!("rms abs err: {:.2e}", rms_err);
println!("max rel err: {:.2e}", rel_err);
assert!(abs_err < 5e-3);
assert!(rms_err < 3e-3);
assert!(rel_err < 0.6);
}
#[test]
fn cossin_error_max_rms_all_phase() {
// Constant amplitude error due to LUT data range.
const AMPLITUDE: f64 = ((1i64 << 31) - (1i64 << 15)) as _;
const MAX_PHASE: f64 = (1i64 << 32) as _;
let mut rms_err = Complex(0., 0.);
let mut sum_err = Complex(0., 0.);
let mut max_err = Complex(0f64, 0.);
let mut sum = Complex(0., 0.);
let mut demod = Complex(0., 0.);
let mut rms_err = Complex(0f64, 0f64);
let mut sum_err = Complex(0f64, 0f64);
let mut max_err = Complex(0f64, 0f64);
let mut sum = Complex(0f64, 0f64);
let mut demod = Complex(0f64, 0f64);
// use std::{fs::File, io::{BufWriter, prelude::*}, path::Path};
// let mut file = BufWriter::new(File::create(Path::new("data.bin")).unwrap());
// log2 of the number of phase values to check
const PHASE_DEPTH: usize = 20;
for phase in 0..(1 << PHASE_DEPTH) {

12
dsp/src/lib.rs
View File

@ -2,10 +2,6 @@
#![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))]
use core::ops::{Add, Mul, Neg};
use serde::{Deserialize, Serialize};
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
pub struct Complex<T>(pub T, pub T);
/// Bit shift, round up half.
///
@ -116,13 +112,19 @@ where
.fold(y0, |y, xa| y + xa)
}
mod atan2;
mod complex;
mod cossin;
pub mod iir;
pub mod iir_int;
pub mod lockin;
pub mod pll;
pub mod reciprocal_pll;
pub mod trig;
pub mod unwrap;
pub use atan2::atan2;
pub use complex::Complex;
pub use cossin::cossin;
#[cfg(test)]
pub mod testing;

21
dsp/src/lockin.rs
View File

@ -1,8 +1,4 @@
use super::{
iir_int,
trig::{atan2, cossin},
Complex,
};
use super::{cossin, iir_int, Complex};
use serde::{Deserialize, Serialize};
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
@ -37,19 +33,4 @@ impl Lockin {
),
)
}
pub fn iq(&self) -> Complex<i32> {
Complex(self.iir_state[0].get_y(0), self.iir_state[1].get_y(0))
}
pub fn power(&self) -> i32 {
let iq = self.iq();
(((iq.0 as i64) * (iq.0 as i64) + (iq.1 as i64) * (iq.1 as i64)) >> 32)
as i32
}
pub fn phase(&self) -> i32 {
let iq = self.iq();
atan2(iq.1, iq.0)
}
}

4
dsp/tests/lockin.rs
View File

@ -1,7 +1,7 @@
use dsp::{
atan2, cossin,
iir_int::{IIRState, IIR},
reciprocal_pll::TimestampHandler,
trig::{atan2, cossin},
Complex,
};
@ -185,7 +185,7 @@ fn adc_batch_timestamps(
/// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html
///
/// # Args
/// * `fc` - Corner frequency, or 3dB cutoff frequency (in Hz).
/// * `fc` - Corner frequency, or 3dB cutoff frequency (in units of sample rate).
/// * `q` - Quality factor (1/sqrt(2) for critical).
/// * `k` - DC gain.
///

14
src/bin/lockin.rs
View File

@ -70,12 +70,12 @@ const APP: () = {
stabilizer.dacs.0.start();
stabilizer.dacs.1.start();
// Start sampling ADCs.
stabilizer.adc_dac_timer.start();
// Start recording digital input timestamps.
stabilizer.timestamp_timer.start();
// Start sampling ADCs.
stabilizer.adc_dac_timer.start();
init::LateResources {
afes: stabilizer.afes,
adcs: stabilizer.adcs,
@ -127,7 +127,7 @@ const APP: () = {
.pll
.update(c.resources.timestamper.latest_timestamp());
// Harmonic index to demodulate
// Harmonic index of the LO: -1 to _de_modulate the fundamental
let harmonic: i32 = -1;
// Demodulation LO phase offset
let phase_offset: i32 = 0;
@ -139,13 +139,13 @@ const APP: () = {
// Convert to signed, MSB align the ADC sample.
let input = (adc_samples[0][i] as i16 as i32) << 16;
// Obtain demodulated, filtered IQ sample.
lockin.update(input, sample_phase);
let output = lockin.update(input, sample_phase);
// Advance the sample phase.
sample_phase = sample_phase.wrapping_add(sample_frequency);
// Convert from IQ to power and phase.
let mut power = lockin.power() as _;
let mut phase = lockin.phase() as _;
let mut power = output.power() as _;
let mut phase = output.phase() as _;
// Filter power and phase through IIR filters.
// Note: Normalization to be done in filters. Phase will wrap happily.