Adding WIP telemetry implementation for dual-iir
parent
1d65edc72a
commit
330f67d3c8
|
|
@ -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() }
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
AFE0, AFE1,
|
||||
digital_input_stamper, eeprom, pounder, system_timer, timers, DdsOutput,
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
|
|
|
|||
|
|
@ -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<
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue