From 9cca1497b7590b82f80740c69cd498e42fa0fbf7 Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Mon, 28 Jun 2021 13:16:54 +0200
Subject: [PATCH] Adding signal generator control to dual-iir

---
 src/bin/dual-iir.rs              | 65 +++++++++++++++++++++++---------
 src/hardware/signal_generator.rs | 18 +++++++--
 2 files changed, 63 insertions(+), 20 deletions(-)

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<InternalConf>,
 }
 
-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.