Merge branch 'master' into feature/telemetry

This commit is contained in:
Ryan Summers 2021-04-15 15:43:40 +02:00
commit f442418f16
6 changed files with 75 additions and 48 deletions

View File

@ -67,7 +67,7 @@ fn iir_bench() {
let mut xy = iir::Vec5::default(); let mut xy = iir::Vec5::default();
println!( println!(
"int::IIR::update(s, x): {}", "int::IIR::update(s, x): {}",
bench_env(0.32241, |x| dut.update(&mut xy, *x)) bench_env(0.32241, |x| dut.update(&mut xy, *x, true))
); );
} }

View File

@ -117,7 +117,7 @@ impl IIR {
/// # Arguments /// # Arguments
/// * `xy` - Current filter state. /// * `xy` - Current filter state.
/// * `x0` - New input. /// * `x0` - New input.
pub fn update(&self, xy: &mut Vec5, x0: f32) -> f32 { pub fn update(&self, xy: &mut Vec5, x0: f32, hold: bool) -> f32 {
let n = self.ba.len(); let n = self.ba.len();
debug_assert!(xy.len() == n); debug_assert!(xy.len() == n);
// `xy` contains x0 x1 y0 y1 y2 // `xy` contains x0 x1 y0 y1 y2
@ -128,7 +128,11 @@ impl IIR {
// Store x0 x0 x1 x2 y1 y2 // Store x0 x0 x1 x2 y1 y2
xy[0] = x0; xy[0] = x0;
// Compute y0 by multiply-accumulate // Compute y0 by multiply-accumulate
let y0 = macc(self.y_offset, xy, &self.ba); let y0 = if hold {
xy[n / 2 + 1]
} else {
macc(self.y_offset, xy, &self.ba)
};
// Limit y0 // Limit y0
let y0 = max(self.y_min, min(self.y_max, y0)); let y0 = max(self.y_min, min(self.y_max, y0));
// Store y0 x0 x1 y0 y1 y2 // Store y0 x0 x1 y0 y1 y2

View File

@ -2,8 +2,6 @@
#![no_std] #![no_std]
#![no_main] #![no_main]
use stm32h7xx_hal as hal;
use stabilizer::hardware; use stabilizer::hardware;
use miniconf::{minimq, Miniconf, MqttInterface}; use miniconf::{minimq, Miniconf, MqttInterface};
@ -12,7 +10,7 @@ use serde::{Deserialize, Serialize};
use dsp::iir; use dsp::iir;
use hardware::{ use hardware::{
Adc0Input, Adc1Input, AfeGain, CycleCounter, Dac0Output, Dac1Output, Adc0Input, Adc1Input, AfeGain, CycleCounter, Dac0Output, Dac1Output,
NetworkStack, SystemTimer, AFE0, AFE1, DigitalInput0, DigitalInput1, SystemTimer, InputPin, NetworkStack, AFE0, AFE1,
}; };
const SCALE: f32 = i16::MAX as _; const SCALE: f32 = i16::MAX as _;
@ -20,13 +18,6 @@ const SCALE: f32 = i16::MAX as _;
// The number of cascaded IIR biquads per channel. Select 1 or 2! // The number of cascaded IIR biquads per channel. Select 1 or 2!
const IIR_CASCADE_LENGTH: usize = 1; const IIR_CASCADE_LENGTH: usize = 1;
#[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)] #[derive(Serialize, Clone)]
pub struct Telemetry { pub struct Telemetry {
latest_samples: [i16; 2], latest_samples: [i16; 2],
@ -34,11 +25,22 @@ pub struct Telemetry {
digital_inputs: [bool; 2], digital_inputs: [bool; 2],
} }
#[derive(Clone, Copy, Debug, Deserialize, Miniconf)]
pub struct Settings {
afe: [AfeGain; 2],
iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2],
allow_hold: bool,
force_hold: bool,
telemetry_period_secs: u16,
}
impl Default for Settings { impl Default for Settings {
fn default() -> Self { fn default() -> Self {
Self { Self {
afe: [AfeGain::G1, AfeGain::G1], afe: [AfeGain::G1, AfeGain::G1],
iir_ch: [[iir::IIR::new(1., -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2], iir_ch: [[iir::IIR::new(1., -SCALE, SCALE); IIR_CASCADE_LENGTH]; 2],
allow_hold: false,
force_hold: false,
telemetry_period_secs: 10, telemetry_period_secs: 10,
} }
} }
@ -58,6 +60,7 @@ impl Default for Telemetry {
const APP: () = { const APP: () = {
struct Resources { struct Resources {
afes: (AFE0, AFE1), afes: (AFE0, AFE1),
digital_inputs: (DigitalInput0, DigitalInput1),
adcs: (Adc0Input, Adc1Input), adcs: (Adc0Input, Adc1Input),
dacs: (Dac0Output, Dac1Output), dacs: (Dac0Output, Dac1Output),
mqtt_interface: mqtt_interface:
@ -69,11 +72,9 @@ const APP: () = {
// Format: iir_state[ch][cascade-no][coeff] // Format: iir_state[ch][cascade-no][coeff]
#[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])] #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])]
iir_state: [[iir::Vec5; 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],
} }
#[init(schedule = [telemetry])] #[init(spawn=[telemetry, settings_update])]
fn init(c: init::Context) -> init::LateResources { fn init(c: init::Context) -> init::LateResources {
// Configure the microcontroller // Configure the microcontroller
let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device); let (mut stabilizer, _pounder) = hardware::setup(c.core, c.device);
@ -96,6 +97,10 @@ const APP: () = {
.unwrap() .unwrap()
}; };
// Spawn a settings update for default settings.
c.spawn.settings_update().unwrap();
c.spawn.telemetry().unwrap();
// Enable ADC/DAC events // Enable ADC/DAC events
stabilizer.adcs.0.start(); stabilizer.adcs.0.start();
stabilizer.adcs.1.start(); stabilizer.adcs.1.start();
@ -105,16 +110,15 @@ const APP: () = {
// Start sampling ADCs. // Start sampling ADCs.
stabilizer.adc_dac_timer.start(); stabilizer.adc_dac_timer.start();
c.schedule.telemetry(c.start).unwrap();
init::LateResources { init::LateResources {
mqtt_interface, mqtt_interface,
afes: stabilizer.afes, afes: stabilizer.afes,
adcs: stabilizer.adcs, adcs: stabilizer.adcs,
dacs: stabilizer.dacs, dacs: stabilizer.dacs,
clock: stabilizer.cycle_counter, clock: stabilizer.cycle_counter,
settings: Settings::default(),
telemetry: Telemetry::default(), telemetry: Telemetry::default(),
digital_inputs: stabilizer.digital_inputs,
settings: Settings::default(),
} }
} }
@ -134,7 +138,7 @@ const APP: () = {
/// ///
/// Because the ADC and DAC operate at the same rate, these two constraints actually implement /// 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. /// the same time bounds, meeting one also means the other is also met.
#[task(binds=DMA1_STR4, resources=[adcs, dacs, iir_state, iir_ch, telemetry], priority=2)] #[task(binds=DMA1_STR4, resources=[adcs, digital_inputs, dacs, iir_state, settings, telemetry], priority=2)]
fn process(c: process::Context) { fn process(c: process::Context) {
let adc_samples = [ let adc_samples = [
c.resources.adcs.0.acquire_buffer(), c.resources.adcs.0.acquire_buffer(),
@ -146,13 +150,19 @@ const APP: () = {
c.resources.dacs.1.acquire_buffer(), c.resources.dacs.1.acquire_buffer(),
]; ];
let hold = c.resources.settings.force_hold
|| (c.resources.digital_inputs.1.is_high().unwrap()
&& c.resources.settings.allow_hold);
for channel in 0..adc_samples.len() { for channel in 0..adc_samples.len() {
for sample in 0..adc_samples[0].len() { for sample in 0..adc_samples[0].len() {
let x = f32::from(adc_samples[channel][sample] as i16); let mut y = f32::from(adc_samples[channel][sample] as i16);
let mut y = x;
for i in 0..c.resources.iir_state[channel].len() { for i in 0..c.resources.iir_state[channel].len() {
y = c.resources.iir_ch[channel][i] y = c.resources.settings.iir_ch[channel][i].update(
.update(&mut c.resources.iir_state[channel][i], y); &mut c.resources.iir_state[channel][i],
y,
hold,
);
} }
// Note(unsafe): The filter limits ensure that the value is in range. // Note(unsafe): The filter limits ensure that the value is in range.
// The truncation introduces 1/2 LSB distortion. // The truncation introduces 1/2 LSB distortion.
@ -169,6 +179,11 @@ const APP: () = {
c.resources.telemetry.latest_outputs = c.resources.telemetry.latest_outputs =
[dac_samples[0][0] as i16, dac_samples[1][0] as i16]; [dac_samples[0][0] as i16, dac_samples[1][0] as i16];
c.resources.telemetry.digital_inputs = [
c.resources.digital_inputs.0.is_high().unwrap(),
c.resources.digital_inputs.1.is_high().unwrap(),
];
} }
#[idle(resources=[mqtt_interface, clock], spawn=[settings_update])] #[idle(resources=[mqtt_interface, clock], spawn=[settings_update])]
@ -206,15 +221,12 @@ const APP: () = {
} }
} }
#[task(priority = 1, resources=[mqtt_interface, afes, settings, iir_ch])] #[task(priority = 1, resources=[mqtt_interface, afes, settings])]
fn settings_update(mut 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;
// Update the IIR channels. // Update the IIR channels.
c.resources.iir_ch.lock(|iir| *iir = settings.iir_ch); c.resources.settings.lock(|current| *current = *settings);
// Update currently-cached settings.
*c.resources.settings = *settings;
// Update AFEs // Update AFEs
c.resources.afes.0.set_gain(settings.afe[0]); c.resources.afes.0.set_gain(settings.afe[0]);
@ -223,11 +235,7 @@ const APP: () = {
#[task(priority = 1, resources=[mqtt_interface, settings, telemetry], schedule=[telemetry])] #[task(priority = 1, resources=[mqtt_interface, settings, telemetry], schedule=[telemetry])]
fn telemetry(mut c: telemetry::Context) { fn telemetry(mut c: telemetry::Context) {
let telemetry = c.resources.telemetry.lock(|telemetry| { let telemetry = c.resources.telemetry.lock(|telemetry| telemetry.clone());
// TODO: Incorporate digital input status.
telemetry.digital_inputs = [false, false];
telemetry.clone()
});
// Serialize telemetry outside of a critical section to prevent blocking the processing // Serialize telemetry outside of a critical section to prevent blocking the processing
// task. // task.
@ -242,20 +250,16 @@ const APP: () = {
client.publish("dt/sinara/dual-iir/telemetry", telemetry.as_bytes(), minimq::QoS::AtMostOnce, &[]).ok() 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. // Schedule the telemetry task in the future.
c.schedule c.schedule.telemetry( c.scheduled + SystemTimer::ticks_from_secs(telemetry_period as u32))
.telemetry(
c.scheduled
+ SystemTimer::ticks_from_secs(
c.resources.settings.telemetry_period_secs as u32,
),
)
.unwrap(); .unwrap();
} }
#[task(binds = ETH, priority = 1)] #[task(binds = ETH, priority = 1)]
fn eth(_: eth::Context) { fn eth(_: eth::Context) {
unsafe { hal::ethernet::interrupt_handler() } unsafe { stm32h7xx_hal::ethernet::interrupt_handler() }
} }
#[task(binds = SPI2, priority = 3)] #[task(binds = SPI2, priority = 3)]

View File

@ -13,8 +13,8 @@ use embedded_hal::digital::v2::{InputPin, OutputPin};
use super::{ use super::{
adc, afe, cycle_counter::CycleCounter, dac, design_parameters, adc, afe, cycle_counter::CycleCounter, dac, design_parameters,
digital_input_stamper, eeprom, pounder, system_timer, timers, DdsOutput, digital_input_stamper, eeprom, pounder, timers, system_timer, DdsOutput, DigitalInput0,
NetworkStack, AFE0, AFE1, DigitalInput1, NetworkStack, AFE0, AFE1,
}; };
pub struct NetStorage { pub struct NetStorage {
@ -69,6 +69,7 @@ pub struct StabilizerDevices {
pub timestamp_timer: timers::TimestampTimer, pub timestamp_timer: timers::TimestampTimer,
pub net: NetworkDevices, pub net: NetworkDevices,
pub cycle_counter: CycleCounter, pub cycle_counter: CycleCounter,
pub digital_inputs: (DigitalInput0, DigitalInput1),
} }
/// The available Pounder-specific hardware interfaces. /// The available Pounder-specific hardware interfaces.
@ -449,6 +450,12 @@ pub fn setup(
) )
}; };
let digital_inputs = {
let di0 = gpiog.pg9.into_floating_input();
let di1 = gpioc.pc15.into_floating_input();
(di0, di1)
};
let mut eeprom_i2c = { let mut eeprom_i2c = {
let sda = gpiof.pf0.into_alternate_af4().set_open_drain(); let sda = gpiof.pf0.into_alternate_af4().set_open_drain();
let scl = gpiof.pf1.into_alternate_af4().set_open_drain(); let scl = gpiof.pf1.into_alternate_af4().set_open_drain();
@ -882,6 +889,7 @@ pub fn setup(
adc_dac_timer: sampling_timer, adc_dac_timer: sampling_timer,
timestamp_timer, timestamp_timer,
cycle_counter, cycle_counter,
digital_inputs,
}; };
// info!("Version {} {}", build_info::PKG_VERSION, build_info::GIT_VERSION.unwrap()); // info!("Version {} {}", build_info::PKG_VERSION, build_info::GIT_VERSION.unwrap());

View File

@ -1,6 +1,9 @@
///! Module for all hardware-specific setup of Stabilizer ///! Module for all hardware-specific setup of Stabilizer
use stm32h7xx_hal as hal; use stm32h7xx_hal as hal;
// Re-export for the DigitalInputs below:
pub use embedded_hal::digital::v2::InputPin;
#[cfg(feature = "semihosting")] #[cfg(feature = "semihosting")]
use panic_semihosting as _; use panic_semihosting as _;
@ -36,6 +39,14 @@ pub type AFE1 = afe::ProgrammableGainAmplifier<
hal::gpio::gpiod::PD15<hal::gpio::Output<hal::gpio::PushPull>>, hal::gpio::gpiod::PD15<hal::gpio::Output<hal::gpio::PushPull>>,
>; >;
// Type alias for digital input 0 (DI0).
pub type DigitalInput0 =
hal::gpio::gpiog::PG9<hal::gpio::Input<hal::gpio::Floating>>;
// Type alias for digital input 1 (DI1).
pub type DigitalInput1 =
hal::gpio::gpioc::PC15<hal::gpio::Input<hal::gpio::Floating>>;
pub type NetworkStack = smoltcp_nal::NetworkStack< pub type NetworkStack = smoltcp_nal::NetworkStack<
'static, 'static,
'static, 'static,

View File

@ -10,14 +10,14 @@ impl SystemTimer {
timer.pause(); timer.pause();
// Have the system timer operate at a tick rate of 10KHz (100uS per tick). With this // 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. // configuration and a 65535 period, we get an overflow once every 6.5 seconds.
timer.set_tick_freq(1.mhz()); timer.set_tick_freq(10.khz());
timer.apply_freq(); timer.apply_freq();
timer.resume(); timer.resume();
} }
pub fn ticks_from_secs(secs: u32) -> i32 { pub fn ticks_from_secs(secs: u32) -> i32 {
(secs * 1_000_000) as i32 (secs * 10_000) as i32
} }
} }
@ -26,8 +26,8 @@ impl rtic::Monotonic for SystemTimer {
fn ratio() -> rtic::Fraction { fn ratio() -> rtic::Fraction {
rtic::Fraction { rtic::Fraction {
numerator: 1, numerator: 40_000,
denominator: 400, denominator: 1,
} }
} }