Updating settings update function

Cargo.lock

@@ -351,7 +351,6 @@ dependencies = [
 [[package]]
 name = "derive_stringset"
 version = "0.1.0"
-source = "git+https://github.com/vertigo-designs/miniconf.git?branch=develop#256098cafc6db93b456b5014c8ef1b835b5e239d"
 dependencies = [
  "proc-macro2",
  "quote",
@@ -365,10 +364,10 @@ dependencies = [
  "criterion",
  "generic-array 0.14.4",
  "libm",
+ "miniconf",
  "ndarray",
  "rand",
  "serde",
- "serde-json-core 0.1.0",
 ]
 
 [[package]]
@@ -620,13 +619,12 @@ dependencies = [
 [[package]]
 name = "miniconf"
 version = "0.1.0"
-source = "git+https://github.com/vertigo-designs/miniconf.git?branch=develop#256098cafc6db93b456b5014c8ef1b835b5e239d"
 dependencies = [
  "derive_stringset",
  "heapless",
  "minimq",
  "serde",
- "serde-json-core 0.2.0",
+ "serde-json-core",
 ]
 
 [[package]]
@@ -966,16 +964,6 @@ dependencies = [
  "serde_derive",
 ]
 
-[[package]]
-name = "serde-json-core"
-version = "0.1.0"
-source = "registry+https://github.com/rust-lang/crates.io-index"
-checksum = "cbf406405ada9ef326ca78677324ac66994ff348fc48a16030be08caeed29825"
-dependencies = [
- "heapless",
- "serde",
-]
-
 [[package]]
 name = "serde-json-core"
 version = "0.2.0"

dsp/src/iir.rs

@@ -1,4 +1,5 @@
-use serde::{Deserialize, Serialize};
+use serde::Deserialize;
+use miniconf::StringSet;
 
 use super::{abs, copysign, macc, max, min};
 use core::f32;
@@ -11,8 +12,7 @@ use core::f32;
 /// To represent the IIR coefficients, this contains the feed-forward
 /// coefficients (b0, b1, b2) followd by the negated feed-back coefficients
 /// (-a1, -a2), all five normalized such that a0 = 1.
-#[derive(Copy, Clone, Default, Deserialize, Serialize)]
+pub type Vec5 = [f32; 5];
-pub struct Vec5(pub [f32; 5]);
 
 /// IIR configuration.
 ///
@@ -39,7 +39,7 @@ pub struct Vec5(pub [f32; 5]);
 ///   Therefore it can trivially implement bump-less transfer.
 /// * Cascading multiple IIR filters allows stable and robust
 ///   implementation of transfer functions beyond bequadratic terms.
-#[derive(Copy, Clone, Default, Deserialize, Serialize)]
+#[derive(Copy, Clone, Debug, Default, Deserialize, StringSet)]
 pub struct IIR {
     pub ba: Vec5,
     pub y_offset: f32,
@@ -50,7 +50,7 @@ pub struct IIR {
 impl IIR {
     pub const fn new(gain: f32, y_min: f32, y_max: f32) -> Self {
         Self {
-            ba: Vec5([gain, 0., 0., 0., 0.]),
+            ba: [gain, 0., 0., 0., 0.],
             y_offset: 0.,
             y_min,
             y_max,
@@ -84,13 +84,13 @@ impl IIR {
             }
             (a1, b0, b1)
         };
-        self.ba.0.copy_from_slice(&[b0, b1, 0., a1, 0.]);
+        self.ba.copy_from_slice(&[b0, b1, 0., a1, 0.]);
         Ok(())
     }
 
     /// Compute the overall (DC feed-forward) gain.
     pub fn get_k(&self) -> f32 {
-        self.ba.0[..3].iter().sum()
+        self.ba[..3].iter().sum()
     }
 
     /// Compute input-referred (`x`) offset from output (`y`) offset.
@@ -118,21 +118,21 @@ impl IIR {
     /// * `xy` - Current filter state.
     /// * `x0` - New input.
     pub fn update(&self, xy: &mut Vec5, x0: f32) -> f32 {
-        let n = self.ba.0.len();
+        let n = self.ba.len();
-        debug_assert!(xy.0.len() == n);
+        debug_assert!(xy.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.0.copy_within(0..n - 1, 1);
+        xy.copy_within(0..n - 1, 1);
         // Store x0            x0 x1 x2 y1 y2
-        xy.0[0] = x0;
+        xy[0] = x0;
         // Compute y0 by multiply-accumulate
-        let y0 = macc(self.y_offset, &xy.0, &self.ba.0);
+        let y0 = macc(self.y_offset, xy, &self.ba);
         // Limit y0
         let y0 = max(self.y_min, min(self.y_max, y0));
         // Store y0            x0 x1 y0 y1 y2
-        xy.0[n / 2] = y0;
+        xy[n / 2] = y0;
         y0
     }
 }

src/bin/dual-iir.rs

@@ -15,7 +15,7 @@ use serde::Deserialize;
 use dsp::iir;
 use hardware::{
     Adc0Input, Adc1Input, CycleCounter, Dac0Output, Dac1Output, NetworkStack,
-    AFE0, AFE1,
+    AFE0, AFE1, AfeGain,
 };
 
 const SCALE: f32 = i16::MAX as _;
@@ -25,12 +25,18 @@ const IIR_CASCADE_LENGTH: usize = 1;
 
 #[derive(Debug, Deserialize, StringSet)]
 pub struct Settings {
-    test: u32,
+    afe: [AfeGain; 2],
+    iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
+    iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
 }
 
-impl Settings {
+impl Default for Settings {
-    pub fn new() -> Self {
+    fn default() -> Self {
-        Self { test: 0 }
+        Self {
+            afe: [AfeGain::G1, AfeGain::G1],
+            iir_state: [[[0.; 5]; IIR_CASCADE_LENGTH]; 2],
+            iir_ch: [[iir::IIR::new(1., -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2],
+        }
     }
 }
 
@@ -45,7 +51,7 @@ const APP: () = {
         clock: CycleCounter,
 
         // Format: iir_state[ch][cascade-no][coeff]
-        #[init([[iir::Vec5([0.; 5]); IIR_CASCADE_LENGTH]; 2])]
+        #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])]
         iir_state: [[iir::Vec5; IIR_CASCADE_LENGTH]; 2],
         #[init([[iir::IIR::new(1., -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2])]
         iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
@@ -67,7 +73,7 @@ const APP: () = {
                 .unwrap()
             };
 
-            MqttInterface::new(mqtt_client, "stabilizer", Settings::new())
+            MqttInterface::new(mqtt_client, "stabilizer", Settings::default())
                 .unwrap()
         };
 
@@ -160,11 +166,19 @@ const APP: () = {
         }
     }
 
-    #[task(priority = 1, resources=[mqtt_interface, afes, iir_ch])]
+    #[task(priority = 1, resources=[mqtt_interface, afes, iir_ch, iir_state])]
-    fn settings_update(c: settings_update::Context) {
+    fn settings_update(mut c: settings_update::Context) {
-        let _settings = &c.resources.mqtt_interface.settings;
+        let settings = &c.resources.mqtt_interface.settings;
-        //c.resources.iir_ch.lock(|iir| *iir = settings.iir);
+
-        // TODO: Update AFEs
+        // Update the IIR channels.
+        c.resources.iir_ch.lock(|iir| *iir = settings.iir_ch);
+
+        // Update the IIR states.
+        c.resources.iir_state.lock(|iir| *iir = settings.iir_state);
+
+        // Update AFEs
+        c.resources.afes.0.set_gain(settings.afe[0]);
+        c.resources.afes.1.set_gain(settings.afe[1]);
     }
 
     #[task(binds = ETH, priority = 1)]

src/hardware/afe.rs

@@ -1,9 +1,10 @@
 use serde::{Deserialize, Serialize};
+use miniconf::StringSet;
 
 use core::convert::TryFrom;
 use enum_iterator::IntoEnumIterator;
 
-#[derive(Copy, Clone, Debug, Serialize, Deserialize, IntoEnumIterator)]
+#[derive(Copy, Clone, Debug, Serialize, Deserialize, IntoEnumIterator, StringSet)]
 pub enum Gain {
     G1 = 0b00,
     G2 = 0b01,

src/hardware/cycle_counter.rs

@@ -3,7 +3,7 @@ use rtic::cyccnt::{Duration, Instant, U32Ext};
 use stm32h7xx_hal::time::Hertz;
 
 pub struct CycleCounter {
-    next_tick: Instant,
+    next_tick: Option<Instant>,
     ticks: u32,
     increment: Duration,
 }
@@ -20,14 +20,18 @@ impl CycleCounter {
         Self {
             increment,
             ticks: 0,
-            next_tick: Instant::now() + increment,
+            next_tick: None,
         }
     }
 
     pub fn current_ms(&mut self) -> u32 {
+        if self.next_tick.is_none() {
+            self.next_tick = Some(Instant::now() + self.increment);
+        }
+
         let now = Instant::now();
-        while now > self.next_tick {
+        while now > self.next_tick.unwrap() {
-            self.next_tick += self.increment;
+            *self.next_tick.as_mut().unwrap() += self.increment;
             self.ticks += 1;
         }