diff --git a/dsp/benches/micro.rs b/dsp/benches/micro.rs index 0902a4c..22a2540 100644 --- a/dsp/benches/micro.rs +++ b/dsp/benches/micro.rs @@ -67,7 +67,7 @@ fn iir_bench() { let mut xy = iir::Vec5::default(); println!( "int::IIR::update(s, x): {}", - bench_env(0.32241, |x| dut.update(&mut xy, *x)) + bench_env(0.32241, |x| dut.update(&mut xy, *x, true)) ); } 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 e8f12a0..1de0ff0 100644 --- a/src/bin/dual-iir.rs +++ b/src/bin/dual-iir.rs @@ -2,8 +2,6 @@ #![no_std] #![no_main] -use stm32h7xx_hal as hal; - use stabilizer::hardware; use miniconf::{minimq, Miniconf, MqttInterface}; @@ -12,7 +10,7 @@ use serde::{Deserialize, Serialize}; use dsp::iir; use hardware::{ Adc0Input, Adc1Input, AfeGain, CycleCounter, Dac0Output, Dac1Output, - NetworkStack, SystemTimer, AFE0, AFE1, + DigitalInput0, DigitalInput1, SystemTimer, InputPin, NetworkStack, AFE0, AFE1, }; const SCALE: f32 = i16::MAX as _; @@ -20,13 +18,6 @@ const SCALE: f32 = i16::MAX as _; // The number of cascaded IIR biquads per channel. Select 1 or 2! const IIR_CASCADE_LENGTH: usize = 1; -#[derive(Debug, Deserialize, Miniconf, Copy, Clone)] -pub struct Settings { - afe: [AfeGain; 2], - iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], - telemetry_period_secs: u16, -} - #[derive(Serialize, Clone)] pub struct Telemetry { latest_samples: [i16; 2], @@ -34,11 +25,22 @@ pub struct Telemetry { digital_inputs: [bool; 2], } +#[derive(Clone, Copy, Debug, Deserialize, Miniconf)] +pub struct Settings { + afe: [AfeGain; 2], + iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], + allow_hold: bool, + force_hold: bool, + telemetry_period_secs: u16, +} + impl Default for Settings { fn default() -> Self { Self { afe: [AfeGain::G1, AfeGain::G1], iir_ch: [[iir::IIR::new(1., -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2], + allow_hold: false, + force_hold: false, telemetry_period_secs: 10, } } @@ -58,6 +60,7 @@ impl Default for Telemetry { const APP: () = { struct Resources { afes: (AFE0, AFE1), + digital_inputs: (DigitalInput0, DigitalInput1), adcs: (Adc0Input, Adc1Input), dacs: (Dac0Output, Dac1Output), mqtt_interface: @@ -69,11 +72,9 @@ const APP: () = { // Format: iir_state[ch][cascade-no][coeff] #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])] iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2], - #[init([[iir::IIR::new(1., -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2])] - iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], } - #[init(schedule = [telemetry])] + #[init(spawn=[telemetry, settings_update])] fn init(c: init::Context) -> init::LateResources { // Configure the microcontroller let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); @@ -96,6 +97,10 @@ const APP: () = { .unwrap() }; + // Spawn a settings update for default settings. + c.spawn.settings_update().unwrap(); + c.spawn.telemetry().unwrap(); + // Enable ADC/DAC events stabilizer.adcs.0.start(); stabilizer.adcs.1.start(); @@ -105,16 +110,15 @@ const APP: () = { // Start sampling ADCs. stabilizer.adc_dac_timer.start(); - c.schedule.telemetry(c.start).unwrap(); - init::LateResources { mqtt_interface, afes: stabilizer.afes, adcs: stabilizer.adcs, dacs: stabilizer.dacs, clock: stabilizer.cycle_counter, - settings: Settings::default(), telemetry: Telemetry::default(), + digital_inputs: stabilizer.digital_inputs, + settings: Settings::default(), } } @@ -134,7 +138,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, telemetry], priority=2)] + #[task(binds=DMA1_STR4, resources=[adcs, digital_inputs, dacs, iir_state, settings, telemetry], priority=2)] fn process(c: process::Context) { let adc_samples = [ c.resources.adcs.0.acquire_buffer(), @@ -146,13 +150,19 @@ const APP: () = { c.resources.dacs.1.acquire_buffer(), ]; + let hold = c.resources.settings.force_hold + || (c.resources.digital_inputs.1.is_high().unwrap() + && c.resources.settings.allow_hold); + 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.settings.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. @@ -169,6 +179,11 @@ const APP: () = { c.resources.telemetry.latest_outputs = [dac_samples[0][0] as i16, dac_samples[1][0] as i16]; + + c.resources.telemetry.digital_inputs = [ + c.resources.digital_inputs.0.is_high().unwrap(), + c.resources.digital_inputs.1.is_high().unwrap(), + ]; } #[idle(resources=[mqtt_interface, clock], spawn=[settings_update])] @@ -206,15 +221,12 @@ const APP: () = { } } - #[task(priority = 1, resources=[mqtt_interface, afes, settings, iir_ch])] + #[task(priority = 1, resources=[mqtt_interface, afes, settings])] fn settings_update(mut c: settings_update::Context) { let settings = &c.resources.mqtt_interface.settings; // Update the IIR channels. - c.resources.iir_ch.lock(|iir| *iir = settings.iir_ch); - - // Update currently-cached settings. - *c.resources.settings = *settings; + c.resources.settings.lock(|current| *current = *settings); // Update AFEs c.resources.afes.0.set_gain(settings.afe[0]); @@ -223,11 +235,7 @@ const APP: () = { #[task(priority = 1, resources=[mqtt_interface, settings, telemetry], schedule=[telemetry])] fn telemetry(mut c: telemetry::Context) { - let telemetry = c.resources.telemetry.lock(|telemetry| { - // TODO: Incorporate digital input status. - telemetry.digital_inputs = [false, false]; - telemetry.clone() - }); + let telemetry = c.resources.telemetry.lock(|telemetry| telemetry.clone()); // Serialize telemetry outside of a critical section to prevent blocking the processing // task. @@ -242,20 +250,16 @@ const APP: () = { client.publish("dt/sinara/dual-iir/telemetry", telemetry.as_bytes(), minimq::QoS::AtMostOnce, &[]).ok() }); + let telemetry_period = c.resources.settings.lock(|settings| settings.telemetry_period_secs); + // Schedule the telemetry task in the future. - c.schedule - .telemetry( - c.scheduled - + SystemTimer::ticks_from_secs( - c.resources.settings.telemetry_period_secs as u32, - ), - ) + c.schedule.telemetry( c.scheduled + SystemTimer::ticks_from_secs(telemetry_period as u32)) .unwrap(); } #[task(binds = ETH, priority = 1)] fn eth(_: eth::Context) { - unsafe { hal::ethernet::interrupt_handler() } + unsafe { stm32h7xx_hal::ethernet::interrupt_handler() } } #[task(binds = SPI2, priority = 3)] diff --git a/src/hardware/configuration.rs b/src/hardware/configuration.rs index 31d47d2..b612bf7 100644 --- a/src/hardware/configuration.rs +++ b/src/hardware/configuration.rs @@ -13,8 +13,8 @@ use embedded_hal::digital::v2::{InputPin, OutputPin}; use super::{ adc, afe, cycle_counter::CycleCounter, dac, design_parameters, - digital_input_stamper, eeprom, pounder, system_timer, timers, DdsOutput, - NetworkStack, AFE0, AFE1, + digital_input_stamper, eeprom, pounder, timers, system_timer, 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. @@ -449,6 +450,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(); @@ -882,6 +889,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 668305e..c24b5e3 100644 --- a/src/hardware/mod.rs +++ b/src/hardware/mod.rs @@ -1,6 +1,9 @@ ///! Module for all hardware-specific setup of Stabilizer use stm32h7xx_hal as hal; +// Re-export for the DigitalInputs below: +pub use embedded_hal::digital::v2::InputPin; + #[cfg(feature = "semihosting")] use panic_semihosting as _; @@ -36,6 +39,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, diff --git a/src/hardware/system_timer.rs b/src/hardware/system_timer.rs index b964a67..0d243e3 100644 --- a/src/hardware/system_timer.rs +++ b/src/hardware/system_timer.rs @@ -10,14 +10,14 @@ impl SystemTimer { timer.pause(); // Have the system timer operate at a tick rate of 10KHz (100uS per tick). With this // configuration and a 65535 period, we get an overflow once every 6.5 seconds. - timer.set_tick_freq(1.mhz()); + timer.set_tick_freq(10.khz()); timer.apply_freq(); timer.resume(); } pub fn ticks_from_secs(secs: u32) -> i32 { - (secs * 1_000_000) as i32 + (secs * 10_000) as i32 } } @@ -26,8 +26,8 @@ impl rtic::Monotonic for SystemTimer { fn ratio() -> rtic::Fraction { rtic::Fraction { - numerator: 1, - denominator: 400, + numerator: 40_000, + denominator: 1, } }