From cb16e9a85f35a20f6c67ac1feb1f97952992aa99 Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Wed, 17 Feb 2021 12:59:24 +0100 Subject: [PATCH] Updating settings update function --- Cargo.lock | 16 ++------------- dsp/src/iir.rs | 26 ++++++++++++------------ src/bin/dual-iir.rs | 38 ++++++++++++++++++++++++----------- src/hardware/afe.rs | 3 ++- src/hardware/cycle_counter.rs | 12 +++++++---- 5 files changed, 51 insertions(+), 44 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 34941fc..27dd8d0 100644 --- a/Cargo.lock +++ b/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" diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index 4206c2b..7c0efc3 100644 --- a/dsp/src/iir.rs +++ b/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 struct Vec5(pub [f32; 5]); +pub type Vec5 = [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(); - debug_assert!(xy.0.len() == n); + let n = self.ba.len(); + 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 } } diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs index 911b63d..0470d83 100644 --- a/src/bin/dual-iir.rs +++ b/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 { - pub fn new() -> Self { - Self { test: 0 } +impl Default for Settings { + fn default() -> Self { + 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])] - fn settings_update(c: settings_update::Context) { - let _settings = &c.resources.mqtt_interface.settings; - //c.resources.iir_ch.lock(|iir| *iir = settings.iir); - // TODO: Update AFEs + #[task(priority = 1, resources=[mqtt_interface, afes, iir_ch, iir_state])] + fn settings_update(mut c: settings_update::Context) { + let settings = &c.resources.mqtt_interface.settings; + + // 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)] diff --git a/src/hardware/afe.rs b/src/hardware/afe.rs index 5dd725a..451dfa7 100644 --- a/src/hardware/afe.rs +++ b/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, diff --git a/src/hardware/cycle_counter.rs b/src/hardware/cycle_counter.rs index 38db550..53535ff 100644 --- a/src/hardware/cycle_counter.rs +++ b/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, 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 { - self.next_tick += self.increment; + while now > self.next_tick.unwrap() { + *self.next_tick.as_mut().unwrap() += self.increment; self.ticks += 1; }