GitHub
commit
12563ff9ab
195
Cargo.lock
generated
195
Cargo.lock
generated
@ -347,6 +347,9 @@ version = "0.1.0"
|
||||
dependencies = [
|
||||
"criterion",
|
||||
"libm",
|
||||
"ndarray",
|
||||
"ndarray-stats",
|
||||
"rand 0.8.3",
|
||||
"serde",
|
||||
]
|
||||
|
||||
@ -423,6 +426,28 @@ 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 1.0.0",
|
||||
"libc",
|
||||
"wasi 0.9.0+wasi-snapshot-preview1",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "getrandom"
|
||||
version = "0.2.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "c9495705279e7140bf035dde1f6e750c162df8b625267cd52cc44e0b156732c8"
|
||||
dependencies = [
|
||||
"cfg-if 1.0.0",
|
||||
"libc",
|
||||
"wasi 0.10.2+wasi-snapshot-preview1",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "half"
|
||||
version = "1.6.0"
|
||||
@ -533,6 +558,15 @@ version = "0.7.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "c75de51135344a4f8ed3cfe2720dc27736f7711989703a0b43aadf3753c55577"
|
||||
|
||||
[[package]]
|
||||
name = "matrixmultiply"
|
||||
version = "0.2.4"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "916806ba0031cd542105d916a97c8572e1fa6dd79c9c51e7eb43a09ec2dd84c1"
|
||||
dependencies = [
|
||||
"rawpointer",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "mcp23017"
|
||||
version = "0.1.1"
|
||||
@ -571,6 +605,62 @@ version = "1.0.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "546c37ac5d9e56f55e73b677106873d9d9f5190605e41a856503623648488cae"
|
||||
|
||||
[[package]]
|
||||
name = "ndarray"
|
||||
version = "0.14.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "6c0d5c9540a691d153064dc47a4db2504587a75eae07bf1d73f7a596ebc73c04"
|
||||
dependencies = [
|
||||
"matrixmultiply",
|
||||
"num-complex",
|
||||
"num-integer",
|
||||
"num-traits",
|
||||
"rawpointer",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "ndarray-stats"
|
||||
version = "0.4.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "22c95a780960082c5746f6bf0ab22d4a3b8cee72bf580acfe9f1e10bc5ea8152"
|
||||
dependencies = [
|
||||
"indexmap",
|
||||
"itertools",
|
||||
"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"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "747d632c0c558b87dbabbe6a82f3b4ae03720d0646ac5b7b4dae89394be5f2c5"
|
||||
dependencies = [
|
||||
"num-traits",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num-integer"
|
||||
version = "0.1.44"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "d2cc698a63b549a70bc047073d2949cce27cd1c7b0a4a862d08a8031bc2801db"
|
||||
dependencies = [
|
||||
"autocfg",
|
||||
"num-traits",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "num-traits"
|
||||
version = "0.2.14"
|
||||
@ -630,6 +720,12 @@ dependencies = [
|
||||
"web-sys",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "ppv-lite86"
|
||||
version = "0.2.10"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "ac74c624d6b2d21f425f752262f42188365d7b8ff1aff74c82e45136510a4857"
|
||||
|
||||
[[package]]
|
||||
name = "proc-macro2"
|
||||
version = "1.0.24"
|
||||
@ -654,6 +750,93 @@ 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"
|
||||
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",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rand_chacha"
|
||||
version = "0.3.0"
|
||||
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",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rand_core"
|
||||
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",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rand_hc"
|
||||
version = "0.3.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "3190ef7066a446f2e7f42e239d161e905420ccab01eb967c9eb27d21b2322a73"
|
||||
dependencies = [
|
||||
"rand_core 0.6.1",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rawpointer"
|
||||
version = "0.2.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3"
|
||||
|
||||
[[package]]
|
||||
name = "rayon"
|
||||
version = "1.5.0"
|
||||
@ -975,6 +1158,18 @@ 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"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "fd6fbd9a79829dd1ad0cc20627bf1ed606756a7f77edff7b66b7064f9cb327c6"
|
||||
|
||||
[[package]]
|
||||
name = "wasm-bindgen"
|
||||
version = "0.2.69"
|
||||
|
||||
@ -10,6 +10,9 @@ serde = { version = "1.0", features = ["derive"], default-features = false }
|
||||
|
||||
[dev-dependencies]
|
||||
criterion = "0.3"
|
||||
rand = "0.8"
|
||||
ndarray = "0.14"
|
||||
ndarray-stats = "0.4"
|
||||
|
||||
[[bench]]
|
||||
name = "trig"
|
||||
|
||||
@ -1,17 +1,58 @@
|
||||
use super::atan2;
|
||||
use super::{atan2, cossin};
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
|
||||
#[derive(Copy, Clone, Default, PartialEq, Debug, 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
|
||||
/// Return a Complex on the unit circle given an angle.
|
||||
///
|
||||
/// Example:
|
||||
///
|
||||
/// ```
|
||||
/// use dsp::Complex;
|
||||
/// Complex::<i32>::from_angle(0);
|
||||
/// Complex::<i32>::from_angle(1 << 30); // pi/2
|
||||
/// Complex::<i32>::from_angle(-1 << 30); // -pi/2
|
||||
/// ```
|
||||
#[inline(always)]
|
||||
pub fn from_angle(angle: i32) -> Complex<i32> {
|
||||
cossin(angle)
|
||||
}
|
||||
|
||||
pub fn phase(&self) -> i32 {
|
||||
/// Return the absolute square (the squared magnitude).
|
||||
///
|
||||
/// Note: Normalization is `1 << 31`, i.e. Q0.31.
|
||||
///
|
||||
/// Example:
|
||||
///
|
||||
/// ```
|
||||
/// use dsp::Complex;
|
||||
/// assert_eq!(Complex(i32::MAX, 0).abs_sqr(), i32::MAX - 1);
|
||||
/// assert_eq!(Complex(i32::MIN + 1, 0).abs_sqr(), i32::MAX - 1);
|
||||
/// ```
|
||||
pub fn abs_sqr(&self) -> i32 {
|
||||
(((self.0 as i64) * (self.0 as i64)
|
||||
+ (self.1 as i64) * (self.1 as i64))
|
||||
>> 31) as i32
|
||||
}
|
||||
|
||||
/// Return the angle.
|
||||
///
|
||||
/// Note: Normalization is `1 << 31 == pi`.
|
||||
///
|
||||
/// Example:
|
||||
///
|
||||
/// ```
|
||||
/// use dsp::Complex;
|
||||
/// assert_eq!(Complex(i32::MAX, 0).arg(), 0);
|
||||
/// assert_eq!(Complex(-i32::MAX, 1).arg(), i32::MAX);
|
||||
/// assert_eq!(Complex(-i32::MAX, -1).arg(), -i32::MAX);
|
||||
/// assert_eq!(Complex(0, -i32::MAX).arg(), -i32::MAX >> 1);
|
||||
/// assert_eq!(Complex(0, i32::MAX).arg(), (i32::MAX >> 1) + 1);
|
||||
/// assert_eq!(Complex(i32::MAX, i32::MAX).arg(), (i32::MAX >> 2) + 1);
|
||||
/// ```
|
||||
pub fn arg(&self) -> i32 {
|
||||
atan2(self.1, self.0)
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
use super::{cossin, iir_int, Complex};
|
||||
use super::{iir_int, Complex};
|
||||
use serde::{Deserialize, Serialize};
|
||||
|
||||
#[derive(Copy, Clone, Default, Deserialize, Serialize)]
|
||||
@ -19,7 +19,7 @@ impl Lockin {
|
||||
|
||||
pub fn update(&mut self, signal: i32, phase: i32) -> Complex<i32> {
|
||||
// Get the LO signal for demodulation.
|
||||
let m = cossin(phase);
|
||||
let m = Complex::from_angle(phase);
|
||||
|
||||
// Mix with the LO signal, filter with the IIR lowpass,
|
||||
// return IQ (in-phase and quadrature) data.
|
||||
@ -35,12 +35,28 @@ impl Lockin {
|
||||
),
|
||||
)
|
||||
}
|
||||
|
||||
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::{
|
||||
atan2,
|
||||
iir_int::IIRState,
|
||||
lockin::Lockin,
|
||||
rpll::RPLL,
|
||||
|
||||
232
dsp/src/rpll.rs
232
dsp/src/rpll.rs
@ -3,13 +3,14 @@
|
||||
/// Consumes noisy, quantized timestamps of a reference signal and reconstructs
|
||||
/// the phase and frequency of the update() invocations with respect to (and in units of
|
||||
/// 1 << 32 of) that reference.
|
||||
/// In other words, `update()` rate ralative to reference frequency,
|
||||
/// `u32::MAX` corresponding to both being equal.
|
||||
#[derive(Copy, Clone, Default)]
|
||||
pub struct RPLL {
|
||||
dt2: u8, // 1 << dt2 is the counter rate to update() rate ratio
|
||||
t: i32, // current counter time
|
||||
x: i32, // previous timestamp
|
||||
ff: i32, // current frequency estimate from frequency loop
|
||||
f: i32, // current frequency estimate from both frequency and phase loop
|
||||
ff: u32, // current frequency estimate from frequency loop
|
||||
f: u32, // current frequency estimate from both frequency and phase loop
|
||||
y: i32, // current phase estimate
|
||||
}
|
||||
|
||||
@ -18,14 +19,12 @@ impl RPLL {
|
||||
///
|
||||
/// Args:
|
||||
/// * dt2: inverse update() rate. 1 << dt2 is the counter rate to update() rate ratio.
|
||||
/// * t: Counter time. Counter value at the first update() call. Typically 0.
|
||||
///
|
||||
/// Returns:
|
||||
/// Initialized RPLL instance.
|
||||
pub fn new(dt2: u8, t: i32) -> RPLL {
|
||||
pub fn new(dt2: u8) -> RPLL {
|
||||
RPLL {
|
||||
dt2,
|
||||
t,
|
||||
..Default::default()
|
||||
}
|
||||
}
|
||||
@ -43,42 +42,231 @@ impl RPLL {
|
||||
///
|
||||
/// Returns:
|
||||
/// A tuple containing the current phase (wrapping at the i32 boundary, pi) and
|
||||
/// frequency (wrapping at the i32 boundary, Nyquist) estimate.
|
||||
/// frequency.
|
||||
pub fn update(
|
||||
&mut self,
|
||||
input: Option<i32>,
|
||||
shift_frequency: u8,
|
||||
shift_phase: u8,
|
||||
) -> (i32, i32) {
|
||||
) -> (i32, u32) {
|
||||
debug_assert!(shift_frequency > self.dt2);
|
||||
debug_assert!(shift_phase > self.dt2);
|
||||
debug_assert!(shift_phase >= self.dt2);
|
||||
// Advance phase
|
||||
self.y = self.y.wrapping_add(self.f);
|
||||
self.y = self.y.wrapping_add(self.f as i32);
|
||||
if let Some(x) = input {
|
||||
// Reference period in counter cycles
|
||||
let dx = x.wrapping_sub(self.x);
|
||||
// Store timestamp for next time.
|
||||
self.x = x;
|
||||
// Phase using the current frequency estimate
|
||||
let p_sig_long = (self.ff as i64).wrapping_mul(dx as i64);
|
||||
let p_sig_64 = self.ff as u64 * dx as u64;
|
||||
// Add half-up rounding bias and apply gain/attenuation
|
||||
let p_sig = (p_sig_long.wrapping_add(1i64 << (shift_frequency - 1))
|
||||
>> shift_frequency) as i32;
|
||||
let p_sig = ((p_sig_64 + (1u32 << (shift_frequency - 1)) as u64)
|
||||
>> shift_frequency) as u32;
|
||||
// Reference phase (1 << dt2 full turns) with gain/attenuation applied
|
||||
let p_ref = 1i32 << (32 + self.dt2 - shift_frequency);
|
||||
let p_ref = 1u32 << (32 + self.dt2 - shift_frequency);
|
||||
// Update frequency lock
|
||||
self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig));
|
||||
self.ff = self.ff.wrapping_add(p_ref.wrapping_sub(p_sig) as u32);
|
||||
// Time in counter cycles between timestamp and "now"
|
||||
let dt = self.t.wrapping_sub(x);
|
||||
let dt = (x.wrapping_neg() & ((1 << self.dt2) - 1)) as u32;
|
||||
// Reference phase estimate "now"
|
||||
let y_ref = (self.f >> self.dt2).wrapping_mul(dt);
|
||||
// Phase error
|
||||
let dy = y_ref.wrapping_sub(self.y);
|
||||
let y_ref = (self.f >> self.dt2).wrapping_mul(dt) as i32;
|
||||
// Phase error with gain
|
||||
let dy = y_ref.wrapping_sub(self.y) >> (shift_phase - self.dt2);
|
||||
// Current frequency estimate from frequency lock and phase error
|
||||
self.f = self.ff.wrapping_add(dy >> (shift_phase - self.dt2));
|
||||
self.f = self.ff.wrapping_add(dy as u32);
|
||||
}
|
||||
// Advance time
|
||||
self.t = self.t.wrapping_add(1 << self.dt2);
|
||||
(self.y, self.f)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod test {
|
||||
use super::RPLL;
|
||||
use ndarray::prelude::*;
|
||||
use ndarray_stats::QuantileExt;
|
||||
use rand::{prelude::*, rngs::StdRng};
|
||||
use std::vec::Vec;
|
||||
|
||||
#[test]
|
||||
fn make() {
|
||||
let _ = RPLL::new(8);
|
||||
}
|
||||
|
||||
struct Harness {
|
||||
rpll: RPLL,
|
||||
dt2: u8,
|
||||
shift_frequency: u8,
|
||||
shift_phase: u8,
|
||||
noise: i32,
|
||||
period: i32,
|
||||
next: i32,
|
||||
next_noisy: i32,
|
||||
time: i32,
|
||||
rng: StdRng,
|
||||
}
|
||||
|
||||
impl Harness {
|
||||
fn default() -> Self {
|
||||
Harness {
|
||||
rpll: RPLL::new(8),
|
||||
dt2: 8,
|
||||
shift_frequency: 9,
|
||||
shift_phase: 8,
|
||||
noise: 0,
|
||||
period: 333,
|
||||
next: 111,
|
||||
next_noisy: 111,
|
||||
time: 0,
|
||||
rng: StdRng::seed_from_u64(42),
|
||||
}
|
||||
}
|
||||
|
||||
fn run(&mut self, n: usize) -> (Vec<f32>, Vec<f32>) {
|
||||
let mut y = Vec::<f32>::new();
|
||||
let mut f = Vec::<f32>::new();
|
||||
for _ in 0..n {
|
||||
let timestamp = if self.time - self.next_noisy >= 0 {
|
||||
assert!(self.time - self.next_noisy < 1 << self.dt2);
|
||||
self.next = self.next.wrapping_add(self.period);
|
||||
let timestamp = self.next_noisy;
|
||||
let p_noise = self.rng.gen_range(-self.noise..=self.noise);
|
||||
self.next_noisy = self.next.wrapping_add(p_noise);
|
||||
Some(timestamp)
|
||||
} else {
|
||||
None
|
||||
};
|
||||
let (yi, fi) = self.rpll.update(
|
||||
timestamp,
|
||||
self.shift_frequency,
|
||||
self.shift_phase,
|
||||
);
|
||||
|
||||
let y_ref = (self.time.wrapping_sub(self.next) as i64
|
||||
* (1i64 << 32)
|
||||
/ self.period as i64) as i32;
|
||||
// phase error
|
||||
y.push(yi.wrapping_sub(y_ref) as f32 / 2f32.powi(32));
|
||||
|
||||
let p_ref = 1 << 32 + self.dt2;
|
||||
let p_sig = fi as u64 * self.period as u64;
|
||||
// relative frequency error
|
||||
f.push(
|
||||
p_sig.wrapping_sub(p_ref) as i64 as f32
|
||||
/ 2f32.powi(32 + self.dt2 as i32),
|
||||
);
|
||||
|
||||
// advance time
|
||||
self.time = self.time.wrapping_add(1 << self.dt2);
|
||||
}
|
||||
(y, f)
|
||||
}
|
||||
|
||||
fn measure(&mut self, n: usize, limits: [f32; 4]) {
|
||||
assert!(self.period >= 1 << self.dt2);
|
||||
assert!(self.dt2 <= self.shift_frequency);
|
||||
assert!(self.period < 1 << self.shift_frequency);
|
||||
assert!(self.period < 1 << self.shift_frequency + 1);
|
||||
let t_settle = (1 << self.shift_frequency - self.dt2 + 4)
|
||||
+ (1 << self.shift_phase - self.dt2 + 4);
|
||||
self.run(t_settle);
|
||||
|
||||
let (y, f) = self.run(n);
|
||||
let y = Array::from(y);
|
||||
let f = Array::from(f);
|
||||
// println!("{:?} {:?}", f, y);
|
||||
|
||||
let fm = f.mean().unwrap();
|
||||
let fs = f.std_axis(Axis(0), 0.).into_scalar();
|
||||
let ym = y.mean().unwrap();
|
||||
let ys = y.std_axis(Axis(0), 0.).into_scalar();
|
||||
|
||||
println!("f: {:.2e}±{:.2e}; y: {:.2e}±{:.2e}", fm, fs, ym, ys);
|
||||
|
||||
let m = [fm, fs, ym, ys];
|
||||
|
||||
print!("relative: ");
|
||||
for i in 0..m.len() {
|
||||
let rel = m[i].abs() / limits[i].abs();
|
||||
print!("{:.2e} ", rel);
|
||||
assert!(
|
||||
rel <= 1.,
|
||||
"idx {}, have |{}| > want {}",
|
||||
i,
|
||||
m[i],
|
||||
limits[i]
|
||||
);
|
||||
}
|
||||
println!();
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn default() {
|
||||
let mut h = Harness::default();
|
||||
|
||||
h.measure(1 << 16, [1e-11, 4e-8, 2e-8, 2e-8]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn noisy() {
|
||||
let mut h = Harness::default();
|
||||
h.noise = 10;
|
||||
h.shift_frequency = 23;
|
||||
h.shift_phase = 22;
|
||||
|
||||
h.measure(1 << 16, [3e-9, 3e-6, 4e-4, 2e-4]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn narrow_fast() {
|
||||
let mut h = Harness::default();
|
||||
h.period = 990;
|
||||
h.next = 351;
|
||||
h.next_noisy = h.next;
|
||||
h.noise = 5;
|
||||
h.shift_frequency = 23;
|
||||
h.shift_phase = 22;
|
||||
|
||||
h.measure(1 << 16, [2e-9, 2e-6, 1e-3, 1e-4]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn narrow_slow() {
|
||||
let mut h = Harness::default();
|
||||
h.period = 1818181;
|
||||
h.next = 35281;
|
||||
h.next_noisy = h.next;
|
||||
h.noise = 1000;
|
||||
h.shift_frequency = 23;
|
||||
h.shift_phase = 22;
|
||||
|
||||
h.measure(1 << 16, [2e-5, 6e-4, 2e-4, 2e-4]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn wide_fast() {
|
||||
let mut h = Harness::default();
|
||||
h.period = 990;
|
||||
h.next = 351;
|
||||
h.next_noisy = h.next;
|
||||
h.noise = 5;
|
||||
h.shift_frequency = 10;
|
||||
h.shift_phase = 9;
|
||||
|
||||
h.measure(1 << 16, [5e-7, 3e-2, 3e-2, 2e-2]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn wide_slow() {
|
||||
let mut h = Harness::default();
|
||||
h.period = 1818181;
|
||||
h.next = 35281;
|
||||
h.next_noisy = h.next;
|
||||
h.noise = 1000;
|
||||
h.shift_frequency = 21;
|
||||
h.shift_phase = 20;
|
||||
|
||||
h.measure(1 << 16, [2e-4, 6e-3, 2e-4, 2e-3]);
|
||||
}
|
||||
}
|
||||
|
||||
@ -53,7 +53,7 @@ const APP: () = {
|
||||
// Configure the microcontroller
|
||||
let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
|
||||
|
||||
let pll = RPLL::new(ADC_SAMPLE_TICKS_LOG2 + SAMPLE_BUFFER_SIZE_LOG2, 0);
|
||||
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
|
||||
@ -119,30 +119,30 @@ const APP: () = {
|
||||
|
||||
let (pll_phase, pll_frequency) = c.resources.pll.update(
|
||||
c.resources.timestamper.latest_timestamp().map(|t| t as i32),
|
||||
22, // relative PLL frequency bandwidth: 2**-22, TODO: expose
|
||||
23, // relative PLL frequency bandwidth: 2**-23, TODO: expose
|
||||
22, // relative PLL phase bandwidth: 2**-22, TODO: expose
|
||||
);
|
||||
|
||||
// Harmonic index of the LO: -1 to _de_modulate the fundamental
|
||||
// Harmonic index of the LO: -1 to _de_modulate the fundamental (complex conjugate)
|
||||
let harmonic: i32 = -1;
|
||||
// Demodulation LO phase offset
|
||||
let phase_offset: i32 = 0;
|
||||
let sample_frequency =
|
||||
(pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2).wrapping_mul(harmonic);
|
||||
let mut sample_phase =
|
||||
let sample_frequency = ((pll_frequency >> SAMPLE_BUFFER_SIZE_LOG2)
|
||||
as i32) // TODO: maybe rounding bias
|
||||
.wrapping_mul(harmonic);
|
||||
let sample_phase =
|
||||
phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic));
|
||||
|
||||
for i in 0..adc_samples[0].len() {
|
||||
// Convert to signed, MSB align the ADC sample.
|
||||
let input = (adc_samples[0][i] as i16 as i32) << 16;
|
||||
// Obtain demodulated, filtered IQ sample.
|
||||
let output = lockin.update(input, sample_phase);
|
||||
// Advance the sample phase.
|
||||
sample_phase = sample_phase.wrapping_add(sample_frequency);
|
||||
|
||||
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,
|
||||
) {
|
||||
// Convert from IQ to power and phase.
|
||||
let mut power = output.power() as _;
|
||||
let mut phase = output.phase() as _;
|
||||
let mut power = output.abs_sqr() as _;
|
||||
let mut phase = output.arg() as _;
|
||||
|
||||
// Filter power and phase through IIR filters.
|
||||
// Note: Normalization to be done in filters. Phase will wrap happily.
|
||||
@ -153,11 +153,13 @@ const APP: () = {
|
||||
|
||||
// Note(unsafe): range clipping to i16 is ensured by IIR filters above.
|
||||
// Convert to DAC data.
|
||||
unsafe {
|
||||
dac_samples[0][i] =
|
||||
power.to_int_unchecked::<i16>() as u16 ^ 0x8000;
|
||||
dac_samples[1][i] =
|
||||
phase.to_int_unchecked::<i16>() as u16 ^ 0x8000;
|
||||
for i in 0..dac_samples[0].len() {
|
||||
unsafe {
|
||||
dac_samples[0][i] =
|
||||
(power.to_int_unchecked::<i32>() >> 16) as u16 ^ 0x8000;
|
||||
dac_samples[1][i] =
|
||||
(phase.to_int_unchecked::<i32>() >> 16) as u16 ^ 0x8000;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user