dual-iir: add closure nesting helper macro
This commit is contained in:
parent
2a9657f98c
commit
c13859d486
@ -52,6 +52,15 @@ impl Default for Settings {
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
macro_rules! flatten_closures {
|
||||||
|
($fn:ident, $e:ident, $fun:block) => {
|
||||||
|
$e.$fn(|$e| $fun )
|
||||||
|
};
|
||||||
|
($fn:ident, $e:ident, $($es:ident),+, $fun:block) => {
|
||||||
|
$e.$fn(|$e| flatten_closures!($fn, $($es),*, $fun))
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = stabilizer::hardware::SystemTimer)]
|
#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = stabilizer::hardware::SystemTimer)]
|
||||||
const APP: () = {
|
const APP: () = {
|
||||||
struct Resources {
|
struct Resources {
|
||||||
@ -141,55 +150,43 @@ const APP: () = {
|
|||||||
let hold =
|
let hold =
|
||||||
settings.force_hold || (digital_inputs[1] && settings.allow_hold);
|
settings.force_hold || (digital_inputs[1] && settings.allow_hold);
|
||||||
|
|
||||||
adc0.with_buffer(|a0| {
|
flatten_closures!(with_buffer, adc0, adc1, dac0, dac1, {
|
||||||
adc1.with_buffer(|a1| {
|
let adc_samples = [adc0, adc1];
|
||||||
dac0.with_buffer(|d0| {
|
let dac_samples = [dac0, dac1];
|
||||||
dac1.with_buffer(|d1| {
|
|
||||||
let adc_samples = [a0, a1];
|
|
||||||
let dac_samples = [d0, d1];
|
|
||||||
|
|
||||||
// Preserve instruction and data ordering w.r.t. DMA flag access.
|
// Preserve instruction and data ordering w.r.t. DMA flag access.
|
||||||
fence(Ordering::SeqCst);
|
fence(Ordering::SeqCst);
|
||||||
|
|
||||||
for channel in 0..adc_samples.len() {
|
for channel in 0..adc_samples.len() {
|
||||||
adc_samples[channel]
|
adc_samples[channel]
|
||||||
.iter()
|
.iter()
|
||||||
.zip(dac_samples[channel].iter_mut())
|
.zip(dac_samples[channel].iter_mut())
|
||||||
.map(|(ai, di)| {
|
.map(|(ai, di)| {
|
||||||
let x = f32::from(*ai as i16);
|
let x = f32::from(*ai as i16);
|
||||||
let y = settings.iir_ch[channel]
|
let y = settings.iir_ch[channel]
|
||||||
.iter()
|
.iter()
|
||||||
.zip(iir_state[channel].iter_mut())
|
.zip(iir_state[channel].iter_mut())
|
||||||
.fold(x, |yi, (ch, state)| {
|
.fold(x, |yi, (ch, state)| {
|
||||||
ch.update(state, yi, hold)
|
ch.update(state, yi, hold)
|
||||||
});
|
});
|
||||||
// Note(unsafe): The filter limits must ensure that
|
// Note(unsafe): The filter limits must ensure that the value is in range.
|
||||||
// the value is in range.
|
// The truncation introduces 1/2 LSB distortion.
|
||||||
// The truncation introduces 1/2 LSB distortion.
|
let y: i16 = unsafe { y.to_int_unchecked() };
|
||||||
let y: i16 =
|
// Convert to DAC code
|
||||||
unsafe { y.to_int_unchecked() };
|
*di = DacCode::from(y).0;
|
||||||
// Convert to DAC code
|
|
||||||
*di = DacCode::from(y).0;
|
|
||||||
})
|
|
||||||
.last();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Update telemetry measurements.
|
|
||||||
telemetry.adcs = [
|
|
||||||
AdcCode(adc_samples[0][0]),
|
|
||||||
AdcCode(adc_samples[1][0]),
|
|
||||||
];
|
|
||||||
|
|
||||||
telemetry.dacs = [
|
|
||||||
DacCode(dac_samples[0][0]),
|
|
||||||
DacCode(dac_samples[1][0]),
|
|
||||||
];
|
|
||||||
|
|
||||||
// Preserve instruction and data ordering w.r.t. DMA flag access.
|
|
||||||
fence(Ordering::SeqCst);
|
|
||||||
})
|
})
|
||||||
})
|
.last();
|
||||||
})
|
}
|
||||||
|
|
||||||
|
// Update telemetry measurements.
|
||||||
|
telemetry.adcs =
|
||||||
|
[AdcCode(adc_samples[0][0]), AdcCode(adc_samples[1][0])];
|
||||||
|
|
||||||
|
telemetry.dacs =
|
||||||
|
[DacCode(dac_samples[0][0]), DacCode(dac_samples[1][0])];
|
||||||
|
|
||||||
|
// Preserve instruction and data ordering w.r.t. DMA flag access.
|
||||||
|
fence(Ordering::SeqCst);
|
||||||
});
|
});
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1,8 +1,5 @@
|
|||||||
#![no_std]
|
#![no_std]
|
||||||
#![cfg_attr(feature = "nightly", feature(core_intrinsics))]
|
#![cfg_attr(feature = "nightly", feature(core_intrinsics))]
|
||||||
|
|
||||||
#[macro_use]
|
|
||||||
extern crate log;
|
|
||||||
|
|
||||||
pub mod hardware;
|
pub mod hardware;
|
||||||
pub mod net;
|
pub mod net;
|
||||||
|
Loading…
Reference in New Issue
Block a user