diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs index efec9c1..ba0a508 100644 --- a/src/bin/dual-iir.rs +++ b/src/bin/dual-iir.rs @@ -16,6 +16,7 @@ use stabilizer::{ hal, system_timer::SystemTimer, DigitalInput0, DigitalInput1, AFE0, AFE1, + signal_generator::SignalGenerator, }, net::{ data_stream::{BlockGenerator, StreamTarget}, @@ -39,6 +40,13 @@ pub struct Settings { force_hold: bool, telemetry_period: u16, stream_target: StreamTarget, + signal_generator: signal_generator::Config; + output_mode: [OutputMode; 2], +} + +pub struct OutputMode { + IirFilter, + SignalGenerator, } impl Default for Settings { @@ -59,6 +67,8 @@ impl Default for Settings { // The default telemetry period in seconds. telemetry_period: 10, + signal_generator: signal_generator::Config::default(), + stream_target: StreamTarget::default(), } } @@ -172,24 +182,42 @@ const APP: () = { fence(Ordering::SeqCst); for channel in 0..adc_samples.len() { - adc_samples[channel] - .iter() - .zip(dac_samples[channel].iter_mut()) - .map(|(ai, di)| { - let x = f32::from(*ai as i16); - let y = settings.iir_ch[channel] + match settings.output_mode[channel] { + OutputMode::IirFilter => { + adc_samples[channel] .iter() - .zip(iir_state[channel].iter_mut()) - .fold(x, |yi, (ch, state)| { - ch.update(state, yi, hold) - }); - // Note(unsafe): The filter limits must ensure that the value is in range. - // The truncation introduces 1/2 LSB distortion. - let y: i16 = unsafe { y.to_int_unchecked() }; - // Convert to DAC code - *di = DacCode::from(y).0; - }) - .last(); + .zip(dac_samples[channel].iter_mut()) + .map(|(ai, di)| { + let x = f32::from(*ai as i16); + let y = settings.iir_ch[channel] + .iter() + .zip(iir_state[channel].iter_mut()) + .fold(x, |yi, (ch, state)| { + ch.update(state, yi, hold) + }); + // Note(unsafe): The filter limits must ensure that the value is in range. + // The truncation introduces 1/2 LSB distortion. + let y: i16 = unsafe { y.to_int_unchecked() }; + // Convert to DAC code + *di = DacCode::from(y).0; + }) + .last(); + } + OutputMode::SignalGenerator => { + // Do not generate the samples twice, or we may mess up phasing of the + // signal generator. Instead, copy the previously-generated signal. + // TODO: Is there a nicer way we can handle this edge case? + if (channel == 1) && settings.output_mode[0] == OutputMode::SignalGenerator { + adc_samples[1].copy_from_slice(adc_samples[0]); + } else { + signal_generator.generate(&mut adc_samples[channel]); + } + } + } + } + + if !settings.output_mode.iter().any(|&mode| mode == OutputMode::SignalGenerator) { + signal_generator.skip(adc_samples[0].len()); } // Stream the data. @@ -230,6 +258,9 @@ const APP: () = { c.resources.afes.0.set_gain(settings.afe[0]); c.resources.afes.1.set_gain(settings.afe[1]); + // Update the signal generator + c.resources.signal_generator.update_waveform(settings.signal_generator); + let target = settings.stream_target.into(); c.resources.network.direct_stream(target); } diff --git a/src/hardware/signal_generator.rs b/src/hardware/signal_generator.rs index 0ca57a2..7cbe331 100644 --- a/src/hardware/signal_generator.rs +++ b/src/hardware/signal_generator.rs @@ -66,13 +66,13 @@ struct InternalConf { } #[derive(Debug)] -pub struct Generator { +pub struct SignalGenerator { index: u32, config: InternalConf, pending_config: Option, } -impl Default for Generator { +impl Default for SignalGenerator { fn default() -> Self { Self { config: Config::default().into(), @@ -82,7 +82,7 @@ impl Default for Generator { } } -impl Generator { +impl SignalGenerator { /// Construct a new signal generator with some specific config. /// /// # Args @@ -108,6 +108,18 @@ impl Generator { } } + /// Skip `count` elements of the generator + pub fn skip(&mut self, count: usize) { + let index = self.index.wrapping_add(count); + + // If we skip past the period of the signal, apply any pending config. + if index > self.config.period && let Some(config) = self.pendig_config.take() { + self.config = config; + } + + self.index = index % self.config.period; + } + /// Get the next value in the generator sequence. pub fn next(&mut self) -> i16 { // When phase wraps, apply any new settings.