kai
/
pounder_test
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

lockin: port to fast double buffered DMA

master
Robert Jördens 2021-06-01 13:17:40 +02:00
parent c5a2704c41
commit b90f4ad185
1 changed files with 60 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

111
src/bin/lockin.rs
View File

@ -78,6 +78,15 @@ impl Default for Settings {
} }
} }
macro_rules! flatten_closures {
($fn:ident, $e:ident, $fun:block) => {
$e.$fn(|$e| $fun ).unwrap()
};
($fn:ident, $e:ident, $($es:ident),+, $fun:block) => {
$e.$fn(|$e| flatten_closures!($fn, $($es),*, $fun)).unwrap()
};
}
#[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 {
@ -159,26 +168,22 @@ const APP: () = {
#[task(binds=DMA1_STR4, resources=[adcs, dacs, lockin, timestamper, pll, settings, telemetry], priority=2)] #[task(binds=DMA1_STR4, resources=[adcs, dacs, lockin, timestamper, pll, settings, telemetry], priority=2)]
#[inline(never)] #[inline(never)]
#[link_section = ".itcm.process"] #[link_section = ".itcm.process"]
fn process(c: process::Context) { fn process(mut c: process::Context) {
let adc_samples = [ let process::Resources {
c.resources.adcs.0.acquire_buffer(), adcs: (ref mut adc0, ref mut adc1),
c.resources.adcs.1.acquire_buffer(), dacs: (ref mut dac0, ref mut dac1),
]; ref settings,
ref mut telemetry,
let mut dac_samples = [ ref mut lockin,
c.resources.dacs.0.acquire_buffer(), ref mut pll,
c.resources.dacs.1.acquire_buffer(), ref mut timestamper,
]; } = c.resources;
let lockin = c.resources.lockin;
let settings = c.resources.settings;
let (reference_phase, reference_frequency) = match settings.lockin_mode let (reference_phase, reference_frequency) = match settings.lockin_mode
{ {
LockinMode::External => { LockinMode::External => {
let timestamp = let timestamp = timestamper.latest_timestamp().unwrap_or(None); // Ignore data from timer capture overflows.
c.resources.timestamper.latest_timestamp().unwrap_or(None); // Ignore data from timer capture overflows. let (pll_phase, pll_frequency) = pll.update(
let (pll_phase, pll_frequency) = c.resources.pll.update(
timestamp.map(|t| t as i32), timestamp.map(|t| t as i32),
settings.pll_tc[0], settings.pll_tc[0],
settings.pll_tc[1], settings.pll_tc[1],
@ -205,45 +210,49 @@ const APP: () = {
reference_phase.wrapping_mul(settings.lockin_harmonic), reference_phase.wrapping_mul(settings.lockin_harmonic),
); );
let output: Complex<i32> = adc_samples[0] flatten_closures!(with_buffer, adc0, adc1, dac0, dac1, {
.iter() let adc_samples = [adc0, adc1];
// Zip in the LO phase. let mut dac_samples = [dac0, dac1];
.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, settings.lockin_tc)
})
// Decimate
.last()
.unwrap()
* 2; // Full scale assuming the 2f component is gone.
// Convert to DAC data. let output: Complex<i32> = adc_samples[0]
for (channel, samples) in dac_samples.iter_mut().enumerate() { .iter()
for (i, sample) in samples.iter_mut().enumerate() { // Zip in the LO phase.
let value = match settings.output_conf[channel] { .zip(Accu::new(sample_phase, sample_frequency))
Conf::Magnitude => output.abs_sqr() as i32 >> 16, // Convert to signed, MSB align the ADC sample, update the Lockin (demodulate, filter)
Conf::Phase => output.arg() >> 16, .map(|(&sample, phase)| {
Conf::LogPower => (output.log2() << 24) as i32 >> 16, let s = (sample as i16 as i32) << 16;
Conf::ReferenceFrequency => { lockin.update(s, phase, settings.lockin_tc)
reference_frequency as i32 >> 16 })
} // Decimate
Conf::InPhase => output.re >> 16, .last()
Conf::Quadrature => output.im >> 16, .unwrap()
Conf::Modulation => DAC_SEQUENCE[i] as i32, * 2; // Full scale assuming the 2f component is gone.
};
*sample = DacCode::from(value as i16).0; // Convert to DAC data.
for (channel, samples) in dac_samples.iter_mut().enumerate() {
for (i, sample) in samples.iter_mut().enumerate() {
let value = match settings.output_conf[channel] {
Conf::Magnitude => output.abs_sqr() as i32 >> 16,
Conf::Phase => output.arg() >> 16,
Conf::LogPower => (output.log2() << 24) as i32 >> 16,
Conf::ReferenceFrequency => {
reference_frequency as i32 >> 16
}
Conf::InPhase => output.re >> 16,
Conf::Quadrature => output.im >> 16,
Conf::Modulation => DAC_SEQUENCE[i] as i32,
};
*sample = DacCode::from(value as i16).0;
}
} }
} // Update telemetry measurements.
telemetry.adcs =
[AdcCode(adc_samples[0][0]), AdcCode(adc_samples[1][0])];
// Update telemetry measurements. telemetry.dacs =
c.resources.telemetry.adcs = [DacCode(dac_samples[0][0]), DacCode(dac_samples[1][0])];
[AdcCode(adc_samples[0][0]), AdcCode(adc_samples[1][0])]; });
c.resources.telemetry.dacs =
[DacCode(dac_samples[0][0]), DacCode(dac_samples[1][0])];
} }
#[idle(resources=[network], spawn=[settings_update])] #[idle(resources=[network], spawn=[settings_update])]