From 9d90d7b0d2d3f0b97453541c32b64226985733ba Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Mon, 18 Jan 2021 17:20:33 +0100
Subject: [PATCH 01/14] Adding WIP apps

---
 Cargo.toml              |   8 ++
 src/{main.rs => iir.rs} |   0
 src/lockin.rs           | 280 ++++++++++++++++++++++++++++++++++++++++
 3 files changed, 288 insertions(+)
 rename src/{main.rs => iir.rs} (100%)
 create mode 100644 src/lockin.rs

diff --git a/Cargo.toml b/Cargo.toml
index f4f90fa..64979a8 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -75,3 +75,11 @@ opt-level = 3
 debug = true
 lto = true
 codegen-units = 1
+
+[[bin]]
+name = "iir"
+path = "src/iir.rs"
+
+[[bin]]
+name = "lockin"
+path = "src/lockin.rs"
diff --git a/src/main.rs b/src/iir.rs
similarity index 100%
rename from src/main.rs
rename to src/iir.rs
diff --git a/src/lockin.rs b/src/lockin.rs
new file mode 100644
index 0000000..29cdb1f
--- /dev/null
+++ b/src/lockin.rs
@@ -0,0 +1,280 @@
+#![deny(warnings)]
+#![no_std]
+#![no_main]
+#![cfg_attr(feature = "nightly", feature(core_intrinsics))]
+
+use stm32h7xx_hal as hal;
+
+#[macro_use]
+extern crate log;
+
+use rtic::cyccnt::{Instant, U32Ext};
+
+use heapless::{consts::*, String};
+
+// The number of ticks in the ADC sampling timer. The timer runs at 100MHz, so the step size is
+// equal to 10ns per tick.
+// Currently, the sample rate is equal to: Fsample = 100/256 MHz = 390.625 KHz
+const ADC_SAMPLE_TICKS: u16 = 256;
+
+// The desired ADC sample processing buffer size.
+const SAMPLE_BUFFER_SIZE: usize = 8;
+
+// A constant sinusoid to send on the DAC output.
+const DAC_SEQUENCE: [8; i16] = [0, 0.707, 1, 0.707, 0, -0.707, -1, -0.707];
+
+#[macro_use]
+mod server;
+mod hardware;
+use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
+use dsp::iir;
+
+const SCALE: f32 = ((1 << 15) - 1) as f32;
+
+const TCP_RX_BUFFER_SIZE: usize = 8192;
+const TCP_TX_BUFFER_SIZE: usize = 8192;
+
+#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
+const APP: () = {
+    struct Resources {
+        afes: (AFE0, AFE1),
+        adcs: (Adc0Input, Adc1Input),
+        dacs: (Dac0Output, Dac1Output),
+        net_interface: hardware::Ethernet,
+
+        // Format: iir_state[ch][coeff]
+        #[init([[0.; 5]; 2])]
+        iir_state: [iir::IIRState; 2],
+        #[init([[iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
+        iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
+    }
+
+    #[init]
+    fn init(c: init::Context) -> init::LateResources {
+        // Configure the microcontroller
+        let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
+
+        // Enable ADC/DAC events
+        stabilizer.adcs.0.start();
+        stabilizer.adcs.1.start();
+        stabilizer.dacs.0.start();
+        stabilizer.dacs.1.start();
+
+        // Start sampling ADCs.
+        stabilizer.adc_dac_timer.start();
+
+        init::LateResources {
+            afes: stabilizer.afes,
+            adcs: stabilizer.adcs,
+            dacs: stabilizer.dacs,
+            net_interface: stabilizer.net.interface,
+        }
+    }
+
+    /// Main DSP processing routine for Stabilizer.
+    ///
+    /// # Note
+    /// Processing time for the DSP application code is bounded by the following constraints:
+    ///
+    /// DSP application code starts after the ADC has generated a batch of samples and must be
+    /// completed by the time the next batch of ADC samples has been acquired (plus the FIFO buffer
+    /// time). If this constraint is not met, firmware will panic due to an ADC input overrun.
+    ///
+    /// The DSP application code must also fill out the next DAC output buffer in time such that the
+    /// DAC can switch to it when it has completed the current buffer. If this constraint is not met
+    /// it's possible that old DAC codes will be generated on the output and the output samples will
+    /// be delayed by 1 batch.
+    ///
+    /// 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)]
+    fn process(c: process::Context) {
+        let adc_samples = [
+            c.resources.adcs.0.acquire_buffer(),
+            c.resources.adcs.1.acquire_buffer(),
+        ];
+        let dac_samples = [
+            c.resources.dacs.0.acquire_buffer(),
+            c.resources.dacs.1.acquire_buffer(),
+        ];
+
+        // DAC0 always generates a fixed sinusoidal output.
+        for value in DAC_SEQUENCE.iter() {
+            let y = value * i16::MAX;
+            // Note(unsafe): The DAC_SEQUENCE values are guaranteed to be normalized.
+            let y = unsafe { y.to_int_unchecked::<i16>() };
+
+            // Convert to DAC code
+            dac_samples[0][sample] = y as u16 ^ 0x8000;
+        }
+
+        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;
+                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);
+                }
+                // Note(unsafe): The filter limits ensure that the value is in range.
+                // The truncation introduces 1/2 LSB distortion.
+                let y = unsafe { y.to_int_unchecked::<i16>() };
+                // Convert to DAC code
+                dac_samples[channel][sample] = y as u16 ^ 0x8000;
+            }
+        }
+    }
+
+    #[idle(resources=[net_interface, iir_state, iir_ch, afes])]
+    fn idle(mut c: idle::Context) -> ! {
+        let mut socket_set_entries: [_; 8] = Default::default();
+        let mut sockets =
+            smoltcp::socket::SocketSet::new(&mut socket_set_entries[..]);
+
+        let mut rx_storage = [0; TCP_RX_BUFFER_SIZE];
+        let mut tx_storage = [0; TCP_TX_BUFFER_SIZE];
+        let tcp_handle = {
+            let tcp_rx_buffer =
+                smoltcp::socket::TcpSocketBuffer::new(&mut rx_storage[..]);
+            let tcp_tx_buffer =
+                smoltcp::socket::TcpSocketBuffer::new(&mut tx_storage[..]);
+            let tcp_socket =
+                smoltcp::socket::TcpSocket::new(tcp_rx_buffer, tcp_tx_buffer);
+            sockets.add(tcp_socket)
+        };
+
+        let mut server = server::Server::new();
+
+        let mut time = 0u32;
+        let mut next_ms = Instant::now();
+
+        // TODO: Replace with reference to CPU clock from CCDR.
+        next_ms += 400_000.cycles();
+
+        loop {
+            let tick = Instant::now() > next_ms;
+
+            if tick {
+                next_ms += 400_000.cycles();
+                time += 1;
+            }
+
+            {
+                let socket =
+                    &mut *sockets.get::<smoltcp::socket::TcpSocket>(tcp_handle);
+                if socket.state() == smoltcp::socket::TcpState::CloseWait {
+                    socket.close();
+                } else if !(socket.is_open() || socket.is_listening()) {
+                    socket
+                        .listen(1235)
+                        .unwrap_or_else(|e| warn!("TCP listen error: {:?}", e));
+                } else {
+                    server.poll(socket, |req| {
+                        info!("Got request: {:?}", req);
+                        route_request!(req,
+                            readable_attributes: [
+                                "stabilizer/iir/state": (|| {
+                                    let state = c.resources.iir_state.lock(|iir_state|
+                                        server::Status {
+                                            t: time,
+                                            x0: iir_state[0][0],
+                                            y0: iir_state[0][2],
+                                            x1: iir_state[1][0],
+                                            y1: iir_state[1][2],
+                                    });
+
+                                    Ok::<server::Status, ()>(state)
+                                }),
+                                "stabilizer/afe0/gain": (|| c.resources.afes.0.get_gain()),
+                                "stabilizer/afe1/gain": (|| c.resources.afes.1.get_gain())
+                            ],
+
+                            modifiable_attributes: [
+                                "stabilizer/iir0/state": server::IirRequest, (|req: server::IirRequest| {
+                                    c.resources.iir_ch.lock(|iir_ch| {
+                                        if req.channel > 1 {
+                                            return Err(());
+                                        }
+
+                                        iir_ch[req.channel as usize] = req.iir;
+
+                                        Ok::<server::IirRequest, ()>(req)
+                                    })
+                                }),
+                                "stabilizer/iir1/state": server::IirRequest, (|req: server::IirRequest| {
+                                    c.resources.iir_ch.lock(|iir_ch| {
+                                        if req.channel > 1 {
+                                            return Err(());
+                                        }
+
+                                        iir_ch[req.channel as usize] = req.iir;
+
+                                        Ok::<server::IirRequest, ()>(req)
+                                    })
+                                }),
+                                }),
+                                "stabilizer/afe0/gain": hardware::AfeGain, (|gain| {
+                                    c.resources.afes.0.set_gain(gain);
+                                    Ok::<(), ()>(())
+                                }),
+                                "stabilizer/afe1/gain": hardware::AfeGain, (|gain| {
+                                    c.resources.afes.1.set_gain(gain);
+                                    Ok::<(), ()>(())
+                                })
+                            ]
+                        )
+                    });
+                }
+            }
+
+            let sleep = match c.resources.net_interface.poll(
+                &mut sockets,
+                smoltcp::time::Instant::from_millis(time as i64),
+            ) {
+                Ok(changed) => !changed,
+                Err(smoltcp::Error::Unrecognized) => true,
+                Err(e) => {
+                    info!("iface poll error: {:?}", e);
+                    true
+                }
+            };
+
+            if sleep {
+                cortex_m::asm::wfi();
+            }
+        }
+    }
+
+    #[task(binds = ETH, priority = 1)]
+    fn eth(_: eth::Context) {
+        unsafe { hal::ethernet::interrupt_handler() }
+    }
+
+    #[task(binds = SPI2, priority = 3)]
+    fn spi2(_: spi2::Context) {
+        panic!("ADC0 input overrun");
+    }
+
+    #[task(binds = SPI3, priority = 3)]
+    fn spi3(_: spi3::Context) {
+        panic!("ADC0 input overrun");
+    }
+
+    #[task(binds = SPI4, priority = 3)]
+    fn spi4(_: spi4::Context) {
+        panic!("DAC0 output error");
+    }
+
+    #[task(binds = SPI5, priority = 3)]
+    fn spi5(_: spi5::Context) {
+        panic!("DAC1 output error");
+    }
+
+    extern "C" {
+        // hw interrupt handlers for RTIC to use for scheduling tasks
+        // one per priority
+        fn DCMI();
+        fn JPEG();
+        fn SDMMC();
+    }
+};

