From 19b606c385a34929e067622fb399c82912ada79e Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Thu, 6 May 2021 13:08:10 +0200 Subject: [PATCH] Refactoring lockin binaries --- .github/workflows/release.yml | 3 +- README.md | 4 +- src/bin/lockin-internal.rs | 143 ---------------------- src/bin/{lockin-external.rs => lockin.rs} | 82 ++++++++++--- 4 files changed, 69 insertions(+), 163 deletions(-) delete mode 100644 src/bin/lockin-internal.rs rename src/bin/{lockin-external.rs => lockin.rs} (71%) diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml index 5be1311..475c05e 100644 --- a/.github/workflows/release.yml +++ b/.github/workflows/release.yml @@ -24,8 +24,7 @@ jobs: - run: > zip bin.zip target/*/release/dual-iir - target/*/release/lockin-external - target/*/release/lockin-internal + target/*/release/lockin - id: create_release uses: actions/create-release@v1 env: diff --git a/README.md b/README.md index f91976a..80cda1d 100644 --- a/README.md +++ b/README.md @@ -29,9 +29,7 @@ to implement different use cases. Several applications are provides by default * anti-windup * derivative kick avoidance -### Lockin external - -### Lockin internal +### Lockin ## Minimal bootstrapping documentation 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 71% rename from src/bin/lockin-external.rs rename to src/bin/lockin.rs index 26634c0..53c51c2 100644 --- a/src/bin/lockin-external.rs +++ b/src/bin/lockin.rs @@ -18,6 +18,13 @@ use stabilizer::hardware::{ use miniconf::Miniconf; use stabilizer::net::{Action, MqttInterface}; +// A constant sinusoid to send on the DAC output. +// Full-scale gives a +/- 10.24V amplitude waveform. Scale it down to give +/- 1V. +const ONE: i16 = ((1.0 / 10.24) * 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]; + #[derive(Copy, Clone, Debug, Deserialize, Miniconf)] enum Conf { PowerPhase, @@ -25,9 +32,16 @@ enum Conf { Quadrature, } +#[derive(Copy, Clone, Debug, Miniconf, Deserialize, PartialEq)] +enum LockinMode { + Internal, + External, +} + #[derive(Copy, Clone, Debug, Deserialize, Miniconf)] pub struct Settings { afe: [AfeGain; 2], + lockin_mode: LockinMode, pll_tc: [u8; 2], @@ -36,6 +50,7 @@ pub struct Settings { lockin_phase: i32, output_conf: [Conf; 2], + telemetry_period_secs: u16, } impl Default for Settings { @@ -43,6 +58,8 @@ impl Default for Settings { Self { afe: [AfeGain::G1; 2], + lockin_mode: LockinMode::External, + pll_tc: [21, 21], // frequency and phase settling time (log2 counter cycles) lockin_tc: 6, // lockin lowpass time constant @@ -50,6 +67,7 @@ impl Default for Settings { lockin_phase: 0, // Demodulation LO phase offset output_conf: [Conf::Quadrature; 2], + telemetry_period_secs: 10, } } } @@ -145,21 +163,49 @@ const APP: () = { let lockin = c.resources.lockin; let settings = c.resources.settings; - let timestamp = - c.resources.timestamper.latest_timestamp().unwrap_or(None); // Ignore data from timer capture overflows. - let (pll_phase, pll_frequency) = c.resources.pll.update( - timestamp.map(|t| t as i32), - settings.pll_tc[0], - settings.pll_tc[1], - ); + let mut pll_frequency = 0; - let sample_frequency = ((pll_frequency - >> design_parameters::SAMPLE_BUFFER_SIZE_LOG2) - as i32) - .wrapping_mul(settings.lockin_harmonic); - let sample_phase = settings - .lockin_phase - .wrapping_add(pll_phase.wrapping_mul(settings.lockin_harmonic)); + let (sample_phase, sample_frequency) = match settings.lockin_mode { + LockinMode::External => { + let timestamp = + c.resources.timestamper.latest_timestamp().unwrap_or(None); // Ignore data from timer capture overflows. + let (pll_phase, frequency) = c.resources.pll.update( + timestamp.map(|t| t as i32), + settings.pll_tc[0], + settings.pll_tc[1], + ); + + pll_frequency = frequency; + + let sample_frequency = ((pll_frequency + >> design_parameters::SAMPLE_BUFFER_SIZE_LOG2) + as i32) + .wrapping_mul(settings.lockin_harmonic); + let sample_phase = settings.lockin_phase.wrapping_add( + pll_phase.wrapping_mul(settings.lockin_harmonic), + ); + + (sample_phase, sample_frequency) + } + + LockinMode::Internal => { + // Reference phase and frequency are known. + let pll_phase = 0i32; + let pll_frequency = + 1i32 << (32 - design_parameters::SAMPLE_BUFFER_SIZE_LOG2); + + // Demodulation LO phase offset + let phase_offset: i32 = 1 << 30; + + let sample_frequency = (pll_frequency as i32) + .wrapping_mul(settings.lockin_harmonic); + let sample_phase = phase_offset.wrapping_add( + pll_phase.wrapping_mul(settings.lockin_harmonic), + ); + + (sample_phase, sample_frequency) + } + }; let output: Complex = adc_samples[0] .iter() @@ -190,7 +236,13 @@ const APP: () = { // Convert to DAC data. for i in 0..dac_samples[0].len() { - dac_samples[0][i] = (output[0] >> 16) as u16 ^ 0x8000; + // When operating in internal lockin mode, DAC0 is always used for generating the + // reference signal. + if settings.lockin_mode == LockinMode::Internal { + dac_samples[0][i] = DAC_SEQUENCE[i] as u16 ^ 0x8000; + } else { + dac_samples[0][i] = (output[0] >> 16) as u16 ^ 0x8000; + } dac_samples[1][i] = (output[1] >> 16) as u16 ^ 0x8000; } }