Adding WIP telemetry implementation for dual-iir

src/bin/dual-iir.rs

@@ -7,12 +7,12 @@ use stm32h7xx_hal as hal;
 use stabilizer::hardware;
 
 use miniconf::{minimq, Miniconf, MqttInterface};
-use serde::Deserialize;
+use serde::{Deserialize, Serialize};
 
 use dsp::iir;
 use hardware::{
     Adc0Input, Adc1Input, AfeGain, CycleCounter, Dac0Output, Dac1Output,
-    NetworkStack, AFE0, AFE1,
+    NetworkStack, SystemTimer, AFE0, AFE1,
 };
 
 const SCALE: f32 = i16::MAX as _;
@@ -20,10 +20,18 @@ const SCALE: f32 = i16::MAX as _;
 // The number of cascaded IIR biquads per channel. Select 1 or 2!
 const IIR_CASCADE_LENGTH: usize = 1;
 
-#[derive(Debug, Deserialize, Miniconf)]
+#[derive(Debug, Deserialize, Miniconf, Copy, Clone)]
 pub struct Settings {
     afe: [AfeGain; 2],
     iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
+    telemetry_period_secs: u16,
+}
+
+#[derive(Serialize, Clone)]
+pub struct Telemetry {
+    latest_samples: [i16; 2],
+    latest_outputs: [i16; 2],
+    digital_inputs: [bool; 2],
 }
 
 impl Default for Settings {
@@ -31,11 +39,22 @@ impl Default for Settings {
         Self {
             afe: [AfeGain::G1, AfeGain::G1],
             iir_ch: [[iir::IIR::new(1., -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2],
+            telemetry_period_secs: 10,
         }
     }
 }
 
-#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
+impl Default for Telemetry {
+    fn default() -> Self {
+        Self {
+            latest_samples: [0, 0],
+            latest_outputs: [0, 0],
+            digital_inputs: [false, false],
+        }
+    }
+}
+
+#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = crate::hardware::SystemTimer)]
 const APP: () = {
     struct Resources {
         afes: (AFE0, AFE1),
@@ -43,6 +62,8 @@ const APP: () = {
         dacs: (Dac0Output, Dac1Output),
         mqtt_interface:
             MqttInterface<Settings, NetworkStack, minimq::consts::U256>,
+        telemetry: Telemetry,
+        settings: Settings,
         clock: CycleCounter,
 
         // Format: iir_state[ch][cascade-no][coeff]
@@ -52,7 +73,7 @@ const APP: () = {
         iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
     }
 
-    #[init]
+    #[init(schedule = [telemetry])]
     fn init(c: init::Context) -> init::LateResources {
         // Configure the microcontroller
         let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
@@ -82,7 +103,9 @@ const APP: () = {
         stabilizer.dacs.1.start();
 
         // Start sampling ADCs.
-        stabilizer.adc_dac_timer.start();
+        //stabilizer.adc_dac_timer.start();
+
+        c.schedule.telemetry(c.start).unwrap();
 
         init::LateResources {
             mqtt_interface,
@@ -90,6 +113,8 @@ const APP: () = {
             adcs: stabilizer.adcs,
             dacs: stabilizer.dacs,
             clock: stabilizer.cycle_counter,
+            settings: Settings::default(),
+            telemetry: Telemetry::default(),
         }
     }
 
@@ -109,7 +134,7 @@ 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=[adcs, dacs, iir_state, iir_ch, telemetry], priority=2)]
     fn process(c: process::Context) {
         let adc_samples = [
             c.resources.adcs.0.acquire_buffer(),
@@ -136,6 +161,14 @@ const APP: () = {
                 dac_samples[channel][sample] = y as u16 ^ 0x8000;
             }
         }
+
+        // Update telemetry measurements.
+        // TODO: Should we report these as voltages?
+        c.resources.telemetry.latest_samples =
+            [adc_samples[0][0] as i16, adc_samples[1][0] as i16];
+
+        c.resources.telemetry.latest_outputs =
+            [dac_samples[0][0] as i16, dac_samples[1][0] as i16];
     }
 
     #[idle(resources=[mqtt_interface, clock], spawn=[settings_update])]
@@ -162,7 +195,7 @@ const APP: () = {
                     if update {
                         c.spawn.settings_update().unwrap();
                     } else if sleep {
-                        cortex_m::asm::wfi();
+                        //cortex_m::asm::wfi();
                     }
                 }
                 Err(miniconf::MqttError::Network(
@@ -173,18 +206,53 @@ const APP: () = {
         }
     }
 
-    #[task(priority = 1, resources=[mqtt_interface, afes, iir_ch])]
+    #[task(priority = 1, resources=[mqtt_interface, afes, settings, iir_ch])]
     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 currently-cached settings.
+        *c.resources.settings = *settings;
+
         // Update AFEs
         c.resources.afes.0.set_gain(settings.afe[0]);
         c.resources.afes.1.set_gain(settings.afe[1]);
     }
 
+    #[task(priority = 1, resources=[mqtt_interface, settings, telemetry], schedule=[telemetry])]
+    fn telemetry(mut c: telemetry::Context) {
+        let telemetry = c.resources.telemetry.lock(|telemetry| {
+            // TODO: Incorporate digital input status.
+            telemetry.digital_inputs = [false, false];
+            telemetry.clone()
+        });
+
+        // Serialize telemetry outside of a critical section to prevent blocking the processing
+        // task.
+        let _telemetry = miniconf::serde_json_core::to_string::<
+            heapless::consts::U256,
+            _,
+        >(&telemetry)
+        .unwrap();
+
+        //c.resources.mqtt_interface.client(|client| {
+        //    // TODO: Incorporate current MQTT prefix instead of hard-coded value.
+        //    client.publish("dt/sinara/dual-iir/telemetry", telemetry.as_bytes(), minimq::QoS::AtMostOnce, &[]).ok()
+        //});
+
+        // Schedule the telemetry task in the future.
+        c.schedule
+            .telemetry(
+                c.scheduled
+                    + SystemTimer::ticks_from_secs(
+                        c.resources.settings.telemetry_period_secs as u32,
+                    ),
+            )
+            .unwrap();
+    }
+
     #[task(binds = ETH, priority = 1)]
     fn eth(_: eth::Context) {
         unsafe { hal::ethernet::interrupt_handler() }

src/hardware/configuration.rs

@@ -13,8 +13,8 @@ use embedded_hal::digital::v2::{InputPin, OutputPin};
 
 use super::{
     adc, afe, cycle_counter::CycleCounter, dac, design_parameters,
-    digital_input_stamper, eeprom, pounder, timers, DdsOutput, NetworkStack,
+    digital_input_stamper, eeprom, pounder, system_timer, timers, DdsOutput,
-    AFE0, AFE1,
+    NetworkStack, AFE0, AFE1,
 };
 
 pub struct NetStorage {
@@ -96,7 +96,7 @@ static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new();
 /// `Some(devices)` if pounder is detected, where `devices` is a `PounderDevices` structure
 /// containing all of the pounder hardware interfaces in a disabled state.
 pub fn setup(
-    mut core: rtic::export::Peripherals,
+    mut core: rtic::Peripherals,
     device: stm32h7xx_hal::stm32::Peripherals,
 ) -> (StabilizerDevices, Option<PounderDevices>) {
     let pwr = device.PWR.constrain();
@@ -139,7 +139,17 @@ pub fn setup(
         init_log(logger).unwrap();
     }
 
-    let mut delay = hal::delay::Delay::new(core.SYST, ccdr.clocks);
+    // Set up the system timer for RTIC scheduling.
+    {
+        let tim15 =
+            device
+                .TIM15
+                .timer(10.khz(), ccdr.peripheral.TIM15, &ccdr.clocks);
+        system_timer::SystemTimer::initialize(tim15);
+    }
+
+    let mut delay =
+        asm_delay::AsmDelay::new(asm_delay::bitrate::MegaHertz(2 * 400));
 
     let gpioa = device.GPIOA.split(ccdr.peripheral.GPIOA);
     let gpiob = device.GPIOB.split(ccdr.peripheral.GPIOB);

src/hardware/design_parameters.rs

@@ -51,4 +51,4 @@ pub const SAMPLE_BUFFER_SIZE_LOG2: u8 = 3;
 pub const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2;
 
 // The MQTT broker IPv4 address
-pub const MQTT_BROKER: [u8; 4] = [10, 34, 16, 10];
+pub const MQTT_BROKER: [u8; 4] = [10, 35, 16, 10];

src/hardware/mod.rs

@@ -13,6 +13,7 @@ pub mod design_parameters;
 mod digital_input_stamper;
 mod eeprom;
 pub mod pounder;
+mod system_timer;
 mod timers;
 
 pub use adc::{Adc0Input, Adc1Input};
@@ -21,6 +22,7 @@ pub use cycle_counter::CycleCounter;
 pub use dac::{Dac0Output, Dac1Output};
 pub use digital_input_stamper::InputStamper;
 pub use pounder::DdsOutput;
+pub use system_timer::SystemTimer;
 
 // Type alias for the analog front-end (AFE) for ADC0.
 pub type AFE0 = afe::ProgrammableGainAmplifier<

src/hardware/system_timer.rs

@@ -0,0 +1,68 @@
+use hal::prelude::*;
+use stm32h7xx_hal as hal;
+
+static mut OVERFLOWS: u32 = 0;
+
+pub struct SystemTimer {}
+
+impl SystemTimer {
+    pub fn initialize(mut timer: hal::timer::Timer<hal::device::TIM15>) {
+        timer.pause();
+        // Have the system timer operate at a tick rate of 10KHz (100uS per tick). With this
+        // configuration and a 65535 period, we get an overflow once every 6.5 seconds.
+        timer.set_tick_freq(10.khz());
+        timer.apply_freq();
+
+        timer.resume();
+    }
+
+    pub fn ticks_from_secs(secs: u32) -> i32 {
+        (secs * 10_000) as i32
+    }
+}
+
+impl rtic::Monotonic for SystemTimer {
+    type Instant = i32;
+
+    fn ratio() -> rtic::Fraction {
+        rtic::Fraction {
+            numerator: 1,
+            denominator: 40000,
+        }
+    }
+
+    fn now() -> i32 {
+        let regs = unsafe { &*hal::device::TIM15::ptr() };
+
+        loop {
+            // Check for overflows
+            if regs.sr.read().uif().bit_is_set() {
+                regs.sr.modify(|_, w| w.uif().clear_bit());
+                unsafe {
+                    OVERFLOWS += 1;
+                }
+            }
+
+            let current_value = regs.cnt.read().bits();
+
+            // If the overflow is still unset, return our latest count, as it indicates we weren't
+            // pre-empted.
+            if regs.sr.read().uif().bit_is_clear() {
+                unsafe {
+                    return (OVERFLOWS * 65535 + current_value) as i32;
+                }
+            }
+        }
+    }
+
+    unsafe fn reset() {
+        // Note: The timer must be safely configured in `SystemTimer::initialize()`.
+        let regs = &*hal::device::TIM15::ptr();
+
+        regs.cnt.reset();
+    }
+
+    fn zero() -> i32 {
+        0
+    }
+}