lowpass: saturating math since it's free

This commit is contained in:
Robert Jördens 2021-02-18 18:50:31 +01:00
parent c0457787bb
commit 33b9b41405
3 changed files with 10 additions and 7 deletions

View File

@ -14,19 +14,20 @@ impl<N: ArrayLength<i32>> Lowpass<N> {
/// Update the filter with a new sample.
///
/// # Args
/// * `x`: Input data. Needs 1 bit headroom.
/// * `k`: Log2 time constant, 0..=31.
/// * `x`: Input data. Needs 1 bit headroom but will saturate cleanly beyond that.
/// * `k`: Log2 time constant, 1..=31.
///
/// # Return
/// Filtered output y.
pub fn update(&mut self, x: i32, k: u8) -> i32 {
debug_assert!(k & 31 == k);
debug_assert!((k - 1) & 31 == k - 1);
// This is an unrolled and optimized first-order IIR loop
// that works for all possible time constants.
// Note T-DF-I and the zeros at Nyquist.
let mut x = x;
for y in self.y.iter_mut() {
let dy = (x - *y + (1 << (k - 1))) >> k;
let dy = x.saturating_sub(*y).saturating_add(1 << (k - 1)) >> k;
*y += dy;
x = *y - (dy >> 1);
}

View File

@ -118,12 +118,13 @@ const APP: () = {
.zip(Accu::new(sample_phase, sample_frequency))
// Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter)
.map(|(&sample, phase)| {
let s = (sample as i16 as i32) << (15 + 1);
let s = (sample as i16 as i32) << 16;
lockin.update(s, phase, time_constant)
})
// Decimate
.last()
.unwrap();
.unwrap()
* 2; // Full scale assuming the 2f component is gone.
#[allow(dead_code)]
enum Conf {

View File

@ -101,12 +101,13 @@ const APP: () = {
.zip(Accu::new(sample_phase, sample_frequency))
// Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter)
.map(|(&sample, phase)| {
let s = (sample as i16 as i32) << (15 + 1);
let s = (sample as i16 as i32) << 16;
lockin.update(s, phase, time_constant)
})
// Decimate
.last()
.unwrap();
.unwrap()
* 2; // Full scale assuming the 2f component is gone.
for value in dac_samples[1].iter_mut() {
*value = (output.arg() >> 16) as u16 ^ 0x8000;