lowpass: i32, no multiplies
This commit is contained in:
parent
208ba8379a
commit
30c2c2aac2
@ -4,12 +4,12 @@ use generic_array::typenum::U3;
|
|||||||
#[derive(Clone, Default)]
|
#[derive(Clone, Default)]
|
||||||
pub struct Lockin {
|
pub struct Lockin {
|
||||||
state: [Lowpass<U3>; 2],
|
state: [Lowpass<U3>; 2],
|
||||||
k: u32,
|
k: u8,
|
||||||
}
|
}
|
||||||
|
|
||||||
impl Lockin {
|
impl Lockin {
|
||||||
/// Create a new Lockin with given IIR coefficients.
|
/// Create a new Lockin with given IIR coefficients.
|
||||||
pub fn new(k: u32) -> Self {
|
pub fn new(k: u8) -> Self {
|
||||||
let lp = Lowpass::default();
|
let lp = Lowpass::default();
|
||||||
Self {
|
Self {
|
||||||
state: [lp.clone(), lp.clone()],
|
state: [lp.clone(), lp.clone()],
|
||||||
|
@ -15,27 +15,19 @@ impl<N: ArrayLength<i32>> Lowpass<N> {
|
|||||||
///
|
///
|
||||||
/// # Args
|
/// # Args
|
||||||
/// * `x`: Input data
|
/// * `x`: Input data
|
||||||
/// * `k`: Cutoff, `u32::MAX` being Nyquist
|
/// * `k`: Log2 time constant
|
||||||
///
|
///
|
||||||
/// # Returns
|
/// # Return
|
||||||
/// Filtered output y
|
/// Filtered output y
|
||||||
pub fn update(&mut self, x: i32, k: u32) -> i32 {
|
pub fn update(&mut self, x: i32, k: u8) -> i32 {
|
||||||
let mut x1 = self.xy[0];
|
|
||||||
self.xy[0] = x;
|
|
||||||
let mut x0 = x;
|
|
||||||
|
|
||||||
// This is an unrolled and optimized first-order IIR loop
|
// This is an unrolled and optimized first-order IIR loop
|
||||||
// that works for all possible time constants.
|
// that works for all possible time constants.
|
||||||
|
// Note the zero(s) at Nyquist and the benign overflow (DF-I).
|
||||||
|
let mut x0 = x;
|
||||||
|
let mut x1 = self.xy[0];
|
||||||
|
self.xy[0] = x;
|
||||||
for y1 in self.xy[1..].iter_mut() {
|
for y1 in self.xy[1..].iter_mut() {
|
||||||
// Optimized first order lowpass expression
|
x0 = *y1 + (((x0 >> 1) + (x1 >> 1) - *y1 + (1 << k - 1)) >> k);
|
||||||
// Note the zero at Nyquist
|
|
||||||
let mut y0 =
|
|
||||||
((x0 >> 1) as i64 + (x1 >> 1) as i64 - *y1 as i64) * k as i64;
|
|
||||||
y0 += (*y1 as i64) << 32;
|
|
||||||
y0 += 1i64 << 31; // Half-up rounding bias
|
|
||||||
|
|
||||||
// Store and advance
|
|
||||||
x0 = (y0 >> 32) as i32;
|
|
||||||
x1 = *y1;
|
x1 = *y1;
|
||||||
*y1 = x0;
|
*y1 = x0;
|
||||||
}
|
}
|
||||||
|
@ -34,7 +34,7 @@ const APP: () = {
|
|||||||
+ design_parameters::SAMPLE_BUFFER_SIZE_LOG2,
|
+ design_parameters::SAMPLE_BUFFER_SIZE_LOG2,
|
||||||
);
|
);
|
||||||
|
|
||||||
let lockin = Lockin::new(1 << 22);
|
let lockin = Lockin::new(10);
|
||||||
|
|
||||||
// Enable ADC/DAC events
|
// Enable ADC/DAC events
|
||||||
stabilizer.adcs.0.start();
|
stabilizer.adcs.0.start();
|
||||||
|
@ -28,7 +28,7 @@ const APP: () = {
|
|||||||
// Configure the microcontroller
|
// Configure the microcontroller
|
||||||
let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
|
let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
|
||||||
|
|
||||||
let lockin = Lockin::new(1 << 22); // TODO: expose
|
let lockin = Lockin::new(10); // TODO: expose
|
||||||
|
|
||||||
// Enable ADC/DAC events
|
// Enable ADC/DAC events
|
||||||
stabilizer.adcs.1.start();
|
stabilizer.adcs.1.start();
|
||||||
|
Loading…
Reference in New Issue
Block a user