diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index ee9639e..a6af191 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -117,7 +117,7 @@ impl IIR { /// # Arguments /// * `xy` - Current filter state. /// * `x0` - New input. - pub fn update(&self, xy: &mut Vec5, x0: f32) -> f32 { + pub fn update(&self, xy: &mut Vec5, x0: f32, hold: bool) -> f32 { let n = self.ba.len(); debug_assert!(xy.len() == n); // `xy` contains x0 x1 y0 y1 y2 @@ -128,7 +128,11 @@ impl IIR { // Store x0 x0 x1 x2 y1 y2 xy[0] = x0; // Compute y0 by multiply-accumulate - let y0 = macc(self.y_offset, xy, &self.ba); + let y0 = if hold { + xy[n / 2 + 1] + } else { + macc(self.y_offset, xy, &self.ba) + }; // Limit y0 let y0 = max(self.y_min, min(self.y_max, y0)); // Store y0 x0 x1 y0 y1 y2 diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs index 4a27d48..520d5ec 100644 --- a/src/bin/dual-iir.rs +++ b/src/bin/dual-iir.rs @@ -4,6 +4,8 @@ use stm32h7xx_hal as hal; +pub use embedded_hal::digital::v2::InputPin; + use stabilizer::hardware; use miniconf::{minimq, Miniconf, MqttInterface}; @@ -12,7 +14,7 @@ use serde::Deserialize; use dsp::iir; use hardware::{ Adc0Input, Adc1Input, AfeGain, CycleCounter, Dac0Output, Dac1Output, - NetworkStack, AFE0, AFE1, + DigitalInput1, NetworkStack, AFE0, AFE1, }; const SCALE: f32 = i16::MAX as _; @@ -39,6 +41,7 @@ impl Default for Settings { const APP: () = { struct Resources { afes: (AFE0, AFE1), + di1: DigitalInput1, adcs: (Adc0Input, Adc1Input), dacs: (Dac0Output, Dac1Output), mqtt_interface: @@ -90,6 +93,7 @@ const APP: () = { adcs: stabilizer.adcs, dacs: stabilizer.dacs, clock: stabilizer.cycle_counter, + di1: stabilizer.digital_inputs.1, } } @@ -109,7 +113,7 @@ const APP: () = { /// /// Because the ADC and DAC operate at the same rate, these two constraints actually implement /// the same time bounds, meeting one also means the other is also met. - #[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch], priority=2)] + #[task(binds=DMA1_STR4, resources=[adcs, di1, dacs, iir_state, iir_ch], priority=2)] fn process(c: process::Context) { let adc_samples = [ c.resources.adcs.0.acquire_buffer(), @@ -121,13 +125,17 @@ const APP: () = { c.resources.dacs.1.acquire_buffer(), ]; + let hold = c.resources.di1.is_high().unwrap(); + for channel in 0..adc_samples.len() { for sample in 0..adc_samples[0].len() { - let x = f32::from(adc_samples[channel][sample] as i16); - let mut y = x; + let mut y = f32::from(adc_samples[channel][sample] as i16); for i in 0..c.resources.iir_state[channel].len() { - y = c.resources.iir_ch[channel][i] - .update(&mut c.resources.iir_state[channel][i], y); + y = c.resources.iir_ch[channel][i].update( + &mut c.resources.iir_state[channel][i], + y, + hold, + ); } // Note(unsafe): The filter limits ensure that the value is in range. // The truncation introduces 1/2 LSB distortion. diff --git a/src/hardware/configuration.rs b/src/hardware/configuration.rs index 0351db9..31fb16d 100644 --- a/src/hardware/configuration.rs +++ b/src/hardware/configuration.rs @@ -9,12 +9,12 @@ use stm32h7xx_hal::{ use smoltcp_nal::smoltcp; -use embedded_hal::digital::v2::{InputPin, OutputPin}; +pub use embedded_hal::digital::v2::{InputPin, OutputPin}; use super::{ adc, afe, cycle_counter::CycleCounter, dac, design_parameters, - digital_input_stamper, eeprom, pounder, timers, DdsOutput, NetworkStack, - AFE0, AFE1, + digital_input_stamper, eeprom, pounder, timers, DdsOutput, DigitalInput0, + DigitalInput1, NetworkStack, AFE0, AFE1, }; pub struct NetStorage { @@ -69,6 +69,7 @@ pub struct StabilizerDevices { pub timestamp_timer: timers::TimestampTimer, pub net: NetworkDevices, pub cycle_counter: CycleCounter, + pub digital_inputs: (DigitalInput0, DigitalInput1), } /// The available Pounder-specific hardware interfaces. @@ -439,6 +440,12 @@ pub fn setup( ) }; + let digital_inputs = { + let di0 = gpiog.pg9.into_floating_input(); + let di1 = gpioc.pc15.into_floating_input(); + (di0, di1) + }; + let mut eeprom_i2c = { let sda = gpiof.pf0.into_alternate_af4().set_open_drain(); let scl = gpiof.pf1.into_alternate_af4().set_open_drain(); @@ -872,6 +879,7 @@ pub fn setup( adc_dac_timer: sampling_timer, timestamp_timer, cycle_counter, + digital_inputs, }; // info!("Version {} {}", build_info::PKG_VERSION, build_info::GIT_VERSION.unwrap()); diff --git a/src/hardware/mod.rs b/src/hardware/mod.rs index cc7a34a..34f092f 100644 --- a/src/hardware/mod.rs +++ b/src/hardware/mod.rs @@ -34,6 +34,14 @@ pub type AFE1 = afe::ProgrammableGainAmplifier< hal::gpio::gpiod::PD15>, >; +// Type alias for digital input 0 (DI0). +pub type DigitalInput0 = + hal::gpio::gpiog::PG9>; + +// Type alias for digital input 1 (DI1). +pub type DigitalInput1 = + hal::gpio::gpioc::PC15>; + pub type NetworkStack = smoltcp_nal::NetworkStack< 'static, 'static,