Merge branch 'master' into feature/lockin-app-refactor

.github/workflows/ci.yml

@@ -29,6 +29,11 @@ jobs:
       - uses: actions-rs/clippy-check@v1
         with:
           token: ${{ secrets.GITHUB_TOKEN }}
+      - name: cargo check
+        uses: actions-rs/cargo@v1
+        with:
+          command: check
+          args: --verbose
 
   compile:
     runs-on: ubuntu-latest
@@ -37,6 +42,12 @@ jobs:
         toolchain:
           - stable
           - beta
+        bin:
+          - dual-iir
+          - lockin
+        features:
+          - ''
+          - pounder_v1_1
     steps:
       - uses: actions/checkout@v2
       - name: Install Rust ${{ matrix.toolchain }}
@@ -46,20 +57,11 @@ jobs:
           target: thumbv7em-none-eabihf
           override: true
           components: llvm-tools-preview
-      - name: cargo check
-        uses: actions-rs/cargo@v1
-        with:
-          command: check
-          args: --verbose
-      - name: cargo build
-        uses: actions-rs/cargo@v1
-        with:
-          command: build
       - name: cargo build release
         uses: actions-rs/cargo@v1
         with:
           command: build
-          args: --release
+          args: --release --features "${{ matrix.features }}" --bin ${{ matrix.bin }}
       - name: cargo-binutils
         uses: actions-rs/cargo@v1
         with:
@@ -69,25 +71,19 @@ jobs:
         uses: actions-rs/cargo@v1
         with:
           command: size
-          args: --release
+          args: --release --features "${{ matrix.features }}" --bin ${{ matrix.bin }}
       - name: cargo objcopy
         uses: actions-rs/cargo@v1
         with:
           command: objcopy
-          args: --release --verbose -- -O binary stabilizer-release.bin
+          args: --release --features "${{ matrix.features }}" --bin ${{ matrix.bin }} --verbose -- -O binary ${{ matrix.bin }}-release.bin
       - uses: actions/upload-artifact@v2
-        if: ${{ matrix.toolchain == 'stable' }}
+        if: ${{ matrix.toolchain == 'stable' && matrix.features == '' }}
         with:
-          name: stabilizer_${{ github.sha }}
+          name: stabilizer_${{ matrix.bin }}
           path: |
-            target/*/release/stabilizer
+            target/*/release/${{ matrix.bin }}
-            stabilizer-release.bin
+            ${{ matrix.bin }}-release.bin
-
-      - name: Build (Pounder v1.1)
-        uses: actions-rs/cargo@v1
-        with:
-          command: build
-          args: --features pounder_v1_1
 
   test:
     runs-on: ubuntu-latest

.github/workflows/hitl.yml

@@ -15,4 +15,4 @@ jobs:
           token: ${{ secrets.DISPATCH_PAT }}
           event-type: stabilizer
           repository: quartiq/hitl
-          client-payload: '{"ref": "${{ github.ref }}", "sha": "${{ github.sha }}"}'
+          client-payload: '{"github": ${{ toJson(github) }}}'

Cargo.lock

@@ -520,9 +520,9 @@ checksum = "c7d73b3f436185384286bd8098d17ec07c9a7d2388a6599f824d8502b529702a"
 
 [[package]]
 name = "log"
-version = "0.4.11"
+version = "0.4.13"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "4fabed175da42fed1fa0746b0ea71f412aa9d35e76e95e59b192c64b9dc2bf8b"
+checksum = "fcf3805d4480bb5b86070dcfeb9e2cb2ebc148adb753c5cca5f884d1d65a42b2"
 dependencies = [
  "cfg-if 0.1.10",
 ]
@@ -767,9 +767,9 @@ checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3"
 
 [[package]]
 name = "serde"
-version = "1.0.118"
+version = "1.0.120"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "06c64263859d87aa2eb554587e2d23183398d617427327cf2b3d0ed8c69e4800"
+checksum = "166b2349061381baf54a58e4b13c89369feb0ef2eaa57198899e2312aac30aab"
 dependencies = [
  "serde_derive",
 ]