From ac06f811ab4bf88e791ec33cd10b06a2cd785835 Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Mon, 18 Jan 2021 18:02:00 +0100
Subject: [PATCH 02/14] Adding framework for initial lockin demo

---
 src/hardware/adc.rs |  2 +
 src/lockin.rs       | 91 ++++++++++++++++++---------------------------
 2 files changed, 39 insertions(+), 54 deletions(-)

diff --git a/src/hardware/adc.rs b/src/hardware/adc.rs
index 188e436..d0d7d4f 100644
--- a/src/hardware/adc.rs
+++ b/src/hardware/adc.rs
@@ -328,6 +328,7 @@ macro_rules! adc_input {
                 }
 
                 /// Enable the ADC DMA transfer sequence.
+                #[allow(dead_code)]
                 pub fn start(&mut self) {
                     self.transfer.start(|spi| {
                         spi.enable_dma_rx();
@@ -345,6 +346,7 @@ macro_rules! adc_input {
                 ///
                 /// # Returns
                 /// A reference to the underlying buffer that has been filled with ADC samples.
+                #[allow(dead_code)]
                 pub fn acquire_buffer(&mut self) -> &[u16; SAMPLE_BUFFER_SIZE] {
                     // Wait for the transfer to fully complete before continuing.  Note: If a device
                     // hangs up, check that this conditional is passing correctly, as there is no
diff --git a/src/lockin.rs b/src/lockin.rs
index 29cdb1f..9de067e 100644
--- a/src/lockin.rs
+++ b/src/lockin.rs
@@ -21,12 +21,12 @@ const ADC_SAMPLE_TICKS: u16 = 256;
 const SAMPLE_BUFFER_SIZE: usize = 8;
 
 // A constant sinusoid to send on the DAC output.
-const DAC_SEQUENCE: [8; i16] = [0, 0.707, 1, 0.707, 0, -0.707, -1, -0.707];
+const DAC_SEQUENCE: [f32; 8] = [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
 
 #[macro_use]
 mod server;
 mod hardware;
-use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
+use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
 use dsp::iir;
 
 const SCALE: f32 = ((1 << 15) - 1) as f32;
@@ -38,15 +38,15 @@ const TCP_TX_BUFFER_SIZE: usize = 8192;
 const APP: () = {
     struct Resources {
         afes: (AFE0, AFE1),
-        adcs: (Adc0Input, Adc1Input),
+        adc1: Adc1Input,
         dacs: (Dac0Output, Dac1Output),
         net_interface: hardware::Ethernet,
 
-        // Format: iir_state[ch][coeff]
-        #[init([[0.; 5]; 2])]
-        iir_state: [iir::IIRState; 2],
-        #[init([[iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])]
-        iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
+        #[init([0.; 5])]
+        iir_state: iir::IIRState,
+
+        #[init(iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE })]
+        iir: iir::IIR,
     }
 
     #[init]
@@ -55,7 +55,6 @@ const APP: () = {
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
         // Enable ADC/DAC events
-        stabilizer.adcs.0.start();
         stabilizer.adcs.1.start();
         stabilizer.dacs.0.start();
         stabilizer.dacs.1.start();
@@ -65,7 +64,7 @@ const APP: () = {
 
         init::LateResources {
             afes: stabilizer.afes,
-            adcs: stabilizer.adcs,
+            adc1: stabilizer.adcs.1,
             dacs: stabilizer.dacs,
             net_interface: stabilizer.net.interface,
         }
@@ -87,45 +86,41 @@ 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=[adc1, dacs, iir_state, iir], priority=2)]
     fn process(c: process::Context) {
-        let adc_samples = [
-            c.resources.adcs.0.acquire_buffer(),
-            c.resources.adcs.1.acquire_buffer(),
-        ];
+        let _adc_samples = c.resources.adc1.acquire_buffer();
         let dac_samples = [
             c.resources.dacs.0.acquire_buffer(),
             c.resources.dacs.1.acquire_buffer(),
         ];
 
         // DAC0 always generates a fixed sinusoidal output.
-        for value in DAC_SEQUENCE.iter() {
-            let y = value * i16::MAX;
+        for (i, value) in DAC_SEQUENCE.iter().enumerate() {
+            let y = value * i16::MAX as f32;
             // Note(unsafe): The DAC_SEQUENCE values are guaranteed to be normalized.
             let y = unsafe { y.to_int_unchecked::<i16>() };
 
             // Convert to DAC code
-            dac_samples[0][sample] = y as u16 ^ 0x8000;
+            dac_samples[0][i] = y as u16 ^ 0x8000;
         }
 
-        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;
-                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);
-                }
-                // Note(unsafe): The filter limits ensure that the value is in range.
-                // The truncation introduces 1/2 LSB distortion.
-                let y = unsafe { y.to_int_unchecked::<i16>() };
-                // Convert to DAC code
-                dac_samples[channel][sample] = y as u16 ^ 0x8000;
-            }
+        // TODO: Introduce a "dummy" PLL here.
+
+        // TODO: Demodulate the ADC0 input samples with the dummy PLL.
+
+        // TODO: Filter the demodulated ADC values
+
+        // TODO: Compute phase of the last sample
+
+        // TODO: Place last sample phase into DAC1s output buffer.
+        let y = 0.0;
+
+        for value in dac_samples[1].iter_mut() {
+            *value = y as u16 ^ 0x8000
         }
     }
 
-    #[idle(resources=[net_interface, iir_state, iir_ch, afes])]
+    #[idle(resources=[net_interface, iir_state, iir, afes])]
     fn idle(mut c: idle::Context) -> ! {
         let mut socket_set_entries: [_; 8] = Default::default();
         let mut sockets =
@@ -177,10 +172,10 @@ const APP: () = {
                                     let state = c.resources.iir_state.lock(|iir_state|
                                         server::Status {
                                             t: time,
-                                            x0: iir_state[0][0],
-                                            y0: iir_state[0][2],
-                                            x1: iir_state[1][0],
-                                            y1: iir_state[1][2],
+                                            x0: iir_state[0],
+                                            y0: iir_state[2],
+                                            x1: iir_state[0],
+                                            y1: iir_state[2],
                                     });
 
                                     Ok::<server::Status, ()>(state)
@@ -190,29 +185,17 @@ const APP: () = {
                             ],
 
                             modifiable_attributes: [
-                                "stabilizer/iir0/state": server::IirRequest, (|req: server::IirRequest| {
-                                    c.resources.iir_ch.lock(|iir_ch| {
-                                        if req.channel > 1 {
+                                "stabilizer/iir/state": server::IirRequest, (|req: server::IirRequest| {
+                                    c.resources.iir.lock(|iir| {
+                                        if req.channel >= 1 {
                                             return Err(());
                                         }
 
-                                        iir_ch[req.channel as usize] = req.iir;
+                                        *iir = req.iir;
 
                                         Ok::<server::IirRequest, ()>(req)
                                     })
                                 }),
-                                "stabilizer/iir1/state": server::IirRequest, (|req: server::IirRequest| {
-                                    c.resources.iir_ch.lock(|iir_ch| {
-                                        if req.channel > 1 {
-                                            return Err(());
-                                        }
-
-                                        iir_ch[req.channel as usize] = req.iir;
-
-                                        Ok::<server::IirRequest, ()>(req)
-                                    })
-                                }),
-                                }),
                                 "stabilizer/afe0/gain": hardware::AfeGain, (|gain| {
                                     c.resources.afes.0.set_gain(gain);
                                     Ok::<(), ()>(())
@@ -257,7 +240,7 @@ const APP: () = {
 
     #[task(binds = SPI3, priority = 3)]
     fn spi3(_: spi3::Context) {
-        panic!("ADC0 input overrun");
+        panic!("ADC1 input overrun");
     }
 
     #[task(binds = SPI4, priority = 3)]

From f1f15aca65f8fe4677205adff8ef433aa6ba0c1c Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Wed, 20 Jan 2021 12:49:07 +0100
Subject: [PATCH 03/14] Refactoring app structure

---
 src/{iir.rs => bin/dual-iir.rs} | 19 +++++--------------
 src/{ => bin}/lockin.rs         |  0
 src/lib.rs                      | 16 ++++++++++++++++
 src/server.rs                   | 10 ++++------
 4 files changed, 25 insertions(+), 20 deletions(-)
 rename src/{iir.rs => bin/dual-iir.rs} (96%)
 rename src/{ => bin}/lockin.rs (100%)
 create mode 100644 src/lib.rs

diff --git a/src/iir.rs b/src/bin/dual-iir.rs
similarity index 96%
rename from src/iir.rs
rename to src/bin/dual-iir.rs
index 3b86c37..fd8f5e4 100644
--- a/src/iir.rs
+++ b/src/bin/dual-iir.rs
@@ -12,20 +12,8 @@ use rtic::cyccnt::{Instant, U32Ext};
 
 use heapless::{consts::*, String};
 
-// The number of ticks in the ADC sampling timer. The timer runs at 100MHz, so the step size is
-// equal to 10ns per tick.
-// Currently, the sample rate is equal to: Fsample = 100/256 MHz = 390.625 KHz
-const ADC_SAMPLE_TICKS: u16 = 256;
+use stabilizer::{hardware, server};
 
-// The desired ADC sample processing buffer size.
-const SAMPLE_BUFFER_SIZE: usize = 8;
-
-// The number of cascaded IIR biquads per channel. Select 1 or 2!
-const IIR_CASCADE_LENGTH: usize = 1;
-
-#[macro_use]
-mod server;
-mod hardware;
 use dsp::iir;
 use hardware::{Adc0Input, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
 
@@ -34,6 +22,9 @@ const SCALE: f32 = ((1 << 15) - 1) as f32;
 const TCP_RX_BUFFER_SIZE: usize = 8192;
 const TCP_TX_BUFFER_SIZE: usize = 8192;
 
+// The number of cascaded IIR biquads per channel. Select 1 or 2!
+const IIR_CASCADE_LENGTH: usize = 1;
+
 #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
 const APP: () = {
     struct Resources {
@@ -162,7 +153,7 @@ const APP: () = {
                 } else {
                     server.poll(socket, |req| {
                         info!("Got request: {:?}", req);
-                        route_request!(req,
+                        stabilizer::route_request!(req,
                             readable_attributes: [
                                 "stabilizer/iir/state": (|| {
                                     let state = c.resources.iir_state.lock(|iir_state|
diff --git a/src/lockin.rs b/src/bin/lockin.rs
similarity index 100%
rename from src/lockin.rs
rename to src/bin/lockin.rs
diff --git a/src/lib.rs b/src/lib.rs
new file mode 100644
index 0000000..a51024a
--- /dev/null
+++ b/src/lib.rs
@@ -0,0 +1,16 @@
+#![no_std]
+
+pub mod server;
+
+#[macro_use]
+extern crate log;
+
+pub mod hardware;
+
+// The number of ticks in the ADC sampling timer. The timer runs at 100MHz, so the step size is
+// equal to 10ns per tick.
+// Currently, the sample rate is equal to: Fsample = 100/256 MHz = 390.625 KHz
+const ADC_SAMPLE_TICKS: u16 = 256;
+
+// The desired ADC sample processing buffer size.
+const SAMPLE_BUFFER_SIZE: usize = 8;
diff --git a/src/server.rs b/src/server.rs
index f434060..5e6950d 100644
--- a/src/server.rs
+++ b/src/server.rs
@@ -1,14 +1,12 @@
-use heapless::{consts::*, String, Vec};
-
 use core::fmt::Write;
-
+use heapless::{consts::*, String, Vec};
 use serde::{Deserialize, Serialize};
-
 use serde_json_core::{de::from_slice, ser::to_string};
-
-use super::iir;
 use smoltcp as net;
 
+use dsp::iir;
+
+#[macro_export]
 macro_rules! route_request {
     ($request:ident,
             readable_attributes: [$($read_attribute:tt: $getter:tt),*],

From b2cbc6791df6c9fcef2fbafd12f25b3cc59ed8f9 Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Wed, 20 Jan 2021 12:55:45 +0100
Subject: [PATCH 04/14] Restructuring

---
 Cargo.toml        |  8 --------
 src/bin/lockin.rs | 18 +++++-------------
 2 files changed, 5 insertions(+), 21 deletions(-)

diff --git a/Cargo.toml b/Cargo.toml
index 64979a8..f4f90fa 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -75,11 +75,3 @@ opt-level = 3
 debug = true
 lto = true
 codegen-units = 1
-
-[[bin]]
-name = "iir"
-path = "src/iir.rs"
-
-[[bin]]
-name = "lockin"
-path = "src/lockin.rs"
diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index 9de067e..6b07901 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -12,21 +12,13 @@ use rtic::cyccnt::{Instant, U32Ext};
 
 use heapless::{consts::*, String};
 
-// The number of ticks in the ADC sampling timer. The timer runs at 100MHz, so the step size is
-// equal to 10ns per tick.
-// Currently, the sample rate is equal to: Fsample = 100/256 MHz = 390.625 KHz
-const ADC_SAMPLE_TICKS: u16 = 256;
-
-// The desired ADC sample processing buffer size.
-const SAMPLE_BUFFER_SIZE: usize = 8;
-
 // A constant sinusoid to send on the DAC output.
 const DAC_SEQUENCE: [f32; 8] = [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
 
-#[macro_use]
-mod server;
-mod hardware;
-use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
+use stabilizer::{
+    hardware::{self, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1},
+    server
+};
 use dsp::iir;
 
 const SCALE: f32 = ((1 << 15) - 1) as f32;
@@ -166,7 +158,7 @@ const APP: () = {
                 } else {
                     server.poll(socket, |req| {
                         info!("Got request: {:?}", req);
-                        route_request!(req,
+                        stabilizer::route_request!(req,
                             readable_attributes: [
                                 "stabilizer/iir/state": (|| {
                                     let state = c.resources.iir_state.lock(|iir_state|

From 2ef27b8187c3e09f03dc2945e34c98749914bc20 Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Wed, 20 Jan 2021 12:55:55 +0100
Subject: [PATCH 05/14] Formatting

---
 src/bin/lockin.rs | 7 ++++---
 1 file changed, 4 insertions(+), 3 deletions(-)

diff --git a/src/bin/lockin.rs b/src/bin/lockin.rs
index 6b07901..3bd70fd 100644
--- a/src/bin/lockin.rs
+++ b/src/bin/lockin.rs
@@ -13,13 +13,14 @@ use rtic::cyccnt::{Instant, U32Ext};
 use heapless::{consts::*, String};
 
 // A constant sinusoid to send on the DAC output.
-const DAC_SEQUENCE: [f32; 8] = [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
+const DAC_SEQUENCE: [f32; 8] =
+    [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
 
+use dsp::iir;
 use stabilizer::{
     hardware::{self, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1},
-    server
+    server,
 };
-use dsp::iir;
 
 const SCALE: f32 = ((1 << 15) - 1) as f32;
 

From 7c5a74c35ef95f70b91a5536d152cd495cedff0b Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Tue, 26 Jan 2021 10:52:35 +0100
Subject: [PATCH 06/14] Renaming internal lockin

---
 src/bin/{lockin.rs => lockin-internal-demo.rs} | 0
 1 file changed, 0 insertions(+), 0 deletions(-)
 rename src/bin/{lockin.rs => lockin-internal-demo.rs} (100%)

diff --git a/src/bin/lockin.rs b/src/bin/lockin-internal-demo.rs
similarity index 100%
rename from src/bin/lockin.rs
rename to src/bin/lockin-internal-demo.rs

From e161f498228b24379fb6a67c1a2d372edad39dba Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Tue, 26 Jan 2021 12:21:44 +0100
Subject: [PATCH 07/14] Adding WIP lockin demo

---
 src/bin/lockin-internal-demo.rs | 64 ++++++++++++++++++++++++++-------
 src/lib.rs                      |  1 -
 2 files changed, 51 insertions(+), 14 deletions(-)

diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal-demo.rs
index 3bd70fd..113fafd 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal-demo.rs
@@ -16,10 +16,10 @@ use heapless::{consts::*, String};
 const DAC_SEQUENCE: [f32; 8] =
     [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
 
-use dsp::iir;
+use dsp::{iir, iir_int, lockin::Lockin, reciprocal_pll::TimestampHandler};
+use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
 use stabilizer::{
-    hardware::{self, Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1},
-    server,
+    hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2,
 };
 
 const SCALE: f32 = ((1 << 15) - 1) as f32;
@@ -40,6 +40,9 @@ const APP: () = {
 
         #[init(iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE })]
         iir: iir::IIR,
+
+        pll: TimestampHandler,
+        lockin: Lockin,
     }
 
     #[init]
@@ -47,6 +50,17 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
+        let pll = TimestampHandler::new(
+            4, // relative PLL frequency bandwidth: 2**-4, TODO: expose
+            3, // relative PLL phase bandwidth: 2**-3, TODO: expose
+            ADC_SAMPLE_TICKS_LOG2 as usize,
+            SAMPLE_BUFFER_SIZE_LOG2,
+        );
+
+        let lockin = Lockin::new(
+            &iir_int::IIRState::default(), // TODO: lowpass, expose
+        );
+
         // Enable ADC/DAC events
         stabilizer.adcs.1.start();
         stabilizer.dacs.0.start();
@@ -56,6 +70,8 @@ const APP: () = {
         stabilizer.adc_dac_timer.start();
 
         init::LateResources {
+            lockin,
+            pll,
             afes: stabilizer.afes,
             adc1: stabilizer.adcs.1,
             dacs: stabilizer.dacs,
@@ -79,9 +95,11 @@ 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=[adc1, dacs, iir_state, iir], priority=2)]
-    fn process(c: process::Context) {
-        let _adc_samples = c.resources.adc1.acquire_buffer();
+    ///
+    /// TODO: Document
+    #[task(binds=DMA1_STR4, resources=[adc1, dacs, iir_state, iir, lockin, pll], priority=2)]
+    fn process(mut c: process::Context) {
+        let adc_samples = c.resources.adc1.acquire_buffer();
         let dac_samples = [
             c.resources.dacs.0.acquire_buffer(),
             c.resources.dacs.1.acquire_buffer(),
@@ -97,19 +115,39 @@ const APP: () = {
             dac_samples[0][i] = y as u16 ^ 0x8000;
         }
 
-        // TODO: Introduce a "dummy" PLL here.
+        // TODO: Verify that the DAC code is always generated at T=0
+        let (pll_phase, pll_frequency) = c.resources.pll.update(Some(0));
 
-        // TODO: Demodulate the ADC0 input samples with the dummy PLL.
+        // Harmonic index of the LO: -1 to _de_modulate the fundamental
+        let harmonic: i32 = -1;
 
-        // TODO: Filter the demodulated ADC values
+        // Demodulation LO phase offset
+        let phase_offset: i32 = 0;
+        let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
+        let mut sample_phase = phase_offset
+            .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
 
-        // TODO: Compute phase of the last sample
+        let mut phase = 0f32;
 
-        // TODO: Place last sample phase into DAC1s output buffer.
-        let y = 0.0;
+        for sample in adc_samples.iter() {
+            // Convert to signed, MSB align the ADC sample.
+            let input = (*sample as i16 as i32) << 16;
+
+            // Obtain demodulated, filtered IQ sample.
+            let output = c.resources.lockin.update(input, sample_phase);
+
+            // Advance the sample phase.
+            sample_phase = sample_phase.wrapping_add(sample_frequency);
+
+            // Convert from IQ to phase.
+            phase = output.phase() as _;
+        }
+
+        // Filter phase through an IIR.
+        phase = c.resources.iir.update(&mut c.resources.iir_state, phase);
 
         for value in dac_samples[1].iter_mut() {
-            *value = y as u16 ^ 0x8000
+            *value = phase as u16 ^ 0x8000
         }
     }
 
diff --git a/src/lib.rs b/src/lib.rs
index 9a73582..254e626 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,6 +1,5 @@
 #![no_std]
 
-
 #[macro_use]
 extern crate log;
 

From 51085d175e07f8e55b8a0310076d5c17d7eb4b18 Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Tue, 26 Jan 2021 12:23:17 +0100
Subject: [PATCH 08/14] Removing dead-code allowance

---
 src/hardware/adc.rs | 2 --
 1 file changed, 2 deletions(-)

diff --git a/src/hardware/adc.rs b/src/hardware/adc.rs
index d0d7d4f..188e436 100644
--- a/src/hardware/adc.rs
+++ b/src/hardware/adc.rs
@@ -328,7 +328,6 @@ macro_rules! adc_input {
                 }
 
                 /// Enable the ADC DMA transfer sequence.
-                #[allow(dead_code)]
                 pub fn start(&mut self) {
                     self.transfer.start(|spi| {
                         spi.enable_dma_rx();
@@ -346,7 +345,6 @@ macro_rules! adc_input {
                 ///
                 /// # Returns
                 /// A reference to the underlying buffer that has been filled with ADC samples.
-                #[allow(dead_code)]
                 pub fn acquire_buffer(&mut self) -> &[u16; SAMPLE_BUFFER_SIZE] {
                     // Wait for the transfer to fully complete before continuing.  Note: If a device
                     // hangs up, check that this conditional is passing correctly, as there is no

From c030b97714b3027d0e035dc9463b3ca2ba1361a3 Mon Sep 17 00:00:00 2001
From: Ryan Summers <summers.ryan.m@gmail.com>
Date: Tue, 26 Jan 2021 12:49:45 +0100
Subject: [PATCH 09/14] Apply suggestions from code review
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Co-authored-by: Robert Jördens <rj@quartiq.de>
---
 src/bin/lockin-internal-demo.rs | 11 ++---------
 1 file changed, 2 insertions(+), 9 deletions(-)

diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal-demo.rs
index 113fafd..6acfc36 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal-demo.rs
@@ -41,7 +41,6 @@ const APP: () = {
         #[init(iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE })]
         iir: iir::IIR,
 
-        pll: TimestampHandler,
         lockin: Lockin,
     }
 
@@ -50,13 +49,6 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
-        let pll = TimestampHandler::new(
-            4, // relative PLL frequency bandwidth: 2**-4, TODO: expose
-            3, // relative PLL phase bandwidth: 2**-3, TODO: expose
-            ADC_SAMPLE_TICKS_LOG2 as usize,
-            SAMPLE_BUFFER_SIZE_LOG2,
-        );
-
         let lockin = Lockin::new(
             &iir_int::IIRState::default(), // TODO: lowpass, expose
         );
@@ -116,7 +108,8 @@ const APP: () = {
         }
 
         // TODO: Verify that the DAC code is always generated at T=0
-        let (pll_phase, pll_frequency) = c.resources.pll.update(Some(0));
+        let pll_phase = 0i32;
+        let pll_frequency = 1i32 << (32 - 3); // 1/8 of the sample rate
 
         // Harmonic index of the LO: -1 to _de_modulate the fundamental
         let harmonic: i32 = -1;

From c628b8d57aa2bbcd383e1e14769e43a1438722a8 Mon Sep 17 00:00:00 2001
From: Ryan Summers <summers.ryan.m@gmail.com>
Date: Fri, 29 Jan 2021 09:55:23 +0100
Subject: [PATCH 10/14] Update src/bin/lockin-internal-demo.rs
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Co-authored-by: Robert Jördens <rj@quartiq.de>
---
 src/bin/lockin-internal-demo.rs | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal-demo.rs
index 6acfc36..e36c676 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal-demo.rs
@@ -50,7 +50,7 @@ const APP: () = {
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
         let lockin = Lockin::new(
-            &iir_int::IIRState::default(), // TODO: lowpass, expose
+            &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose
         );
 
         // Enable ADC/DAC events

From 1ebbe0f6d77a01cd3aba478cc78a5248601bc27e Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Fri, 29 Jan 2021 10:11:56 +0100
Subject: [PATCH 11/14] Cleaning up demo

---
 src/bin/lockin-internal-demo.rs | 133 ++------------------------------
 1 file changed, 8 insertions(+), 125 deletions(-)

diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal-demo.rs
index e36c676..969f332 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal-demo.rs
@@ -3,37 +3,22 @@
 #![no_main]
 #![cfg_attr(feature = "nightly", feature(core_intrinsics))]
 
-use stm32h7xx_hal as hal;
-
-#[macro_use]
-extern crate log;
-
-use rtic::cyccnt::{Instant, U32Ext};
-
-use heapless::{consts::*, String};
-
 // A constant sinusoid to send on the DAC output.
 const DAC_SEQUENCE: [f32; 8] =
     [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
 
-use dsp::{iir, iir_int, lockin::Lockin, reciprocal_pll::TimestampHandler};
+use dsp::{iir, iir_int, lockin::Lockin};
 use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
-use stabilizer::{
-    hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2,
-};
+use stabilizer::hardware;
 
 const SCALE: f32 = ((1 << 15) - 1) as f32;
 
-const TCP_RX_BUFFER_SIZE: usize = 8192;
-const TCP_TX_BUFFER_SIZE: usize = 8192;
-
 #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
 const APP: () = {
     struct Resources {
         afes: (AFE0, AFE1),
         adc1: Adc1Input,
         dacs: (Dac0Output, Dac1Output),
-        net_interface: hardware::Ethernet,
 
         #[init([0.; 5])]
         iir_state: iir::IIRState,
@@ -63,11 +48,9 @@ const APP: () = {
 
         init::LateResources {
             lockin,
-            pll,
             afes: stabilizer.afes,
             adc1: stabilizer.adcs.1,
             dacs: stabilizer.dacs,
-            net_interface: stabilizer.net.interface,
         }
     }
 
@@ -89,7 +72,7 @@ const APP: () = {
     /// the same time bounds, meeting one also means the other is also met.
     ///
     /// TODO: Document
-    #[task(binds=DMA1_STR4, resources=[adc1, dacs, iir_state, iir, lockin, pll], priority=2)]
+    #[task(binds=DMA1_STR4, resources=[adc1, dacs, iir_state, iir, lockin], priority=2)]
     fn process(mut c: process::Context) {
         let adc_samples = c.resources.adc1.acquire_buffer();
         let dac_samples = [
@@ -144,117 +127,17 @@ const APP: () = {
         }
     }
 
-    #[idle(resources=[net_interface, iir_state, iir, afes])]
-    fn idle(mut c: idle::Context) -> ! {
-        let mut socket_set_entries: [_; 8] = Default::default();
-        let mut sockets =
-            smoltcp::socket::SocketSet::new(&mut socket_set_entries[..]);
-
-        let mut rx_storage = [0; TCP_RX_BUFFER_SIZE];
-        let mut tx_storage = [0; TCP_TX_BUFFER_SIZE];
-        let tcp_handle = {
-            let tcp_rx_buffer =
-                smoltcp::socket::TcpSocketBuffer::new(&mut rx_storage[..]);
-            let tcp_tx_buffer =
-                smoltcp::socket::TcpSocketBuffer::new(&mut tx_storage[..]);
-            let tcp_socket =
-                smoltcp::socket::TcpSocket::new(tcp_rx_buffer, tcp_tx_buffer);
-            sockets.add(tcp_socket)
-        };
-
-        let mut server = server::Server::new();
-
-        let mut time = 0u32;
-        let mut next_ms = Instant::now();
-
-        // TODO: Replace with reference to CPU clock from CCDR.
-        next_ms += 400_000.cycles();
-
+    #[idle(resources=[iir_state, iir, afes])]
+    fn idle(_: idle::Context) -> ! {
         loop {
-            let tick = Instant::now() > next_ms;
-
-            if tick {
-                next_ms += 400_000.cycles();
-                time += 1;
-            }
-
-            {
-                let socket =
-                    &mut *sockets.get::<smoltcp::socket::TcpSocket>(tcp_handle);
-                if socket.state() == smoltcp::socket::TcpState::CloseWait {
-                    socket.close();
-                } else if !(socket.is_open() || socket.is_listening()) {
-                    socket
-                        .listen(1235)
-                        .unwrap_or_else(|e| warn!("TCP listen error: {:?}", e));
-                } else {
-                    server.poll(socket, |req| {
-                        info!("Got request: {:?}", req);
-                        stabilizer::route_request!(req,
-                            readable_attributes: [
-                                "stabilizer/iir/state": (|| {
-                                    let state = c.resources.iir_state.lock(|iir_state|
-                                        server::Status {
-                                            t: time,
-                                            x0: iir_state[0],
-                                            y0: iir_state[2],
-                                            x1: iir_state[0],
-                                            y1: iir_state[2],
-                                    });
-
-                                    Ok::<server::Status, ()>(state)
-                                }),
-                                "stabilizer/afe0/gain": (|| c.resources.afes.0.get_gain()),
-                                "stabilizer/afe1/gain": (|| c.resources.afes.1.get_gain())
-                            ],
-
-                            modifiable_attributes: [
-                                "stabilizer/iir/state": server::IirRequest, (|req: server::IirRequest| {
-                                    c.resources.iir.lock(|iir| {
-                                        if req.channel >= 1 {
-                                            return Err(());
-                                        }
-
-                                        *iir = req.iir;
-
-                                        Ok::<server::IirRequest, ()>(req)
-                                    })
-                                }),
-                                "stabilizer/afe0/gain": hardware::AfeGain, (|gain| {
-                                    c.resources.afes.0.set_gain(gain);
-                                    Ok::<(), ()>(())
-                                }),
-                                "stabilizer/afe1/gain": hardware::AfeGain, (|gain| {
-                                    c.resources.afes.1.set_gain(gain);
-                                    Ok::<(), ()>(())
-                                })
-                            ]
-                        )
-                    });
-                }
-            }
-
-            let sleep = match c.resources.net_interface.poll(
-                &mut sockets,
-                smoltcp::time::Instant::from_millis(time as i64),
-            ) {
-                Ok(changed) => !changed,
-                Err(smoltcp::Error::Unrecognized) => true,
-                Err(e) => {
-                    info!("iface poll error: {:?}", e);
-                    true
-                }
-            };
-
-            if sleep {
-                cortex_m::asm::wfi();
-            }
+            // TODO: Implement network interface.
+            cortex_m::asm::wfi();
         }
     }
 
     #[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)]

From ab7d725235708727f80005f29c8fb170c94a792a Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Fri, 29 Jan 2021 11:01:21 +0100
Subject: [PATCH 12/14] Updating lockin demo after testing

---
 src/bin/lockin-internal-demo.rs | 22 +++++++++++++++++-----
 1 file changed, 17 insertions(+), 5 deletions(-)

diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal-demo.rs
index 969f332..a8761d2 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal-demo.rs
@@ -9,10 +9,12 @@ const DAC_SEQUENCE: [f32; 8] =
 
 use dsp::{iir, iir_int, lockin::Lockin};
 use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
-use stabilizer::hardware;
+use stabilizer::{ADC_SAMPLE_TICKS, hardware};
 
 const SCALE: f32 = ((1 << 15) - 1) as f32;
 
+const PHASE_SCALING: f32 = 1e12;
+
 #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
 const APP: () = {
     struct Resources {
@@ -34,8 +36,18 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
+        // The desired corner frequency is always
+        let desired_corner_frequency = 10e3;
+        let gain = 1000.0;
+
+        // Calculate the IIR corner freuqency parameter as a function of the sample rate.
+        let corner_frequency = {
+            let sample_rate = 1.0 / (10e-9 * ADC_SAMPLE_TICKS as f32);
+            desired_corner_frequency / sample_rate
+        };
+
         let lockin = Lockin::new(
-            &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose
+            &iir_int::IIRState::lowpass(corner_frequency, 0.707, gain), // TODO: expose
         );
 
         // Enable ADC/DAC events
@@ -82,7 +94,8 @@ const APP: () = {
 
         // DAC0 always generates a fixed sinusoidal output.
         for (i, value) in DAC_SEQUENCE.iter().enumerate() {
-            let y = value * i16::MAX as f32;
+            // Full-scale gives a +/- 12V amplitude waveform. Scale it down to give +/- 100mV.
+            let y = value * i16::MAX as f32 / 120.0;
             // Note(unsafe): The DAC_SEQUENCE values are guaranteed to be normalized.
             let y = unsafe { y.to_int_unchecked::<i16>() };
 
@@ -90,7 +103,6 @@ const APP: () = {
             dac_samples[0][i] = y as u16 ^ 0x8000;
         }
 
-        // TODO: Verify that the DAC code is always generated at T=0
         let pll_phase = 0i32;
         let pll_frequency = 1i32 << (32 - 3); // 1/8 of the sample rate
 
@@ -120,7 +132,7 @@ const APP: () = {
         }
 
         // Filter phase through an IIR.
-        phase = c.resources.iir.update(&mut c.resources.iir_state, phase);
+        phase = c.resources.iir.update(&mut c.resources.iir_state, phase) * PHASE_SCALING;
 
         for value in dac_samples[1].iter_mut() {
             *value = phase as u16 ^ 0x8000

From b152343aafd29b7da68caf7caee7243b7a5f5903 Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Fri, 29 Jan 2021 11:05:46 +0100
Subject: [PATCH 13/14] Style

---
 src/bin/lockin-internal-demo.rs | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal-demo.rs
index a8761d2..8611a2c 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal-demo.rs
@@ -9,7 +9,7 @@ const DAC_SEQUENCE: [f32; 8] =
 
 use dsp::{iir, iir_int, lockin::Lockin};
 use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
-use stabilizer::{ADC_SAMPLE_TICKS, hardware};
+use stabilizer::{hardware, ADC_SAMPLE_TICKS};
 
 const SCALE: f32 = ((1 << 15) - 1) as f32;
 
@@ -132,7 +132,8 @@ const APP: () = {
         }
 
         // Filter phase through an IIR.
-        phase = c.resources.iir.update(&mut c.resources.iir_state, phase) * PHASE_SCALING;
+        phase = c.resources.iir.update(&mut c.resources.iir_state, phase)
+            * PHASE_SCALING;
 
         for value in dac_samples[1].iter_mut() {
             *value = phase as u16 ^ 0x8000

From 8b46c3c768cb54e21810054154446fd7dafce503 Mon Sep 17 00:00:00 2001
From: Ryan Summers <ryan.summers@vertigo-designs.com>
Date: Fri, 29 Jan 2021 18:55:54 +0100
Subject: [PATCH 14/14] Updating internal lockin demo

---
 src/bin/lockin-internal-demo.rs | 47 +++++++++------------------------
 1 file changed, 12 insertions(+), 35 deletions(-)

diff --git a/src/bin/lockin-internal-demo.rs b/src/bin/lockin-internal-demo.rs
index 8611a2c..54a429f 100644
--- a/src/bin/lockin-internal-demo.rs
+++ b/src/bin/lockin-internal-demo.rs
@@ -7,13 +7,9 @@
 const DAC_SEQUENCE: [f32; 8] =
     [0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707];
 
-use dsp::{iir, iir_int, lockin::Lockin};
+use dsp::{iir_int, lockin::Lockin};
 use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1};
-use stabilizer::{hardware, ADC_SAMPLE_TICKS};
-
-const SCALE: f32 = ((1 << 15) - 1) as f32;
-
-const PHASE_SCALING: f32 = 1e12;
+use stabilizer::hardware;
 
 #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
 const APP: () = {
@@ -22,12 +18,6 @@ const APP: () = {
         adc1: Adc1Input,
         dacs: (Dac0Output, Dac1Output),
 
-        #[init([0.; 5])]
-        iir_state: iir::IIRState,
-
-        #[init(iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE })]
-        iir: iir::IIR,
-
         lockin: Lockin,
     }
 
@@ -36,18 +26,8 @@ const APP: () = {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
 
-        // The desired corner frequency is always
-        let desired_corner_frequency = 10e3;
-        let gain = 1000.0;
-
-        // Calculate the IIR corner freuqency parameter as a function of the sample rate.
-        let corner_frequency = {
-            let sample_rate = 1.0 / (10e-9 * ADC_SAMPLE_TICKS as f32);
-            desired_corner_frequency / sample_rate
-        };
-
         let lockin = Lockin::new(
-            &iir_int::IIRState::lowpass(corner_frequency, 0.707, gain), // TODO: expose
+            &iir_int::IIRState::lowpass(1e-3, 0.707, 2.), // TODO: expose
         );
 
         // Enable ADC/DAC events
@@ -84,8 +64,8 @@ const APP: () = {
     /// the same time bounds, meeting one also means the other is also met.
     ///
     /// TODO: Document
-    #[task(binds=DMA1_STR4, resources=[adc1, dacs, iir_state, iir, lockin], priority=2)]
-    fn process(mut c: process::Context) {
+    #[task(binds=DMA1_STR4, resources=[adc1, dacs, lockin], priority=2)]
+    fn process(c: process::Context) {
         let adc_samples = c.resources.adc1.acquire_buffer();
         let dac_samples = [
             c.resources.dacs.0.acquire_buffer(),
@@ -103,19 +83,19 @@ const APP: () = {
             dac_samples[0][i] = y as u16 ^ 0x8000;
         }
 
-        let pll_phase = 0i32;
+        let pll_phase = 0;
         let pll_frequency = 1i32 << (32 - 3); // 1/8 of the sample rate
 
         // Harmonic index of the LO: -1 to _de_modulate the fundamental
         let harmonic: i32 = -1;
 
         // Demodulation LO phase offset
-        let phase_offset: i32 = 0;
+        let phase_offset: i32 = (0.7495 * i32::MAX as f32) as i32;
         let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
         let mut sample_phase = phase_offset
             .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
 
-        let mut phase = 0f32;
+        let mut phase = 0i16;
 
         for sample in adc_samples.iter() {
             // Convert to signed, MSB align the ADC sample.
@@ -127,20 +107,17 @@ const APP: () = {
             // Advance the sample phase.
             sample_phase = sample_phase.wrapping_add(sample_frequency);
 
-            // Convert from IQ to phase.
-            phase = output.phase() as _;
+            // Convert from IQ to phase. Scale the phase so that it fits in the DAC range. We do
+            // this by shifting it down into the 16-bit range.
+            phase = (output.phase() >> 16) as i16;
         }
 
-        // Filter phase through an IIR.
-        phase = c.resources.iir.update(&mut c.resources.iir_state, phase)
-            * PHASE_SCALING;
-
         for value in dac_samples[1].iter_mut() {
             *value = phase as u16 ^ 0x8000
         }
     }
 
-    #[idle(resources=[iir_state, iir, afes])]
+    #[idle(resources=[afes])]
     fn idle(_: idle::Context) -> ! {
         loop {
             // TODO: Implement network interface.