diff --git a/src/bin/dual-iir.rs b/src/bin/dual-iir.rs index 83cf764..9f48989 100644 --- a/src/bin/dual-iir.rs +++ b/src/bin/dual-iir.rs @@ -5,7 +5,7 @@ use stabilizer::{hardware, net}; use miniconf::{minimq, Miniconf}; -use serde::{Deserialize, Serialize}; +use serde::Deserialize; use dsp::iir; use hardware::{ @@ -20,13 +20,6 @@ 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(Serialize, Clone)] -pub struct Telemetry { - latest_samples: [i16; 2], - latest_outputs: [i16; 2], - digital_inputs: [bool; 2], -} - #[derive(Clone, Copy, Debug, Deserialize, Miniconf)] pub struct Settings { afe: [AfeGain; 2], @@ -48,17 +41,7 @@ impl Default for Settings { } } -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)] +#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = stabilizer::hardware::SystemTimer)] const APP: () = { struct Resources { afes: (AFE0, AFE1), @@ -66,7 +49,7 @@ const APP: () = { adcs: (Adc0Input, Adc1Input), dacs: (Dac0Output, Dac1Output), mqtt_config: MiniconfInterface, - telemetry: Telemetry, + telemetry: net::Telemetry, settings: Settings, // Format: iir_state[ch][cascade-no][coeff] @@ -107,7 +90,7 @@ const APP: () = { afes: stabilizer.afes, adcs: stabilizer.adcs, dacs: stabilizer.dacs, - telemetry: Telemetry::default(), + telemetry: net::Telemetry::default(), digital_inputs: stabilizer.digital_inputs, mqtt_config, settings: Settings::default(), diff --git a/src/bin/lockin-internal.rs b/src/bin/lockin-internal.rs deleted file mode 100644 index ee9da2f..0000000 --- a/src/bin/lockin-internal.rs +++ /dev/null @@ -1,143 +0,0 @@ -#![deny(warnings)] -#![no_std] -#![no_main] - -use dsp::{Accu, Complex, ComplexExt, Lockin}; -use generic_array::typenum::U2; -use hardware::{Adc1Input, Dac0Output, Dac1Output, AFE0, AFE1}; -use stabilizer::{hardware, hardware::design_parameters}; - -// A constant sinusoid to send on the DAC output. -// Full-scale gives a +/- 10V amplitude waveform. Scale it down to give +/- 1V. -const ONE: i16 = (0.1 * u16::MAX as f32) as _; -const SQRT2: i16 = (ONE as f32 * 0.707) as _; -const DAC_SEQUENCE: [i16; design_parameters::SAMPLE_BUFFER_SIZE] = - [ONE, SQRT2, 0, -SQRT2, -ONE, -SQRT2, 0, SQRT2]; - -#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)] -const APP: () = { - struct Resources { - afes: (AFE0, AFE1), - adc: Adc1Input, - dacs: (Dac0Output, Dac1Output), - - lockin: Lockin, - } - - #[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.1.start(); - stabilizer.dacs.0.start(); - stabilizer.dacs.1.start(); - - // Start sampling ADCs. - stabilizer.adc_dac_timer.start(); - - init::LateResources { - lockin: Lockin::default(), - afes: stabilizer.afes, - adc: stabilizer.adcs.1, - dacs: stabilizer.dacs, - } - } - - /// Main DSP processing routine. - /// - /// See `dual-iir` for general notes on processing time and timing. - /// - /// This is an implementation of an internal-reference lockin on the ADC1 signal. - /// The reference at f_sample/8 is output on DAC0 and the phase of the demodulated - /// signal on DAC1. - #[task(binds=DMA1_STR4, resources=[adc, dacs, lockin], priority=2)] - fn process(c: process::Context) { - let lockin = c.resources.lockin; - let adc_samples = c.resources.adc.acquire_buffer(); - let dac_samples = [ - c.resources.dacs.0.acquire_buffer(), - c.resources.dacs.1.acquire_buffer(), - ]; - - // Reference phase and frequency are known. - let pll_phase = 0i32; - let pll_frequency = - 1i32 << (32 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2); - - // Harmonic index of the LO: -1 to _de_modulate the fundamental (complex conjugate) - let harmonic: i32 = -1; - - // Demodulation LO phase offset - let phase_offset: i32 = 1 << 30; - - // Log2 lowpass time constant. - let time_constant: u8 = 8; - - let sample_frequency = (pll_frequency as i32).wrapping_mul(harmonic); - let sample_phase = - phase_offset.wrapping_add(pll_phase.wrapping_mul(harmonic)); - - let output: Complex = adc_samples - .iter() - // Zip in the LO phase. - .zip(Accu::new(sample_phase, sample_frequency)) - // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter) - .map(|(&sample, phase)| { - let s = (sample as i16 as i32) << 16; - lockin.update(s, phase, time_constant) - }) - // Decimate - .last() - .unwrap() - * 2; // Full scale assuming the 2f component is gone. - - // Convert to DAC data. - for (i, data) in DAC_SEQUENCE.iter().enumerate() { - // DAC0 always generates a fixed sinusoidal output. - dac_samples[0][i] = *data as u16 ^ 0x8000; - dac_samples[1][i] = (output.arg() >> 16) as u16 ^ 0x8000; - } - } - - #[idle(resources=[afes])] - fn idle(_: idle::Context) -> ! { - loop { - cortex_m::asm::wfi(); - } - } - - #[task(binds = ETH, priority = 1)] - fn eth(_: eth::Context) { - unsafe { stm32h7xx_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!("ADC1 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(); - } -}; diff --git a/src/bin/lockin-external.rs b/src/bin/lockin.rs similarity index 75% rename from src/bin/lockin-external.rs rename to src/bin/lockin.rs index 082b0d2..f2f7181 100644 --- a/src/bin/lockin-external.rs +++ b/src/bin/lockin.rs @@ -2,6 +2,7 @@ #![no_std] #![no_main] +use embedded_hal::digital::v2::InputPin; use generic_array::typenum::U4; use serde::Deserialize; @@ -12,10 +13,11 @@ use stabilizer::net; use stabilizer::hardware::{ design_parameters, setup, Adc0Input, Adc1Input, AfeGain, Dac0Output, - Dac1Output, InputStamper, AFE0, AFE1, + Dac1Output, DigitalInput0, DigitalInput1, InputStamper, SystemTimer, AFE0, + AFE1, }; -use miniconf::Miniconf; +use miniconf::{minimq, Miniconf}; use stabilizer::net::{Action, MiniconfInterface}; #[derive(Copy, Clone, Debug, Deserialize, Miniconf)] @@ -36,6 +38,7 @@ pub struct Settings { lockin_phase: i32, output_conf: [Conf; 2], + telemetry_period_secs: u16, } impl Default for Settings { @@ -50,11 +53,12 @@ impl Default for Settings { lockin_phase: 0, // Demodulation LO phase offset output_conf: [Conf::Quadrature; 2], + telemetry_period_secs: 10, } } } -#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)] +#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = stabilizer::hardware::SystemTimer)] const APP: () = { struct Resources { afes: (AFE0, AFE1), @@ -62,13 +66,15 @@ const APP: () = { dacs: (Dac0Output, Dac1Output), mqtt_config: MiniconfInterface, settings: Settings, + telemetry: net::Telemetry, + digital_inputs: (DigitalInput0, DigitalInput1), timestamper: InputStamper, pll: RPLL, lockin: Lockin, } - #[init(spawn=[settings_update])] + #[init(spawn=[settings_update, telemetry])] fn init(c: init::Context) -> init::LateResources { // Configure the microcontroller let (mut stabilizer, _pounder) = setup(c.core, c.device); @@ -91,8 +97,9 @@ const APP: () = { + design_parameters::SAMPLE_BUFFER_SIZE_LOG2, ); - // Spawn a settings update for default settings. + // Spawn a settings and telemetry update for default settings. c.spawn.settings_update().unwrap(); + c.spawn.telemetry().unwrap(); // Enable ADC/DAC events stabilizer.adcs.0.start(); @@ -113,8 +120,10 @@ const APP: () = { afes: stabilizer.afes, adcs: stabilizer.adcs, dacs: stabilizer.dacs, + digital_inputs: stabilizer.digital_inputs, mqtt_config, timestamper: stabilizer.timestamper, + telemetry: net::Telemetry::default(), settings, @@ -130,7 +139,7 @@ const APP: () = { /// This is an implementation of a externally (DI0) referenced PLL lockin on the ADC0 signal. /// It outputs either I/Q or power/phase on DAC0/DAC1. Data is normalized to full scale. /// PLL bandwidth, filter bandwidth, slope, and x/y or power/phase post-filters are available. - #[task(binds=DMA1_STR4, resources=[adcs, dacs, lockin, timestamper, pll, settings], priority=2)] + #[task(binds=DMA1_STR4, resources=[adcs, dacs, lockin, timestamper, pll, settings, telemetry], priority=2)] fn process(c: process::Context) { let adc_samples = [ c.resources.adcs.0.acquire_buffer(), @@ -193,6 +202,14 @@ const APP: () = { dac_samples[0][i] = (output[0] >> 16) as u16 ^ 0x8000; dac_samples[1][i] = (output[1] >> 16) 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_config], spawn=[settings_update])] @@ -222,6 +239,50 @@ const APP: () = { c.resources.settings.lock(|current| *current = *settings); } + #[task(priority = 1, resources=[mqtt_config, digital_inputs, settings, telemetry], schedule=[telemetry])] + fn telemetry(mut c: telemetry::Context) { + let mut telemetry = + c.resources.telemetry.lock(|telemetry| telemetry.clone()); + + telemetry.digital_inputs = [ + c.resources.digital_inputs.0.is_high().unwrap(), + c.resources.digital_inputs.1.is_high().unwrap(), + ]; + + // 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_config.mqtt.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() + }); + + let telemetry_period = c + .resources + .settings + .lock(|settings| settings.telemetry_period_secs); + + // Schedule the telemetry task in the future. + c.schedule + .telemetry( + c.scheduled + + SystemTimer::ticks_from_secs(telemetry_period as u32), + ) + .unwrap(); + } + #[task(binds = ETH, priority = 1)] fn eth(_: eth::Context) { unsafe { stm32h7xx_hal::ethernet::interrupt_handler() } diff --git a/src/net/mod.rs b/src/net/mod.rs index 13b514c..2a4ba63 100644 --- a/src/net/mod.rs +++ b/src/net/mod.rs @@ -7,6 +7,9 @@ use core::fmt::Write; use heapless::{consts, String}; use miniconf::minimq; +mod telemetry; +pub use telemetry::Telemetry; + /// Potential actions for firmware to take. pub enum Action { /// Indicates that firmware can sleep for the next event. diff --git a/src/net/telemetry.rs b/src/net/telemetry.rs new file mode 100644 index 0000000..5c948d5 --- /dev/null +++ b/src/net/telemetry.rs @@ -0,0 +1,18 @@ +use serde::Serialize; + +#[derive(Serialize, Clone)] +pub struct Telemetry { + pub latest_samples: [i16; 2], + pub latest_outputs: [i16; 2], + pub digital_inputs: [bool; 2], +} + +impl Default for Telemetry { + fn default() -> Self { + Self { + latest_samples: [0, 0], + latest_outputs: [0, 0], + digital_inputs: [false, false], + } + } +}