@@ -796,9 +796,9 @@ dependencies = [
 
 [[package]]
 name = "serde_derive"
-version = "1.0.118"
+version = "1.0.120"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "c84d3526699cd55261af4b941e4e725444df67aa4f9e6a3564f18030d12672df"
+checksum = "0ca2a8cb5805ce9e3b95435e3765b7b553cecc762d938d409434338386cb5775"
 dependencies = [
  "proc-macro2",
  "quote",
@@ -891,9 +891,9 @@ dependencies = [
 
 [[package]]
 name = "syn"
-version = "1.0.53"
+version = "1.0.58"
 source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "8833e20724c24de12bbaba5ad230ea61c3eafb05b881c7c9d3cfe8638b187e68"
+checksum = "cc60a3d73ea6594cd712d830cc1f0390fd71542d8c8cd24e70cc54cdfd5e05d5"
 dependencies = [
  "proc-macro2",
  "quote",

dsp/benches/trig.rs

@@ -1,6 +1,6 @@
 use core::f32::consts::PI;
 use criterion::{black_box, criterion_group, criterion_main, Criterion};
-use dsp::trig::{atan2, cossin};
+use dsp::{atan2, cossin};
 
 fn atan2_bench(c: &mut Criterion) {
     let xi = (10 << 16) as i32;

dsp/src/atan2.rs

@@ -0,0 +1,135 @@
+/// 2-argument arctangent function.
+///
+/// This implementation uses all integer arithmetic for fast
+/// computation. It is designed to have high accuracy near the axes
+/// and lower away from the axes. It is additionally designed so that
+/// the error changes slowly with respect to the angle.
+///
+/// # Arguments
+///
+/// * `y` - Y-axis component.
+/// * `x` - X-axis component.
+///
+/// # Returns
+///
+/// The angle between the x-axis and the ray to the point (x,y). The
+/// result range is from i32::MIN to i32::MAX, where i32::MIN
+/// represents -pi and, equivalently, +pi. i32::MAX represents one
+/// count less than +pi.
+pub fn atan2(y: i32, x: i32) -> i32 {
+    let sign = (x < 0, y < 0);
+
+    let mut y = y.wrapping_abs() as u32;
+    let mut x = x.wrapping_abs() as u32;
+
+    let y_greater = y > x;
+    if y_greater {
+        core::mem::swap(&mut y, &mut x);
+    }
+
+    let z = (16 - y.leading_zeros() as i32).max(0);
+
+    x >>= z;
+    if x == 0 {
+        return 0;
+    }
+    y >>= z;
+    let r = (y << 16) / x;
+    debug_assert!(r <= 1 << 16);
+
+    // Uses the general procedure described in the following
+    // Mathematics stack exchange answer:
+    //
+    // https://math.stackexchange.com/a/1105038/583981
+    //
+    // The atan approximation method has been modified to be cheaper
+    // to compute and to be more compatible with integer
+    // arithmetic. The approximation technique used here is
+    //
+    // pi / 4 * r + C * r * (1 - abs(r))
+    //
+    // which is taken from Rajan 2006: Efficient Approximations for
+    // the Arctangent Function.
+    //
+    // The least mean squared error solution is C = 0.279 (no the 0.285 that
+    // Rajan uses). K = C*4/pi.
+    // Q5 for K provides sufficient correction accuracy while preserving
+    // as much smoothness of the quadratic correction as possible.
+    const FP_K: usize = 5;
+    const K: u32 = (0.35489 * (1 << FP_K) as f64) as u32;
+    // debug_assert!(K == 11);
+
+    // `r` is unsigned Q16.16 and <= 1
+    // `angle` is signed Q1.31 with 1 << 31 == +- pi
+    // Since K < 0.5 and r*(1 - r) <= 0.25 the correction product can use
+    // 4 bits for K, and 15 bits for r and 1-r to remain within the u32 range.
+    let mut angle = ((r << 13)
+        + ((K * (r >> 1) * ((1 << 15) - (r >> 1))) >> (FP_K + 1)))
+        as i32;
+
+    if y_greater {
+        angle = (1 << 30) - angle;
+    }
+
+    if sign.0 {
+        angle = i32::MAX - angle;
+    }
+
+    if sign.1 {
+        angle = angle.wrapping_neg();
+    }
+
+    angle
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+    use core::f64::consts::PI;
+
+    fn angle_to_axis(angle: f64) -> f64 {
+        let angle = angle % (PI / 2.);
+        (PI / 2. - angle).min(angle)
+    }
+
+    #[test]
+    fn atan2_absolute_error() {
+        const N: usize = 321;
+        let mut test_vals = [0i32; N + 4];
+        let scale = (1i64 << 31) as f64;
+        for i in 0..N {
+            test_vals[i] = (scale * (-1. + 2. * i as f64 / N as f64)) as i32;
+        }
+
+        assert!(test_vals.contains(&i32::MIN));
+        test_vals[N] = i32::MAX;
+        test_vals[N + 1] = 0;
+        test_vals[N + 2] = -1;
+        test_vals[N + 3] = 1;
+
+        let mut rms_err = 0f64;
+        let mut abs_err = 0f64;
+        let mut rel_err = 0f64;
+
+        for &x in test_vals.iter() {
+            for &y in test_vals.iter() {
+                let want = (y as f64 / scale).atan2(x as f64 / scale);
+                let have = atan2(y, x) as f64 * PI / scale;
+
+                let err = (have - want).abs();
+                abs_err = abs_err.max(err);
+                rms_err += err * err;
+                if err > 3e-5 {
+                    rel_err = rel_err.max(err / angle_to_axis(want));
+                }
+            }
+        }
+        rms_err = rms_err.sqrt() / test_vals.len() as f64;
+        println!("max abs err: {:.2e}", abs_err);
+        println!("rms abs err: {:.2e}", rms_err);
+        println!("max rel err: {:.2e}", rel_err);
+        assert!(abs_err < 5e-3);
+        assert!(rms_err < 3e-3);
+        assert!(rel_err < 0.6);
+    }
+}

dsp/src/complex.rs

@@ -0,0 +1,17 @@
+use super::atan2;
+use serde::{Deserialize, Serialize};
+
+#[derive(Copy, Clone, Default, Deserialize, Serialize)]
+pub struct Complex<T>(pub T, pub T);
+
+impl Complex<i32> {
+    pub fn power(&self) -> i32 {
+        (((self.0 as i64) * (self.0 as i64)
+            + (self.1 as i64) * (self.1 as i64))
+            >> 32) as i32
+    }
+
+    pub fn phase(&self) -> i32 {
+        atan2(self.1, self.0)
+    }
+}

dsp/src/cossin.rs

@@ -3,90 +3,6 @@ use core::f64::consts::PI;
 
 include!(concat!(env!("OUT_DIR"), "/cossin_table.rs"));
 
-/// 2-argument arctangent function.
-///
-/// This implementation uses all integer arithmetic for fast
-/// computation. It is designed to have high accuracy near the axes
-/// and lower away from the axes. It is additionally designed so that
-/// the error changes slowly with respect to the angle.
-///
-/// # Arguments
-///
-/// * `y` - Y-axis component.
-/// * `x` - X-axis component.
-///
-/// # Returns
-///
-/// The angle between the x-axis and the ray to the point (x,y). The
-/// result range is from i32::MIN to i32::MAX, where i32::MIN
-/// represents -pi and, equivalently, +pi. i32::MAX represents one
-/// count less than +pi.
-pub fn atan2(y: i32, x: i32) -> i32 {
-    let sign = (x < 0, y < 0);
-
-    let mut y = y.wrapping_abs() as u32;
-    let mut x = x.wrapping_abs() as u32;
-
-    let y_greater = y > x;
-    if y_greater {
-        core::mem::swap(&mut y, &mut x);
-    }
-
-    let z = (16 - y.leading_zeros() as i32).max(0);
-
-    x >>= z;
-    if x == 0 {
-        return 0;
-    }
-    y >>= z;
-    let r = (y << 16) / x;
-    debug_assert!(r <= 1 << 16);
-
-    // Uses the general procedure described in the following
-    // Mathematics stack exchange answer:
-    //
-    // https://math.stackexchange.com/a/1105038/583981
-    //
-    // The atan approximation method has been modified to be cheaper
-    // to compute and to be more compatible with integer
-    // arithmetic. The approximation technique used here is
-    //
-    // pi / 4 * r + C * r * (1 - abs(r))
-    //
-    // which is taken from Rajan 2006: Efficient Approximations for
-    // the Arctangent Function.
-    //
-    // The least mean squared error solution is C = 0.279 (no the 0.285 that
-    // Rajan uses). K = C*4/pi.
-    // Q5 for K provides sufficient correction accuracy while preserving
-    // as much smoothness of the quadratic correction as possible.
-    const FP_K: usize = 5;
-    const K: u32 = (0.35489 * (1 << FP_K) as f64) as u32;
-    // debug_assert!(K == 11);
-
-    // `r` is unsigned Q16.16 and <= 1
-    // `angle` is signed Q1.31 with 1 << 31 == +- pi
-    // Since K < 0.5 and r*(1 - r) <= 0.25 the correction product can use
-    // 4 bits for K, and 15 bits for r and 1-r to remain within the u32 range.
-    let mut angle = ((r << 13)
-        + ((K * (r >> 1) * ((1 << 15) - (r >> 1))) >> (FP_K + 1)))
-        as i32;
-
-    if y_greater {
-        angle = (1 << 30) - angle;
-    }
-
-    if sign.0 {
-        angle = i32::MAX - angle;
-    }
-
-    if sign.1 {
-        angle = angle.wrapping_neg();
-    }
-
-    angle
-}
-
 /// Compute the cosine and sine of an angle.
 /// This is ported from the MiSoC cossin core.
 /// (https://github.com/m-labs/misoc/blob/master/misoc/cores/cossin.py)
@@ -153,7 +69,7 @@ pub fn cossin(phase: i32) -> Complex<i32> {
         sin *= -1;
     }
 
-    (cos, sin)
+    Complex(cos, sin)
 }
 
 #[cfg(test)]
@@ -161,66 +77,21 @@ mod tests {
     use super::*;
     use core::f64::consts::PI;
 
-    fn angle_to_axis(angle: f64) -> f64 {
-        let angle = angle % (PI / 2.);
-        (PI / 2. - angle).min(angle)
-    }
-
-    #[test]
-    fn atan2_absolute_error() {
-        const N: usize = 321;
-        let mut test_vals = [0i32; N + 4];
-        let scale = (1i64 << 31) as f64;
-        for i in 0..N {
-            test_vals[i] = (scale * (-1. + 2. * i as f64 / N as f64)) as i32;
-        }
-
-        assert!(test_vals.contains(&i32::MIN));
-        test_vals[N] = i32::MAX;
-        test_vals[N + 1] = 0;
-        test_vals[N + 2] = -1;
-        test_vals[N + 3] = 1;
-
-        let mut rms_err = 0f64;
-        let mut abs_err = 0f64;
-        let mut rel_err = 0f64;
-
-        for &x in test_vals.iter() {
-            for &y in test_vals.iter() {
-                let want = (y as f64 / scale).atan2(x as f64 / scale);
-                let have = atan2(y, x) as f64 * PI / scale;
-
-                let err = (have - want).abs();
-                abs_err = abs_err.max(err);
-                rms_err += err * err;
-                if err > 3e-5 {
-                    rel_err = rel_err.max(err / angle_to_axis(want));
-                }
-            }
-        }
-        rms_err = rms_err.sqrt() / test_vals.len() as f64;
-        println!("max abs err: {:.2e}", abs_err);
-        println!("rms abs err: {:.2e}", rms_err);
-        println!("max rel err: {:.2e}", rel_err);
-        assert!(abs_err < 5e-3);
-        assert!(rms_err < 3e-3);
-        assert!(rel_err < 0.6);
-    }
-
     #[test]
     fn cossin_error_max_rms_all_phase() {
         // Constant amplitude error due to LUT data range.
-        const AMPLITUDE: f64 = ((1i64 << 31) - (1i64 << 15)) as f64;
+        const AMPLITUDE: f64 = ((1i64 << 31) - (1i64 << 15)) as _;
-        const MAX_PHASE: f64 = (1i64 << 32) as f64;
+        const MAX_PHASE: f64 = (1i64 << 32) as _;
-        let mut rms_err: Complex<f64> = (0., 0.);
+        let mut rms_err = Complex(0f64, 0f64);
-        let mut sum_err: Complex<f64> = (0., 0.);
+        let mut sum_err = Complex(0f64, 0f64);
-        let mut max_err: Complex<f64> = (0., 0.);
+        let mut max_err = Complex(0f64, 0f64);
-        let mut sum: Complex<f64> = (0., 0.);
+        let mut sum = Complex(0f64, 0f64);
-        let mut demod: Complex<f64> = (0., 0.);
+        let mut demod = Complex(0f64, 0f64);
 
         // use std::{fs::File, io::{BufWriter, prelude::*}, path::Path};
         // let mut file = BufWriter::new(File::create(Path::new("data.bin")).unwrap());
 
+        // log2 of the number of phase values to check
         const PHASE_DEPTH: usize = 20;
 
         for phase in 0..(1 << PHASE_DEPTH) {

dsp/src/iir_int.rs

@@ -1,6 +1,7 @@
 use serde::{Deserialize, Serialize};
 
-pub type IIRState = [i32; 5];
+#[derive(Copy, Clone, Default, Deserialize, Serialize)]
+pub struct IIRState(pub [i32; 5]);
 
 fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
     // Rounding bias, half up
@@ -18,7 +19,7 @@ fn macc(y0: i32, x: &[i32], a: &[i32], shift: u32) -> i32 {
 /// See `dsp::iir::IIR` for general implementation details.
 /// Offset and limiting disabled to suit lowpass applications.
 /// Coefficient scaling fixed and optimized.
-#[derive(Copy, Clone, Deserialize, Serialize)]
+#[derive(Copy, Clone, Default, Deserialize, Serialize)]
 pub struct IIR {
     pub ba: IIRState,
     // pub y_offset: i32,
@@ -27,9 +28,9 @@ pub struct IIR {
 }
 
 impl IIR {
-    /// Coefficient fixed point: signed Q2.30.
+    /// Coefficient fixed point format: signed Q2.30.
-    /// Tailored to low-passes PI, II etc.
+    /// Tailored to low-passes, PI, II etc.
-    const SHIFT: u32 = 30;
+    pub const SHIFT: u32 = 30;
 
     /// Feed a new input value into the filter, update the filter state, and
     /// return the new output. Only the state `xy` is modified.
@@ -38,21 +39,21 @@ impl IIR {
     /// * `xy` - Current filter state.
     /// * `x0` - New input.
     pub fn update(&self, xy: &mut IIRState, x0: i32) -> i32 {
-        let n = self.ba.len();
+        let n = self.ba.0.len();
-        debug_assert!(xy.len() == n);
+        debug_assert!(xy.0.len() == n);
         // `xy` contains       x0 x1 y0 y1 y2
         // Increment time      x1 x2 y1 y2 y3
         // Shift               x1 x1 x2 y1 y2
         // This unrolls better than xy.rotate_right(1)
-        xy.copy_within(0..n - 1, 1);
+        xy.0.copy_within(0..n - 1, 1);
         // Store x0            x0 x1 x2 y1 y2
-        xy[0] = x0;
+        xy.0[0] = x0;
         // Compute y0 by multiply-accumulate
-        let y0 = macc(0, xy, &self.ba, IIR::SHIFT);
+        let y0 = macc(0, &xy.0, &self.ba.0, IIR::SHIFT);
         // Limit y0
         // let y0 = y0.max(self.y_min).min(self.y_max);
         // Store y0            x0 x1 y0 y1 y2
-        xy[n / 2] = y0;
+        xy.0[n / 2] = y0;
         y0
     }
 }

dsp/src/lib.rs

@@ -3,8 +3,6 @@
 
 use core::ops::{Add, Mul, Neg};
 
-pub type Complex<T> = (T, T);
-
 /// Bit shift, round up half.
 ///
 /// # Arguments
@@ -114,12 +112,19 @@ where
         .fold(y0, |y, xa| y + xa)
 }
 
+mod atan2;
+mod complex;
+mod cossin;
 pub mod iir;
 pub mod iir_int;
+pub mod lockin;
 pub mod pll;
 pub mod reciprocal_pll;
-pub mod trig;
 pub mod unwrap;
 
+pub use atan2::atan2;
+pub use complex::Complex;
+pub use cossin::cossin;
+
 #[cfg(test)]
-mod testing;
+pub mod testing;

dsp/src/reciprocal_pll.rs

@@ -62,18 +62,26 @@ impl TimestampHandler {
             self.reference_frequency = frequency as u32 as i64;
         }
 
-        let demodulation_frequency = divide_round(
+        let demodulation_frequency: u32;
-            1 << (32 + self.adc_sample_ticks_log2),
+        let demodulation_initial_phase: u32;
-            self.reference_frequency,
+
-        ) as u32;
+        if self.reference_frequency == 0 {
-        let demodulation_initial_phase = divide_round(
+            demodulation_frequency = u32::MAX;
-            (((self.batch_index as i64)
+            demodulation_initial_phase = u32::MAX;
-                << (self.adc_sample_ticks_log2
+        } else {
-                    + self.sample_buffer_size_log2))
+            demodulation_frequency = divide_round(
-                - self.reference_phase)
+                1 << (32 + self.adc_sample_ticks_log2),
-                << 32,
+                self.reference_frequency,
-            self.reference_frequency,
+            ) as u32;
-        ) as u32;
+            demodulation_initial_phase = divide_round(
+                (((self.batch_index as i64)
+                    << (self.adc_sample_ticks_log2
+                        + self.sample_buffer_size_log2))
+                    - self.reference_phase)
+                    << 32,
+                self.reference_frequency,
+            ) as u32;
+        }
 
         if self.batch_index
             < (1 << (32

dsp/src/testing.rs

@@ -1,6 +1,22 @@
 #![allow(dead_code)]
 use super::Complex;
 
+/// Maximum acceptable error between a computed and actual value given fixed and relative
+/// tolerances.
+///
+/// # Args
+/// * `a` - First input.
+/// * `b` - Second input. The relative tolerance is computed with respect to the maximum of the
+/// absolute values of the first and second inputs.
+/// * `rtol` - Relative tolerance.
+/// * `atol` - Fixed tolerance.
+///
+/// # Returns
+/// Maximum acceptable error.
+pub fn max_error(a: f64, b: f64, rtol: f64, atol: f64) -> f64 {
+    rtol * a.abs().max(b.abs()) + atol
+}
+
 pub fn isclose(a: f64, b: f64, rtol: f64, atol: f64) -> bool {
     (a - b).abs() <= a.abs().max(b.abs()) * rtol + atol
 }

src/bin/lockin.rs

@@ -0,0 +1,353 @@
+#![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};
+
+use stabilizer::{
+    hardware, server, ADC_SAMPLE_TICKS_LOG2, SAMPLE_BUFFER_SIZE_LOG2,
+};
+
+use dsp::{iir, iir_int, lockin::Lockin, reciprocal_pll::TimestampHandler};
+use hardware::{
+    Adc0Input, Adc1Input, Dac0Output, Dac1Output, InputStamper, AFE0, AFE1,
+};
+
+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 {
+        afes: (AFE0, AFE1),
+        adcs: (Adc0Input, Adc1Input),
+        dacs: (Dac0Output, Dac1Output),
+        net_interface: hardware::Ethernet,
+
+        // Format: iir_state[ch][cascade-no][coeff]
+        #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])]
+        iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 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],
+
+        timestamper: InputStamper,
+        pll: TimestampHandler,
+        lockin: Lockin,
+    }
+
+    #[init]
+    fn init(c: init::Context) -> init::LateResources {
+        // 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.0.start();
+        stabilizer.adcs.1.start();
+        stabilizer.dacs.0.start();
+        stabilizer.dacs.1.start();
+
+        // Start recording digital input timestamps.
+        stabilizer.timestamp_timer.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,
+            timestamper: stabilizer.timestamper,
+
+            pll,
+            lockin,
+        }
+    }
+
+    /// 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.
+    ///
+    /// TODO: document lockin
+    #[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch, lockin, timestamper, pll], 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(),
+        ];
+
+        let iir_ch = c.resources.iir_ch;
+        let iir_state = c.resources.iir_state;
+        let lockin = c.resources.lockin;
+
+        let (pll_phase, pll_frequency) = c
+            .resources
+            .pll
+            .update(c.resources.timestamper.latest_timestamp());
+
+        // 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 sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic);
+        let mut sample_phase = phase_offset
+            .wrapping_add((pll_phase as i32).wrapping_mul(harmonic));
+
+        for i in 0..adc_samples[0].len() {
+            // Convert to signed, MSB align the ADC sample.
+            let input = (adc_samples[0][i] as i16 as i32) << 16;
+            // Obtain demodulated, filtered IQ sample.
+            let output = lockin.update(input, sample_phase);
+            // Advance the sample phase.
+            sample_phase = sample_phase.wrapping_add(sample_frequency);
+
+            // Convert from IQ to power and phase.
+            let mut power = output.power() as _;
+            let mut phase = output.phase() as _;
+
+            // Filter power and phase through IIR filters.
+            // Note: Normalization to be done in filters. Phase will wrap happily.
+            for j in 0..iir_state[0].len() {
+                power = iir_ch[0][j].update(&mut iir_state[0][j], power);
+                phase = iir_ch[1][j].update(&mut iir_state[1][j], phase);
+            }
+
+            // Note(unsafe): range clipping to i16 is ensured by IIR filters above.
+            // Convert to DAC data.
+            unsafe {
+                dac_samples[0][i] =
+                    power.to_int_unchecked::<i16>() as u16 ^ 0x8000;
+                dac_samples[1][i] =
+                    phase.to_int_unchecked::<i16>() 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);
+                        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][0][0],
+                                            y0: iir_state[0][0][2],
+                                            x1: iir_state[1][0][0],
+                                            y1: iir_state[1][0][2],
+                                    });
+
+                                    Ok::<server::Status, ()>(state)
+                                }),
+                                // "_b" means cascades 2nd IIR
+                                "stabilizer/iir_b/state": (|| { let state = c.resources.iir_state.lock(|iir_state|
+                                        server::Status {
+                                            t: time,
+                                            x0: iir_state[0][IIR_CASCADE_LENGTH-1][0],
+                                            y0: iir_state[0][IIR_CASCADE_LENGTH-1][2],
+                                            x1: iir_state[1][IIR_CASCADE_LENGTH-1][0],
+                                            y1: iir_state[1][IIR_CASCADE_LENGTH-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][0] = 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][0] = req.iir;
+
+                                        Ok::<server::IirRequest, ()>(req)
+                                    })
+                                }),
+                                "stabilizer/iir_b0/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][IIR_CASCADE_LENGTH-1] = req.iir;
+
+                                        Ok::<server::IirRequest, ()>(req)
+                                    })
+                                }),
+                                "stabilizer/iir_b1/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][IIR_CASCADE_LENGTH-1] = 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();
+    }
+};

src/hardware/mod.rs

@@ -20,6 +20,7 @@ mod timers;
 pub use adc::{Adc0Input, Adc1Input};
 pub use afe::Gain as AfeGain;
 pub use dac::{Dac0Output, Dac1Output};
+pub use digital_input_stamper::InputStamper;
 pub use pounder::DdsOutput;
 
 // Type alias for the analog front-end (AFE) for ADC0.

src/lib.rs

@@ -1,16 +1,18 @@
 #![no_std]
 
-pub mod server;
 
 #[macro_use]
 extern crate log;
 
 pub mod hardware;
+pub mod server;
 
 // 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;
+pub const ADC_SAMPLE_TICKS_LOG2: u16 = 8;
+pub const ADC_SAMPLE_TICKS: u16 = 1 << ADC_SAMPLE_TICKS_LOG2;
 
 // The desired ADC sample processing buffer size.
-const SAMPLE_BUFFER_SIZE: usize = 8;
+pub const SAMPLE_BUFFER_SIZE_LOG2: usize = 3;
+pub const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2;