GitHub
commit
7e82fce4f0
99
Cargo.lock
generated
99
Cargo.lock
generated
@ -355,8 +355,7 @@ dependencies = [
|
||||
"criterion",
|
||||
"libm",
|
||||
"ndarray",
|
||||
"ndarray-stats",
|
||||
"rand 0.8.3",
|
||||
"rand",
|
||||
"serde",
|
||||
]
|
||||
|
||||
@ -433,17 +432,6 @@ dependencies = [
|
||||
"version_check",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "getrandom"
|
||||
version = "0.1.16"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "8fc3cb4d91f53b50155bdcfd23f6a4c39ae1969c2ae85982b135750cccaf5fce"
|
||||
dependencies = [
|
||||
"cfg-if",
|
||||
"libc",
|
||||
"wasi 0.9.0+wasi-snapshot-preview1",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "getrandom"
|
||||
version = "0.2.2"
|
||||
@ -452,7 +440,7 @@ checksum = "c9495705279e7140bf035dde1f6e750c162df8b625267cd52cc44e0b156732c8"
|
||||
dependencies = [
|
||||
"cfg-if",
|
||||
"libc",
|
||||
"wasi 0.10.2+wasi-snapshot-preview1",
|
||||
"wasi",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
@ -634,30 +622,6 @@ dependencies = [
|
||||
"rawpointer",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "ndarray-stats"
|
||||
version = "0.4.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "22c95a780960082c5746f6bf0ab22d4a3b8cee72bf580acfe9f1e10bc5ea8152"
|
||||
dependencies = [
|
||||
"indexmap",
|
||||
"itertools 0.9.0",
|
||||
"ndarray",
|
||||
"noisy_float",
|
||||
"num-integer",
|
||||
"num-traits",
|
||||
"rand 0.7.3",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "noisy_float"
|
||||
version = "0.1.13"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "a14c16cde392a1cd18084ffd8348cb8937525130e62f0478d72dcc683698809d"
|
||||
dependencies = [
|
||||
"num-traits",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num-complex"
|
||||
version = "0.3.1"
|
||||
@ -782,19 +746,6 @@ version = "0.2.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "e2a38df5b15c8d5c7e8654189744d8e396bddc18ad48041a500ce52d6948941f"
|
||||
|
||||
[[package]]
|
||||
name = "rand"
|
||||
version = "0.7.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "6a6b1679d49b24bbfe0c803429aa1874472f50d9b363131f0e89fc356b544d03"
|
||||
dependencies = [
|
||||
"getrandom 0.1.16",
|
||||
"libc",
|
||||
"rand_chacha 0.2.2",
|
||||
"rand_core 0.5.1",
|
||||
"rand_hc 0.2.0",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rand"
|
||||
version = "0.8.3"
|
||||
@ -802,19 +753,9 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "0ef9e7e66b4468674bfcb0c81af8b7fa0bb154fa9f28eb840da5c447baeb8d7e"
|
||||
dependencies = [
|
||||
"libc",
|
||||
"rand_chacha 0.3.0",
|
||||
"rand_core 0.6.1",
|
||||
"rand_hc 0.3.0",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rand_chacha"
|
||||
version = "0.2.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "f4c8ed856279c9737206bf725bf36935d8666ead7aa69b52be55af369d193402"
|
||||
dependencies = [
|
||||
"ppv-lite86",
|
||||
"rand_core 0.5.1",
|
||||
"rand_chacha",
|
||||
"rand_core",
|
||||
"rand_hc",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
@ -824,16 +765,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "e12735cf05c9e10bf21534da50a147b924d555dc7a547c42e6bb2d5b6017ae0d"
|
||||
dependencies = [
|
||||
"ppv-lite86",
|
||||
"rand_core 0.6.1",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rand_core"
|
||||
version = "0.5.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "90bde5296fc891b0cef12a6d03ddccc162ce7b2aff54160af9338f8d40df6d19"
|
||||
dependencies = [
|
||||
"getrandom 0.1.16",
|
||||
"rand_core",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
@ -842,16 +774,7 @@ version = "0.6.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "c026d7df8b298d90ccbbc5190bd04d85e159eaf5576caeacf8741da93ccbd2e5"
|
||||
dependencies = [
|
||||
"getrandom 0.2.2",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rand_hc"
|
||||
version = "0.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "ca3129af7b92a17112d59ad498c6f81eaf463253766b90396d39ea7a39d6613c"
|
||||
dependencies = [
|
||||
"rand_core 0.5.1",
|
||||
"getrandom",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
@ -860,7 +783,7 @@ version = "0.3.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "3190ef7066a446f2e7f42e239d161e905420ccab01eb967c9eb27d21b2322a73"
|
||||
dependencies = [
|
||||
"rand_core 0.6.1",
|
||||
"rand_core",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
@ -1190,12 +1113,6 @@ dependencies = [
|
||||
"winapi-util",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "wasi"
|
||||
version = "0.9.0+wasi-snapshot-preview1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "cccddf32554fecc6acb585f82a32a72e28b48f8c4c1883ddfeeeaa96f7d8e519"
|
||||
|
||||
[[package]]
|
||||
name = "wasi"
|
||||
version = "0.10.2+wasi-snapshot-preview1"
|
||||
|
||||
@ -12,7 +12,6 @@ serde = { version = "1.0", features = ["derive"], default-features = false }
|
||||
criterion = "0.3"
|
||||
rand = "0.8"
|
||||
ndarray = "0.14"
|
||||
ndarray-stats = "0.4"
|
||||
|
||||
[[bench]]
|
||||
name = "micro"
|
||||
|
||||
@ -50,7 +50,7 @@ fn pll_bench(c: &mut Criterion) {
|
||||
|
||||
fn iir_int_bench(c: &mut Criterion) {
|
||||
let dut = iir_int::IIR::default();
|
||||
let mut xy = iir_int::IIRState::default();
|
||||
let mut xy = iir_int::Vec5::default();
|
||||
c.bench_function("int_iir::IIR::update(s, x)", |b| {
|
||||
b.iter(|| dut.update(&mut xy, black_box(0x2832)))
|
||||
});
|
||||
@ -58,7 +58,7 @@ fn iir_int_bench(c: &mut Criterion) {
|
||||
|
||||
fn iir_bench(c: &mut Criterion) {
|
||||
let dut = iir::IIR::default();
|
||||
let mut xy = iir::IIRState::default();
|
||||
let mut xy = iir::Vec5::default();
|
||||
c.bench_function("int::IIR::update(s, x)", |b| {
|
||||
b.iter(|| dut.update(&mut xy, black_box(0.32241)))
|
||||
});
|
||||
|
||||
21
dsp/src/accu.rs
Normal file
21
dsp/src/accu.rs
Normal file
@ -0,0 +1,21 @@
|
||||
#[derive(Copy, Clone, Default, PartialEq, Debug)]
|
||||
pub struct Accu {
|
||||
state: i32,
|
||||
step: i32,
|
||||
}
|
||||
|
||||
impl Accu {
|
||||
pub fn new(state: i32, step: i32) -> Self {
|
||||
Self { state, step }
|
||||
}
|
||||
}
|
||||
|
||||
impl Iterator for Accu {
|
||||
type Item = i32;
|
||||
#[inline]
|
||||
fn next(&mut self) -> Option<i32> {
|
||||
let s = self.state;
|
||||
self.state = self.state.wrapping_add(self.step);
|
||||
Some(s)
|
||||
}
|
||||
}
|
||||
@ -1,7 +1,6 @@
|
||||
use super::{atan2, cossin};
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
#[derive(Copy, Clone, Default, PartialEq, Debug, Deserialize, Serialize)]
|
||||
#[derive(Copy, Clone, Default, PartialEq, Debug)]
|
||||
pub struct Complex<T>(pub T, pub T);
|
||||
|
||||
impl Complex<i32> {
|
||||
@ -16,8 +15,9 @@ impl Complex<i32> {
|
||||
/// Complex::<i32>::from_angle(-1 << 30); // -pi/2
|
||||
/// ```
|
||||
#[inline(always)]
|
||||
pub fn from_angle(angle: i32) -> Complex<i32> {
|
||||
cossin(angle)
|
||||
pub fn from_angle(angle: i32) -> Self {
|
||||
let (c, s) = cossin(angle);
|
||||
Self(c, s)
|
||||
}
|
||||
|
||||
/// Return the absolute square (the squared magnitude).
|
||||
|
||||
@ -1,4 +1,3 @@
|
||||
use super::Complex;
|
||||
use core::f64::consts::PI;
|
||||
|
||||
include!(concat!(env!("OUT_DIR"), "/cossin_table.rs"));
|
||||
@ -11,10 +10,10 @@ include!(concat!(env!("OUT_DIR"), "/cossin_table.rs"));
|
||||
/// * `phase` - 32-bit phase.
|
||||
///
|
||||
/// # Returns
|
||||
/// The cos and sin values of the provided phase as a `Complex<i32>`
|
||||
/// value. With a 7-bit deep LUT there is 1e-5 max and 6e-8 RMS error
|
||||
/// The cos and sin values of the provided phase as a `(i32, i32)`
|
||||
/// tuple. 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<i32> {
|
||||
pub fn cossin(phase: i32) -> (i32, i32) {
|
||||
// Phase bits excluding the three highes MSB
|
||||
const OCTANT_BITS: usize = 32 - 3;
|
||||
|
||||
@ -69,12 +68,13 @@ pub fn cossin(phase: i32) -> Complex<i32> {
|
||||
sin *= -1;
|
||||
}
|
||||
|
||||
Complex(cos, sin)
|
||||
(cos, sin)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
use crate::Complex;
|
||||
use core::f64::consts::PI;
|
||||
|
||||
#[test]
|
||||
|
||||
@ -12,7 +12,7 @@ use core::f32;
|
||||
/// coefficients (b0, b1, b2) followd by the negated feed-back coefficients
|
||||
/// (-a1, -a2), all five normalized such that a0 = 1.
|
||||
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
|
||||
pub struct IIRState(pub [f32; 5]);
|
||||
pub struct Vec5(pub [f32; 5]);
|
||||
|
||||
/// IIR configuration.
|
||||
///
|
||||
@ -41,7 +41,7 @@ pub struct IIRState(pub [f32; 5]);
|
||||
/// implementation of transfer functions beyond bequadratic terms.
|
||||
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
|
||||
pub struct IIR {
|
||||
pub ba: IIRState,
|
||||
pub ba: Vec5,
|
||||
pub y_offset: f32,
|
||||
pub y_min: f32,
|
||||
pub y_max: f32,
|
||||
@ -108,7 +108,7 @@ impl IIR {
|
||||
/// # Arguments
|
||||
/// * `xy` - Current filter state.
|
||||
/// * `x0` - New input.
|
||||
pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 {
|
||||
pub fn update(&self, xy: &mut Vec5, x0: f32) -> f32 {
|
||||
let n = self.ba.0.len();
|
||||
debug_assert!(xy.0.len() == n);
|
||||
// `xy` contains x0 x1 y0 y1 y2
|
||||
|
||||
@ -5,9 +5,9 @@ use serde::{Deserialize, Serialize};
|
||||
/// This struct is used to hold the x/y input/output data vector or the b/a coefficient
|
||||
/// vector.
|
||||
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
|
||||
pub struct IIRState(pub [i32; 5]);
|
||||
pub struct Vec5(pub [i32; 5]);
|
||||
|
||||
impl IIRState {
|
||||
impl Vec5 {
|
||||
/// Lowpass biquad filter using cutoff and sampling frequencies. Taken from:
|
||||
/// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html
|
||||
///
|
||||
@ -19,7 +19,7 @@ impl IIRState {
|
||||
///
|
||||
/// # Returns
|
||||
/// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 is set to -1.
|
||||
pub fn lowpass(f: f32, q: f32, k: f32) -> IIRState {
|
||||
pub fn lowpass(f: f32, q: f32, k: f32) -> Self {
|
||||
// 3rd order Taylor approximation of sin and cos.
|
||||
let f = f * 2. * PI;
|
||||
let fsin = f - f * f * f / 6.;
|
||||
@ -31,7 +31,7 @@ impl IIRState {
|
||||
let a1 = (2. * fcos / a0) as _;
|
||||
let a2 = ((alpha - 1.) / a0) as _;
|
||||
|
||||
IIRState([b0, 2 * b0, b0, a1, a2])
|
||||
Self([b0, 2 * b0, b0, a1, a2])
|
||||
}
|
||||
}
|
||||
|
||||
@ -53,7 +53,7 @@ fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
|
||||
/// Coefficient scaling fixed and optimized.
|
||||
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
|
||||
pub struct IIR {
|
||||
pub ba: IIRState,
|
||||
pub ba: Vec5,
|
||||
// pub y_offset: i32,
|
||||
// pub y_min: i32,
|
||||
// pub y_max: i32,
|
||||
@ -70,7 +70,7 @@ impl IIR {
|
||||
/// # Arguments
|
||||
/// * `xy` - Current filter state.
|
||||
/// * `x0` - New input.
|
||||
pub fn update(&self, xy: &mut IIRState, x0: i32) -> i32 {
|
||||
pub fn update(&self, xy: &mut Vec5, x0: i32) -> i32 {
|
||||
let n = self.ba.0.len();
|
||||
debug_assert!(xy.0.len() == n);
|
||||
// `xy` contains x0 x1 y0 y1 y2
|
||||
@ -92,11 +92,11 @@ impl IIR {
|
||||
|
||||
#[cfg(test)]
|
||||
mod test {
|
||||
use super::IIRState;
|
||||
use super::Vec5;
|
||||
|
||||
#[test]
|
||||
fn lowpass_gen() {
|
||||
let ba = IIRState::lowpass(1e-3, 1. / 2f32.sqrt(), 2.);
|
||||
let ba = Vec5::lowpass(1e-3, 1. / 2f32.sqrt(), 2.);
|
||||
println!("{:?}", ba.0);
|
||||
}
|
||||
}
|
||||
|
||||
@ -112,6 +112,7 @@ where
|
||||
.fold(y0, |y, xa| y + xa)
|
||||
}
|
||||
|
||||
pub mod accu;
|
||||
mod atan2;
|
||||
mod complex;
|
||||
mod cossin;
|
||||
@ -122,6 +123,7 @@ pub mod pll;
|
||||
pub mod rpll;
|
||||
pub mod unwrap;
|
||||
|
||||
pub use accu::Accu;
|
||||
pub use atan2::atan2;
|
||||
pub use complex::Complex;
|
||||
pub use cossin::cossin;
|
||||
|
||||
@ -1,176 +1,44 @@
|
||||
use super::{iir_int, Complex};
|
||||
use super::{
|
||||
iir_int::{Vec5, IIR},
|
||||
Complex,
|
||||
};
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
|
||||
pub struct Lockin {
|
||||
iir: iir_int::IIR,
|
||||
iir_state: [iir_int::IIRState; 2],
|
||||
iir: IIR,
|
||||
state: [Vec5; 2],
|
||||
}
|
||||
|
||||
impl Lockin {
|
||||
pub fn new(ba: &iir_int::IIRState) -> Self {
|
||||
let mut iir = iir_int::IIR::default();
|
||||
iir.ba.0.copy_from_slice(&ba.0);
|
||||
Lockin {
|
||||
iir,
|
||||
iir_state: [iir_int::IIRState::default(); 2],
|
||||
/// Create a new Lockin with given IIR coefficients.
|
||||
pub fn new(ba: Vec5) -> Self {
|
||||
Self {
|
||||
iir: IIR {
|
||||
ba,
|
||||
..Default::default()
|
||||
},
|
||||
state: [Vec5::default(); 2],
|
||||
}
|
||||
}
|
||||
|
||||
pub fn update(&mut self, signal: i32, phase: i32) -> Complex<i32> {
|
||||
/// Update the lockin with a sample taken at a given phase.
|
||||
pub fn update(&mut self, sample: i32, phase: i32) -> Complex<i32> {
|
||||
// Get the LO signal for demodulation.
|
||||
let m = Complex::from_angle(phase);
|
||||
let lo = Complex::from_angle(phase);
|
||||
|
||||
// Mix with the LO signal, filter with the IIR lowpass,
|
||||
// return IQ (in-phase and quadrature) data.
|
||||
// Note: 32x32 -> 64 bit multiplications are pretty much free.
|
||||
Complex(
|
||||
self.iir.update(
|
||||
&mut self.iir_state[0],
|
||||
((signal as i64 * m.0 as i64) >> 32) as _,
|
||||
&mut self.state[0],
|
||||
((sample as i64 * lo.0 as i64) >> 32) as _,
|
||||
),
|
||||
self.iir.update(
|
||||
&mut self.iir_state[1],
|
||||
((signal as i64 * m.1 as i64) >> 32) as _,
|
||||
&mut self.state[1],
|
||||
((sample as i64 * lo.1 as i64) >> 32) as _,
|
||||
),
|
||||
)
|
||||
}
|
||||
|
||||
pub fn feed<I: IntoIterator<Item = i32>>(
|
||||
&mut self,
|
||||
signal: I,
|
||||
phase: i32,
|
||||
frequency: i32,
|
||||
) -> Option<Complex<i32>> {
|
||||
let mut phase = phase;
|
||||
|
||||
signal
|
||||
.into_iter()
|
||||
.map(|s| {
|
||||
phase = phase.wrapping_add(frequency);
|
||||
self.update(s, phase)
|
||||
})
|
||||
.last()
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod test {
|
||||
use crate::{
|
||||
iir_int::IIRState,
|
||||
lockin::Lockin,
|
||||
rpll::RPLL,
|
||||
testing::{isclose, max_error},
|
||||
Complex,
|
||||
};
|
||||
|
||||
use std::f64::consts::PI;
|
||||
use std::vec::Vec;
|
||||
|
||||
/// ADC full scale in machine units (16 bit signed).
|
||||
const ADC_SCALE: f64 = ((1 << 15) - 1) as _;
|
||||
|
||||
struct PllLockin {
|
||||
harmonic: i32,
|
||||
phase: i32,
|
||||
lockin: Lockin,
|
||||
}
|
||||
|
||||
impl PllLockin {
|
||||
pub fn new(harmonic: i32, phase: i32, iir: &IIRState) -> Self {
|
||||
PllLockin {
|
||||
harmonic,
|
||||
phase,
|
||||
lockin: Lockin::new(iir),
|
||||
}
|
||||
}
|
||||
|
||||
pub fn update(
|
||||
&mut self,
|
||||
input: Vec<i16>,
|
||||
phase: i32,
|
||||
frequency: i32,
|
||||
) -> Complex<i32> {
|
||||
let sample_frequency = frequency.wrapping_mul(self.harmonic);
|
||||
let mut sample_phase =
|
||||
self.phase.wrapping_add(phase.wrapping_mul(self.harmonic));
|
||||
input
|
||||
.iter()
|
||||
.map(|&s| {
|
||||
let input = (s as i32) << 16;
|
||||
let signal =
|
||||
self.lockin.update(input, sample_phase.wrapping_neg());
|
||||
sample_phase = sample_phase.wrapping_add(sample_frequency);
|
||||
signal
|
||||
})
|
||||
.last()
|
||||
.unwrap_or(Complex::default())
|
||||
}
|
||||
}
|
||||
|
||||
/// Single-frequency sinusoid.
|
||||
#[derive(Copy, Clone)]
|
||||
struct Tone {
|
||||
// Frequency (in Hz).
|
||||
frequency: f64,
|
||||
// Phase offset (in radians).
|
||||
phase: f64,
|
||||
// Amplitude in dBFS (decibels relative to full-scale).
|
||||
// A 16-bit ADC has a minimum dBFS for each sample of -90.
|
||||
amplitude_dbfs: f64,
|
||||
}
|
||||
|
||||
/// Convert dBFS to a linear ratio.
|
||||
fn linear(dbfs: f64) -> f64 {
|
||||
10f64.powf(dbfs / 20.)
|
||||
}
|
||||
|
||||
impl Tone {
|
||||
fn eval(&self, time: f64) -> f64 {
|
||||
linear(self.amplitude_dbfs)
|
||||
* (self.phase + self.frequency * time).cos()
|
||||
}
|
||||
}
|
||||
|
||||
/// Generate a full batch of samples with size `sample_buffer_size` starting at `time_offset`.
|
||||
fn sample_tones(
|
||||
tones: &Vec<Tone>,
|
||||
time_offset: f64,
|
||||
sample_buffer_size: u32,
|
||||
) -> Vec<i16> {
|
||||
(0..sample_buffer_size)
|
||||
.map(|i| {
|
||||
let time = 2. * PI * (time_offset + i as f64);
|
||||
let x: f64 = tones.iter().map(|t| t.eval(time)).sum();
|
||||
assert!(-1. < x && x < 1.);
|
||||
(x * ADC_SCALE) as i16
|
||||
})
|
||||
.collect()
|
||||
}
|
||||
|
||||
/// Total maximum noise amplitude of the input signal after 2nd order lowpass filter.
|
||||
/// Constructive interference is assumed.
|
||||
///
|
||||
/// # Args
|
||||
/// * `tones` - Noise sources at the ADC input.
|
||||
/// * `frequency` - Frequency of the signal of interest.
|
||||
/// * `corner` - Low-pass filter 3dB corner cutoff frequency.
|
||||
///
|
||||
/// # Returns
|
||||
/// Upper bound of the total amplitude of all noise sources in linear units full scale.
|
||||
fn sampled_noise_amplitude(
|
||||
tones: &Vec<Tone>,
|
||||
frequency: f64,
|
||||
corner: f64,
|
||||
) -> f64 {
|
||||
tones
|
||||
.iter()
|
||||
.map(|t| {
|
||||
let df = (t.frequency - frequency) / corner;
|
||||
// Assuming a 2nd order lowpass filter: 40dB/decade.
|
||||
linear(t.amplitude_dbfs - 40. * df.abs().max(1.).log10())
|
||||
})
|
||||
.sum::<f64>()
|
||||
.max(1. / ADC_SCALE / 2.) // 1/2 LSB from quantization
|
||||
}
|
||||
}
|
||||
|
||||
@ -22,8 +22,8 @@ impl RPLL {
|
||||
///
|
||||
/// Returns:
|
||||
/// Initialized RPLL instance.
|
||||
pub fn new(dt2: u8) -> RPLL {
|
||||
RPLL {
|
||||
pub fn new(dt2: u8) -> Self {
|
||||
Self {
|
||||
dt2,
|
||||
..Default::default()
|
||||
}
|
||||
@ -84,7 +84,6 @@ impl RPLL {
|
||||
mod test {
|
||||
use super::RPLL;
|
||||
use ndarray::prelude::*;
|
||||
use ndarray_stats::QuantileExt;
|
||||
use rand::{prelude::*, rngs::StdRng};
|
||||
use std::vec::Vec;
|
||||
|
||||
@ -108,7 +107,7 @@ mod test {
|
||||
|
||||
impl Harness {
|
||||
fn default() -> Self {
|
||||
Harness {
|
||||
Self {
|
||||
rpll: RPLL::new(8),
|
||||
dt2: 8,
|
||||
shift_frequency: 9,
|
||||
@ -254,7 +253,7 @@ mod test {
|
||||
h.shift_frequency = 10;
|
||||
h.shift_phase = 9;
|
||||
|
||||
h.measure(1 << 16, [5e-7, 3e-2, 3e-2, 2e-2]);
|
||||
h.measure(1 << 16, [5e-7, 3e-2, 2e-5, 2e-2]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
|
||||
@ -34,9 +34,9 @@ const APP: () = {
|
||||
net_interface: hardware::Ethernet,
|
||||
|
||||
// Format: iir_state[ch][cascade-no][coeff]
|
||||
#[init([[iir::IIRState([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
|
||||
iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2],
|
||||
#[init([[iir::IIR { ba: iir::IIRState([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
|
||||
#[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
|
||||
iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
|
||||
#[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
|
||||
iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
|
||||
}
|
||||
|
||||
|
||||
@ -16,7 +16,7 @@ use stabilizer::{
|
||||
hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2,
|
||||
};
|
||||
|
||||
use dsp::{iir, iir_int, lockin::Lockin, rpll::RPLL};
|
||||
use dsp::{iir, iir_int, lockin::Lockin, rpll::RPLL, Accu};
|
||||
use hardware::{
|
||||
Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
|
||||
};
|
||||
@ -38,9 +38,9 @@ const APP: () = {
|
||||
net_interface: hardware::Ethernet,
|
||||
|
||||
// Format: iir_state[ch][cascade-no][coeff]
|
||||
#[init([[iir::IIRState([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
|
||||
iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2],
|
||||
#[init([[iir::IIR { ba: iir::IIRState([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
|
||||
#[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
|
||||
iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
|
||||
#[init([[iir::IIR { ba: iir::Vec5([1., 0., 0., 0., 0.]), y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
|
||||
iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
|
||||
|
||||
timestamper: InputStamper,
|
||||
@ -56,7 +56,7 @@ const APP: () = {
|
||||
let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2);
|
||||
|
||||
let lockin = Lockin::new(
|
||||
&iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose
|
||||
iir_int::Vec5::lowpass(1e-3, 0.707, 2.), // TODO: expose
|
||||
);
|
||||
|
||||
// Enable ADC/DAC events
|
||||
@ -133,13 +133,15 @@ const APP: () = {
|
||||
let sample_phase =
|
||||
phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
|
||||
|
||||
if let Some(output) = lockin.feed(
|
||||
adc_samples[0].iter().map(|&i|
|
||||
// Convert to signed, MSB align the ADC sample.
|
||||
(i as i16 as i32) << 16),
|
||||
sample_phase,
|
||||
sample_frequency,
|
||||
) {
|
||||
if let Some(output) = adc_samples[0]
|
||||
.iter()
|
||||
.zip(Accu::new(sample_phase, sample_frequency))
|
||||
// Convert to signed, MSB align the ADC sample.
|
||||
.map(|(&sample, phase)| {
|
||||
lockin.update((sample as i16 as i32) << 16, phase)
|
||||
})
|
||||
.last()
|
||||
{
|
||||
// Convert from IQ to power and phase.
|
||||
let mut power = output.abs_sqr() as _;
|
||||
let mut phase = output.arg() as _;
|
||||
|
||||
@ -7,7 +7,7 @@
|
||||
const DAC_SEQUENCE: [f32; 8] =
|
||||
[0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
|
||||
|
||||
use dsp::{iir_int, lockin::Lockin};
|
||||
use dsp::{iir_int, lockin::Lockin, Accu};
|
||||
use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
|
||||
use stabilizer::hardware;
|
||||
|
||||
@ -27,7 +27,7 @@ const APP: () = {
|
||||
let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
|
||||
|
||||
let lockin = Lockin::new(
|
||||
&iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose
|
||||
iir_int::Vec5::lowpass(1e-3, 0.707, 2.), // TODO: expose
|
||||
);
|
||||
|
||||
// Enable ADC/DAC events
|
||||
@ -66,6 +66,7 @@ const APP: () = {
|
||||
/// TODO: Document
|
||||
#[task(binds=DMA1_STR4, resources=[adc1, dacs, lockin], priority=2)]
|
||||
fn process(c: process::Context) {
|
||||
let lockin = c.resources.lockin;
|
||||
let adc_samples = c.resources.adc1.acquire_buffer();
|
||||
let dac_samples = [
|
||||
c.resources.dacs.0.acquire_buffer(),
|
||||
@ -96,13 +97,15 @@ const APP: () = {
|
||||
let sample_phase = phase_offset
|
||||
.wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
|
||||
|
||||
if let Some(output) = c.resources.lockin.feed(
|
||||
adc_samples.iter().map(|&i|
|
||||
// Convert to signed, MSB align the ADC sample.
|
||||
(i as i16 as i32) << 16),
|
||||
sample_phase,
|
||||
sample_frequency,
|
||||
) {
|
||||
if let Some(output) = adc_samples
|
||||
.iter()
|
||||
.zip(Accu::new(sample_phase, sample_frequency))
|
||||
// Convert to signed, MSB align the ADC sample.
|
||||
.map(|(&sample, phase)| {
|
||||
lockin.update((sample as i16 as i32) << 16, phase)
|
||||
})
|
||||
.last()
|
||||
{
|
||||
// Convert from IQ to power and phase.
|
||||
let _power = output.abs_sqr();
|
||||
let phase = output.arg() >> 16;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user