From 3b953e36aa3ec77ee00de4c70830f196301971b5 Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Wed, 11 Nov 2020 18:42:34 +0100 Subject: [PATCH 01/71] Adding compile-time management of TIM2 channels --- src/adc.rs | 18 ++++++++++++++++-- src/main.rs | 46 +++++++++++++++++++++++++--------------------- 2 files changed, 41 insertions(+), 23 deletions(-) diff --git a/src/adc.rs b/src/adc.rs index e3310f4..d63ebe9 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -14,8 +14,8 @@ ///! both transfers are completed before reading the data. This is usually not significant for ///! busy-waiting because the transfers should complete at approximately the same time. use super::{ - hal, DMAReq, DmaConfig, MemoryToPeripheral, PeripheralToMemory, Priority, - TargetAddress, Transfer, + hal, sampling_timer, DMAReq, DmaConfig, MemoryToPeripheral, + PeripheralToMemory, Priority, TargetAddress, Transfer, }; // The desired ADC input buffer size. This is use configurable. @@ -142,11 +142,18 @@ impl Adc0Input { /// * `trigger_stream` - The DMA stream used to trigger each ADC transfer by writing a word into /// the SPI TX FIFO. /// * `data_stream` - The DMA stream used to read samples received over SPI into a data buffer. + /// * `_trigger_channel` - The ADC sampling timer output compare channel for read triggers. pub fn new( spi: hal::spi::Spi, trigger_stream: hal::dma::dma::Stream0, data_stream: hal::dma::dma::Stream1, + trigger_channel: sampling_timer::Timer2Channel1, ) -> Self { + // Generate DMA events when an output compare of the timer hitting zero (timer roll over) + // occurs. + trigger_channel.listen_dma(); + trigger_channel.to_output_compare(0); + // The trigger stream constantly writes to the TX FIFO using a static word (dont-care // contents). Thus, neither the memory or peripheral address ever change. This is run in // circular mode to be completed at every DMA request. @@ -256,11 +263,18 @@ impl Adc1Input { /// * `spi` - The SPI interface connected to ADC1. /// * `trigger_stream` - The DMA stream used to trigger ADC conversions on the SPI interface. /// * `data_stream` - The DMA stream used to read ADC samples from the SPI RX FIFO. + /// * `trigger_channel` - The ADC sampling timer output compare channel for read triggers. pub fn new( spi: hal::spi::Spi, trigger_stream: hal::dma::dma::Stream2, data_stream: hal::dma::dma::Stream3, + trigger_channel: sampling_timer::Timer2Channel2, ) -> Self { + // Generate DMA events when an output compare of the timer hitting zero (timer roll over) + // occurs. + trigger_channel.listen_dma(); + trigger_channel.to_output_compare(0); + // The trigger stream constantly writes to the TX FIFO using a static word (dont-care // contents). Thus, neither the memory or peripheral address ever change. This is run in // circular mode to be completed at every DMA request. diff --git a/src/main.rs b/src/main.rs index 1805370..59b19fc 100644 --- a/src/main.rs +++ b/src/main.rs @@ -62,6 +62,7 @@ mod dac; mod eeprom; mod iir; mod pounder; +mod sampling_timer; mod server; use adc::{Adc0Input, Adc1Input, AdcInputs}; @@ -187,8 +188,6 @@ const APP: () = { eeprom_i2c: hal::i2c::I2c, - timer: hal::timer::Timer, - // Note: It appears that rustfmt generates a format that GDB cannot recognize, which // results in GDB breakpoints being set improperly. #[rustfmt::skip] @@ -264,6 +263,16 @@ const APP: () = { let dma_streams = hal::dma::dma::StreamsTuple::new(dp.DMA1, ccdr.peripheral.DMA1); + // Configure timer 2 to trigger conversions for the ADC + let timer2 = dp.TIM2.timer( + SAMPLE_FREQUENCY_KHZ.khz(), + ccdr.peripheral.TIM2, + &ccdr.clocks, + ); + + let mut sampling_timer = sampling_timer::SamplingTimer::new(timer2); + let sampling_timer_channels = sampling_timer.channels(); + // Configure the SPI interfaces to the ADCs and DACs. let adcs = { let adc0 = { @@ -296,7 +305,12 @@ const APP: () = { &ccdr.clocks, ); - Adc0Input::new(spi, dma_streams.0, dma_streams.1) + Adc0Input::new( + spi, + dma_streams.0, + dma_streams.1, + sampling_timer_channels.ch1, + ) }; let adc1 = { @@ -329,7 +343,12 @@ const APP: () = { &ccdr.clocks, ); - Adc1Input::new(spi, dma_streams.2, dma_streams.3) + Adc1Input::new( + spi, + dma_streams.2, + dma_streams.3, + sampling_timer_channels.ch2, + ) }; AdcInputs::new(adc0, adc1) @@ -697,22 +716,8 @@ const APP: () = { // Utilize the cycle counter for RTIC scheduling. cp.DWT.enable_cycle_counter(); - // Configure timer 2 to trigger conversions for the ADC - let timer2 = dp.TIM2.timer( - SAMPLE_FREQUENCY_KHZ.khz(), - ccdr.peripheral.TIM2, - &ccdr.clocks, - ); - { - // Listen to the CH1 and CH2 comparison events. These channels should have a value of - // zero loaded into them, so the event should occur whenever the timer overflows. Note - // that we use channels instead of timer updates because each SPI DMA transfer needs a - // unique request line. - let t2_regs = unsafe { &*hal::stm32::TIM2::ptr() }; - t2_regs - .dier - .modify(|_, w| w.cc1de().set_bit().cc2de().set_bit()); - } + // Start sampling ADCs. + sampling_timer.start(); init::LateResources { afe0: afe0, @@ -721,7 +726,6 @@ const APP: () = { adcs, dacs, - timer: timer2, pounder: pounder_devices, eeprom_i2c, From 56bcf1e0aad89a6f9bc4c240ce2cb94ea113c51f Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Wed, 11 Nov 2020 18:44:28 +0100 Subject: [PATCH 02/71] Adding sampling_timer file --- src/sampling_timer.rs | 107 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 src/sampling_timer.rs diff --git a/src/sampling_timer.rs b/src/sampling_timer.rs new file mode 100644 index 0000000..b412b75 --- /dev/null +++ b/src/sampling_timer.rs @@ -0,0 +1,107 @@ +///! The sampling timer is used for managing ADC sampling and external reference timestamping. +use super::hal; + +pub use hal::stm32::tim2::ccmr2_input::CC4S_A; + +/// The timer used for managing ADC sampling. +pub struct SamplingTimer { + timer: hal::timer::Timer, + channels: Option, +} + +impl SamplingTimer { + /// Construct the sampling timer. + pub fn new(mut timer: hal::timer::Timer) -> Self { + timer.pause(); + + Self { + timer, + channels: Some(TimerChannels::new()), + } + } + + /// Get the timer capture/compare channels. + pub fn channels(&mut self) -> TimerChannels { + self.channels.take().unwrap() + } + + /// Start the sampling timer. + pub fn start(&mut self) { + self.timer.reset_counter(); + self.timer.resume(); + } +} + +/// The capture/compare channels for the sampling timer. +/// +/// # Note +/// This should not be instantiated directly. +pub struct TimerChannels { + pub ch1: Timer2Channel1, + pub ch2: Timer2Channel2, + pub ch3: Timer2Channel3, + pub ch4: Timer2Channel4, +} + +impl TimerChannels { + fn new() -> Self { + Self { + ch1: Timer2Channel1 {}, + ch2: Timer2Channel2 {}, + ch3: Timer2Channel3 {}, + ch4: Timer2Channel4 {}, + } + } +} + +/// Representation of CH1 of TIM2. +pub struct Timer2Channel1 {} + +impl Timer2Channel1 { + /// Allow CH1 to generate DMA requests. + pub fn listen_dma(&self) { + let regs = unsafe { &*hal::stm32::TIM2::ptr() }; + regs.dier.modify(|_, w| w.cc1de().set_bit()); + } + + /// Operate CH1 as an output-compare. + /// + /// # Args + /// * `value` - The value to compare the sampling timer's counter against. + pub fn to_output_compare(&self, value: u32) { + let regs = unsafe { &*hal::stm32::TIM2::ptr() }; + assert!(value <= regs.arr.read().bits()); + regs.ccr1.write(|w| w.ccr().bits(value)); + regs.ccmr1_output() + .modify(|_, w| unsafe { w.cc1s().bits(0) }); + } +} + +/// Representation of CH2 of TIM2. +pub struct Timer2Channel2 {} + +impl Timer2Channel2 { + /// Allow CH2 to generate DMA requests. + pub fn listen_dma(&self) { + let regs = unsafe { &*hal::stm32::TIM2::ptr() }; + regs.dier.modify(|_, w| w.cc2de().set_bit()); + } + + /// Operate CH2 as an output-compare. + /// + /// # Args + /// * `value` - The value to compare the sampling timer's counter against. + pub fn to_output_compare(&self, value: u32) { + let regs = unsafe { &*hal::stm32::TIM2::ptr() }; + assert!(value <= regs.arr.read().bits()); + regs.ccr2.write(|w| w.ccr().bits(value)); + regs.ccmr1_output() + .modify(|_, w| unsafe { w.cc2s().bits(0) }); + } +} + +/// Representation of CH3 of TIM2. +pub struct Timer2Channel3 {} + +/// Representation of CH4 of TIM2. +pub struct Timer2Channel4 {} From 91809cf255eff81b86c5c5e530e8a0c9d1ab13fb Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Fri, 13 Nov 2020 10:47:44 +0100 Subject: [PATCH 03/71] Adding DMA support for DAC writes --- Cargo.lock | 2 +- Cargo.toml | 4 +- src/adc.rs | 27 ++-- src/dac.rs | 369 ++++++++++++++++++++++++++++++------------ src/main.rs | 46 +++--- src/sampling_timer.rs | 40 +++++ 6 files changed, 348 insertions(+), 140 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index f247a6c..3c96c4f 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -501,7 +501,7 @@ dependencies = [ [[package]] name = "stm32h7xx-hal" version = "0.8.0" -source = "git+https://github.com/quartiq/stm32h7xx-hal?branch=feature/dma-rtic-example#d8cb6fa5099282665f5e5068a9dcdc9ebaa63240" +source = "git+https://github.com/stm32-rs/stm32h7xx-hal?branch=dma#0bfeeca4ce120c1b7c6d140a7da73a4372b874d8" dependencies = [ "bare-metal 1.0.0", "cast", diff --git a/Cargo.toml b/Cargo.toml index 049e61c..429fe85 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -54,8 +54,8 @@ path = "ad9959" [dependencies.stm32h7xx-hal] features = ["stm32h743v", "rt", "unproven", "ethernet", "quadspi"] -git = "https://github.com/quartiq/stm32h7xx-hal" -branch = "feature/dma-rtic-example" +git = "https://github.com/stm32-rs/stm32h7xx-hal" +branch = "dma" [features] semihosting = ["panic-semihosting", "cortex-m-log/semihosting"] diff --git a/src/adc.rs b/src/adc.rs index d63ebe9..55ed2a8 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -15,12 +15,9 @@ ///! busy-waiting because the transfers should complete at approximately the same time. use super::{ hal, sampling_timer, DMAReq, DmaConfig, MemoryToPeripheral, - PeripheralToMemory, Priority, TargetAddress, Transfer, + PeripheralToMemory, Priority, TargetAddress, Transfer, SAMPLE_BUFFER_SIZE, }; -// The desired ADC input buffer size. This is use configurable. -const INPUT_BUFFER_SIZE: usize = 1; - // The following data is written by the timer ADC sample trigger into each of the SPI TXFIFOs. Note // that because the SPI MOSI line is not connected, this data is dont-care. Data in AXI SRAM is not // initialized on boot, so the contents are random. @@ -32,16 +29,16 @@ static mut SPI_START: [u16; 1] = [0x00]; // processed). Note that the contents of AXI SRAM is uninitialized, so the buffer contents on // startup are undefined. #[link_section = ".axisram.buffers"] -static mut ADC0_BUF0: [u16; INPUT_BUFFER_SIZE] = [0; INPUT_BUFFER_SIZE]; +static mut ADC0_BUF0: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; #[link_section = ".axisram.buffers"] -static mut ADC0_BUF1: [u16; INPUT_BUFFER_SIZE] = [0; INPUT_BUFFER_SIZE]; +static mut ADC0_BUF1: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; #[link_section = ".axisram.buffers"] -static mut ADC1_BUF0: [u16; INPUT_BUFFER_SIZE] = [0; INPUT_BUFFER_SIZE]; +static mut ADC1_BUF0: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; #[link_section = ".axisram.buffers"] -static mut ADC1_BUF1: [u16; INPUT_BUFFER_SIZE] = [0; INPUT_BUFFER_SIZE]; +static mut ADC1_BUF1: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; /// SPI2 is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI2 TX FIFO /// whenever the tim2 update dma request occurs. @@ -110,7 +107,7 @@ impl AdcInputs { /// are returned - one for each ADC sample stream. pub fn transfer_complete_handler( &mut self, - ) -> (&[u16; INPUT_BUFFER_SIZE], &[u16; INPUT_BUFFER_SIZE]) { + ) -> (&[u16; SAMPLE_BUFFER_SIZE], &[u16; SAMPLE_BUFFER_SIZE]) { let adc0_buffer = self.adc0.transfer_complete_handler(); let adc1_buffer = self.adc1.transfer_complete_handler(); (adc0_buffer, adc1_buffer) @@ -119,12 +116,12 @@ impl AdcInputs { /// Represents data associated with ADC0. pub struct Adc0Input { - next_buffer: Option<&'static mut [u16; INPUT_BUFFER_SIZE]>, + next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, transfer: Transfer< hal::dma::dma::Stream1, hal::spi::Spi, PeripheralToMemory, - &'static mut [u16; INPUT_BUFFER_SIZE], + &'static mut [u16; SAMPLE_BUFFER_SIZE], >, _trigger_transfer: Transfer< hal::dma::dma::Stream0, @@ -223,7 +220,7 @@ impl Adc0Input { /// /// # Returns /// A reference to the underlying buffer that has been filled with ADC samples. - pub fn transfer_complete_handler(&mut self) -> &[u16; INPUT_BUFFER_SIZE] { + pub fn transfer_complete_handler(&mut self) -> &[u16; SAMPLE_BUFFER_SIZE] { let next_buffer = self.next_buffer.take().unwrap(); // Wait for the transfer to fully complete before continuing. @@ -241,12 +238,12 @@ impl Adc0Input { /// Represents the data input stream from ADC1 pub struct Adc1Input { - next_buffer: Option<&'static mut [u16; INPUT_BUFFER_SIZE]>, + next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, transfer: Transfer< hal::dma::dma::Stream3, hal::spi::Spi, PeripheralToMemory, - &'static mut [u16; INPUT_BUFFER_SIZE], + &'static mut [u16; SAMPLE_BUFFER_SIZE], >, _trigger_transfer: Transfer< hal::dma::dma::Stream2, @@ -345,7 +342,7 @@ impl Adc1Input { /// /// # Returns /// A reference to the underlying buffer that has been filled with ADC samples. - pub fn transfer_complete_handler(&mut self) -> &[u16; INPUT_BUFFER_SIZE] { + pub fn transfer_complete_handler(&mut self) -> &[u16; SAMPLE_BUFFER_SIZE] { let next_buffer = self.next_buffer.take().unwrap(); // Wait for the transfer to fully complete before continuing. diff --git a/src/dac.rs b/src/dac.rs index 8829385..8057a5a 100644 --- a/src/dac.rs +++ b/src/dac.rs @@ -1,114 +1,279 @@ -///! Stabilizer DAC output control +///! Stabilizer DAC management interface ///! -///! Stabilizer output DACs do not currently rely on DMA requests for generating output. -///! Instead, the DACs utilize an internal queue for storing output codes. A timer then periodically -///! generates an interrupt which triggers an update of the DACs via a write over SPI. -use super::hal; -use heapless::consts; +///! The Stabilizer DAC utilize a DMA channel to generate output updates. A timer channel is +///! configured to generate a DMA write into the SPI TXFIFO, which initiates a SPI transfer and +///! results in DAC update for both channels. +use super::{ + hal, sampling_timer, DMAReq, DmaConfig, MemoryToPeripheral, TargetAddress, + Transfer, SAMPLE_BUFFER_SIZE, +}; -/// Controller structure for managing the DAC outputs. +// The following global buffers are used for the DAC code DMA transfers. Two buffers are used for +// each transfer in a ping-pong buffer configuration (one is being prepared while the other is being +// processed). Note that the contents of AXI SRAM is uninitialized, so the buffer contents on +// startup are undefined. +#[link_section = ".axisram.buffers"] +static mut DAC0_BUF0: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; + +#[link_section = ".axisram.buffers"] +static mut DAC0_BUF1: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; + +#[link_section = ".axisram.buffers"] +static mut DAC1_BUF0: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; + +#[link_section = ".axisram.buffers"] +static mut DAC1_BUF1: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; + +/// SPI4 is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI4 TX FIFO +struct SPI4 {} +impl SPI4 { + pub fn new() -> Self { + Self {} + } +} + +unsafe impl TargetAddress for SPI4 { + /// SPI2 is configured to operate using 16-bit transfer words. + type MemSize = u16; + + /// SPI4 DMA requests are generated whenever TIM2 CH3 comparison occurs. + const REQUEST_LINE: Option = Some(DMAReq::TIM2_CH3 as u8); + + /// Whenever the DMA request occurs, it should write into SPI4's TX FIFO. + fn address(&self) -> u32 { + let regs = unsafe { &*hal::stm32::SPI4::ptr() }; + ®s.txdr as *const _ as u32 + } +} + +/// SPI5 is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI5 TX FIFO +struct SPI5 {} +impl SPI5 { + pub fn new() -> Self { + Self {} + } +} + +unsafe impl TargetAddress for SPI5 { + /// SPI5 is configured to operate using 16-bit transfer words. + type MemSize = u16; + + /// SPI5 DMA requests are generated whenever TIM2 CH4 comparison occurs. + const REQUEST_LINE: Option = Some(DMAReq::TIM2_CH4 as u8); + + /// Whenever the DMA request occurs, it should write into SPI5's TX FIFO + fn address(&self) -> u32 { + let regs = unsafe { &*hal::stm32::SPI5::ptr() }; + ®s.txdr as *const _ as u32 + } +} + +/// Represents both DAC output channels. pub struct DacOutputs { - dac0_spi: hal::spi::Spi, - dac1_spi: hal::spi::Spi, - timer: hal::timer::Timer, - - // The queue is provided a default length of 32 updates, but this queue can be updated by the - // end user to be larger if necessary. - outputs: heapless::spsc::Queue<(u16, u16), consts::U32>, + dac0: Dac0Output, + dac1: Dac1Output, } impl DacOutputs { - /// Construct a new set of DAC output controls - /// - /// # Args - /// * `dac0_spi` - The SPI interface to the DAC0 output. - /// * `dac1_spi` - The SPI interface to the DAC1 output. - /// * `timer` - The timer used to generate periodic events for updating the DACs. - pub fn new( - mut dac0_spi: hal::spi::Spi, - mut dac1_spi: hal::spi::Spi, - mut timer: hal::timer::Timer, - ) -> Self { - // Start the DAC SPI interfaces in infinite transaction mode. CS is configured in - // auto-suspend mode. - dac0_spi.inner().cr1.modify(|_, w| w.cstart().started()); - dac1_spi.inner().cr1.modify(|_, w| w.cstart().started()); - - dac0_spi.listen(hal::spi::Event::Error); - dac1_spi.listen(hal::spi::Event::Error); - - // Stop the timer and begin listening for timeouts. Timeouts will be used as a means to - // generate new DAC outputs. - timer.pause(); - timer.reset_counter(); - timer.clear_irq(); - timer.listen(hal::timer::Event::TimeOut); - - Self { - dac0_spi, - dac1_spi, - outputs: heapless::spsc::Queue::new(), - timer, - } + /// Construct the DAC outputs. + pub fn new(dac0: Dac0Output, dac1: Dac1Output) -> Self { + Self { dac0, dac1 } } - /// Push a set of new DAC output codes to the internal queue. - /// - /// # Note - /// The earlier DAC output codes will be generated within 1 update cycle of the codes. This is a - /// fixed latency currently. - /// - /// This function will panic if too many codes are written. + /// Enqueue the next DAC output codes for transmission. /// /// # Args - /// * `dac0_value` - The value to enqueue for a DAC0 update. - /// * `dac1_value` - The value to enqueue for a DAC1 update. - pub fn push(&mut self, dac0_value: u16, dac1_value: u16) { - self.outputs.enqueue((dac0_value, dac1_value)).unwrap(); - self.timer.resume(); - } - - /// Update the DAC codes with the next set of values in the internal queue. - /// - /// # Note - /// This is intended to be called from the TIM3 update ISR. - /// - /// If the last value in the queue is used, the timer is stopped. - pub fn update(&mut self) { - self.timer.clear_irq(); - match self.outputs.dequeue() { - Some((dac0, dac1)) => self.write(dac0, dac1), - None => { - self.timer.pause(); - self.timer.reset_counter(); - self.timer.clear_irq(); - } - }; - } - - /// Write immediate values to the DAC outputs. - /// - /// # Note - /// The DACs will be updated as soon as the SPI transfer completes, which will be nominally - /// 320nS after this function call. - /// - /// # Args - /// * `dac0_value` - The output code to write to DAC0. - /// * `dac1_value` - The output code to write to DAC1. - pub fn write(&mut self, dac0_value: u16, dac1_value: u16) { - // In order to optimize throughput and minimize latency, the DAC codes are written directly - // into the SPI TX FIFO. No error checking is conducted. Errors are handled via interrupts - // instead. - unsafe { - core::ptr::write_volatile( - &self.dac0_spi.inner().txdr as *const _ as *mut u16, - dac0_value, - ); - - core::ptr::write_volatile( - &self.dac1_spi.inner().txdr as *const _ as *mut u16, - dac1_value, - ); - } + /// * `dac0_codes` - The output codes for DAC0 to enqueue. + /// * `dac1_codes` - The output codes for DAC1 to enqueue. + pub fn next_data( + &mut self, + dac0_codes: &[u16; SAMPLE_BUFFER_SIZE], + dac1_codes: &[u16; SAMPLE_BUFFER_SIZE], + ) { + self.dac0.next_data(dac0_codes); + self.dac1.next_data(dac1_codes); + } +} + +/// Represents data associated with DAC0. +pub struct Dac0Output { + next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, + _spi: hal::spi::Spi, + transfer: Transfer< + hal::dma::dma::Stream4, + SPI4, + MemoryToPeripheral, + &'static mut [u16; SAMPLE_BUFFER_SIZE], + >, + first_transfer: bool, +} + +impl Dac0Output { + /// Construct the DAC0 output channel. + /// + /// # Args + /// * `spi` - The SPI interface used to communicate with the ADC. + /// * `stream` - The DMA stream used to write DAC codes over SPI. + /// * `trigger_channel` - The sampling timer output compare channel for update triggers. + pub fn new( + spi: hal::spi::Spi, + stream: hal::dma::dma::Stream4, + trigger_channel: sampling_timer::Timer2Channel3, + ) -> Self { + // Generate DMA events when an output compare of the timer hitting zero (timer roll over) + // occurs. + trigger_channel.listen_dma(); + trigger_channel.to_output_compare(0); + + // The stream constantly writes to the TX FIFO to write new update codes. + let trigger_config = DmaConfig::default() + .memory_increment(true) + .peripheral_increment(false); + + // Construct the trigger stream to write from memory to the peripheral. + let transfer: Transfer<_, _, MemoryToPeripheral, _> = Transfer::init( + stream, + SPI4::new(), + unsafe { &mut DAC0_BUF0 }, + None, + trigger_config, + ); + + // Listen for any potential SPI error signals, which may indicate that we are not generating + // update codes. + let mut spi = spi.disable(); + spi.listen(hal::spi::Event::Error); + + // Allow the SPI FIFOs to operate using only DMA data channels. + spi.enable_dma_tx(); + + // Enable SPI and start it in infinite transaction mode. + spi.inner().cr1.modify(|_, w| w.spe().set_bit()); + spi.inner().cr1.modify(|_, w| w.cstart().started()); + + Self { + transfer, + next_buffer: unsafe { Some(&mut DAC0_BUF1) }, + _spi: spi, + first_transfer: true, + } + } + + /// Schedule the next set of DAC update codes. + /// + /// # Args + /// * `data` - The next samples to enqueue for transmission. + pub fn next_data(&mut self, data: &[u16; SAMPLE_BUFFER_SIZE]) { + let next_buffer = self.next_buffer.take().unwrap(); + + // Copy data into the next buffer + next_buffer.copy_from_slice(data); + + // If the last transfer was not complete, we didn't write all our previous DAC codes. + // Wait for all the DAC codes to get written as well. + if self.first_transfer { + self.first_transfer = false + } else { + while self.transfer.get_transfer_complete_flag() == false {} + } + + // Start the next transfer. + self.transfer.clear_interrupts(); + let (prev_buffer, _) = + self.transfer.next_transfer(next_buffer).unwrap(); + + self.next_buffer.replace(prev_buffer); + } +} + +/// Represents the data output stream from DAC1. +pub struct Dac1Output { + next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, + _spi: hal::spi::Spi, + transfer: Transfer< + hal::dma::dma::Stream5, + SPI5, + MemoryToPeripheral, + &'static mut [u16; SAMPLE_BUFFER_SIZE], + >, + first_transfer: bool, +} + +impl Dac1Output { + /// Construct a new DAC1 output data stream. + /// + /// # Args + /// * `spi` - The SPI interface connected to DAC1. + /// * `stream` - The DMA stream used to write DAC codes the SPI TX FIFO. + /// * `trigger_channel` - The timer channel used to generate DMA requests for DAC updates. + pub fn new( + spi: hal::spi::Spi, + stream: hal::dma::dma::Stream5, + trigger_channel: sampling_timer::Timer2Channel4, + ) -> Self { + // Generate DMA events when an output compare of the timer hitting zero (timer roll over) + // occurs. + trigger_channel.listen_dma(); + trigger_channel.to_output_compare(0); + + // The trigger stream constantly writes to the TX FIFO to generate DAC updates. + let trigger_config = DmaConfig::default() + .memory_increment(true) + .peripheral_increment(false) + .circular_buffer(true); + + // Construct the stream to write from memory to the peripheral. + let transfer: Transfer<_, _, MemoryToPeripheral, _> = Transfer::init( + stream, + SPI5::new(), + unsafe { &mut DAC1_BUF0 }, + None, + trigger_config, + ); + + // Listen for any SPI errors, as this may indicate that we are not generating updates on the + // DAC. + let mut spi = spi.disable(); + spi.listen(hal::spi::Event::Error); + + // Allow the SPI FIFOs to operate using only DMA data channels. + spi.enable_dma_tx(); + + // Enable SPI and start it in infinite transaction mode. + spi.inner().cr1.modify(|_, w| w.spe().set_bit()); + spi.inner().cr1.modify(|_, w| w.cstart().started()); + + Self { + next_buffer: unsafe { Some(&mut DAC1_BUF1) }, + transfer, + _spi: spi, + first_transfer: true, + } + } + + /// Enqueue the next buffer for transmission to the DAC. + /// + /// # Args + /// * `data` - The next data to write to the DAC. + pub fn next_data(&mut self, data: &[u16; SAMPLE_BUFFER_SIZE]) { + let next_buffer = self.next_buffer.take().unwrap(); + + // Copy data into the next buffer + next_buffer.copy_from_slice(data); + + // If the last transfer was not complete, we didn't write all our previous DAC codes. + // Wait for all the DAC codes to get written as well. + if self.first_transfer { + self.first_transfer = false + } else { + while self.transfer.get_transfer_complete_flag() == false {} + } + + // Start the next transfer. + self.transfer.clear_interrupts(); + let (prev_buffer, _) = + self.transfer.next_transfer(next_buffer).unwrap(); + + self.next_buffer.replace(prev_buffer); } } diff --git a/src/main.rs b/src/main.rs index 59b19fc..6060d4f 100644 --- a/src/main.rs +++ b/src/main.rs @@ -51,8 +51,12 @@ use smoltcp::wire::Ipv4Address; use heapless::{consts::*, String}; +// The desired sampling frequency of the ADCs. const SAMPLE_FREQUENCY_KHZ: u32 = 500; +// The desired ADC sample processing buffer size. +const SAMPLE_BUFFER_SIZE: usize = 1; + #[link_section = ".sram3.eth"] static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new(); @@ -66,7 +70,7 @@ mod sampling_timer; mod server; use adc::{Adc0Input, Adc1Input, AdcInputs}; -use dac::DacOutputs; +use dac::{Dac0Output, Dac1Output, DacOutputs}; #[cfg(not(feature = "semihosting"))] fn init_log() {} @@ -426,13 +430,17 @@ const APP: () = { ) }; - let timer = dp.TIM3.timer( - SAMPLE_FREQUENCY_KHZ.khz(), - ccdr.peripheral.TIM3, - &ccdr.clocks, + let dac0 = Dac0Output::new( + dac0_spi, + dma_streams.4, + sampling_timer_channels.ch3, ); - - DacOutputs::new(dac0_spi, dac1_spi, timer) + let dac1 = Dac1Output::new( + dac1_spi, + dma_streams.5, + sampling_timer_channels.ch4, + ); + DacOutputs::new(dac0, dac1) }; let mut fp_led_0 = gpiod.pd5.into_push_pull_output(); @@ -735,35 +743,33 @@ const APP: () = { } } - #[task(binds = TIM3, resources=[dacs], priority = 3)] - fn dac_update(c: dac_update::Context) { - c.resources.dacs.update(); - } - #[task(binds=DMA1_STR3, resources=[adcs, dacs, iir_state, iir_ch], priority=2)] - fn adc_update(mut c: adc_update::Context) { + fn adc_update(c: adc_update::Context) { let (adc0_samples, adc1_samples) = c.resources.adcs.transfer_complete_handler(); - for (adc0, adc1) in adc0_samples.iter().zip(adc1_samples.iter()) { - let result_adc0 = { + let mut dac0: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; + let mut dac1: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; + + for (i, (adc0, adc1)) in + adc0_samples.iter().zip(adc1_samples.iter()).enumerate() + { + dac0[i] = { let x0 = f32::from(*adc0 as i16); let y0 = c.resources.iir_ch[0] .update(&mut c.resources.iir_state[0], x0); y0 as i16 as u16 ^ 0x8000 }; - let result_adc1 = { + dac1[i] = { let x1 = f32::from(*adc1 as i16); let y1 = c.resources.iir_ch[1] .update(&mut c.resources.iir_state[1], x1); y1 as i16 as u16 ^ 0x8000 }; - - c.resources - .dacs - .lock(|dacs| dacs.push(result_adc0, result_adc1)); } + + c.resources.dacs.next_data(&dac0, &dac1); } #[idle(resources=[net_interface, pounder, mac_addr, eth_mac, iir_state, iir_ch, afe0, afe1])] diff --git a/src/sampling_timer.rs b/src/sampling_timer.rs index b412b75..7b3b557 100644 --- a/src/sampling_timer.rs +++ b/src/sampling_timer.rs @@ -103,5 +103,45 @@ impl Timer2Channel2 { /// Representation of CH3 of TIM2. pub struct Timer2Channel3 {} +impl Timer2Channel3 { + /// Allow CH4 to generate DMA requests. + pub fn listen_dma(&self) { + let regs = unsafe { &*hal::stm32::TIM2::ptr() }; + regs.dier.modify(|_, w| w.cc3de().set_bit()); + } + + /// Operate CH2 as an output-compare. + /// + /// # Args + /// * `value` - The value to compare the sampling timer's counter against. + pub fn to_output_compare(&self, value: u32) { + let regs = unsafe { &*hal::stm32::TIM2::ptr() }; + assert!(value <= regs.arr.read().bits()); + regs.ccr3.write(|w| w.ccr().bits(value)); + regs.ccmr2_output() + .modify(|_, w| unsafe { w.cc3s().bits(0) }); + } +} + /// Representation of CH4 of TIM2. pub struct Timer2Channel4 {} + +impl Timer2Channel4 { + /// Allow CH4 to generate DMA requests. + pub fn listen_dma(&self) { + let regs = unsafe { &*hal::stm32::TIM2::ptr() }; + regs.dier.modify(|_, w| w.cc4de().set_bit()); + } + + /// Operate CH2 as an output-compare. + /// + /// # Args + /// * `value` - The value to compare the sampling timer's counter against. + pub fn to_output_compare(&self, value: u32) { + let regs = unsafe { &*hal::stm32::TIM2::ptr() }; + assert!(value <= regs.arr.read().bits()); + regs.ccr4.write(|w| w.ccr().bits(value)); + regs.ccmr2_output() + .modify(|_, w| unsafe { w.cc4s().bits(0) }); + } +} From 04a0462aad49ac5e0e6393516d26e4ebc87fb1c3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Mon, 16 Nov 2020 17:33:14 +0100 Subject: [PATCH 04/71] README: add matrix badge --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index d111f63..cafbb78 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,5 @@ ![Continuous Integration](https://github.com/quartiq/stabilizer/workflows/Continuous%20Integration/badge.svg) +![QUARTIQ Matrix Chat](https://img.shields.io/matrix/quartiq:matrix.org?style=plastic) # Stabilizer Firmware From a7b6b5c796c09c15927e98c8553d0e80c2593d3a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Mon, 16 Nov 2020 17:36:16 +0100 Subject: [PATCH 05/71] README: remove CI badge, add matrix link --- README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/README.md b/README.md index cafbb78..cf231c0 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,4 @@ -![Continuous Integration](https://github.com/quartiq/stabilizer/workflows/Continuous%20Integration/badge.svg) -![QUARTIQ Matrix Chat](https://img.shields.io/matrix/quartiq:matrix.org?style=plastic) +[![QUARTIQ Matrix Chat](https://img.shields.io/matrix/quartiq:matrix.org)](https://matrix.to/#/#quartiq:matrix.org) # Stabilizer Firmware From 3eb43c6b99107486349acf2ad35e1a6a9ee79267 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sun, 22 Nov 2020 07:45:32 -0800 Subject: [PATCH 06/71] move iir to new dsp crate --- .gitignore | 2 +- Cargo.lock | 8 ++++++ Cargo.toml | 1 + dsp/Cargo.lock | 63 +++++++++++++++++++++++++++++++++++++++++ dsp/Cargo.toml | 10 +++++++ {src => dsp/src}/iir.rs | 0 dsp/src/lib.rs | 3 ++ src/main.rs | 3 +- 8 files changed, 88 insertions(+), 2 deletions(-) create mode 100644 dsp/Cargo.lock create mode 100644 dsp/Cargo.toml rename {src => dsp/src}/iir.rs (100%) create mode 100644 dsp/src/lib.rs diff --git a/.gitignore b/.gitignore index 265b7f5..98097d7 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,2 @@ -/target +**/target .gdb_history diff --git a/Cargo.lock b/Cargo.lock index 014b427..f3ace36 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -185,6 +185,13 @@ dependencies = [ "cortex-m", ] +[[package]] +name = "dsp" +version = "0.1.0" +dependencies = [ + "serde", +] + [[package]] name = "embedded-hal" version = "0.2.4" @@ -457,6 +464,7 @@ dependencies = [ "cortex-m-log", "cortex-m-rt", "cortex-m-rtic", + "dsp", "embedded-hal", "enum-iterator", "heapless", diff --git a/Cargo.toml b/Cargo.toml index a57745e..886f041 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -40,6 +40,7 @@ embedded-hal = "0.2.4" nb = "1.0.0" asm-delay = "0.9.0" enum-iterator = "0.6.0" +dsp = { path = "dsp", version = "0.1.0" } [dependencies.mcp23017] git = "https://github.com/mrd0ll4r/mcp23017.git" diff --git a/dsp/Cargo.lock b/dsp/Cargo.lock new file mode 100644 index 0000000..afad0c4 --- /dev/null +++ b/dsp/Cargo.lock @@ -0,0 +1,63 @@ +# This file is automatically @generated by Cargo. +# It is not intended for manual editing. +[[package]] +name = "dsp" +version = "0.1.0" +dependencies = [ + "serde", +] + +[[package]] +name = "proc-macro2" +version = "1.0.24" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1e0704ee1a7e00d7bb417d0770ea303c1bccbabf0ef1667dae92b5967f5f8a71" +dependencies = [ + "unicode-xid", +] + +[[package]] +name = "quote" +version = "1.0.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "aa563d17ecb180e500da1cfd2b028310ac758de548efdd203e18f283af693f37" +dependencies = [ + "proc-macro2", +] + +[[package]] +name = "serde" +version = "1.0.117" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b88fa983de7720629c9387e9f517353ed404164b1e482c970a90c1a4aaf7dc1a" +dependencies = [ + "serde_derive", +] + +[[package]] +name = "serde_derive" +version = "1.0.117" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cbd1ae72adb44aab48f325a02444a5fc079349a8d804c1fc922aed3f7454c74e" +dependencies = [ + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "syn" +version = "1.0.50" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "443b4178719c5a851e1bde36ce12da21d74a0e60b4d982ec3385a933c812f0f6" +dependencies = [ + "proc-macro2", + "quote", + "unicode-xid", +] + +[[package]] +name = "unicode-xid" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f7fe0bb3479651439c9112f72b6c505038574c9fbb575ed1bf3b797fa39dd564" diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml new file mode 100644 index 0000000..a9b7fe5 --- /dev/null +++ b/dsp/Cargo.toml @@ -0,0 +1,10 @@ +[package] +name = "dsp" +version = "0.1.0" +authors = ["Robert Jördens "] +edition = "2018" + +# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html + +[dependencies] +serde = { version = "1.0", features = ["derive"], default-features = false } diff --git a/src/iir.rs b/dsp/src/iir.rs similarity index 100% rename from src/iir.rs rename to dsp/src/iir.rs diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs new file mode 100644 index 0000000..3c44bbc --- /dev/null +++ b/dsp/src/lib.rs @@ -0,0 +1,3 @@ +#![no_std] + +pub mod iir; diff --git a/src/main.rs b/src/main.rs index a7d3d73..4c945e5 100644 --- a/src/main.rs +++ b/src/main.rs @@ -54,10 +54,11 @@ static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new(); mod afe; mod eeprom; -mod iir; mod pounder; mod server; +use dsp::iir; + #[cfg(not(feature = "semihosting"))] fn init_log() {} From d24dfb302e2770022d99a273af6c4862df2c0feb Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sun, 22 Nov 2020 10:32:40 -0800 Subject: [PATCH 07/71] dsp crate: drop version dependency and remove boilerplate from cargo --- .gitignore | 2 +- Cargo.toml | 2 +- dsp/Cargo.toml | 2 -- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/.gitignore b/.gitignore index 98097d7..265b7f5 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,2 @@ -**/target +/target .gdb_history diff --git a/Cargo.toml b/Cargo.toml index 886f041..7aad310 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -40,7 +40,7 @@ embedded-hal = "0.2.4" nb = "1.0.0" asm-delay = "0.9.0" enum-iterator = "0.6.0" -dsp = { path = "dsp", version = "0.1.0" } +dsp = { path = "dsp" } [dependencies.mcp23017] git = "https://github.com/mrd0ll4r/mcp23017.git" diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml index a9b7fe5..625d0f0 100644 --- a/dsp/Cargo.toml +++ b/dsp/Cargo.toml @@ -4,7 +4,5 @@ version = "0.1.0" authors = ["Robert Jördens "] edition = "2018" -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html - [dependencies] serde = { version = "1.0", features = ["derive"], default-features = false } From 6808d32e0fcb3cc02c92813bfa74d09dcfe8f2e8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sun, 22 Nov 2020 22:58:17 +0100 Subject: [PATCH 08/71] iir: document --- dsp/src/iir.rs | 112 +++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 90 insertions(+), 22 deletions(-) diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index 0c34306..fac1c4c 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -3,15 +3,10 @@ use serde::{Deserialize, Serialize}; use core::f32; -pub type IIRState = [f32; 5]; - -#[derive(Copy, Clone, Deserialize, Serialize)] -pub struct IIR { - pub ba: IIRState, - pub y_offset: f32, - pub y_min: f32, - pub y_max: f32, -} +// These are implemented here because core::f32 doesn't have them (yet). +// They are naive and don't handle inf/nan. +// `compiler-intrinsics`/llvm should have better (robust, universal, and +// faster) implementations. fn abs(x: f32) -> f32 { if x >= 0. { @@ -45,17 +40,72 @@ fn min(x: f32, y: f32) -> f32 { } } +// Multiply-accumulate vectors `x` and `a`. +// +// A.k.a. dot product. +// Rust/LLVM optimize this nicely. fn macc(y0: T, x: &[T], a: &[T]) -> T where T: Add + Mul + Copy, { x.iter() - .zip(a.iter()) - .map(|(&i, &j)| i * j) + .zip(a) + .map(|(&x, &a)| x * a) .fold(y0, |y, xa| y + xa) } +/// IIR state and coefficients type. +/// +/// To represent the IIR state (input and output memory) during the filter update +/// this contains the three inputs (x0, x1, x2) and the two outputs (y1, y2) +/// concatenated. +/// To represent the IIR coefficients, this contains the feed-forward +/// coefficients (b0, b1, b2) followd by the feed-back coefficients (a1, a2), +/// all normalized such that a0 = 1. +pub type IIRState = [f32; 5]; + +/// IIR configuration. +/// +/// Contains the coeeficients `ba`, the output offset `y_offset`, and the +/// output limits `y_min` and `y_max`. +/// +/// This implementation achieves several important properties: +/// +/// * Its transfer function is universal in the sense that any biquadratic +/// transfer function can be implemented (high-passes, gain limits, second +/// order integrators with inherent anti-windup, notches etc) without code +/// changes preserving all features. +/// * It inherits a universal implementation of "integrator anti-windup", also +/// and especially in the presence of set-point changes and in the presence +/// of proportional or derivative gain without any back-off that would reduce +/// steady-state output range. +/// * It has universal derivative-kick (undesired, unlimited, and un-physical +/// amplification of set-point changes by the derivative term) avoidance. +/// * An offset at the input of an IIR filter (a.k.a. "set-point") is +/// equivalent to an offset at the output. They are related by the +/// overall (DC feed-forward) gain of the filter. +/// * It stores only previous outputs and inputs. These have direct and +/// invariant interpretation (independent of gains and offsets). +/// Therefore it can trivially implement bump-less transfer. +/// * Cascading multiple IIR filters allows stable and robust +/// implementation of transfer functions beyond bequadratic terms. +#[derive(Copy, Clone, Deserialize, Serialize)] +pub struct IIR { + pub ba: IIRState, + pub y_offset: f32, + pub y_min: f32, + pub y_max: f32, +} + impl IIR { + /// Configures IIR filter coefficients for proportional-integral behavior + /// with gain limit. + /// + /// # Arguments + /// + /// * `kp` - Proportional gain. Also defines gain sign. + /// * `ki` - Integral gain at Nyquist. Sign taken from `kp`. + /// * `g` - Gain limit. pub fn set_pi(&mut self, kp: f32, ki: f32, g: f32) -> Result<(), &str> { let ki = copysign(ki, kp); let g = copysign(g, kp); @@ -75,33 +125,51 @@ impl IIR { } (a1, b0, b1) }; - self.ba[0] = b0; - self.ba[1] = b1; - self.ba[2] = 0.; - self.ba[3] = a1; - self.ba[4] = 0.; + self.ba.copy_from_slice(&[b0, b1, 0., a1, 0.]); Ok(()) } + /// Compute the overall (DC feed-forward) gain. + pub fn get_k(&self) -> f32 { + self.ba[..3].iter().sum() + } + + /// Compute input-referred (`x`) offset from output (`y`) offset. pub fn get_x_offset(&self) -> Result { - let b: f32 = self.ba[..3].iter().sum(); - if abs(b) < f32::EPSILON { - Err("b is zero") + let k = self.get_k(); + if abs(k) < f32::EPSILON { + Err("k is zero") } else { - Ok(self.y_offset / b) + Ok(self.y_offset / k) } } + /// Convert input (`x`) offset to equivalent output (`y`) offset and apply. + /// + /// # Arguments + /// * `xo`: Input (`x`) offset. pub fn set_x_offset(&mut self, xo: f32) { - let b: f32 = self.ba[..3].iter().sum(); - self.y_offset = xo * b; + self.y_offset = xo * self.get_k(); } + /// Feed a new input value into the filter, update the filter state, and + /// return the new output. Only the state `xy` is modified. + /// + /// # Arguments + /// * `xy` - Current filter state. + /// * `x0` - New input. pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 { + // `xy` contains x0 x1 y0 y1 y2 + // Increment time x1 x2 y1 y2 y3 + // Rotate y3 x1 x2 y1 y2 xy.rotate_right(1); + // Store x0 x0 x1 x2 y1 y2 xy[0] = x0; + // Compute y0 by multiply-accumulate let y0 = macc(self.y_offset, xy, &self.ba); + // Limit y0 let y0 = max(self.y_min, min(self.y_max, y0)); + // Store y0 x0 x1 y0 y1 y2 xy[xy.len() / 2] = y0; y0 } From 11e6688a14adccb955f014749503a6a832b4b643 Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Mon, 23 Nov 2020 14:30:29 +0100 Subject: [PATCH 09/71] Refactoring timer channels to macros, adding safety notes --- Cargo.lock | 1 + Cargo.toml | 1 + src/adc.rs | 45 +++++++--- src/dac.rs | 38 +++++--- src/sampling_timer.rs | 198 ++++++++++++++++++------------------------ 5 files changed, 150 insertions(+), 133 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 3c96c4f..47e2086 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -474,6 +474,7 @@ dependencies = [ "nb 1.0.0", "panic-halt", "panic-semihosting", + "paste", "serde", "serde-json-core", "smoltcp", diff --git a/Cargo.toml b/Cargo.toml index 429fe85..44f2d02 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -40,6 +40,7 @@ embedded-hal = "0.2.4" nb = "1.0.0" asm-delay = "0.9.0" enum-iterator = "0.6.0" +paste = "1" [dependencies.mcp23017] git = "https://github.com/mrd0ll4r/mcp23017.git" diff --git a/src/adc.rs b/src/adc.rs index 55ed2a8..9b2c53b 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -42,13 +42,18 @@ static mut ADC1_BUF1: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; /// SPI2 is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI2 TX FIFO /// whenever the tim2 update dma request occurs. -struct SPI2 {} +struct SPI2 { + _channel: sampling_timer::tim2::Channel1, +} impl SPI2 { - pub fn new() -> Self { - Self {} + pub fn new(_channel: sampling_timer::tim2::Channel1) -> Self { + Self { _channel } } } +// Note(unsafe): This structure is only safe to instantiate once. The DMA request is hard-coded and +// may only be used if ownership of the timer2 channel 1 compare channel is assured, which is +// ensured by maintaining ownership of the channel. unsafe impl TargetAddress for SPI2 { /// SPI2 is configured to operate using 16-bit transfer words. type MemSize = u16; @@ -59,6 +64,8 @@ unsafe impl TargetAddress for SPI2 { /// Whenever the DMA request occurs, it should write into SPI2's TX FIFO to start a DMA /// transfer. fn address(&self) -> u32 { + // Note(unsafe): It is assumed that SPI2 is owned by another DMA transfer and this DMA is + // only used for the transmit-half of DMA. let regs = unsafe { &*hal::stm32::SPI2::ptr() }; ®s.txdr as *const _ as u32 } @@ -66,13 +73,18 @@ unsafe impl TargetAddress for SPI2 { /// SPI3 is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI3 TX FIFO /// whenever the tim2 update dma request occurs. -struct SPI3 {} +struct SPI3 { + _channel: sampling_timer::tim2::Channel2, +} impl SPI3 { - pub fn new() -> Self { - Self {} + pub fn new(_channel: sampling_timer::tim2::Channel2) -> Self { + Self { _channel } } } +// Note(unsafe): This structure is only safe to instantiate once. The DMA request is hard-coded and +// may only be used if ownership of the timer2 channel 2 compare channel is assured, which is +// ensured by maintaining ownership of the channel. unsafe impl TargetAddress for SPI3 { /// SPI3 is configured to operate using 16-bit transfer words. type MemSize = u16; @@ -83,6 +95,8 @@ unsafe impl TargetAddress for SPI3 { /// Whenever the DMA request occurs, it should write into SPI3's TX FIFO to start a DMA /// transfer. fn address(&self) -> u32 { + // Note(unsafe): It is assumed that SPI3 is owned by another DMA transfer and this DMA is + // only used for the transmit-half of DMA. let regs = unsafe { &*hal::stm32::SPI3::ptr() }; ®s.txdr as *const _ as u32 } @@ -144,7 +158,7 @@ impl Adc0Input { spi: hal::spi::Spi, trigger_stream: hal::dma::dma::Stream0, data_stream: hal::dma::dma::Stream1, - trigger_channel: sampling_timer::Timer2Channel1, + trigger_channel: sampling_timer::tim2::Channel1, ) -> Self { // Generate DMA events when an output compare of the timer hitting zero (timer roll over) // occurs. @@ -164,7 +178,10 @@ impl Adc0Input { let mut trigger_transfer: Transfer<_, _, MemoryToPeripheral, _> = Transfer::init( trigger_stream, - SPI2::new(), + SPI2::new(trigger_channel), + // Note(unsafe): Because this is a Memory->Peripheral transfer, this data is never + // actually modified. It technically only needs to be immutably borrowed, but the + // current HAL API only supports mutable borrows. unsafe { &mut SPI_START }, None, trigger_config, @@ -192,6 +209,8 @@ impl Adc0Input { Transfer::init( data_stream, spi, + // Note(unsafe): The ADC0_BUF0 is "owned" by this peripheral. It shall not be used + // anywhere else in the module. unsafe { &mut ADC0_BUF0 }, None, data_config, @@ -210,6 +229,8 @@ impl Adc0Input { trigger_transfer.start(|_| {}); Self { + // Note(unsafe): The ADC0_BUF1 is "owned" by this peripheral. It shall not be used + // anywhere else in the module. next_buffer: unsafe { Some(&mut ADC0_BUF1) }, transfer: data_transfer, _trigger_transfer: trigger_transfer, @@ -265,7 +286,7 @@ impl Adc1Input { spi: hal::spi::Spi, trigger_stream: hal::dma::dma::Stream2, data_stream: hal::dma::dma::Stream3, - trigger_channel: sampling_timer::Timer2Channel2, + trigger_channel: sampling_timer::tim2::Channel2, ) -> Self { // Generate DMA events when an output compare of the timer hitting zero (timer roll over) // occurs. @@ -285,7 +306,7 @@ impl Adc1Input { let mut trigger_transfer: Transfer<_, _, MemoryToPeripheral, _> = Transfer::init( trigger_stream, - SPI3::new(), + SPI3::new(trigger_channel), unsafe { &mut SPI_START }, None, trigger_config, @@ -314,6 +335,8 @@ impl Adc1Input { Transfer::init( data_stream, spi, + // Note(unsafe): The ADC1_BUF0 is "owned" by this peripheral. It shall not be used + // anywhere else in the module. unsafe { &mut ADC1_BUF0 }, None, data_config, @@ -332,6 +355,8 @@ impl Adc1Input { trigger_transfer.start(|_| {}); Self { + // Note(unsafe): The ADC1_BUF1 is "owned" by this peripheral. It shall not be used + // anywhere else in the module. next_buffer: unsafe { Some(&mut ADC1_BUF1) }, transfer: data_transfer, _trigger_transfer: trigger_transfer, diff --git a/src/dac.rs b/src/dac.rs index 8057a5a..d7032db 100644 --- a/src/dac.rs +++ b/src/dac.rs @@ -25,13 +25,18 @@ static mut DAC1_BUF0: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; static mut DAC1_BUF1: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; /// SPI4 is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI4 TX FIFO -struct SPI4 {} +struct SPI4 { + _channel: sampling_timer::tim2::Channel3, +} impl SPI4 { - pub fn new() -> Self { - Self {} + pub fn new(_channel: sampling_timer::tim2::Channel3) -> Self { + Self { _channel } } } +// Note(unsafe): This is safe because the DMA request line is logically owned by this module. +// Additionally, it is only safe if the SPI TX functionality is never used, which is managed by the +// Dac0Output. unsafe impl TargetAddress for SPI4 { /// SPI2 is configured to operate using 16-bit transfer words. type MemSize = u16; @@ -41,19 +46,25 @@ unsafe impl TargetAddress for SPI4 { /// Whenever the DMA request occurs, it should write into SPI4's TX FIFO. fn address(&self) -> u32 { + // Note(unsafe): This is only safe as long as no other users write to the SPI TX FIFO. let regs = unsafe { &*hal::stm32::SPI4::ptr() }; ®s.txdr as *const _ as u32 } } /// SPI5 is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI5 TX FIFO -struct SPI5 {} +struct SPI5 { + _channel: sampling_timer::tim2::Channel4, +} impl SPI5 { - pub fn new() -> Self { - Self {} + pub fn new(_channel: sampling_timer::tim2::Channel4) -> Self { + Self { _channel } } } +// Note(unsafe): This is safe because the DMA request line is logically owned by this module. +// Additionally, it is only safe if the SPI TX functionality is never used, which is managed by the +// Dac1Output. unsafe impl TargetAddress for SPI5 { /// SPI5 is configured to operate using 16-bit transfer words. type MemSize = u16; @@ -63,6 +74,7 @@ unsafe impl TargetAddress for SPI5 { /// Whenever the DMA request occurs, it should write into SPI5's TX FIFO fn address(&self) -> u32 { + // Note(unsafe): This is only safe as long as no other users write to the SPI TX FIFO. let regs = unsafe { &*hal::stm32::SPI5::ptr() }; ®s.txdr as *const _ as u32 } @@ -98,6 +110,7 @@ impl DacOutputs { /// Represents data associated with DAC0. pub struct Dac0Output { next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, + // Note: SPI TX functionality may not be used from this structure to ensure safety with DMA. _spi: hal::spi::Spi, transfer: Transfer< hal::dma::dma::Stream4, @@ -118,7 +131,7 @@ impl Dac0Output { pub fn new( spi: hal::spi::Spi, stream: hal::dma::dma::Stream4, - trigger_channel: sampling_timer::Timer2Channel3, + trigger_channel: sampling_timer::tim2::Channel3, ) -> Self { // Generate DMA events when an output compare of the timer hitting zero (timer roll over) // occurs. @@ -133,7 +146,8 @@ impl Dac0Output { // Construct the trigger stream to write from memory to the peripheral. let transfer: Transfer<_, _, MemoryToPeripheral, _> = Transfer::init( stream, - SPI4::new(), + SPI4::new(trigger_channel), + // Note(unsafe): This buffer is only used once and provided for the DMA transfer. unsafe { &mut DAC0_BUF0 }, None, trigger_config, @@ -153,6 +167,7 @@ impl Dac0Output { Self { transfer, + // Note(unsafe): This buffer is only used once and provided for the next DMA transfer. next_buffer: unsafe { Some(&mut DAC0_BUF1) }, _spi: spi, first_transfer: true, @@ -189,6 +204,7 @@ impl Dac0Output { /// Represents the data output stream from DAC1. pub struct Dac1Output { next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, + // Note: SPI TX functionality may not be used from this structure to ensure safety with DMA. _spi: hal::spi::Spi, transfer: Transfer< hal::dma::dma::Stream5, @@ -209,7 +225,7 @@ impl Dac1Output { pub fn new( spi: hal::spi::Spi, stream: hal::dma::dma::Stream5, - trigger_channel: sampling_timer::Timer2Channel4, + trigger_channel: sampling_timer::tim2::Channel4, ) -> Self { // Generate DMA events when an output compare of the timer hitting zero (timer roll over) // occurs. @@ -225,7 +241,8 @@ impl Dac1Output { // Construct the stream to write from memory to the peripheral. let transfer: Transfer<_, _, MemoryToPeripheral, _> = Transfer::init( stream, - SPI5::new(), + SPI5::new(trigger_channel), + // Note(unsafe): This buffer is only used once and provided to the transfer. unsafe { &mut DAC1_BUF0 }, None, trigger_config, @@ -244,6 +261,7 @@ impl Dac1Output { spi.inner().cr1.modify(|_, w| w.cstart().started()); Self { + // Note(unsafe): This buffer is only used once and provided for the next DMA transfer. next_buffer: unsafe { Some(&mut DAC1_BUF1) }, transfer, _spi: spi, diff --git a/src/sampling_timer.rs b/src/sampling_timer.rs index 7b3b557..4755886 100644 --- a/src/sampling_timer.rs +++ b/src/sampling_timer.rs @@ -1,12 +1,10 @@ ///! The sampling timer is used for managing ADC sampling and external reference timestamping. use super::hal; -pub use hal::stm32::tim2::ccmr2_input::CC4S_A; - /// The timer used for managing ADC sampling. pub struct SamplingTimer { timer: hal::timer::Timer, - channels: Option, + channels: Option, } impl SamplingTimer { @@ -16,12 +14,17 @@ impl SamplingTimer { Self { timer, - channels: Some(TimerChannels::new()), + // Note(unsafe): Once these channels are taken, we guarantee that we do not modify any + // of the underlying timer channel registers, as ownership of the channels is now + // provided through the associated channel structures. We additionally guarantee this + // can only be called once because there is only one Timer2 and this resource takes + // ownership of it once instantiated. + channels: unsafe { Some(tim2::Channels::new()) }, } } /// Get the timer capture/compare channels. - pub fn channels(&mut self) -> TimerChannels { + pub fn channels(&mut self) -> tim2::Channels { self.channels.take().unwrap() } @@ -32,116 +35,85 @@ impl SamplingTimer { } } -/// The capture/compare channels for the sampling timer. -/// -/// # Note -/// This should not be instantiated directly. -pub struct TimerChannels { - pub ch1: Timer2Channel1, - pub ch2: Timer2Channel2, - pub ch3: Timer2Channel3, - pub ch4: Timer2Channel4, +macro_rules! timer_channel { + ($name:ident, $TY:ty, ($ccxde:expr, $ccrx:expr, $ccmrx_output:expr, $ccxs:expr)) => { + pub struct $name {} + + paste::paste! { + impl $name { + /// Construct a new timer channel. + /// + /// Note(unsafe): This function must only be called once. Once constructed, the + /// constructee guarantees to never modify the timer channel. + unsafe fn new() -> Self { + Self {} + } + + /// Allow CH4 to generate DMA requests. + pub fn listen_dma(&self) { + let regs = unsafe { &*<$TY>::ptr() }; + regs.dier.modify(|_, w| w.[< $ccxde >]().set_bit()); + } + + /// Operate CH2 as an output-compare. + /// + /// # Args + /// * `value` - The value to compare the sampling timer's counter against. + pub fn to_output_compare(&self, value: u32) { + let regs = unsafe { &*<$TY>::ptr() }; + assert!(value <= regs.arr.read().bits()); + regs.[< $ccrx >].write(|w| w.ccr().bits(value)); + regs.[< $ccmrx_output >]() + .modify(|_, w| unsafe { w.[< $ccxs >]().bits(0) }); + } + } + } + }; } -impl TimerChannels { - fn new() -> Self { - Self { - ch1: Timer2Channel1 {}, - ch2: Timer2Channel2 {}, - ch3: Timer2Channel3 {}, - ch4: Timer2Channel4 {}, +pub mod tim2 { + use stm32h7xx_hal as hal; + + /// The channels representing the timer. + pub struct Channels { + pub ch1: Channel1, + pub ch2: Channel2, + pub ch3: Channel3, + pub ch4: Channel4, + } + + impl Channels { + /// Construct a new set of channels. + /// + /// Note(unsafe): This is only safe to call once. + pub unsafe fn new() -> Self { + Self { + ch1: Channel1::new(), + ch2: Channel2::new(), + ch3: Channel3::new(), + ch4: Channel4::new(), + } } } -} - -/// Representation of CH1 of TIM2. -pub struct Timer2Channel1 {} - -impl Timer2Channel1 { - /// Allow CH1 to generate DMA requests. - pub fn listen_dma(&self) { - let regs = unsafe { &*hal::stm32::TIM2::ptr() }; - regs.dier.modify(|_, w| w.cc1de().set_bit()); - } - - /// Operate CH1 as an output-compare. - /// - /// # Args - /// * `value` - The value to compare the sampling timer's counter against. - pub fn to_output_compare(&self, value: u32) { - let regs = unsafe { &*hal::stm32::TIM2::ptr() }; - assert!(value <= regs.arr.read().bits()); - regs.ccr1.write(|w| w.ccr().bits(value)); - regs.ccmr1_output() - .modify(|_, w| unsafe { w.cc1s().bits(0) }); - } -} - -/// Representation of CH2 of TIM2. -pub struct Timer2Channel2 {} - -impl Timer2Channel2 { - /// Allow CH2 to generate DMA requests. - pub fn listen_dma(&self) { - let regs = unsafe { &*hal::stm32::TIM2::ptr() }; - regs.dier.modify(|_, w| w.cc2de().set_bit()); - } - - /// Operate CH2 as an output-compare. - /// - /// # Args - /// * `value` - The value to compare the sampling timer's counter against. - pub fn to_output_compare(&self, value: u32) { - let regs = unsafe { &*hal::stm32::TIM2::ptr() }; - assert!(value <= regs.arr.read().bits()); - regs.ccr2.write(|w| w.ccr().bits(value)); - regs.ccmr1_output() - .modify(|_, w| unsafe { w.cc2s().bits(0) }); - } -} - -/// Representation of CH3 of TIM2. -pub struct Timer2Channel3 {} - -impl Timer2Channel3 { - /// Allow CH4 to generate DMA requests. - pub fn listen_dma(&self) { - let regs = unsafe { &*hal::stm32::TIM2::ptr() }; - regs.dier.modify(|_, w| w.cc3de().set_bit()); - } - - /// Operate CH2 as an output-compare. - /// - /// # Args - /// * `value` - The value to compare the sampling timer's counter against. - pub fn to_output_compare(&self, value: u32) { - let regs = unsafe { &*hal::stm32::TIM2::ptr() }; - assert!(value <= regs.arr.read().bits()); - regs.ccr3.write(|w| w.ccr().bits(value)); - regs.ccmr2_output() - .modify(|_, w| unsafe { w.cc3s().bits(0) }); - } -} - -/// Representation of CH4 of TIM2. -pub struct Timer2Channel4 {} - -impl Timer2Channel4 { - /// Allow CH4 to generate DMA requests. - pub fn listen_dma(&self) { - let regs = unsafe { &*hal::stm32::TIM2::ptr() }; - regs.dier.modify(|_, w| w.cc4de().set_bit()); - } - - /// Operate CH2 as an output-compare. - /// - /// # Args - /// * `value` - The value to compare the sampling timer's counter against. - pub fn to_output_compare(&self, value: u32) { - let regs = unsafe { &*hal::stm32::TIM2::ptr() }; - assert!(value <= regs.arr.read().bits()); - regs.ccr4.write(|w| w.ccr().bits(value)); - regs.ccmr2_output() - .modify(|_, w| unsafe { w.cc4s().bits(0) }); - } + + timer_channel!( + Channel1, + hal::stm32::TIM2, + (cc1de, ccr1, ccmr1_output, cc1s) + ); + timer_channel!( + Channel2, + hal::stm32::TIM2, + (cc2de, ccr2, ccmr1_output, cc1s) + ); + timer_channel!( + Channel3, + hal::stm32::TIM2, + (cc3de, ccr3, ccmr2_output, cc3s) + ); + timer_channel!( + Channel4, + hal::stm32::TIM2, + (cc4de, ccr4, ccmr2_output, cc4s) + ); } From b0e0b5144fec39f66c49d9b551d6be5b9d658f25 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Tue, 24 Nov 2020 09:27:47 +0100 Subject: [PATCH 10/71] processing: use faster unsafe truncate --- src/main.rs | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main.rs b/src/main.rs index 4c945e5..6a72552 100644 --- a/src/main.rs +++ b/src/main.rs @@ -779,7 +779,11 @@ const APP: () = { let x0 = f32::from(a as i16); let y0 = c.resources.iir_ch[1].update(&mut c.resources.iir_state[1], x0); - y0 as i16 as u16 ^ 0x8000 + // note(unsafe): The filter limits ensure that the value is in range. + // The truncation introduces 1/2 LSB distortion. + let y0 = unsafe { y0.to_int_unchecked::() }; + // convert to DAC code + y0 as u16 ^ 0x8000 }; c.resources.dac1.send(output).unwrap(); @@ -792,7 +796,11 @@ const APP: () = { let x0 = f32::from(a as i16); let y0 = c.resources.iir_ch[0].update(&mut c.resources.iir_state[0], x0); - y0 as i16 as u16 ^ 0x8000 + // note(unsafe): The filter limits ensure that the value is in range. + // The truncation introduces 1/2 LSB distortion. + let y0 = unsafe { y0.to_int_unchecked::() }; + // convert to DAC code + y0 as u16 ^ 0x8000 }; c.resources.dac0.send(output).unwrap(); From b7c6b6d203559123a0decbdb9eaff680d71c50a4 Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Tue, 24 Nov 2020 16:46:14 +0100 Subject: [PATCH 11/71] Marking AXISRAM as NOLOAD --- memory.x | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/memory.x b/memory.x index df42468..c1569ce 100644 --- a/memory.x +++ b/memory.x @@ -17,7 +17,7 @@ SECTIONS { *(.itcm .itcm.*); . = ALIGN(8); } > ITCM - .axisram : ALIGN(8) { + .axisram (NOLOAD) : ALIGN(8) { *(.axisram .axisram.*); . = ALIGN(8); } > AXISRAM From 720e0291f5582ef678ebfc968f5a87e641cebadd Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Tue, 24 Nov 2020 16:57:36 +0100 Subject: [PATCH 12/71] Removing copy to DAC buffers, adding in-place borrow of output buffers --- src/dac.rs | 54 +++++++++++++++++++++++++++++++++-------------------- src/main.rs | 5 ++--- 2 files changed, 36 insertions(+), 23 deletions(-) diff --git a/src/dac.rs b/src/dac.rs index d7032db..c567bf2 100644 --- a/src/dac.rs +++ b/src/dac.rs @@ -92,18 +92,28 @@ impl DacOutputs { Self { dac0, dac1 } } + /// Borrow the next DAC output buffers to populate the DAC output codes in-place. + /// + /// # Returns + /// (dac0, dac1) where each value is a mutable reference to the output code array for DAC0 and + /// DAC1 respectively. + pub fn prepare_data( + &mut self, + ) -> ( + &mut [u16; SAMPLE_BUFFER_SIZE], + &mut [u16; SAMPLE_BUFFER_SIZE], + ) { + (self.dac0.prepare_buffer(), self.dac1.prepare_buffer()) + } + /// Enqueue the next DAC output codes for transmission. /// - /// # Args - /// * `dac0_codes` - The output codes for DAC0 to enqueue. - /// * `dac1_codes` - The output codes for DAC1 to enqueue. - pub fn next_data( - &mut self, - dac0_codes: &[u16; SAMPLE_BUFFER_SIZE], - dac1_codes: &[u16; SAMPLE_BUFFER_SIZE], - ) { - self.dac0.next_data(dac0_codes); - self.dac1.next_data(dac1_codes); + /// # Note + /// It is assumed that data was populated using `prepare_data()` before this function is + /// called. + pub fn commit_data(&mut self) { + self.dac0.commit_buffer(); + self.dac1.commit_buffer(); } } @@ -174,16 +184,18 @@ impl Dac0Output { } } - /// Schedule the next set of DAC update codes. + /// Mutably borrow the next output buffer to populate it with DAC codes. + pub fn prepare_buffer(&mut self) -> &mut [u16; SAMPLE_BUFFER_SIZE] { + self.next_buffer.as_mut().unwrap() + } + + /// Enqueue the next buffer for transmission to the DAC. /// /// # Args - /// * `data` - The next samples to enqueue for transmission. - pub fn next_data(&mut self, data: &[u16; SAMPLE_BUFFER_SIZE]) { + /// * `data` - The next data to write to the DAC. + pub fn commit_buffer(&mut self) { let next_buffer = self.next_buffer.take().unwrap(); - // Copy data into the next buffer - next_buffer.copy_from_slice(data); - // If the last transfer was not complete, we didn't write all our previous DAC codes. // Wait for all the DAC codes to get written as well. if self.first_transfer { @@ -269,16 +281,18 @@ impl Dac1Output { } } + /// Mutably borrow the next output buffer to populate it with DAC codes. + pub fn prepare_buffer(&mut self) -> &mut [u16; SAMPLE_BUFFER_SIZE] { + self.next_buffer.as_mut().unwrap() + } + /// Enqueue the next buffer for transmission to the DAC. /// /// # Args /// * `data` - The next data to write to the DAC. - pub fn next_data(&mut self, data: &[u16; SAMPLE_BUFFER_SIZE]) { + pub fn commit_buffer(&mut self) { let next_buffer = self.next_buffer.take().unwrap(); - // Copy data into the next buffer - next_buffer.copy_from_slice(data); - // If the last transfer was not complete, we didn't write all our previous DAC codes. // Wait for all the DAC codes to get written as well. if self.first_transfer { diff --git a/src/main.rs b/src/main.rs index 89908f5..e3ddbcf 100644 --- a/src/main.rs +++ b/src/main.rs @@ -748,8 +748,7 @@ const APP: () = { let (adc0_samples, adc1_samples) = c.resources.adcs.transfer_complete_handler(); - let mut dac0: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; - let mut dac1: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; + let (dac0, dac1) = c.resources.dacs.prepare_data(); for (i, (adc0, adc1)) in adc0_samples.iter().zip(adc1_samples.iter()).enumerate() @@ -769,7 +768,7 @@ const APP: () = { }; } - c.resources.dacs.next_data(&dac0, &dac1); + c.resources.dacs.commit_data(); } #[idle(resources=[net_interface, pounder, mac_addr, eth_mac, iir_state, iir_ch, afe0, afe1])] From bf8b950fe6f2a9e4688db1cc3dc7e4b15d5511c7 Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Tue, 24 Nov 2020 17:09:36 +0100 Subject: [PATCH 13/71] Moving constants to a new file --- src/design_parameters.rs | 6 ++++++ src/main.rs | 13 +++++++------ 2 files changed, 13 insertions(+), 6 deletions(-) create mode 100644 src/design_parameters.rs diff --git a/src/design_parameters.rs b/src/design_parameters.rs new file mode 100644 index 0000000..9835568 --- /dev/null +++ b/src/design_parameters.rs @@ -0,0 +1,6 @@ +/// The ADC setup time is the number of seconds after the CSn line goes low before the serial clock +/// may begin. This is used for performing the internal ADC conversion. +pub const ADC_SETUP_TIME: f32 = 220e-9; + +/// The maximum DAC/ADC serial clock line frequency. This is a hardware limit. +pub const ADC_DAC_SCK_MHZ_MAX: u32 = 50; diff --git a/src/main.rs b/src/main.rs index e3ddbcf..ea6ed1c 100644 --- a/src/main.rs +++ b/src/main.rs @@ -63,6 +63,7 @@ static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new(); mod adc; mod afe; mod dac; +mod design_parameters; mod eeprom; mod pounder; mod sampling_timer; @@ -299,12 +300,12 @@ const APP: () = { }) .manage_cs() .suspend_when_inactive() - .cs_delay(220e-9); + .cs_delay(design_parameters::ADC_SETUP_TIME); let spi: hal::spi::Spi<_, _, u16> = dp.SPI2.spi( (spi_sck, spi_miso, hal::spi::NoMosi), config, - 50.mhz(), + design_parameters::ADC_DAC_SCK_MHZ_MAX.mhz(), ccdr.peripheral.SPI2, &ccdr.clocks, ); @@ -337,12 +338,12 @@ const APP: () = { }) .manage_cs() .suspend_when_inactive() - .cs_delay(220e-9); + .cs_delay(design_parameters::ADC_SETUP_TIME); let spi: hal::spi::Spi<_, _, u16> = dp.SPI3.spi( (spi_sck, spi_miso, hal::spi::NoMosi), config, - 50.mhz(), + design_parameters::ADC_DAC_SCK_MHZ_MAX.mhz(), ccdr.peripheral.SPI3, &ccdr.clocks, ); @@ -392,7 +393,7 @@ const APP: () = { dp.SPI4.spi( (spi_sck, spi_miso, hal::spi::NoMosi), config, - 50.mhz(), + design_parameters::ADC_DAC_SCK_MHZ_MAX.mhz(), ccdr.peripheral.SPI4, &ccdr.clocks, ) @@ -424,7 +425,7 @@ const APP: () = { dp.SPI5.spi( (spi_sck, spi_miso, hal::spi::NoMosi), config, - 50.mhz(), + design_parameters::ADC_DAC_SCK_MHZ_MAX.mhz(), ccdr.peripheral.SPI5, &ccdr.clocks, ) From d236ea94c476aa74edcfee1985134d098daa954c Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Tue, 24 Nov 2020 17:21:14 +0100 Subject: [PATCH 14/71] Updating DAC SPI structures to own HAL SPI structure for safety guarantees --- src/adc.rs | 2 ++ src/dac.rs | 81 +++++++++++++++++++++++++++--------------------------- 2 files changed, 43 insertions(+), 40 deletions(-) diff --git a/src/adc.rs b/src/adc.rs index 9b2c53b..552d210 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -307,6 +307,8 @@ impl Adc1Input { Transfer::init( trigger_stream, SPI3::new(trigger_channel), + // Note(unsafe). This transaction is read-only and SPI_START is a dont-care value, + // so it is always safe to share. unsafe { &mut SPI_START }, None, trigger_config, diff --git a/src/dac.rs b/src/dac.rs index c567bf2..89a31bf 100644 --- a/src/dac.rs +++ b/src/dac.rs @@ -24,19 +24,24 @@ static mut DAC1_BUF0: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; #[link_section = ".axisram.buffers"] static mut DAC1_BUF1: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; -/// SPI4 is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI4 TX FIFO +/// SPI4 is used as a type for indicating a DMA transfer into the SPI4 TX FIFO struct SPI4 { + spi: hal::spi::Spi, _channel: sampling_timer::tim2::Channel3, } + impl SPI4 { - pub fn new(_channel: sampling_timer::tim2::Channel3) -> Self { - Self { _channel } + pub fn new( + _channel: sampling_timer::tim2::Channel3, + spi: hal::spi::Spi, + ) -> Self { + Self { _channel, spi } } } // Note(unsafe): This is safe because the DMA request line is logically owned by this module. -// Additionally, it is only safe if the SPI TX functionality is never used, which is managed by the -// Dac0Output. +// Additionally, the SPI is owned by this structure and is known to be configured for u16 word +// sizes. unsafe impl TargetAddress for SPI4 { /// SPI2 is configured to operate using 16-bit transfer words. type MemSize = u16; @@ -46,25 +51,28 @@ unsafe impl TargetAddress for SPI4 { /// Whenever the DMA request occurs, it should write into SPI4's TX FIFO. fn address(&self) -> u32 { - // Note(unsafe): This is only safe as long as no other users write to the SPI TX FIFO. - let regs = unsafe { &*hal::stm32::SPI4::ptr() }; - ®s.txdr as *const _ as u32 + &self.spi.inner().txdr as *const _ as u32 } } /// SPI5 is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI5 TX FIFO struct SPI5 { _channel: sampling_timer::tim2::Channel4, + spi: hal::spi::Spi, } + impl SPI5 { - pub fn new(_channel: sampling_timer::tim2::Channel4) -> Self { - Self { _channel } + pub fn new( + _channel: sampling_timer::tim2::Channel4, + spi: hal::spi::Spi, + ) -> Self { + Self { _channel, spi } } } // Note(unsafe): This is safe because the DMA request line is logically owned by this module. -// Additionally, it is only safe if the SPI TX functionality is never used, which is managed by the -// Dac1Output. +// Additionally, the SPI is owned by this structure and is known to be configured for u16 word +// sizes. unsafe impl TargetAddress for SPI5 { /// SPI5 is configured to operate using 16-bit transfer words. type MemSize = u16; @@ -74,9 +82,7 @@ unsafe impl TargetAddress for SPI5 { /// Whenever the DMA request occurs, it should write into SPI5's TX FIFO fn address(&self) -> u32 { - // Note(unsafe): This is only safe as long as no other users write to the SPI TX FIFO. - let regs = unsafe { &*hal::stm32::SPI5::ptr() }; - ®s.txdr as *const _ as u32 + &self.spi.inner().txdr as *const _ as u32 } } @@ -121,7 +127,6 @@ impl DacOutputs { pub struct Dac0Output { next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, // Note: SPI TX functionality may not be used from this structure to ensure safety with DMA. - _spi: hal::spi::Spi, transfer: Transfer< hal::dma::dma::Stream4, SPI4, @@ -153,16 +158,6 @@ impl Dac0Output { .memory_increment(true) .peripheral_increment(false); - // Construct the trigger stream to write from memory to the peripheral. - let transfer: Transfer<_, _, MemoryToPeripheral, _> = Transfer::init( - stream, - SPI4::new(trigger_channel), - // Note(unsafe): This buffer is only used once and provided for the DMA transfer. - unsafe { &mut DAC0_BUF0 }, - None, - trigger_config, - ); - // Listen for any potential SPI error signals, which may indicate that we are not generating // update codes. let mut spi = spi.disable(); @@ -175,11 +170,20 @@ impl Dac0Output { spi.inner().cr1.modify(|_, w| w.spe().set_bit()); spi.inner().cr1.modify(|_, w| w.cstart().started()); + // Construct the trigger stream to write from memory to the peripheral. + let transfer: Transfer<_, _, MemoryToPeripheral, _> = Transfer::init( + stream, + SPI4::new(trigger_channel, spi), + // Note(unsafe): This buffer is only used once and provided for the DMA transfer. + unsafe { &mut DAC0_BUF0 }, + None, + trigger_config, + ); + Self { transfer, // Note(unsafe): This buffer is only used once and provided for the next DMA transfer. next_buffer: unsafe { Some(&mut DAC0_BUF1) }, - _spi: spi, first_transfer: true, } } @@ -216,8 +220,6 @@ impl Dac0Output { /// Represents the data output stream from DAC1. pub struct Dac1Output { next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, - // Note: SPI TX functionality may not be used from this structure to ensure safety with DMA. - _spi: hal::spi::Spi, transfer: Transfer< hal::dma::dma::Stream5, SPI5, @@ -250,16 +252,6 @@ impl Dac1Output { .peripheral_increment(false) .circular_buffer(true); - // Construct the stream to write from memory to the peripheral. - let transfer: Transfer<_, _, MemoryToPeripheral, _> = Transfer::init( - stream, - SPI5::new(trigger_channel), - // Note(unsafe): This buffer is only used once and provided to the transfer. - unsafe { &mut DAC1_BUF0 }, - None, - trigger_config, - ); - // Listen for any SPI errors, as this may indicate that we are not generating updates on the // DAC. let mut spi = spi.disable(); @@ -272,11 +264,20 @@ impl Dac1Output { spi.inner().cr1.modify(|_, w| w.spe().set_bit()); spi.inner().cr1.modify(|_, w| w.cstart().started()); + // Construct the stream to write from memory to the peripheral. + let transfer: Transfer<_, _, MemoryToPeripheral, _> = Transfer::init( + stream, + SPI5::new(trigger_channel, spi), + // Note(unsafe): This buffer is only used once and provided to the transfer. + unsafe { &mut DAC1_BUF0 }, + None, + trigger_config, + ); + Self { // Note(unsafe): This buffer is only used once and provided for the next DMA transfer. next_buffer: unsafe { Some(&mut DAC1_BUF1) }, transfer, - _spi: spi, first_transfer: true, } } From 7d13627a0cb8f5e5acdeb7f918da9d482886303b Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Wed, 25 Nov 2020 16:29:45 +0100 Subject: [PATCH 15/71] Removing default parameter settings --- src/adc.rs | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/adc.rs b/src/adc.rs index 552d210..6f7243c 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -169,8 +169,6 @@ impl Adc0Input { // contents). Thus, neither the memory or peripheral address ever change. This is run in // circular mode to be completed at every DMA request. let trigger_config = DmaConfig::default() - .memory_increment(false) - .peripheral_increment(false) .priority(Priority::High) .circular_buffer(true); @@ -193,8 +191,7 @@ impl Adc0Input { // stream is used to trigger a transfer completion interrupt. let data_config = DmaConfig::default() .memory_increment(true) - .priority(Priority::VeryHigh) - .peripheral_increment(false); + .priority(Priority::VeryHigh); // A SPI peripheral error interrupt is used to determine if the RX FIFO overflows. This // indicates that samples were dropped due to excessive processing time in the main @@ -297,8 +294,6 @@ impl Adc1Input { // contents). Thus, neither the memory or peripheral address ever change. This is run in // circular mode to be completed at every DMA request. let trigger_config = DmaConfig::default() - .memory_increment(false) - .peripheral_increment(false) .priority(Priority::High) .circular_buffer(true); @@ -321,8 +316,7 @@ impl Adc1Input { let data_config = DmaConfig::default() .memory_increment(true) .transfer_complete_interrupt(true) - .priority(Priority::VeryHigh) - .peripheral_increment(false); + .priority(Priority::VeryHigh); // A SPI peripheral error interrupt is used to determine if the RX FIFO overflows. This // indicates that samples were dropped due to excessive processing time in the main From 88da225e4bcc2ee42a8080ca0d97d62ccd1493e2 Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Wed, 25 Nov 2020 16:43:49 +0100 Subject: [PATCH 16/71] Adding comments about execution hanging to transfer complete waits --- src/adc.rs | 4 ++++ src/main.rs | 8 ++++---- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/adc.rs b/src/adc.rs index 6f7243c..9d344dc 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -242,6 +242,8 @@ impl Adc0Input { let next_buffer = self.next_buffer.take().unwrap(); // Wait for the transfer to fully complete before continuing. + // Note: If a device hangs up, check that this conditional is passing correctly, as there is + // no time-out checks here in the interest of execution speed. while self.transfer.get_transfer_complete_flag() == false {} // Start the next transfer. @@ -367,6 +369,8 @@ impl Adc1Input { let next_buffer = self.next_buffer.take().unwrap(); // Wait for the transfer to fully complete before continuing. + // Note: If a device hangs up, check that this conditional is passing correctly, as there is + // no time-out checks here in the interest of execution speed. while self.transfer.get_transfer_complete_flag() == false {} // Start the next transfer. diff --git a/src/main.rs b/src/main.rs index ea6ed1c..ece4340 100644 --- a/src/main.rs +++ b/src/main.rs @@ -962,22 +962,22 @@ const APP: () = { unsafe { ethernet::interrupt_handler() } } - #[task(binds = SPI2, priority = 1)] + #[task(binds = SPI2, priority = 3)] fn spi2(_: spi2::Context) { panic!("ADC0 input overrun"); } - #[task(binds = SPI3, priority = 1)] + #[task(binds = SPI3, priority = 3)] fn spi3(_: spi3::Context) { panic!("ADC0 input overrun"); } - #[task(binds = SPI4, priority = 1)] + #[task(binds = SPI4, priority = 3)] fn spi4(_: spi4::Context) { panic!("DAC0 output error"); } - #[task(binds = SPI5, priority = 1)] + #[task(binds = SPI5, priority = 3)] fn spi5(_: spi5::Context) { panic!("DAC1 output error"); } From a07be010b633b93f1297a4ee2262006d8436d580 Mon Sep 17 00:00:00 2001 From: Ryan Summers Date: Wed, 25 Nov 2020 16:46:42 +0100 Subject: [PATCH 17/71] Adding comment about checking for flag pass completion --- src/dac.rs | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/dac.rs b/src/dac.rs index 89a31bf..d6325c2 100644 --- a/src/dac.rs +++ b/src/dac.rs @@ -205,6 +205,8 @@ impl Dac0Output { if self.first_transfer { self.first_transfer = false } else { + // Note: If a device hangs up, check that this conditional is passing correctly, as + // there is no time-out checks here in the interest of execution speed. while self.transfer.get_transfer_complete_flag() == false {} } @@ -299,6 +301,8 @@ impl Dac1Output { if self.first_transfer { self.first_transfer = false } else { + // Note: If a device hangs up, check that this conditional is passing correctly, as + // there is no time-out checks here in the interest of execution speed. while self.transfer.get_transfer_complete_flag() == false {} } From d9e4f6a052757abcc6a084d47603d51259c02b2e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 25 Nov 2020 17:24:49 +0100 Subject: [PATCH 18/71] iir: copy_within is better than rotate_right --- dsp/src/iir.rs | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index fac1c4c..8d25c27 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -159,10 +159,12 @@ impl IIR { /// * `xy` - Current filter state. /// * `x0` - New input. pub fn update(&self, xy: &mut IIRState, x0: f32) -> f32 { + let n = self.ba.len(); + debug_assert!(xy.len() == n); // `xy` contains x0 x1 y0 y1 y2 // Increment time x1 x2 y1 y2 y3 // Rotate y3 x1 x2 y1 y2 - xy.rotate_right(1); + xy.copy_within(0..n - 1, 1); // unrolls better than xy.rotate_right(1) // Store x0 x0 x1 x2 y1 y2 xy[0] = x0; // Compute y0 by multiply-accumulate @@ -170,7 +172,7 @@ impl IIR { // Limit y0 let y0 = max(self.y_min, min(self.y_max, y0)); // Store y0 x0 x1 y0 y1 y2 - xy[xy.len() / 2] = y0; + xy[n / 2] = y0; y0 } } From 4c9c65bf2d5d9146e130ce55d7d615f578937091 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 25 Nov 2020 17:33:16 +0100 Subject: [PATCH 19/71] cargo-config: cm7 features --- .cargo/config | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.cargo/config b/.cargo/config index 382c36e..9968846 100644 --- a/.cargo/config +++ b/.cargo/config @@ -1,6 +1,10 @@ [target.'cfg(all(target_arch = "arm", target_os = "none"))'] runner = "gdb-multiarch -q -x openocd.gdb" -rustflags = ["-C", "link-arg=-Tlink.x"] +rustflags = [ + "-C", "link-arg=-Tlink.x", + "-C", "target-cpu=cortex-m7", + "-C", "target-feature=+fp-armv8d16", +] [build] target = "thumbv7em-none-eabihf" From 7e6cabe9c163f2c1e0ea01132cf86aa34a030404 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 25 Nov 2020 16:40:47 +0000 Subject: [PATCH 20/71] build(deps): bump panic-semihosting from 0.5.4 to 0.5.6 Bumps [panic-semihosting](https://github.com/rust-embedded/cortex-m) from 0.5.4 to 0.5.6. - [Release notes](https://github.com/rust-embedded/cortex-m/releases) - [Changelog](https://github.com/rust-embedded/cortex-m/blob/master/CHANGELOG.md) - [Commits](https://github.com/rust-embedded/cortex-m/compare/v0.5.4...v0.5.6) Signed-off-by: dependabot[bot] --- Cargo.lock | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index b4bac53..9520563 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -343,9 +343,9 @@ checksum = "de96540e0ebde571dc55c73d60ef407c653844e6f9a1e2fdbd40c07b9252d812" [[package]] name = "panic-semihosting" -version = "0.5.4" +version = "0.5.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "aed16eb761d0ee9161dd1319cb38c8007813b20f9720a5a682b283e7b8cdfe58" +checksum = "c3d55dedd501dfd02514646e0af4d7016ce36bc12ae177ef52056989966a1eec" dependencies = [ "cortex-m", "cortex-m-semihosting", From 38dfd48c149efaabce5dfa390b7bb6724b187dde Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 25 Nov 2020 17:53:13 +0100 Subject: [PATCH 21/71] iir: fix comment [nfc] --- dsp/src/iir.rs | 2 +- src/main.rs | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index 8d25c27..f58fb64 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -163,7 +163,7 @@ impl IIR { debug_assert!(xy.len() == n); // `xy` contains x0 x1 y0 y1 y2 // Increment time x1 x2 y1 y2 y3 - // Rotate y3 x1 x2 y1 y2 + // Shift x1 x1 x2 y1 y2 xy.copy_within(0..n - 1, 1); // unrolls better than xy.rotate_right(1) // Store x0 x0 x1 x2 y1 y2 xy[0] = x0; diff --git a/src/main.rs b/src/main.rs index ac9f1a6..3173158 100644 --- a/src/main.rs +++ b/src/main.rs @@ -758,10 +758,10 @@ const APP: () = { let x0 = f32::from(*adc0 as i16); let y0 = c.resources.iir_ch[0] .update(&mut c.resources.iir_state[0], x0); - // 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. let y0 = unsafe { y0.to_int_unchecked::() }; - // convert to DAC code + // Convert to DAC code y0 as u16 ^ 0x8000 }; @@ -769,10 +769,10 @@ const APP: () = { let x1 = f32::from(*adc1 as i16); let y1 = c.resources.iir_ch[1] .update(&mut c.resources.iir_state[1], x1); - // 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. let y1 = unsafe { y1.to_int_unchecked::() }; - // convert to DAC code + // Convert to DAC code y1 as u16 ^ 0x8000 }; } From cc64f470049ec363f547f4643c1b3de3613db12d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Wed, 25 Nov 2020 18:55:07 +0100 Subject: [PATCH 22/71] iir: fmt [nfc] --- dsp/src/iir.rs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index f58fb64..04d7c8e 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -164,7 +164,8 @@ impl IIR { // `xy` contains x0 x1 y0 y1 y2 // Increment time x1 x2 y1 y2 y3 // Shift x1 x1 x2 y1 y2 - xy.copy_within(0..n - 1, 1); // unrolls better than xy.rotate_right(1) + // This unrolls better than xy.rotate_right(1) + xy.copy_within(0..n - 1, 1); // Store x0 x0 x1 x2 y1 y2 xy[0] = x0; // Compute y0 by multiply-accumulate From de827acdc8ef9d8db407fea9d6e8a19522a805bc Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 26 Nov 2020 04:04:37 +0000 Subject: [PATCH 23/71] build(deps): bump paste from 1.0.2 to 1.0.3 Bumps [paste](https://github.com/dtolnay/paste) from 1.0.2 to 1.0.3. - [Release notes](https://github.com/dtolnay/paste/releases) - [Commits](https://github.com/dtolnay/paste/compare/1.0.2...1.0.3) Signed-off-by: dependabot[bot] --- Cargo.lock | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 9520563..c804d71 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -353,9 +353,9 @@ dependencies = [ [[package]] name = "paste" -version = "1.0.2" +version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ba7ae1a2180ed02ddfdb5ab70c70d596a26dd642e097bb6fe78b1bde8588ed97" +checksum = "7151b083b0664ed58ed669fcdd92f01c3d2fdbf10af4931a301474950b52bfa9" [[package]] name = "proc-macro2" From 1906185286dbd9cf25eb4810fda6619848f2450d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 26 Nov 2020 10:58:11 +0100 Subject: [PATCH 24/71] adc: macro --- src/adc.rs | 518 +++++++++++++++++----------------------------------- src/main.rs | 10 +- 2 files changed, 177 insertions(+), 351 deletions(-) diff --git a/src/adc.rs b/src/adc.rs index 9d344dc..a16976a 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -27,358 +27,184 @@ static mut SPI_START: [u16; 1] = [0x00]; // The following global buffers are used for the ADC sample DMA transfers. Two buffers are used for // each transfer in a ping-pong buffer configuration (one is being acquired while the other is being // processed). Note that the contents of AXI SRAM is uninitialized, so the buffer contents on -// startup are undefined. +// startup are undefined. The dimension are `ADC_BUF[adc_index][ping_pong_index][sample_index]`. #[link_section = ".axisram.buffers"] -static mut ADC0_BUF0: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; +static mut ADC_BUF: [[[u16; SAMPLE_BUFFER_SIZE]; 2]; 2] = + [[[0; SAMPLE_BUFFER_SIZE]; 2]; 2]; -#[link_section = ".axisram.buffers"] -static mut ADC0_BUF1: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; - -#[link_section = ".axisram.buffers"] -static mut ADC1_BUF0: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; - -#[link_section = ".axisram.buffers"] -static mut ADC1_BUF1: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; - -/// SPI2 is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI2 TX FIFO -/// whenever the tim2 update dma request occurs. -struct SPI2 { - _channel: sampling_timer::tim2::Channel1, -} -impl SPI2 { - pub fn new(_channel: sampling_timer::tim2::Channel1) -> Self { - Self { _channel } - } -} - -// Note(unsafe): This structure is only safe to instantiate once. The DMA request is hard-coded and -// may only be used if ownership of the timer2 channel 1 compare channel is assured, which is -// ensured by maintaining ownership of the channel. -unsafe impl TargetAddress for SPI2 { - /// SPI2 is configured to operate using 16-bit transfer words. - type MemSize = u16; - - /// SPI2 DMA requests are generated whenever TIM2 CH1 comparison occurs. - const REQUEST_LINE: Option = Some(DMAReq::TIM2_CH1 as u8); - - /// Whenever the DMA request occurs, it should write into SPI2's TX FIFO to start a DMA - /// transfer. - fn address(&self) -> u32 { - // Note(unsafe): It is assumed that SPI2 is owned by another DMA transfer and this DMA is - // only used for the transmit-half of DMA. - let regs = unsafe { &*hal::stm32::SPI2::ptr() }; - ®s.txdr as *const _ as u32 - } -} - -/// SPI3 is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI3 TX FIFO -/// whenever the tim2 update dma request occurs. -struct SPI3 { - _channel: sampling_timer::tim2::Channel2, -} -impl SPI3 { - pub fn new(_channel: sampling_timer::tim2::Channel2) -> Self { - Self { _channel } - } -} - -// Note(unsafe): This structure is only safe to instantiate once. The DMA request is hard-coded and -// may only be used if ownership of the timer2 channel 2 compare channel is assured, which is -// ensured by maintaining ownership of the channel. -unsafe impl TargetAddress for SPI3 { - /// SPI3 is configured to operate using 16-bit transfer words. - type MemSize = u16; - - /// SPI3 DMA requests are generated whenever TIM2 CH2 comparison occurs. - const REQUEST_LINE: Option = Some(DMAReq::TIM2_CH2 as u8); - - /// Whenever the DMA request occurs, it should write into SPI3's TX FIFO to start a DMA - /// transfer. - fn address(&self) -> u32 { - // Note(unsafe): It is assumed that SPI3 is owned by another DMA transfer and this DMA is - // only used for the transmit-half of DMA. - let regs = unsafe { &*hal::stm32::SPI3::ptr() }; - ®s.txdr as *const _ as u32 - } -} - -/// Represents both ADC input channels. -pub struct AdcInputs { - adc0: Adc0Input, - adc1: Adc1Input, -} - -impl AdcInputs { - /// Construct the ADC inputs. - pub fn new(adc0: Adc0Input, adc1: Adc1Input) -> Self { - Self { adc0, adc1 } - } - - /// Interrupt handler to handle when the sample collection DMA transfer completes. - /// - /// # Returns - /// (adc0, adc1) where adcN is a reference to the collected ADC samples. Two array references - /// are returned - one for each ADC sample stream. - pub fn transfer_complete_handler( - &mut self, - ) -> (&[u16; SAMPLE_BUFFER_SIZE], &[u16; SAMPLE_BUFFER_SIZE]) { - let adc0_buffer = self.adc0.transfer_complete_handler(); - let adc1_buffer = self.adc1.transfer_complete_handler(); - (adc0_buffer, adc1_buffer) - } -} - -/// Represents data associated with ADC0. -pub struct Adc0Input { - next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, - transfer: Transfer< - hal::dma::dma::Stream1, - hal::spi::Spi, - PeripheralToMemory, - &'static mut [u16; SAMPLE_BUFFER_SIZE], - >, - _trigger_transfer: Transfer< - hal::dma::dma::Stream0, - SPI2, - MemoryToPeripheral, - &'static mut [u16; 1], - >, -} - -impl Adc0Input { - /// Construct the ADC0 input channel. - /// - /// # Args - /// * `spi` - The SPI interface used to communicate with the ADC. - /// * `trigger_stream` - The DMA stream used to trigger each ADC transfer by writing a word into - /// the SPI TX FIFO. - /// * `data_stream` - The DMA stream used to read samples received over SPI into a data buffer. - /// * `_trigger_channel` - The ADC sampling timer output compare channel for read triggers. - pub fn new( - spi: hal::spi::Spi, - trigger_stream: hal::dma::dma::Stream0, - data_stream: hal::dma::dma::Stream1, - trigger_channel: sampling_timer::tim2::Channel1, - ) -> Self { - // Generate DMA events when an output compare of the timer hitting zero (timer roll over) - // occurs. - trigger_channel.listen_dma(); - trigger_channel.to_output_compare(0); - - // The trigger stream constantly writes to the TX FIFO using a static word (dont-care - // contents). Thus, neither the memory or peripheral address ever change. This is run in - // circular mode to be completed at every DMA request. - let trigger_config = DmaConfig::default() - .priority(Priority::High) - .circular_buffer(true); - - // Construct the trigger stream to write from memory to the peripheral. - let mut trigger_transfer: Transfer<_, _, MemoryToPeripheral, _> = - Transfer::init( - trigger_stream, - SPI2::new(trigger_channel), - // Note(unsafe): Because this is a Memory->Peripheral transfer, this data is never - // actually modified. It technically only needs to be immutably borrowed, but the - // current HAL API only supports mutable borrows. - unsafe { &mut SPI_START }, - None, - trigger_config, - ); - - // The data stream constantly reads from the SPI RX FIFO into a RAM buffer. The peripheral - // stalls reads of the SPI RX FIFO until data is available, so the DMA transfer completes - // after the requested number of samples have been collected. Note that only ADC1's data - // stream is used to trigger a transfer completion interrupt. - let data_config = DmaConfig::default() - .memory_increment(true) - .priority(Priority::VeryHigh); - - // A SPI peripheral error interrupt is used to determine if the RX FIFO overflows. This - // indicates that samples were dropped due to excessive processing time in the main - // application (e.g. a second DMA transfer completes before the first was done with - // processing). This is used as a flow control indicator to guarantee that no ADC samples - // are lost. - let mut spi = spi.disable(); - spi.listen(hal::spi::Event::Error); - - // The data transfer is always a transfer of data from the peripheral to a RAM buffer. - let mut data_transfer: Transfer<_, _, PeripheralToMemory, _> = - Transfer::init( - data_stream, - spi, - // Note(unsafe): The ADC0_BUF0 is "owned" by this peripheral. It shall not be used - // anywhere else in the module. - unsafe { &mut ADC0_BUF0 }, - None, - data_config, - ); - - data_transfer.start(|spi| { - // Allow the SPI FIFOs to operate using only DMA data channels. - spi.enable_dma_rx(); - spi.enable_dma_tx(); - - // Enable SPI and start it in infinite transaction mode. - spi.inner().cr1.modify(|_, w| w.spe().set_bit()); - spi.inner().cr1.modify(|_, w| w.cstart().started()); - }); - - trigger_transfer.start(|_| {}); - - Self { - // Note(unsafe): The ADC0_BUF1 is "owned" by this peripheral. It shall not be used - // anywhere else in the module. - next_buffer: unsafe { Some(&mut ADC0_BUF1) }, - transfer: data_transfer, - _trigger_transfer: trigger_transfer, +macro_rules! adc_input { + ($name:ident, $index:literal, $trigger_stream:ident, $data_stream:ident, + $spi:ident, $trigger_channel:ident, $dma_req:ident) => { + /// SPI is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI TX FIFO + /// whenever the tim2 update dma request occurs. + struct $spi { + _channel: sampling_timer::tim2::$trigger_channel, } - } - - /// Handle a transfer completion. - /// - /// # Returns - /// A reference to the underlying buffer that has been filled with ADC samples. - pub fn transfer_complete_handler(&mut self) -> &[u16; SAMPLE_BUFFER_SIZE] { - let next_buffer = self.next_buffer.take().unwrap(); - - // Wait for the transfer to fully complete before continuing. - // Note: If a device hangs up, check that this conditional is passing correctly, as there is - // no time-out checks here in the interest of execution speed. - while self.transfer.get_transfer_complete_flag() == false {} - - // Start the next transfer. - self.transfer.clear_interrupts(); - let (prev_buffer, _) = - self.transfer.next_transfer(next_buffer).unwrap(); - - self.next_buffer.replace(prev_buffer); - self.next_buffer.as_ref().unwrap() - } -} - -/// Represents the data input stream from ADC1 -pub struct Adc1Input { - next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, - transfer: Transfer< - hal::dma::dma::Stream3, - hal::spi::Spi, - PeripheralToMemory, - &'static mut [u16; SAMPLE_BUFFER_SIZE], - >, - _trigger_transfer: Transfer< - hal::dma::dma::Stream2, - SPI3, - MemoryToPeripheral, - &'static mut [u16; 1], - >, -} - -impl Adc1Input { - /// Construct a new ADC1 input data stream. - /// - /// # Args - /// * `spi` - The SPI interface connected to ADC1. - /// * `trigger_stream` - The DMA stream used to trigger ADC conversions on the SPI interface. - /// * `data_stream` - The DMA stream used to read ADC samples from the SPI RX FIFO. - /// * `trigger_channel` - The ADC sampling timer output compare channel for read triggers. - pub fn new( - spi: hal::spi::Spi, - trigger_stream: hal::dma::dma::Stream2, - data_stream: hal::dma::dma::Stream3, - trigger_channel: sampling_timer::tim2::Channel2, - ) -> Self { - // Generate DMA events when an output compare of the timer hitting zero (timer roll over) - // occurs. - trigger_channel.listen_dma(); - trigger_channel.to_output_compare(0); - - // The trigger stream constantly writes to the TX FIFO using a static word (dont-care - // contents). Thus, neither the memory or peripheral address ever change. This is run in - // circular mode to be completed at every DMA request. - let trigger_config = DmaConfig::default() - .priority(Priority::High) - .circular_buffer(true); - - // Construct the trigger stream to write from memory to the peripheral. - let mut trigger_transfer: Transfer<_, _, MemoryToPeripheral, _> = - Transfer::init( - trigger_stream, - SPI3::new(trigger_channel), - // Note(unsafe). This transaction is read-only and SPI_START is a dont-care value, - // so it is always safe to share. - unsafe { &mut SPI_START }, - None, - trigger_config, - ); - - // The data stream constantly reads from the SPI RX FIFO into a RAM buffer. The peripheral - // stalls reads of the SPI RX FIFO until data is available, so the DMA transfer completes - // after the requested number of samples have been collected. Note that only ADC1's data - // stream is used to trigger a transfer completion interrupt. - let data_config = DmaConfig::default() - .memory_increment(true) - .transfer_complete_interrupt(true) - .priority(Priority::VeryHigh); - - // A SPI peripheral error interrupt is used to determine if the RX FIFO overflows. This - // indicates that samples were dropped due to excessive processing time in the main - // application (e.g. a second DMA transfer completes before the first was done with - // processing). This is used as a flow control indicator to guarantee that no ADC samples - // are lost. - let mut spi = spi.disable(); - spi.listen(hal::spi::Event::Error); - - // The data transfer is always a transfer of data from the peripheral to a RAM buffer. - let mut data_transfer: Transfer<_, _, PeripheralToMemory, _> = - Transfer::init( - data_stream, - spi, - // Note(unsafe): The ADC1_BUF0 is "owned" by this peripheral. It shall not be used - // anywhere else in the module. - unsafe { &mut ADC1_BUF0 }, - None, - data_config, - ); - - data_transfer.start(|spi| { - // Allow the SPI FIFOs to operate using only DMA data channels. - spi.enable_dma_rx(); - spi.enable_dma_tx(); - - // Enable SPI and start it in infinite transaction mode. - spi.inner().cr1.modify(|_, w| w.spe().set_bit()); - spi.inner().cr1.modify(|_, w| w.cstart().started()); - }); - - trigger_transfer.start(|_| {}); - - Self { - // Note(unsafe): The ADC1_BUF1 is "owned" by this peripheral. It shall not be used - // anywhere else in the module. - next_buffer: unsafe { Some(&mut ADC1_BUF1) }, - transfer: data_transfer, - _trigger_transfer: trigger_transfer, + impl $spi { + pub fn new( + _channel: sampling_timer::tim2::$trigger_channel, + ) -> Self { + Self { _channel } + } } - } - /// Handle a transfer completion. - /// - /// # Returns - /// A reference to the underlying buffer that has been filled with ADC samples. - pub fn transfer_complete_handler(&mut self) -> &[u16; SAMPLE_BUFFER_SIZE] { - let next_buffer = self.next_buffer.take().unwrap(); + // Note(unsafe): This structure is only safe to instantiate once. The DMA request is hard-coded and + // may only be used if ownership of the timer2 $trigger_channel compare channel is assured, which is + // ensured by maintaining ownership of the channel. + unsafe impl TargetAddress for $spi { + /// SPI is configured to operate using 16-bit transfer words. + type MemSize = u16; - // Wait for the transfer to fully complete before continuing. - // Note: If a device hangs up, check that this conditional is passing correctly, as there is - // no time-out checks here in the interest of execution speed. - while self.transfer.get_transfer_complete_flag() == false {} + /// SPI DMA requests are generated whenever TIM2 CH1 comparison occurs. + const REQUEST_LINE: Option = Some(DMAReq::$dma_req as u8); - // Start the next transfer. - self.transfer.clear_interrupts(); - let (prev_buffer, _) = - self.transfer.next_transfer(next_buffer).unwrap(); + /// Whenever the DMA request occurs, it should write into SPI's TX FIFO to start a DMA + /// transfer. + fn address(&self) -> u32 { + // Note(unsafe): It is assumed that SPI is owned by another DMA transfer and this DMA is + // only used for the transmit-half of DMA. + let regs = unsafe { &*hal::stm32::$spi::ptr() }; + ®s.txdr as *const _ as u32 + } + } - self.next_buffer.replace(prev_buffer); - self.next_buffer.as_ref().unwrap() - } + /// Represents data associated with ADC0. + pub struct $name { + next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, + transfer: Transfer< + hal::dma::dma::$data_stream, + hal::spi::Spi, + PeripheralToMemory, + &'static mut [u16; SAMPLE_BUFFER_SIZE], + >, + _trigger_transfer: Transfer< + hal::dma::dma::$trigger_stream, + $spi, + MemoryToPeripheral, + &'static mut [u16; 1], + >, + } + + impl $name { + /// Construct the ADC0 input channel. + /// + /// # Args + /// * `spi` - The SPI interface used to communicate with the ADC. + /// * `trigger_stream` - The DMA stream used to trigger each ADC transfer by writing a word into + /// the SPI TX FIFO. + /// * `data_stream` - The DMA stream used to read samples received over SPI into a data buffer. + /// * `_trigger_channel` - The ADC sampling timer output compare channel for read triggers. + pub fn new( + spi: hal::spi::Spi, + trigger_stream: hal::dma::dma::$trigger_stream< + hal::stm32::DMA1, + >, + data_stream: hal::dma::dma::$data_stream, + trigger_channel: sampling_timer::tim2::$trigger_channel, + ) -> Self { + // Generate DMA events when an output compare of the timer hitting zero (timer roll over) + // occurs. + trigger_channel.listen_dma(); + trigger_channel.to_output_compare(0); + + // The trigger stream constantly writes to the TX FIFO using a static word (dont-care + // contents). Thus, neither the memory or peripheral address ever change. This is run in + // circular mode to be completed at every DMA request. + let trigger_config = DmaConfig::default() + .priority(Priority::High) + .circular_buffer(true); + + // Construct the trigger stream to write from memory to the peripheral. + let mut trigger_transfer: Transfer< + _, + _, + MemoryToPeripheral, + _, + > = Transfer::init( + trigger_stream, + $spi::new(trigger_channel), + // Note(unsafe): Because this is a Memory->Peripheral transfer, this data is never + // actually modified. It technically only needs to be immutably borrowed, but the + // current HAL API only supports mutable borrows. + unsafe { &mut SPI_START }, + None, + trigger_config, + ); + + // The data stream constantly reads from the SPI RX FIFO into a RAM buffer. The peripheral + // stalls reads of the SPI RX FIFO until data is available, so the DMA transfer completes + // after the requested number of samples have been collected. Note that only ADC1's (sic!) + // data stream is used to trigger a transfer completion interrupt. + let data_config = DmaConfig::default() + .memory_increment(true) + .priority(Priority::VeryHigh); + + // A SPI peripheral error interrupt is used to determine if the RX FIFO overflows. This + // indicates that samples were dropped due to excessive processing time in the main + // application (e.g. a second DMA transfer completes before the first was done with + // processing). This is used as a flow control indicator to guarantee that no ADC samples + // are lost. + let mut spi = spi.disable(); + spi.listen(hal::spi::Event::Error); + + // The data transfer is always a transfer of data from the peripheral to a RAM buffer. + let mut data_transfer: Transfer<_, _, PeripheralToMemory, _> = + Transfer::init( + data_stream, + spi, + // Note(unsafe): The ADC_BUF[$index][0] is "owned" by this peripheral. + // It shall not be used anywhere else in the module. + unsafe { &mut ADC_BUF[$index][0] }, + None, + data_config, + ); + + data_transfer.start(|spi| { + // Allow the SPI FIFOs to operate using only DMA data channels. + spi.enable_dma_rx(); + spi.enable_dma_tx(); + + // Enable SPI and start it in infinite transaction mode. + spi.inner().cr1.modify(|_, w| w.spe().set_bit()); + spi.inner().cr1.modify(|_, w| w.cstart().started()); + }); + + trigger_transfer.start(|_| {}); + + Self { + // Note(unsafe): The ADC_BUF[$index][1] is "owned" by this peripheral. It shall not be used + // anywhere else in the module. + next_buffer: unsafe { Some(&mut ADC_BUF[$index][1]) }, + transfer: data_transfer, + _trigger_transfer: trigger_transfer, + } + } + + /// Handle a transfer completion. + /// + /// # Returns + /// A reference to the underlying buffer that has been filled with ADC samples. + pub fn transfer_complete_handler( + &mut self, + ) -> &[u16; SAMPLE_BUFFER_SIZE] { + let next_buffer = self.next_buffer.take().unwrap(); + + // Wait for the transfer to fully complete before continuing. + // Note: If a device hangs up, check that this conditional is passing correctly, as there is + // no time-out checks here in the interest of execution speed. + while self.transfer.get_transfer_complete_flag() == false {} + + // Start the next transfer. + self.transfer.clear_interrupts(); + let (prev_buffer, _) = + self.transfer.next_transfer(next_buffer).unwrap(); + + self.next_buffer.replace(prev_buffer); + self.next_buffer.as_ref().unwrap() + } + } + }; } + +adc_input!(Adc0Input, 0, Stream0, Stream1, SPI2, Channel1, TIM2_CH1); +adc_input!(Adc1Input, 1, Stream2, Stream3, SPI3, Channel2, TIM2_CH2); diff --git a/src/main.rs b/src/main.rs index ece4340..4bf67cf 100644 --- a/src/main.rs +++ b/src/main.rs @@ -69,7 +69,7 @@ mod pounder; mod sampling_timer; mod server; -use adc::{Adc0Input, Adc1Input, AdcInputs}; +use adc::{Adc0Input, Adc1Input}; use dac::{Dac0Output, Dac1Output, DacOutputs}; use dsp::iir; @@ -188,7 +188,7 @@ const APP: () = { afe0: AFE0, afe1: AFE1, - adcs: AdcInputs, + adcs: (Adc0Input, Adc1Input), dacs: DacOutputs, eeprom_i2c: hal::i2c::I2c, @@ -356,7 +356,7 @@ const APP: () = { ) }; - AdcInputs::new(adc0, adc1) + (adc0, adc1) }; let dacs = { @@ -746,8 +746,8 @@ const APP: () = { #[task(binds=DMA1_STR3, resources=[adcs, dacs, iir_state, iir_ch], priority=2)] fn adc_update(c: adc_update::Context) { - let (adc0_samples, adc1_samples) = - c.resources.adcs.transfer_complete_handler(); + let adc0_samples = c.resources.adcs.0.transfer_complete_handler(); + let adc1_samples = c.resources.adcs.1.transfer_complete_handler(); let (dac0, dac1) = c.resources.dacs.prepare_data(); From c04180635b9b50e347c8bfba48c9c6370d06c2b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 26 Nov 2020 11:16:08 +0100 Subject: [PATCH 25/71] dacs: macros --- src/dac.rs | 427 ++++++++++++++++------------------------------------ src/main.rs | 12 +- 2 files changed, 139 insertions(+), 300 deletions(-) diff --git a/src/dac.rs b/src/dac.rs index d6325c2..7f99001 100644 --- a/src/dac.rs +++ b/src/dac.rs @@ -11,306 +11,143 @@ use super::{ // The following global buffers are used for the DAC code DMA transfers. Two buffers are used for // each transfer in a ping-pong buffer configuration (one is being prepared while the other is being // processed). Note that the contents of AXI SRAM is uninitialized, so the buffer contents on -// startup are undefined. +// startup are undefined. The dimension are `ADC_BUF[adc_index][ping_pong_index][sample_index]`. #[link_section = ".axisram.buffers"] -static mut DAC0_BUF0: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; +static mut DAC_BUF: [[[u16; SAMPLE_BUFFER_SIZE]; 2]; 2] = + [[[0; SAMPLE_BUFFER_SIZE]; 2]; 2]; -#[link_section = ".axisram.buffers"] -static mut DAC0_BUF1: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; - -#[link_section = ".axisram.buffers"] -static mut DAC1_BUF0: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; - -#[link_section = ".axisram.buffers"] -static mut DAC1_BUF1: [u16; SAMPLE_BUFFER_SIZE] = [0; SAMPLE_BUFFER_SIZE]; - -/// SPI4 is used as a type for indicating a DMA transfer into the SPI4 TX FIFO -struct SPI4 { - spi: hal::spi::Spi, - _channel: sampling_timer::tim2::Channel3, -} - -impl SPI4 { - pub fn new( - _channel: sampling_timer::tim2::Channel3, - spi: hal::spi::Spi, - ) -> Self { - Self { _channel, spi } - } -} - -// Note(unsafe): This is safe because the DMA request line is logically owned by this module. -// Additionally, the SPI is owned by this structure and is known to be configured for u16 word -// sizes. -unsafe impl TargetAddress for SPI4 { - /// SPI2 is configured to operate using 16-bit transfer words. - type MemSize = u16; - - /// SPI4 DMA requests are generated whenever TIM2 CH3 comparison occurs. - const REQUEST_LINE: Option = Some(DMAReq::TIM2_CH3 as u8); - - /// Whenever the DMA request occurs, it should write into SPI4's TX FIFO. - fn address(&self) -> u32 { - &self.spi.inner().txdr as *const _ as u32 - } -} - -/// SPI5 is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI5 TX FIFO -struct SPI5 { - _channel: sampling_timer::tim2::Channel4, - spi: hal::spi::Spi, -} - -impl SPI5 { - pub fn new( - _channel: sampling_timer::tim2::Channel4, - spi: hal::spi::Spi, - ) -> Self { - Self { _channel, spi } - } -} - -// Note(unsafe): This is safe because the DMA request line is logically owned by this module. -// Additionally, the SPI is owned by this structure and is known to be configured for u16 word -// sizes. -unsafe impl TargetAddress for SPI5 { - /// SPI5 is configured to operate using 16-bit transfer words. - type MemSize = u16; - - /// SPI5 DMA requests are generated whenever TIM2 CH4 comparison occurs. - const REQUEST_LINE: Option = Some(DMAReq::TIM2_CH4 as u8); - - /// Whenever the DMA request occurs, it should write into SPI5's TX FIFO - fn address(&self) -> u32 { - &self.spi.inner().txdr as *const _ as u32 - } -} - -/// Represents both DAC output channels. -pub struct DacOutputs { - dac0: Dac0Output, - dac1: Dac1Output, -} - -impl DacOutputs { - /// Construct the DAC outputs. - pub fn new(dac0: Dac0Output, dac1: Dac1Output) -> Self { - Self { dac0, dac1 } - } - - /// Borrow the next DAC output buffers to populate the DAC output codes in-place. - /// - /// # Returns - /// (dac0, dac1) where each value is a mutable reference to the output code array for DAC0 and - /// DAC1 respectively. - pub fn prepare_data( - &mut self, - ) -> ( - &mut [u16; SAMPLE_BUFFER_SIZE], - &mut [u16; SAMPLE_BUFFER_SIZE], - ) { - (self.dac0.prepare_buffer(), self.dac1.prepare_buffer()) - } - - /// Enqueue the next DAC output codes for transmission. - /// - /// # Note - /// It is assumed that data was populated using `prepare_data()` before this function is - /// called. - pub fn commit_data(&mut self) { - self.dac0.commit_buffer(); - self.dac1.commit_buffer(); - } -} - -/// Represents data associated with DAC0. -pub struct Dac0Output { - next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, - // Note: SPI TX functionality may not be used from this structure to ensure safety with DMA. - transfer: Transfer< - hal::dma::dma::Stream4, - SPI4, - MemoryToPeripheral, - &'static mut [u16; SAMPLE_BUFFER_SIZE], - >, - first_transfer: bool, -} - -impl Dac0Output { - /// Construct the DAC0 output channel. - /// - /// # Args - /// * `spi` - The SPI interface used to communicate with the ADC. - /// * `stream` - The DMA stream used to write DAC codes over SPI. - /// * `trigger_channel` - The sampling timer output compare channel for update triggers. - pub fn new( - spi: hal::spi::Spi, - stream: hal::dma::dma::Stream4, - trigger_channel: sampling_timer::tim2::Channel3, - ) -> Self { - // Generate DMA events when an output compare of the timer hitting zero (timer roll over) - // occurs. - trigger_channel.listen_dma(); - trigger_channel.to_output_compare(0); - - // The stream constantly writes to the TX FIFO to write new update codes. - let trigger_config = DmaConfig::default() - .memory_increment(true) - .peripheral_increment(false); - - // Listen for any potential SPI error signals, which may indicate that we are not generating - // update codes. - let mut spi = spi.disable(); - spi.listen(hal::spi::Event::Error); - - // Allow the SPI FIFOs to operate using only DMA data channels. - spi.enable_dma_tx(); - - // Enable SPI and start it in infinite transaction mode. - spi.inner().cr1.modify(|_, w| w.spe().set_bit()); - spi.inner().cr1.modify(|_, w| w.cstart().started()); - - // Construct the trigger stream to write from memory to the peripheral. - let transfer: Transfer<_, _, MemoryToPeripheral, _> = Transfer::init( - stream, - SPI4::new(trigger_channel, spi), - // Note(unsafe): This buffer is only used once and provided for the DMA transfer. - unsafe { &mut DAC0_BUF0 }, - None, - trigger_config, - ); - - Self { - transfer, - // Note(unsafe): This buffer is only used once and provided for the next DMA transfer. - next_buffer: unsafe { Some(&mut DAC0_BUF1) }, - first_transfer: true, - } - } - - /// Mutably borrow the next output buffer to populate it with DAC codes. - pub fn prepare_buffer(&mut self) -> &mut [u16; SAMPLE_BUFFER_SIZE] { - self.next_buffer.as_mut().unwrap() - } - - /// Enqueue the next buffer for transmission to the DAC. - /// - /// # Args - /// * `data` - The next data to write to the DAC. - pub fn commit_buffer(&mut self) { - let next_buffer = self.next_buffer.take().unwrap(); - - // If the last transfer was not complete, we didn't write all our previous DAC codes. - // Wait for all the DAC codes to get written as well. - if self.first_transfer { - self.first_transfer = false - } else { - // Note: If a device hangs up, check that this conditional is passing correctly, as - // there is no time-out checks here in the interest of execution speed. - while self.transfer.get_transfer_complete_flag() == false {} +macro_rules! dac_output { + ($name:ident, $index:literal, $data_stream:ident, + $spi:ident, $trigger_channel:ident, $dma_req:ident) => { + /// SPI is used as a type for indicating a DMA transfer into the SPI TX FIFO + struct $spi { + spi: hal::spi::Spi, + _channel: sampling_timer::tim2::$trigger_channel, } - // Start the next transfer. - self.transfer.clear_interrupts(); - let (prev_buffer, _) = - self.transfer.next_transfer(next_buffer).unwrap(); - - self.next_buffer.replace(prev_buffer); - } -} - -/// Represents the data output stream from DAC1. -pub struct Dac1Output { - next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, - transfer: Transfer< - hal::dma::dma::Stream5, - SPI5, - MemoryToPeripheral, - &'static mut [u16; SAMPLE_BUFFER_SIZE], - >, - first_transfer: bool, -} - -impl Dac1Output { - /// Construct a new DAC1 output data stream. - /// - /// # Args - /// * `spi` - The SPI interface connected to DAC1. - /// * `stream` - The DMA stream used to write DAC codes the SPI TX FIFO. - /// * `trigger_channel` - The timer channel used to generate DMA requests for DAC updates. - pub fn new( - spi: hal::spi::Spi, - stream: hal::dma::dma::Stream5, - trigger_channel: sampling_timer::tim2::Channel4, - ) -> Self { - // Generate DMA events when an output compare of the timer hitting zero (timer roll over) - // occurs. - trigger_channel.listen_dma(); - trigger_channel.to_output_compare(0); - - // The trigger stream constantly writes to the TX FIFO to generate DAC updates. - let trigger_config = DmaConfig::default() - .memory_increment(true) - .peripheral_increment(false) - .circular_buffer(true); - - // Listen for any SPI errors, as this may indicate that we are not generating updates on the - // DAC. - let mut spi = spi.disable(); - spi.listen(hal::spi::Event::Error); - - // Allow the SPI FIFOs to operate using only DMA data channels. - spi.enable_dma_tx(); - - // Enable SPI and start it in infinite transaction mode. - spi.inner().cr1.modify(|_, w| w.spe().set_bit()); - spi.inner().cr1.modify(|_, w| w.cstart().started()); - - // Construct the stream to write from memory to the peripheral. - let transfer: Transfer<_, _, MemoryToPeripheral, _> = Transfer::init( - stream, - SPI5::new(trigger_channel, spi), - // Note(unsafe): This buffer is only used once and provided to the transfer. - unsafe { &mut DAC1_BUF0 }, - None, - trigger_config, - ); - - Self { - // Note(unsafe): This buffer is only used once and provided for the next DMA transfer. - next_buffer: unsafe { Some(&mut DAC1_BUF1) }, - transfer, - first_transfer: true, - } - } - - /// Mutably borrow the next output buffer to populate it with DAC codes. - pub fn prepare_buffer(&mut self) -> &mut [u16; SAMPLE_BUFFER_SIZE] { - self.next_buffer.as_mut().unwrap() - } - - /// Enqueue the next buffer for transmission to the DAC. - /// - /// # Args - /// * `data` - The next data to write to the DAC. - pub fn commit_buffer(&mut self) { - let next_buffer = self.next_buffer.take().unwrap(); - - // If the last transfer was not complete, we didn't write all our previous DAC codes. - // Wait for all the DAC codes to get written as well. - if self.first_transfer { - self.first_transfer = false - } else { - // Note: If a device hangs up, check that this conditional is passing correctly, as - // there is no time-out checks here in the interest of execution speed. - while self.transfer.get_transfer_complete_flag() == false {} + impl $spi { + pub fn new( + _channel: sampling_timer::tim2::$trigger_channel, + spi: hal::spi::Spi, + ) -> Self { + Self { _channel, spi } + } } - // Start the next transfer. - self.transfer.clear_interrupts(); - let (prev_buffer, _) = - self.transfer.next_transfer(next_buffer).unwrap(); + // Note(unsafe): This is safe because the DMA request line is logically owned by this module. + // Additionally, the SPI is owned by this structure and is known to be configured for u16 word + // sizes. + unsafe impl TargetAddress for $spi { + /// SPI is configured to operate using 16-bit transfer words. + type MemSize = u16; - self.next_buffer.replace(prev_buffer); - } + /// SPI DMA requests are generated whenever TIM2 CH3 comparison occurs. + const REQUEST_LINE: Option = Some(DMAReq::$dma_req as u8); + + /// Whenever the DMA request occurs, it should write into SPI's TX FIFO. + fn address(&self) -> u32 { + &self.spi.inner().txdr as *const _ as u32 + } + } + + /// Represents data associated with DAC0. + pub struct $name { + next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, + // Note: SPI TX functionality may not be used from this structure to ensure safety with DMA. + transfer: Transfer< + hal::dma::dma::$data_stream, + $spi, + MemoryToPeripheral, + &'static mut [u16; SAMPLE_BUFFER_SIZE], + >, + first_transfer: bool, + } + + impl $name { + /// Construct the DAC output channel. + /// + /// # Args + /// * `spi` - The SPI interface used to communicate with the ADC. + /// * `stream` - The DMA stream used to write DAC codes over SPI. + /// * `trigger_channel` - The sampling timer output compare channel for update triggers. + pub fn new( + spi: hal::spi::Spi, + stream: hal::dma::dma::$data_stream, + trigger_channel: sampling_timer::tim2::$trigger_channel, + ) -> Self { + // Generate DMA events when an output compare of the timer hitting zero (timer roll over) + // occurs. + trigger_channel.listen_dma(); + trigger_channel.to_output_compare(0); + + // The stream constantly writes to the TX FIFO to write new update codes. + let trigger_config = DmaConfig::default() + .memory_increment(true) + .peripheral_increment(false); + + // Listen for any potential SPI error signals, which may indicate that we are not generating + // update codes. + let mut spi = spi.disable(); + spi.listen(hal::spi::Event::Error); + + // Allow the SPI FIFOs to operate using only DMA data channels. + spi.enable_dma_tx(); + + // Enable SPI and start it in infinite transaction mode. + spi.inner().cr1.modify(|_, w| w.spe().set_bit()); + spi.inner().cr1.modify(|_, w| w.cstart().started()); + + // Construct the trigger stream to write from memory to the peripheral. + let transfer: Transfer<_, _, MemoryToPeripheral, _> = + Transfer::init( + stream, + $spi::new(trigger_channel, spi), + // Note(unsafe): This buffer is only used once and provided for the DMA transfer. + unsafe { &mut DAC_BUF[$index][0] }, + None, + trigger_config, + ); + + Self { + transfer, + // Note(unsafe): This buffer is only used once and provided for the next DMA transfer. + next_buffer: unsafe { Some(&mut DAC_BUF[$index][1]) }, + first_transfer: true, + } + } + + /// Mutably borrow the next output buffer to populate it with DAC codes. + pub fn prepare_buffer(&mut self) -> &mut [u16; SAMPLE_BUFFER_SIZE] { + self.next_buffer.as_mut().unwrap() + } + + /// Enqueue the next buffer for transmission to the DAC. + /// + /// # Args + /// * `data` - The next data to write to the DAC. + pub fn commit_buffer(&mut self) { + let next_buffer = self.next_buffer.take().unwrap(); + + // If the last transfer was not complete, we didn't write all our previous DAC codes. + // Wait for all the DAC codes to get written as well. + if self.first_transfer { + self.first_transfer = false + } else { + // Note: If a device hangs up, check that this conditional is passing correctly, as + // there is no time-out checks here in the interest of execution speed. + while self.transfer.get_transfer_complete_flag() == false {} + } + + // Start the next transfer. + self.transfer.clear_interrupts(); + let (prev_buffer, _) = + self.transfer.next_transfer(next_buffer).unwrap(); + + self.next_buffer.replace(prev_buffer); + } + } + }; } + +dac_output!(Dac0Output, 0, Stream4, SPI4, Channel3, TIM2_CH3); +dac_output!(Dac1Output, 1, Stream5, SPI5, Channel4, TIM2_CH4); diff --git a/src/main.rs b/src/main.rs index 4bf67cf..4dd91e3 100644 --- a/src/main.rs +++ b/src/main.rs @@ -70,7 +70,7 @@ mod sampling_timer; mod server; use adc::{Adc0Input, Adc1Input}; -use dac::{Dac0Output, Dac1Output, DacOutputs}; +use dac::{Dac0Output, Dac1Output}; use dsp::iir; #[cfg(not(feature = "semihosting"))] @@ -189,7 +189,7 @@ const APP: () = { afe1: AFE1, adcs: (Adc0Input, Adc1Input), - dacs: DacOutputs, + dacs: (Dac0Output, Dac1Output), eeprom_i2c: hal::i2c::I2c, @@ -441,7 +441,7 @@ const APP: () = { dma_streams.5, sampling_timer_channels.ch4, ); - DacOutputs::new(dac0, dac1) + (dac0, dac1) }; let mut fp_led_0 = gpiod.pd5.into_push_pull_output(); @@ -749,7 +749,8 @@ const APP: () = { let adc0_samples = c.resources.adcs.0.transfer_complete_handler(); let adc1_samples = c.resources.adcs.1.transfer_complete_handler(); - let (dac0, dac1) = c.resources.dacs.prepare_data(); + let dac0 = c.resources.dacs.0.prepare_buffer(); + let dac1 = c.resources.dacs.1.prepare_buffer(); for (i, (adc0, adc1)) in adc0_samples.iter().zip(adc1_samples.iter()).enumerate() @@ -769,7 +770,8 @@ const APP: () = { }; } - c.resources.dacs.commit_data(); + c.resources.dacs.0.commit_buffer(); + c.resources.dacs.1.commit_buffer(); } #[idle(resources=[net_interface, pounder, mac_addr, eth_mac, iir_state, iir_ch, afe0, afe1])] From 911fb661461eb97df144b402bb8c4dc9dfa1cea1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 26 Nov 2020 11:29:16 +0100 Subject: [PATCH 26/71] main: process() code duplication a bit --- src/main.rs | 38 ++++++++++++++++---------------------- 1 file changed, 16 insertions(+), 22 deletions(-) diff --git a/src/main.rs b/src/main.rs index 4dd91e3..a4e2fc4 100644 --- a/src/main.rs +++ b/src/main.rs @@ -745,29 +745,23 @@ const APP: () = { } #[task(binds=DMA1_STR3, resources=[adcs, dacs, iir_state, iir_ch], priority=2)] - fn adc_update(c: adc_update::Context) { - let adc0_samples = c.resources.adcs.0.transfer_complete_handler(); - let adc1_samples = c.resources.adcs.1.transfer_complete_handler(); + fn process(c: process::Context) { + let adc_samples = [ + c.resources.adcs.0.transfer_complete_handler(), + c.resources.adcs.1.transfer_complete_handler(), + ]; + let dac_samples = [ + c.resources.dacs.0.prepare_buffer(), + c.resources.dacs.1.prepare_buffer(), + ]; - let dac0 = c.resources.dacs.0.prepare_buffer(); - let dac1 = c.resources.dacs.1.prepare_buffer(); - - for (i, (adc0, adc1)) in - adc0_samples.iter().zip(adc1_samples.iter()).enumerate() - { - dac0[i] = { - let x0 = f32::from(*adc0 as i16); - let y0 = c.resources.iir_ch[0] - .update(&mut c.resources.iir_state[0], x0); - y0 as i16 as u16 ^ 0x8000 - }; - - dac1[i] = { - let x1 = f32::from(*adc1 as i16); - let y1 = c.resources.iir_ch[1] - .update(&mut c.resources.iir_state[1], x1); - y1 as i16 as u16 ^ 0x8000 - }; + for channel in 0..adc_samples.len() { + for sample in 0..adc_samples[0].len() { + let x = f32::from(adc_samples[channel][sample] as i16); + let y = c.resources.iir_ch[channel] + .update(&mut c.resources.iir_state[channel], x); + dac_samples[channel][sample] = y as i16 as u16 ^ 0x8000; + } } c.resources.dacs.0.commit_buffer(); From 71898308968ef39c82e87d021a18044afcc69b2c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 26 Nov 2020 11:33:08 +0100 Subject: [PATCH 27/71] main: put AFEs into a tuple --- src/main.rs | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/src/main.rs b/src/main.rs index a4e2fc4..d7f30c7 100644 --- a/src/main.rs +++ b/src/main.rs @@ -185,8 +185,7 @@ macro_rules! route_request { #[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)] const APP: () = { struct Resources { - afe0: AFE0, - afe1: AFE1, + afes: (AFE0, AFE1), adcs: (Adc0Input, Adc1Input), dacs: (Dac0Output, Dac1Output), @@ -729,8 +728,7 @@ const APP: () = { sampling_timer.start(); init::LateResources { - afe0: afe0, - afe1: afe1, + afes: (afe0, afe1), adcs, dacs, @@ -768,7 +766,7 @@ const APP: () = { c.resources.dacs.1.commit_buffer(); } - #[idle(resources=[net_interface, pounder, mac_addr, eth_mac, iir_state, iir_ch, afe0, afe1])] + #[idle(resources=[net_interface, pounder, mac_addr, eth_mac, iir_state, iir_ch, afes])] fn idle(mut c: idle::Context) -> ! { let mut socket_set_entries: [_; 8] = Default::default(); let mut sockets = @@ -828,8 +826,8 @@ const APP: () = { Ok::(state) }), - "stabilizer/afe0/gain": (|| c.resources.afe0.get_gain()), - "stabilizer/afe1/gain": (|| c.resources.afe1.get_gain()), + "stabilizer/afe0/gain": (|| c.resources.afes.0.get_gain()), + "stabilizer/afe1/gain": (|| c.resources.afes.1.get_gain()), "pounder/in0": (|| { match c.resources.pounder { Some(pounder) => @@ -924,10 +922,10 @@ const APP: () = { } }), "stabilizer/afe0/gain": afe::Gain, (|gain| { - Ok::<(), ()>(c.resources.afe0.set_gain(gain)) + Ok::<(), ()>(c.resources.afes.0.set_gain(gain)) }), "stabilizer/afe1/gain": afe::Gain, (|gain| { - Ok::<(), ()>(c.resources.afe1.set_gain(gain)) + Ok::<(), ()>(c.resources.afes.1.set_gain(gain)) }) ] ) From 754ebed50db05d05859bc2b6076c527bbfbb0e06 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 26 Nov 2020 11:55:15 +0100 Subject: [PATCH 28/71] adc: transfer complete interrupt on Adc1 --- src/adc.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/src/adc.rs b/src/adc.rs index a16976a..2849f00 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -137,6 +137,7 @@ macro_rules! adc_input { // data stream is used to trigger a transfer completion interrupt. let data_config = DmaConfig::default() .memory_increment(true) + .transfer_complete_interrupt($index == 1) .priority(Priority::VeryHigh); // A SPI peripheral error interrupt is used to determine if the RX FIFO overflows. This From d8c6f39d0f7207e5b1ea58de532227242270295f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 26 Nov 2020 13:51:39 +0100 Subject: [PATCH 29/71] adc/dac: make the buffer handling more symmetric --- src/adc.rs | 26 ++++++++++++++++++-------- src/dac.rs | 18 +++++++++++------- src/main.rs | 17 ++++++++++------- 3 files changed, 39 insertions(+), 22 deletions(-) diff --git a/src/adc.rs b/src/adc.rs index 2849f00..145c5af 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -181,27 +181,37 @@ macro_rules! adc_input { } } - /// Handle a transfer completion. + /// Obtain a buffer filled with ADC samples. /// /// # Returns /// A reference to the underlying buffer that has been filled with ADC samples. - pub fn transfer_complete_handler( + pub fn acquire_buffer( &mut self, - ) -> &[u16; SAMPLE_BUFFER_SIZE] { - let next_buffer = self.next_buffer.take().unwrap(); - + ) -> &'static mut [u16; SAMPLE_BUFFER_SIZE] { // Wait for the transfer to fully complete before continuing. // Note: If a device hangs up, check that this conditional is passing correctly, as there is // no time-out checks here in the interest of execution speed. - while self.transfer.get_transfer_complete_flag() == false {} + while !self.transfer.get_transfer_complete_flag() {} + + let next_buffer = self.next_buffer.take().unwrap(); // Start the next transfer. self.transfer.clear_interrupts(); let (prev_buffer, _) = self.transfer.next_transfer(next_buffer).unwrap(); - self.next_buffer.replace(prev_buffer); - self.next_buffer.as_ref().unwrap() + prev_buffer + } + + /// Release a buffer of ADC samples to the pool. + /// + /// # Args + /// * `next_buffer` - Buffer of ADC samples to be re-used. + pub fn release_buffer( + &mut self, + next_buffer: &'static mut [u16; SAMPLE_BUFFER_SIZE], + ) { + self.next_buffer.replace(next_buffer); // .unwrap_none() https://github.com/rust-lang/rust/issues/62633 } } }; diff --git a/src/dac.rs b/src/dac.rs index 7f99001..2c61b10 100644 --- a/src/dac.rs +++ b/src/dac.rs @@ -116,18 +116,21 @@ macro_rules! dac_output { } } - /// Mutably borrow the next output buffer to populate it with DAC codes. - pub fn prepare_buffer(&mut self) -> &mut [u16; SAMPLE_BUFFER_SIZE] { - self.next_buffer.as_mut().unwrap() + /// Acquire the next output buffer to populate it with DAC codes. + pub fn acquire_buffer( + &mut self, + ) -> &'static mut [u16; SAMPLE_BUFFER_SIZE] { + self.next_buffer.take().unwrap() } /// Enqueue the next buffer for transmission to the DAC. /// /// # Args /// * `data` - The next data to write to the DAC. - pub fn commit_buffer(&mut self) { - let next_buffer = self.next_buffer.take().unwrap(); - + pub fn release_buffer( + &mut self, + next_buffer: &'static mut [u16; SAMPLE_BUFFER_SIZE], + ) { // If the last transfer was not complete, we didn't write all our previous DAC codes. // Wait for all the DAC codes to get written as well. if self.first_transfer { @@ -135,7 +138,7 @@ macro_rules! dac_output { } else { // Note: If a device hangs up, check that this conditional is passing correctly, as // there is no time-out checks here in the interest of execution speed. - while self.transfer.get_transfer_complete_flag() == false {} + while !self.transfer.get_transfer_complete_flag() {} } // Start the next transfer. @@ -143,6 +146,7 @@ macro_rules! dac_output { let (prev_buffer, _) = self.transfer.next_transfer(next_buffer).unwrap(); + // .unwrap_none() https://github.com/rust-lang/rust/issues/62633 self.next_buffer.replace(prev_buffer); } } diff --git a/src/main.rs b/src/main.rs index d7f30c7..925e277 100644 --- a/src/main.rs +++ b/src/main.rs @@ -745,12 +745,12 @@ const APP: () = { #[task(binds=DMA1_STR3, resources=[adcs, dacs, iir_state, iir_ch], priority=2)] fn process(c: process::Context) { let adc_samples = [ - c.resources.adcs.0.transfer_complete_handler(), - c.resources.adcs.1.transfer_complete_handler(), + c.resources.adcs.0.acquire_buffer(), + c.resources.adcs.1.acquire_buffer(), ]; let dac_samples = [ - c.resources.dacs.0.prepare_buffer(), - c.resources.dacs.1.prepare_buffer(), + c.resources.dacs.0.acquire_buffer(), + c.resources.dacs.1.acquire_buffer(), ]; for channel in 0..adc_samples.len() { @@ -761,9 +761,12 @@ const APP: () = { dac_samples[channel][sample] = y as i16 as u16 ^ 0x8000; } } - - c.resources.dacs.0.commit_buffer(); - c.resources.dacs.1.commit_buffer(); + let [adc0, adc1] = adc_samples; + c.resources.adcs.0.release_buffer(adc0); + c.resources.adcs.0.release_buffer(adc1); + let [dac0, dac1] = dac_samples; + c.resources.dacs.0.release_buffer(dac0); + c.resources.dacs.1.release_buffer(dac1); } #[idle(resources=[net_interface, pounder, mac_addr, eth_mac, iir_state, iir_ch, afes])] From 468929690df7493baef5ae6537f09bee9443ec73 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 26 Nov 2020 14:19:09 +0100 Subject: [PATCH 30/71] iir: vminnm/vmaxnm --- Cargo.toml | 2 +- dsp/Cargo.toml | 3 +++ dsp/src/iir.rs | 27 +++++++++++++++++++++++++++ dsp/src/lib.rs | 1 + src/main.rs | 3 +++ 5 files changed, 35 insertions(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index 301956c..896eecf 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -62,7 +62,7 @@ branch = "dma" [features] semihosting = ["panic-semihosting", "cortex-m-log/semihosting"] bkpt = [ ] -nightly = ["cortex-m/inline-asm"] +nightly = ["cortex-m/inline-asm", "dsp/nightly"] [profile.dev] codegen-units = 1 diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml index 625d0f0..c8ef52b 100644 --- a/dsp/Cargo.toml +++ b/dsp/Cargo.toml @@ -6,3 +6,6 @@ edition = "2018" [dependencies] serde = { version = "1.0", features = ["derive"], default-features = false } + +[features] +nightly = [] diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index 04d7c8e..e081556 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -24,6 +24,7 @@ fn copysign(x: f32, y: f32) -> f32 { } } +#[cfg(not(feature = "nightly"))] fn max(x: f32, y: f32) -> f32 { if x > y { x @@ -32,6 +33,7 @@ fn max(x: f32, y: f32) -> f32 { } } +#[cfg(not(feature = "nightly"))] fn min(x: f32, y: f32) -> f32 { if x < y { x @@ -40,6 +42,31 @@ fn min(x: f32, y: f32) -> f32 { } } +#[cfg(feature = "nightly")] +fn max(x: f32, y: f32) -> f32 { + let o: f32; + unsafe { + asm!("vmaxnm.f32 {}, {}, {}", + lateout(sreg) o, in(sreg) x, in(sreg) y, + options(pure, nomem, nostack, preserves_flags) + ); + } + o +} + +#[cfg(feature = "nightly")] +fn min(x: f32, y: f32) -> f32 { + let o: f32; + unsafe { + asm!("vminnm.f32 {}, {}, {}", + lateout(sreg) o, in(sreg) x, in(sreg) y, + options(pure, nomem, nostack, preserves_flags) + ); + } + o +} + + // Multiply-accumulate vectors `x` and `a`. // // A.k.a. dot product. diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 3c44bbc..ac25d1e 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -1,3 +1,4 @@ #![no_std] +#![cfg_attr(feature = "nightly", feature(asm))] pub mod iir; diff --git a/src/main.rs b/src/main.rs index 3173158..15046b8 100644 --- a/src/main.rs +++ b/src/main.rs @@ -13,6 +13,9 @@ fn panic(_info: &core::panic::PanicInfo) -> ! { let gpiod = unsafe { &*hal::stm32::GPIOD::ptr() }; gpiod.odr.modify(|_, w| w.odr6().high().odr12().high()); // FP_LED_1, FP_LED_3 + #[cfg(feature = "nightly")] + core::intrinsics::abort(); + #[cfg(not(feature = "nightly"))] unsafe { core::intrinsics::abort(); } From ea3e343c39d7768fa6050c774ee360ec9473add4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 26 Nov 2020 14:30:09 +0100 Subject: [PATCH 31/71] cargo fmt [nfc] --- dsp/src/iir.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index e081556..33d8fdf 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -66,7 +66,6 @@ fn min(x: f32, y: f32) -> f32 { o } - // Multiply-accumulate vectors `x` and `a`. // // A.k.a. dot product. From 8cf380a488e5964a58ee420bb2312d2877dcc632 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 26 Nov 2020 14:40:24 +0100 Subject: [PATCH 32/71] dac/adc: doc cleanup [nfc] --- src/adc.rs | 6 +++--- src/dac.rs | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/adc.rs b/src/adc.rs index 145c5af..b275d27 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -35,7 +35,7 @@ static mut ADC_BUF: [[[u16; SAMPLE_BUFFER_SIZE]; 2]; 2] = macro_rules! adc_input { ($name:ident, $index:literal, $trigger_stream:ident, $data_stream:ident, $spi:ident, $trigger_channel:ident, $dma_req:ident) => { - /// SPI is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI TX FIFO + /// $spi is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI TX FIFO /// whenever the tim2 update dma request occurs. struct $spi { _channel: sampling_timer::tim2::$trigger_channel, @@ -68,7 +68,7 @@ macro_rules! adc_input { } } - /// Represents data associated with ADC0. + /// Represents data associated with ADC. pub struct $name { next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, transfer: Transfer< @@ -86,7 +86,7 @@ macro_rules! adc_input { } impl $name { - /// Construct the ADC0 input channel. + /// Construct the ADC input channel. /// /// # Args /// * `spi` - The SPI interface used to communicate with the ADC. diff --git a/src/dac.rs b/src/dac.rs index 2c61b10..95ae8b0 100644 --- a/src/dac.rs +++ b/src/dac.rs @@ -19,7 +19,7 @@ static mut DAC_BUF: [[[u16; SAMPLE_BUFFER_SIZE]; 2]; 2] = macro_rules! dac_output { ($name:ident, $index:literal, $data_stream:ident, $spi:ident, $trigger_channel:ident, $dma_req:ident) => { - /// SPI is used as a type for indicating a DMA transfer into the SPI TX FIFO + /// $spi is used as a type for indicating a DMA transfer into the SPI TX FIFO struct $spi { spi: hal::spi::Spi, _channel: sampling_timer::tim2::$trigger_channel, @@ -50,7 +50,7 @@ macro_rules! dac_output { } } - /// Represents data associated with DAC0. + /// Represents data associated with DAC. pub struct $name { next_buffer: Option<&'static mut [u16; SAMPLE_BUFFER_SIZE]>, // Note: SPI TX functionality may not be used from this structure to ensure safety with DMA. From fe764a47a5c26bb79dd7cc77d2d12df7dffd8d92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 26 Nov 2020 14:48:02 +0100 Subject: [PATCH 33/71] gha: clippy-check --- .github/workflows/ci.yml | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 40d2a76..c2498fc 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -33,15 +33,14 @@ jobs: - uses: actions-rs/toolchain@v1 with: profile: minimal - toolchain: stable + toolchain: nightly target: thumbv7em-none-eabihf override: true components: clippy - - name: cargo clippy - uses: actions-rs/cargo@v1 + - uses: actions-rs/clippy-check@v1 continue-on-error: true with: - command: clippy + token: ${{ secrets.GITHUB_TOKEN }} compile: runs-on: ubuntu-latest From ab50f550623627d377a7fef7bc7dc0861bed4999 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 26 Nov 2020 15:41:19 +0100 Subject: [PATCH 34/71] adc/dac: docstrings --- src/adc.rs | 2 +- src/dac.rs | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/adc.rs b/src/adc.rs index b275d27..02f686f 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -55,7 +55,7 @@ macro_rules! adc_input { /// SPI is configured to operate using 16-bit transfer words. type MemSize = u16; - /// SPI DMA requests are generated whenever TIM2 CH1 comparison occurs. + /// SPI DMA requests are generated whenever TIM2 CHx ($dma_req) comparison occurs. const REQUEST_LINE: Option = Some(DMAReq::$dma_req as u8); /// Whenever the DMA request occurs, it should write into SPI's TX FIFO to start a DMA diff --git a/src/dac.rs b/src/dac.rs index 95ae8b0..c586e2f 100644 --- a/src/dac.rs +++ b/src/dac.rs @@ -41,7 +41,7 @@ macro_rules! dac_output { /// SPI is configured to operate using 16-bit transfer words. type MemSize = u16; - /// SPI DMA requests are generated whenever TIM2 CH3 comparison occurs. + /// SPI DMA requests are generated whenever TIM2 CHx ($dma_req) comparison occurs. const REQUEST_LINE: Option = Some(DMAReq::$dma_req as u8); /// Whenever the DMA request occurs, it should write into SPI's TX FIFO. From 7fc6f5c4adfa3276ce92a40f3785130dda90d812 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 26 Nov 2020 16:24:42 +0100 Subject: [PATCH 35/71] clippy lints --- .github/workflows/ci.yml | 2 +- ad9959/src/lib.rs | 125 ++++++++++++++------------------------- src/adc.rs | 4 +- src/dac.rs | 4 +- src/main.rs | 16 +++-- src/pounder/mod.rs | 2 +- src/server.rs | 6 +- 7 files changed, 62 insertions(+), 97 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index c2498fc..1f131c8 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -17,7 +17,7 @@ jobs: - uses: actions-rs/toolchain@v1 with: profile: minimal - toolchain: stable + toolchain: nightly override: true components: rustfmt - name: cargo fmt --check diff --git a/ad9959/src/lib.rs b/ad9959/src/lib.rs index d12041c..0f6b237 100644 --- a/ad9959/src/lib.rs +++ b/ad9959/src/lib.rs @@ -127,30 +127,27 @@ where system_clock_multiplier: 1, }; - ad9959.io_update.set_low().or_else(|_| Err(Error::Pin))?; + ad9959.io_update.set_low().or(Err(Error::Pin))?; // Reset the AD9959 - reset_pin.set_high().or_else(|_| Err(Error::Pin))?; + reset_pin.set_high().or(Err(Error::Pin))?; // Delay for a clock cycle to allow the device to reset. ad9959 .delay .delay_ms((1000.0 / clock_frequency as f32) as u8); - reset_pin.set_low().or_else(|_| Err(Error::Pin))?; + reset_pin.set_low().or(Err(Error::Pin))?; ad9959 .interface .configure_mode(Mode::SingleBitTwoWire) - .map_err(|_| Error::Interface)?; + .or(Err(Error::Interface))?; // Program the interface configuration in the AD9959. Default to all channels enabled. let mut csr: [u8; 1] = [0xF0]; csr[0].set_bits(1..3, desired_mode as u8); - ad9959 - .interface - .write(Register::CSR as u8, &csr) - .map_err(|_| Error::Interface)?; + ad9959.write(Register::CSR, &csr)?; // Latch the configuration registers to make them active. ad9959.latch_configuration()?; @@ -158,14 +155,11 @@ where ad9959 .interface .configure_mode(desired_mode) - .map_err(|_| Error::Interface)?; + .or(Err(Error::Interface))?; // Read back the CSR to ensure it specifies the mode correctly. let mut updated_csr: [u8; 1] = [0]; - ad9959 - .interface - .read(Register::CSR as u8, &mut updated_csr) - .map_err(|_| Error::Interface)?; + ad9959.read(Register::CSR, &mut updated_csr)?; if updated_csr[0] != csr[0] { return Err(Error::Check); } @@ -175,14 +169,26 @@ where Ok(ad9959) } + fn read(&mut self, reg: Register, data: &mut [u8]) -> Result<(), Error> { + self.interface + .read(reg as u8, data) + .or(Err(Error::Interface)) + } + + fn write(&mut self, reg: Register, data: &[u8]) -> Result<(), Error> { + self.interface + .write(reg as u8, data) + .or(Err(Error::Interface)) + } + /// Latch the DDS configuration to ensure it is active on the output channels. fn latch_configuration(&mut self) -> Result<(), Error> { - self.io_update.set_high().or_else(|_| Err(Error::Pin))?; + self.io_update.set_high().or(Err(Error::Pin))?; // The SYNC_CLK is 1/4 the system clock frequency. The IO_UPDATE pin must be latched for one // full SYNC_CLK pulse to register. For safety, we latch for 5 here. self.delay .delay_ms((5000.0 / self.system_clock_frequency()) as u8); - self.io_update.set_low().or_else(|_| Err(Error::Pin))?; + self.io_update.set_low().or(Err(Error::Pin))?; Ok(()) } @@ -214,17 +220,13 @@ where // TODO: Update / disable any enabled channels? let mut fr1: [u8; 3] = [0, 0, 0]; - self.interface - .read(Register::FR1 as u8, &mut fr1) - .map_err(|_| Error::Interface)?; + self.read(Register::FR1, &mut fr1)?; fr1[0].set_bits(2..=6, multiplier); let vco_range = frequency > 255e6; fr1[0].set_bit(7, vco_range); - self.interface - .write(Register::FR1 as u8, &fr1) - .map_err(|_| Error::Interface)?; + self.write(Register::FR1, &fr1)?; self.system_clock_multiplier = multiplier; Ok(self.system_clock_frequency()) @@ -238,9 +240,7 @@ where /// Get the current reference clock multiplier. pub fn get_reference_clock_multiplier(&mut self) -> Result { let mut fr1: [u8; 3] = [0, 0, 0]; - self.interface - .read(Register::FR1 as u8, &mut fr1) - .map_err(|_| Error::Interface)?; + self.read(Register::FR1, &mut fr1)?; Ok(fr1[0].get_bits(2..=6) as u8) } @@ -254,46 +254,34 @@ where /// True if the self test succeeded. False otherwise. pub fn self_test(&mut self) -> Result { let mut csr: [u8; 1] = [0]; - self.interface - .read(Register::CSR as u8, &mut csr) - .map_err(|_| Error::Interface)?; + self.read(Register::CSR, &mut csr)?; let old_csr = csr[0]; // Enable all channels. csr[0].set_bits(4..8, 0xF); - self.interface - .write(Register::CSR as u8, &csr) - .map_err(|_| Error::Interface)?; + self.write(Register::CSR, &csr)?; // Read back the enable. csr[0] = 0; - self.interface - .read(Register::CSR as u8, &mut csr) - .map_err(|_| Error::Interface)?; + self.read(Register::CSR, &mut csr)?; if csr[0].get_bits(4..8) != 0xF { return Ok(false); } // Clear all channel enables. csr[0].set_bits(4..8, 0x0); - self.interface - .write(Register::CSR as u8, &csr) - .map_err(|_| Error::Interface)?; + self.write(Register::CSR, &csr)?; // Read back the enable. csr[0] = 0xFF; - self.interface - .read(Register::CSR as u8, &mut csr) - .map_err(|_| Error::Interface)?; + self.read(Register::CSR, &mut csr)?; if csr[0].get_bits(4..8) != 0 { return Ok(false); } // Restore the CSR. csr[0] = old_csr; - self.interface - .write(Register::CSR as u8, &csr) - .map_err(|_| Error::Interface)?; + self.write(Register::CSR, &csr)?; Ok(true) } @@ -307,13 +295,9 @@ where /// Enable an output channel. pub fn enable_channel(&mut self, channel: Channel) -> Result<(), Error> { let mut csr: [u8; 1] = [0]; - self.interface - .read(Register::CSR as u8, &mut csr) - .map_err(|_| Error::Interface)?; + self.read(Register::CSR, &mut csr)?; csr[0].set_bit(channel as usize + 4, true); - self.interface - .write(Register::CSR as u8, &csr) - .map_err(|_| Error::Interface)?; + self.write(Register::CSR, &csr)?; Ok(()) } @@ -321,13 +305,9 @@ where /// Disable an output channel. pub fn disable_channel(&mut self, channel: Channel) -> Result<(), Error> { let mut csr: [u8; 1] = [0]; - self.interface - .read(Register::CSR as u8, &mut csr) - .map_err(|_| Error::Interface)?; + self.read(Register::CSR, &mut csr)?; csr[0].set_bit(channel as usize + 4, false); - self.interface - .write(Register::CSR as u8, &csr) - .map_err(|_| Error::Interface)?; + self.write(Register::CSR, &csr)?; Ok(()) } @@ -335,9 +315,7 @@ where /// Determine if an output channel is enabled. pub fn is_enabled(&mut self, channel: Channel) -> Result { let mut csr: [u8; 1] = [0; 1]; - self.interface - .read(Register::CSR as u8, &mut csr) - .map_err(|_| Error::Interface)?; + self.read(Register::CSR, &mut csr)?; Ok(csr[0].get_bit(channel as usize + 4)) } @@ -357,28 +335,20 @@ where // Disable all other outputs so that we can update the configuration register of only the // specified channel. let mut csr: [u8; 1] = [0]; - self.interface - .read(Register::CSR as u8, &mut csr) - .map_err(|_| Error::Interface)?; + self.read(Register::CSR, &mut csr)?; let mut new_csr = csr; new_csr[0].set_bits(4..8, 0); new_csr[0].set_bit(4 + channel as usize, true); - self.interface - .write(Register::CSR as u8, &new_csr) - .map_err(|_| Error::Interface)?; + self.write(Register::CSR, &new_csr)?; - self.interface - .write(register as u8, &data) - .map_err(|_| Error::Interface)?; + self.write(register, &data)?; // Latch the configuration and restore the previous CSR. Note that the re-enable of the // channel happens immediately, so the CSR update does not need to be latched. self.latch_configuration()?; - self.interface - .write(Register::CSR as u8, &csr) - .map_err(|_| Error::Interface)?; + self.write(Register::CSR, &csr)?; Ok(()) } @@ -398,27 +368,18 @@ where // Disable all other channels in the CSR so that we can read the configuration register of // only the desired channel. let mut csr: [u8; 1] = [0]; - self.interface - .read(Register::CSR as u8, &mut csr) - .map_err(|_| Error::Interface)?; + self.read(Register::CSR, &mut csr)?; let mut new_csr = csr; new_csr[0].set_bits(4..8, 0); new_csr[0].set_bit(4 + channel as usize, true); - self.interface - .write(Register::CSR as u8, &new_csr) - .map_err(|_| Error::Interface)?; - - self.interface - .read(register as u8, &mut data) - .map_err(|_| Error::Interface)?; + self.write(Register::CSR, &new_csr)?; + self.read(register, &mut data)?; // Restore the previous CSR. Note that the re-enable of the channel happens immediately, so // the CSR update does not need to be latched. - self.interface - .write(Register::CSR as u8, &csr) - .map_err(|_| Error::Interface)?; + self.write(Register::CSR, &csr)?; Ok(()) } diff --git a/src/adc.rs b/src/adc.rs index 9d344dc..87702b0 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -244,7 +244,7 @@ impl Adc0Input { // Wait for the transfer to fully complete before continuing. // Note: If a device hangs up, check that this conditional is passing correctly, as there is // no time-out checks here in the interest of execution speed. - while self.transfer.get_transfer_complete_flag() == false {} + while !self.transfer.get_transfer_complete_flag() {} // Start the next transfer. self.transfer.clear_interrupts(); @@ -371,7 +371,7 @@ impl Adc1Input { // Wait for the transfer to fully complete before continuing. // Note: If a device hangs up, check that this conditional is passing correctly, as there is // no time-out checks here in the interest of execution speed. - while self.transfer.get_transfer_complete_flag() == false {} + while !self.transfer.get_transfer_complete_flag() {} // Start the next transfer. self.transfer.clear_interrupts(); diff --git a/src/dac.rs b/src/dac.rs index d6325c2..ef1e267 100644 --- a/src/dac.rs +++ b/src/dac.rs @@ -207,7 +207,7 @@ impl Dac0Output { } else { // Note: If a device hangs up, check that this conditional is passing correctly, as // there is no time-out checks here in the interest of execution speed. - while self.transfer.get_transfer_complete_flag() == false {} + while !self.transfer.get_transfer_complete_flag() {} } // Start the next transfer. @@ -303,7 +303,7 @@ impl Dac1Output { } else { // Note: If a device hangs up, check that this conditional is passing correctly, as // there is no time-out checks here in the interest of execution speed. - while self.transfer.get_transfer_complete_flag() == false {} + while !self.transfer.get_transfer_complete_flag() {} } // Start the next transfer. diff --git a/src/main.rs b/src/main.rs index ece4340..a90516c 100644 --- a/src/main.rs +++ b/src/main.rs @@ -140,6 +140,7 @@ macro_rules! route_request { match $request.attribute { $( $read_attribute => { + #[allow(clippy::redundant_closure_call)] let value = match $getter() { Ok(data) => data, Err(_) => return server::Response::error($request.attribute, @@ -168,6 +169,7 @@ macro_rules! route_request { "Failed to decode value"), }; + #[allow(clippy::redundant_closure_call)] match $setter(new_value) { Ok(_) => server::Response::success($request.attribute, &$request.value), Err(_) => server::Response::error($request.attribute, @@ -678,7 +680,7 @@ const APP: () = { dp.ETHERNET_MTL, dp.ETHERNET_DMA, &mut DES_RING, - mac_addr.clone(), + mac_addr, ccdr.peripheral.ETH1MAC, &ccdr.clocks, ) @@ -729,8 +731,8 @@ const APP: () = { sampling_timer.start(); init::LateResources { - afe0: afe0, - afe1: afe1, + afe0, + afe1, adcs, dacs, @@ -928,10 +930,12 @@ const APP: () = { } }), "stabilizer/afe0/gain": afe::Gain, (|gain| { - Ok::<(), ()>(c.resources.afe0.set_gain(gain)) + c.resources.afe0.set_gain(gain); + Ok::<(), ()>(()) }), "stabilizer/afe1/gain": afe::Gain, (|gain| { - Ok::<(), ()>(c.resources.afe1.set_gain(gain)) + c.resources.afe1.set_gain(gain); + Ok::<(), ()>(()) }) ] ) @@ -943,7 +947,7 @@ const APP: () = { &mut sockets, net::time::Instant::from_millis(time as i64), ) { - Ok(changed) => changed == false, + Ok(changed) => !changed, Err(net::Error::Unrecognized) => true, Err(e) => { info!("iface poll error: {:?}", e); diff --git a/src/pounder/mod.rs b/src/pounder/mod.rs index 6940e0e..8054243 100644 --- a/src/pounder/mod.rs +++ b/src/pounder/mod.rs @@ -17,7 +17,7 @@ const ATT_RST_N_PIN: u8 = 8 + 5; const ATT_LE3_PIN: u8 = 8 + 3; const ATT_LE2_PIN: u8 = 8 + 2; const ATT_LE1_PIN: u8 = 8 + 1; -const ATT_LE0_PIN: u8 = 8 + 0; +const ATT_LE0_PIN: u8 = 8; #[derive(Debug, Copy, Clone)] pub enum Error { diff --git a/src/server.rs b/src/server.rs index 3184426..2803805 100644 --- a/src/server.rs +++ b/src/server.rs @@ -89,7 +89,7 @@ impl Response { /// Args: /// * `attrbute` - The attribute of the success. /// * `value` - The value of the attribute. - pub fn success<'a, 'b>(attribute: &'a str, value: &'b str) -> Self { + pub fn success(attribute: &str, value: &str) -> Self { let mut res = Self { code: 200, attribute: String::from(attribute), @@ -106,7 +106,7 @@ impl Response { /// Args: /// * `attrbute` - The attribute of the success. /// * `message` - The message denoting the error. - pub fn error<'a, 'b>(attribute: &'a str, message: &'b str) -> Self { + pub fn error(attribute: &str, message: &str) -> Self { let mut res = Self { code: 400, attribute: String::from(attribute), @@ -123,7 +123,7 @@ impl Response { /// Args: /// * `attrbute` - The attribute of the success. /// * `message` - The message denoting the status. - pub fn custom<'a>(code: i32, message: &'a str) -> Self { + pub fn custom(code: i32, message: &str) -> Self { let mut res = Self { code, attribute: String::from(""), From 128e7dd78ea57b0bc2be3cc440fc4bd92ff91c90 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 26 Nov 2020 16:45:57 +0100 Subject: [PATCH 36/71] more nightly clippy lints --- ad9959/src/lib.rs | 4 ++-- src/pounder/attenuators.rs | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ad9959/src/lib.rs b/ad9959/src/lib.rs index 0f6b237..446825f 100644 --- a/ad9959/src/lib.rs +++ b/ad9959/src/lib.rs @@ -208,7 +208,7 @@ where ) -> Result { self.reference_clock_frequency = reference_clock_frequency; - if multiplier != 1 && (multiplier > 20 || multiplier < 4) { + if multiplier != 1 && !(4..=20).contains(&multiplier) { return Err(Error::Bounds); } @@ -438,7 +438,7 @@ where channel: Channel, amplitude: f32, ) -> Result { - if amplitude < 0.0 || amplitude > 1.0 { + if !(0.0..=1.0).contains(&litude) { return Err(Error::Bounds); } diff --git a/src/pounder/attenuators.rs b/src/pounder/attenuators.rs index 2ffa75f..156da9f 100644 --- a/src/pounder/attenuators.rs +++ b/src/pounder/attenuators.rs @@ -19,7 +19,7 @@ pub trait AttenuatorInterface { channel: Channel, attenuation: f32, ) -> Result { - if attenuation > 31.5 || attenuation < 0.0 { + if !(0.0..=31.5).contains(&attenuation) { return Err(Error::Bounds); } From 74349e5d68f4663deff5b9df9d1ac52d069ff294 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 27 Nov 2020 10:36:30 +0100 Subject: [PATCH 37/71] iir: more generic math helpers, use core::intrinsics --- dsp/src/iir.rs | 54 +++++++++++++++++++++++++------------------------- dsp/src/lib.rs | 2 +- 2 files changed, 28 insertions(+), 28 deletions(-) diff --git a/dsp/src/iir.rs b/dsp/src/iir.rs index 33d8fdf..c6f2100 100644 --- a/dsp/src/iir.rs +++ b/dsp/src/iir.rs @@ -1,4 +1,4 @@ -use core::ops::{Add, Mul}; +use core::ops::{Add, Mul, Neg}; use serde::{Deserialize, Serialize}; use core::f32; @@ -8,16 +8,24 @@ use core::f32; // `compiler-intrinsics`/llvm should have better (robust, universal, and // faster) implementations. -fn abs(x: f32) -> f32 { - if x >= 0. { +fn abs(x: T) -> T +where + T: PartialOrd + Default + Neg, +{ + if x >= T::default() { x } else { -x } } -fn copysign(x: f32, y: f32) -> f32 { - if (x >= 0. && y >= 0.) || (x <= 0. && y <= 0.) { +fn copysign(x: T, y: T) -> T +where + T: PartialOrd + Default + Neg, +{ + if (x >= T::default() && y >= T::default()) + || (x <= T::default() && y <= T::default()) + { x } else { -x @@ -25,7 +33,10 @@ fn copysign(x: f32, y: f32) -> f32 { } #[cfg(not(feature = "nightly"))] -fn max(x: f32, y: f32) -> f32 { +fn max(x: T, y: T) -> T +where + T: PartialOrd, +{ if x > y { x } else { @@ -34,7 +45,10 @@ fn max(x: f32, y: f32) -> f32 { } #[cfg(not(feature = "nightly"))] -fn min(x: f32, y: f32) -> f32 { +fn min(x: T, y: T) -> T +where + T: PartialOrd, +{ if x < y { x } else { @@ -44,26 +58,12 @@ fn min(x: f32, y: f32) -> f32 { #[cfg(feature = "nightly")] fn max(x: f32, y: f32) -> f32 { - let o: f32; - unsafe { - asm!("vmaxnm.f32 {}, {}, {}", - lateout(sreg) o, in(sreg) x, in(sreg) y, - options(pure, nomem, nostack, preserves_flags) - ); - } - o + core::intrinsics::maxnumf32(x, y) } #[cfg(feature = "nightly")] fn min(x: f32, y: f32) -> f32 { - let o: f32; - unsafe { - asm!("vminnm.f32 {}, {}, {}", - lateout(sreg) o, in(sreg) x, in(sreg) y, - options(pure, nomem, nostack, preserves_flags) - ); - } - o + core::intrinsics::minnumf32(x, y) } // Multiply-accumulate vectors `x` and `a`. @@ -76,7 +76,7 @@ where { x.iter() .zip(a) - .map(|(&x, &a)| x * a) + .map(|(x, a)| *x * *a) .fold(y0, |y, xa| y + xa) } @@ -84,10 +84,10 @@ where /// /// To represent the IIR state (input and output memory) during the filter update /// this contains the three inputs (x0, x1, x2) and the two outputs (y1, y2) -/// concatenated. +/// concatenated. Lower indices correspond to more recent samples. /// To represent the IIR coefficients, this contains the feed-forward -/// coefficients (b0, b1, b2) followd by the feed-back coefficients (a1, a2), -/// all normalized such that a0 = 1. +/// coefficients (b0, b1, b2) followd by the negated feed-back coefficients +/// (-a1, -a2), all five normalized such that a0 = 1. pub type IIRState = [f32; 5]; /// IIR configuration. diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index ac25d1e..b2acf34 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -1,4 +1,4 @@ #![no_std] -#![cfg_attr(feature = "nightly", feature(asm))] +#![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] pub mod iir; From feb229ddd5befd9fdb6fa873458db502d722e82c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Mon, 30 Nov 2020 12:02:14 +0100 Subject: [PATCH 38/71] cargo: add docs for target cpu/features --- .cargo/config | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.cargo/config b/.cargo/config index 9968846..ea1d6c0 100644 --- a/.cargo/config +++ b/.cargo/config @@ -2,8 +2,14 @@ runner = "gdb-multiarch -q -x openocd.gdb" rustflags = [ "-C", "link-arg=-Tlink.x", +# The target (below) defaults to cortex-m4 +# There currently are two different options to go beyond that: +# 1. cortex-m7 has the right flags and instructions (FPU) but no instruction schedule yet "-C", "target-cpu=cortex-m7", +# 2. cortex-m4 with the additional fpv5 instructions and a potentially +# better-than-nothing instruction schedule "-C", "target-feature=+fp-armv8d16", +# When combined they are equivalent to (1) alone ] [build] From b1301a61849ef0fe7669073ed178130d72d66f53 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Mon, 30 Nov 2020 15:04:31 +0100 Subject: [PATCH 39/71] dac,adc: spelling --- src/adc.rs | 4 ++-- src/dac.rs | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/adc.rs b/src/adc.rs index 02f686f..43fb0a5 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -27,7 +27,7 @@ static mut SPI_START: [u16; 1] = [0x00]; // The following global buffers are used for the ADC sample DMA transfers. Two buffers are used for // each transfer in a ping-pong buffer configuration (one is being acquired while the other is being // processed). Note that the contents of AXI SRAM is uninitialized, so the buffer contents on -// startup are undefined. The dimension are `ADC_BUF[adc_index][ping_pong_index][sample_index]`. +// startup are undefined. The dimensions are `ADC_BUF[adc_index][ping_pong_index][sample_index]`. #[link_section = ".axisram.buffers"] static mut ADC_BUF: [[[u16; SAMPLE_BUFFER_SIZE]; 2]; 2] = [[[0; SAMPLE_BUFFER_SIZE]; 2]; 2]; @@ -35,7 +35,7 @@ static mut ADC_BUF: [[[u16; SAMPLE_BUFFER_SIZE]; 2]; 2] = macro_rules! adc_input { ($name:ident, $index:literal, $trigger_stream:ident, $data_stream:ident, $spi:ident, $trigger_channel:ident, $dma_req:ident) => { - /// $spi is used as a ZST (zero-sized type) for indicating a DMA transfer into the SPI TX FIFO + /// $spi is used as a type for indicating a DMA transfer into the SPI TX FIFO /// whenever the tim2 update dma request occurs. struct $spi { _channel: sampling_timer::tim2::$trigger_channel, diff --git a/src/dac.rs b/src/dac.rs index c586e2f..d96109c 100644 --- a/src/dac.rs +++ b/src/dac.rs @@ -11,7 +11,7 @@ use super::{ // The following global buffers are used for the DAC code DMA transfers. Two buffers are used for // each transfer in a ping-pong buffer configuration (one is being prepared while the other is being // processed). Note that the contents of AXI SRAM is uninitialized, so the buffer contents on -// startup are undefined. The dimension are `ADC_BUF[adc_index][ping_pong_index][sample_index]`. +// startup are undefined. The dimensions are `ADC_BUF[adc_index][ping_pong_index][sample_index]`. #[link_section = ".axisram.buffers"] static mut DAC_BUF: [[[u16; SAMPLE_BUFFER_SIZE]; 2]; 2] = [[[0; SAMPLE_BUFFER_SIZE]; 2]; 2]; From 34d59dac5dd876b1f1ae2ea20333f1cc35cdfb70 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Mon, 30 Nov 2020 15:38:14 +0100 Subject: [PATCH 40/71] adc: merge acquire_buffer and release_buffer again --- src/adc.rs | 17 +++-------------- src/main.rs | 3 --- 2 files changed, 3 insertions(+), 17 deletions(-) diff --git a/src/adc.rs b/src/adc.rs index 43fb0a5..e9120aa 100644 --- a/src/adc.rs +++ b/src/adc.rs @@ -185,9 +185,7 @@ macro_rules! adc_input { /// /// # Returns /// A reference to the underlying buffer that has been filled with ADC samples. - pub fn acquire_buffer( - &mut self, - ) -> &'static mut [u16; SAMPLE_BUFFER_SIZE] { + pub fn acquire_buffer(&mut self) -> &[u16; SAMPLE_BUFFER_SIZE] { // Wait for the transfer to fully complete before continuing. // Note: If a device hangs up, check that this conditional is passing correctly, as there is // no time-out checks here in the interest of execution speed. @@ -200,18 +198,9 @@ macro_rules! adc_input { let (prev_buffer, _) = self.transfer.next_transfer(next_buffer).unwrap(); - prev_buffer - } + self.next_buffer.replace(prev_buffer); // .unwrap_none() https://github.com/rust-lang/rust/issues/62633 - /// Release a buffer of ADC samples to the pool. - /// - /// # Args - /// * `next_buffer` - Buffer of ADC samples to be re-used. - pub fn release_buffer( - &mut self, - next_buffer: &'static mut [u16; SAMPLE_BUFFER_SIZE], - ) { - self.next_buffer.replace(next_buffer); // .unwrap_none() https://github.com/rust-lang/rust/issues/62633 + self.next_buffer.as_ref().unwrap() } } }; diff --git a/src/main.rs b/src/main.rs index 0aaf669..e6f83b5 100644 --- a/src/main.rs +++ b/src/main.rs @@ -763,9 +763,6 @@ const APP: () = { dac_samples[channel][sample] = y as i16 as u16 ^ 0x8000; } } - let [adc0, adc1] = adc_samples; - c.resources.adcs.0.release_buffer(adc0); - c.resources.adcs.0.release_buffer(adc1); let [dac0, dac1] = dac_samples; c.resources.dacs.0.release_buffer(dac0); c.resources.dacs.1.release_buffer(dac1); From 24222821b52696520df2d3f9555e2fad36a62d19 Mon Sep 17 00:00:00 2001 From: Niklas Kuhrmeyer Date: Thu, 3 Dec 2020 14:10:28 +0100 Subject: [PATCH 41/71] Added cascaded IIR with server commands for up to 2 cascaded IIRs per channel. --- src/main.rs | 67 +++++++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 55 insertions(+), 12 deletions(-) diff --git a/src/main.rs b/src/main.rs index 845a9b4..f2c3baa 100644 --- a/src/main.rs +++ b/src/main.rs @@ -60,6 +60,9 @@ const SAMPLE_FREQUENCY_KHZ: u32 = 500; // The desired ADC sample processing buffer size. const SAMPLE_BUFFER_SIZE: usize = 1; +// The number of cascaded IIR biquads per channel. Select 1 or 2! +const IIR_CASCADE_LENGTH: usize = 1; + #[link_section = ".sram3.eth"] static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new(); @@ -210,10 +213,11 @@ const APP: () = { pounder: Option>, - #[init([[0.; 5]; 2])] - iir_state: [iir::IIRState; 2], - #[init([iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; 2])] - iir_ch: [iir::IIR; 2], + // Format: iir_state[ch][cascade-no][coeff] + #[init([[[0.; 5]; IIR_CASCADE_LENGTH];2])] + iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2], + #[init([[iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])] + iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], } #[init] @@ -761,8 +765,11 @@ const APP: () = { for channel in 0..adc_samples.len() { for sample in 0..adc_samples[0].len() { let x = f32::from(adc_samples[channel][sample] as i16); - let y = c.resources.iir_ch[channel] - .update(&mut c.resources.iir_state[channel], x); + let mut y = x; + for i in 0..c.resources.iir_state[channel].len() { + y = c.resources.iir_ch[channel][i] + .update(&mut c.resources.iir_state[channel][i], y); + } // Note(unsafe): The filter limits ensure that the value is in range. // The truncation introduces 1/2 LSB distortion. let y = unsafe { y.to_int_unchecked::() }; @@ -776,6 +783,7 @@ const APP: () = { } #[idle(resources=[net_interface, pounder, mac_addr, eth_mac, iir_state, iir_ch, afes])] + fn idle(mut c: idle::Context) -> ! { let mut socket_set_entries: [_; 8] = Default::default(); let mut sockets = @@ -827,10 +835,23 @@ const APP: () = { let state = c.resources.iir_state.lock(|iir_state| server::Status { t: time, - x0: iir_state[0][0], - y0: iir_state[0][2], - x1: iir_state[1][0], - y1: iir_state[1][2], + x0: iir_state[0][0][0], + y0: iir_state[0][0][2], + x1: iir_state[1][0][0], + y1: iir_state[1][0][2], + }); + + Ok::(state) + }), + // "_b" means cascades 2nd IIR + "stabilizer/iir_b/state": (|| { + let state = c.resources.iir_state.lock(|iir_state| + server::Status { + t: time, + x0: iir_state[0][IIR_CASCADE_LENGTH-1][0], + y0: iir_state[0][IIR_CASCADE_LENGTH-1][2], + x1: iir_state[1][IIR_CASCADE_LENGTH-1][0], + y1: iir_state[1][IIR_CASCADE_LENGTH-1][2], }); Ok::(state) @@ -880,7 +901,7 @@ const APP: () = { return Err(()); } - iir_ch[req.channel as usize] = req.iir; + iir_ch[req.channel as usize][0] = req.iir; Ok::(req) }) @@ -891,7 +912,29 @@ const APP: () = { return Err(()); } - iir_ch[req.channel as usize] = req.iir; + iir_ch[req.channel as usize][0] = req.iir; + + Ok::(req) + }) + }), + "stabilizer/iir_b0/state": server::IirRequest, (|req: server::IirRequest| { + c.resources.iir_ch.lock(|iir_ch| { + if req.channel > 1 { + return Err(()); + } + + iir_ch[req.channel as usize][IIR_CASCADE_LENGTH-1] = req.iir; + + Ok::(req) + }) + }), + "stabilizer/iir_b1/state": server::IirRequest,(|req: server::IirRequest| { + c.resources.iir_ch.lock(|iir_ch| { + if req.channel > 1 { + return Err(()); + } + + iir_ch[req.channel as usize][IIR_CASCADE_LENGTH-1] = req.iir; Ok::(req) }) From 644d85c115f0658cfc61b0324643dd9ac85dbe76 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Thu, 3 Dec 2020 19:09:54 +0100 Subject: [PATCH 42/71] pll: init --- dsp/src/lib.rs | 3 +- dsp/src/pll.rs | 77 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 79 insertions(+), 1 deletion(-) create mode 100644 dsp/src/pll.rs diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index b2acf34..9b9e966 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -1,4 +1,5 @@ -#![no_std] +#![cfg_attr(not(test), no_std)] #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] pub mod iir; +pub mod pll; diff --git a/dsp/src/pll.rs b/dsp/src/pll.rs new file mode 100644 index 0000000..8cef99b --- /dev/null +++ b/dsp/src/pll.rs @@ -0,0 +1,77 @@ +use serde::{Deserialize, Serialize}; + +/// Type-II, sampled phase, discrete time PLL +/// +/// This PLL tracks the frequency and phase of an input signal with respect to the sampling clock. +/// The transfer function is I^2,I from input phase to output phase and P,I from input phase to +/// output frequency. +/// +/// The PLL locks to any frequency (i.e. it locks to the alias in the first Nyquist zone) and is +/// stable for any gain (1 <= shift <= 30). It has a single parameter that determines the loop +/// bandwidth in octave steps. The gain can be changed freely between updates. +/// +/// The frequency settling time constant for an (any) frequency jump is `1 << shift` update cycles. +/// The phase settling time in response to a frequency jump is about twice that. The loop bandwidth +/// is about `1/(2*pi*(1 << shift))` in units of the sample rate. +/// +/// All math is naturally wrapping 32 bit integer. Phase and frequency are understood modulo that +/// overflow in the first Nyquist zone. Expressing the IIR equations in other ways (e.g. single +/// (T)-DF-{I,II} biquad/IIR) would break on overflow. +/// +/// There are no floating point rounding errors here. But there is integer quantization/truncation +/// error of the `shift` lowest bits leading to a phase offset for very low gains. Truncation +/// bias is applied. Rounding is "half up". +/// +/// This PLL does not unwrap phase slips during lock acquisition. This can and should be +/// implemented elsewhere by (down) scaling and then unwrapping the input phase and (up) scaling +/// and wrapping output phase and frequency. This affects dynamic range accordingly. +/// +/// The extension to I^3,I^2,I behavior to track chirps phase-accurately or to i64 data to +/// increase resolution for extremely narrowband applications is obvious. +#[derive(Copy, Clone, Default, Deserialize, Serialize)] +pub struct PLLState { + // last input phase + x: i32, + // filtered frequency + f: i32, + // filtered output phase + y: i32, +} + +impl PLLState { + /// Update the PLL with a new phase sample. + /// + /// Args: + /// * `input`: New input phase sample. + /// * `shift`: Error scaling. The frequency gain per update is `1/(1 << shift)`. The phase gain + /// is always twice the frequency gain. + /// + /// Returns: + /// A tuple of instantaneous phase and frequency (the current phase increment). + pub fn update(&mut self, x: i32, shift: u8) -> (i32, i32) { + debug_assert!(shift >= 1 && shift <= 31); + let bias = 1i32 << shift; + let e = x.wrapping_sub(self.f); + self.f = self.f.wrapping_add( + (bias >> 1).wrapping_add(e).wrapping_sub(self.x) >> shift, + ); + self.x = x; + let f = self.f.wrapping_add( + bias.wrapping_add(e).wrapping_sub(self.y) >> shift - 1, + ); + self.y = self.y.wrapping_add(f); + (self.y, f) + } +} + +#[cfg(test)] +mod tests { + use super::*; + #[test] + fn mini() { + let mut p = PLLState::default(); + let (y, f) = p.update(0x10000, 10); + assert_eq!(y, 0xc2); + assert_eq!(f, y); + } +} From b34c8bb8a1287c7c037a80fb070c3523ccb08557 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 18 Nov 2020 14:55:55 -0800 Subject: [PATCH 43/71] add github CI test workflow --- .github/workflows/ci.yml | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 1f131c8..253a307 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -77,6 +77,25 @@ jobs: command: build args: --release --features semihosting + test: + runs-on: ubuntu-latest + strategy: + matrix: + toolchain: + - stable + - beta + steps: + - uses: actions/checkout@v2 + - name: Install Rust ${{ matrix.toolchain }} + uses: actions-rs/toolchain@v1 + with: + toolchain: ${{ matrix.toolchain }} + - name: cargo test + uses: actions-rs/cargo@v1 + with: + command: test + args: --manifest-path dsp/Cargo.toml --target=x86_64-unknown-linux-gnu + # Tell bors about it # https://github.com/rtic-rs/cortex-m-rtic/blob/8a4f9c6b8ae91bebeea0791680f89375a78bffc6/.github/workflows/build.yml#L566-L603 ci-success: From 9a83d565ae44697a2e3a452e5946298d794c623b Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sun, 22 Nov 2020 14:32:43 -0800 Subject: [PATCH 44/71] add dsp/target to gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 265b7f5..68b644a 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ /target +/dsp/target .gdb_history From 85adc8b1e1e348c6c3fd8c3d2377f5d9e4fdc58e Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sun, 22 Nov 2020 14:34:38 -0800 Subject: [PATCH 45/71] add lockin module --- Cargo.lock | 7 + dsp/Cargo.lock | 7 + dsp/Cargo.toml | 1 + dsp/src/lib.rs | 1 + dsp/src/lockin.rs | 534 ++++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 550 insertions(+) create mode 100644 dsp/src/lockin.rs diff --git a/Cargo.lock b/Cargo.lock index c804d71..080623c 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -189,6 +189,7 @@ dependencies = [ name = "dsp" version = "0.1.0" dependencies = [ + "libm", "serde", ] @@ -297,6 +298,12 @@ dependencies = [ "hashbrown", ] +[[package]] +name = "libm" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c7d73b3f436185384286bd8098d17ec07c9a7d2388a6599f824d8502b529702a" + [[package]] name = "log" version = "0.4.11" diff --git a/dsp/Cargo.lock b/dsp/Cargo.lock index afad0c4..cda08c3 100644 --- a/dsp/Cargo.lock +++ b/dsp/Cargo.lock @@ -4,9 +4,16 @@ name = "dsp" version = "0.1.0" dependencies = [ + "libm", "serde", ] +[[package]] +name = "libm" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c7d73b3f436185384286bd8098d17ec07c9a7d2388a6599f824d8502b529702a" + [[package]] name = "proc-macro2" version = "1.0.24" diff --git a/dsp/Cargo.toml b/dsp/Cargo.toml index c8ef52b..2d0c15c 100644 --- a/dsp/Cargo.toml +++ b/dsp/Cargo.toml @@ -5,6 +5,7 @@ authors = ["Robert Jördens "] edition = "2018" [dependencies] +libm = "0.2.1" serde = { version = "1.0", features = ["derive"], default-features = false } [features] diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 9b9e966..ffb021c 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -2,4 +2,5 @@ #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] pub mod iir; +pub mod lockin; pub mod pll; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs new file mode 100644 index 0000000..3645754 --- /dev/null +++ b/dsp/src/lockin.rs @@ -0,0 +1,534 @@ +//! Lock-in amplifier. +//! +//! Lock-in processing is performed through a combination of the +//! following modular processing blocks: demodulation, filtering, +//! decimation and computing the magnitude and phase from the in-phase +//! and quadrature signals. These processing blocks are mutually +//! independent. +//! +//! # Terminology +//! +//! * _demodulation signal_ - A copy of the reference signal that is +//! optionally frequency scaled and phase shifted. There are two +//! copies of this signal. The first copy is in-phase with the +//! reference signal (before any optional phase shifting). The second +//! is 90 degrees out of phase (in quadrature) with the first +//! copy. The demodulation signals are used to demodulate the ADC +//! sampled signal. +//! * _in-phase_ and _quadrature_ - These terms are used to delineate +//! between the two components of the demodulation signal and the +//! resulting two signals at any step downstream of the demodulation +//! step. The in-phase signal is in-phase with the reference signal +//! prior to any phase shifts. The quadrature signal is 90 degrees out +//! of phase with the in-phase signal. +//! * _internal clock_ - A fast internal clock used to increment a +//! counter for determining the 0-phase points of a reference signal. +//! * _reference signal_ - A constant-frequency signal used to derive +//! the demodulation signal. +//! * _timestamp_ - Timestamps record the timing of the reference +//! signal's 0-phase points. For instance, if a reference signal is +//! provided externally, a fast internal clock increments a +//! counter. When the external reference reaches the 0-phase point +//! (e.g., a positive edge), the value of the counter is recorded as a +//! timestamp. These timestamps are used to determine the frequency +//! and phase of the reference signal. +//! +//! # Usage +//! +//! The first step is to initialize a `Lockin` instance with +//! `Lockin::new()`. This provides the lock-in algorithms with +//! necessary information about the demodulation and filtering steps, +//! such as whether to demodulate with a harmonic of the reference +//! signal and the IIR biquad filter to use. There are then 4 +//! different processing steps that can be used: +//! +//! * `demodulate` - Computes the phase of the demodulation signal +//! corresponding to each ADC sample, uses this phase to compute the +//! in-phase and quadrature demodulation signals, and multiplies these +//! demodulation signals by the ADC-sampled signal. This is a method +//! of `Lockin` since it requires information about how to modify the +//! reference signal for demodulation. +//! * `filter` - Performs IIR biquad filtering of in-phase and +//! quadrature signals. This is commonly performed on the in-phase and +//! quadrature components provided by the demodulation step, but can +//! be performed at any other point in the processing chain or omitted +//! entirely. `filter` is a method of `Lockin` since it must hold onto +//! the filter configuration and state. +//! * `decimate` - This decimates the in-phase and quadrature signals +//! to reduce the load on the DAC output. It does not require any +//! state information and is therefore a normal function. +//! * `magnitude_phase` - Computes the magnitude and phase of the +//! component of the ADC-sampled signal whose frequency is equal to +//! the demodulation frequency. This does not require any state +//! information and is therefore a normal function. + +use super::iir::{IIRState, IIR}; +use core::f32::consts::PI; + +/// The number of ADC samples in one batch. +pub const ADC_SAMPLE_BUFFER_SIZE: usize = 16; +/// The maximum number of timestamps in the period for one ADC +/// batch. Each timestamp corresponds to the time of an external +/// reference clock edge. +pub const TIMESTAMP_BUFFER_SIZE: usize = ADC_SAMPLE_BUFFER_SIZE / 2; +/// The number of outputs sent to the DAC for each ADC batch. +pub const DECIMATED_BUFFER_SIZE: usize = 1; + +/// Performs lock-in amplifier processing of a signal. +pub struct Lockin { + phase_offset: f32, + sample_period: u32, + harmonic: u32, + timestamps: [Option; 2], + iir: [IIR; 2], + iirstate: [IIRState; 2], +} + +impl Lockin { + /// Initialize a new `Lockin` instance. + /// + /// # Arguments + /// + /// * `phase_offset` - Phase offset (in radians) applied to the + /// demodulation signal. + /// * `sample_period` - ADC sampling period in terms of the + /// internal clock period. + /// * `harmonic` - Integer scaling factor used to adjust the + /// demodulation frequency. E.g., 2 would demodulate with the + /// first harmonic. + /// * `iir` - IIR biquad filter. Two identical copies of this IIR + /// filter are used: one for the in-phase signal and the other for + /// the quadrature signal. + /// + /// # Returns + /// + /// New `Lockin` instance. + pub fn new( + phase_offset: f32, + sample_period: u32, + harmonic: u32, + iir: IIR, + ) -> Self { + Lockin { + phase_offset: phase_offset, + sample_period: sample_period, + harmonic: harmonic, + timestamps: [None, None], + iir: [iir, iir], + iirstate: [[0.; 5]; 2], + } + } + + /// Demodulate an input signal with in-phase and quadrature + /// reference signals. + /// + /// # Arguments + /// + /// * `adc_samples` - One batch of ADC samples. + /// * `timestamps` - Counter values corresponding to the edges of + /// an external reference signal. The counter is incremented by a + /// fast internal clock. + /// * `valid_timestamps` - The number of valid timestamps in + /// `timestamps`. Only `×tamps[..valid_timestamps]` are used; + /// every other value in the `timestamps` array is ignored. + /// + /// # Returns + /// + /// The demodulated in-phase and quadrature signals as an + /// `Option`. When there are an insufficient number of timestamps + /// to perform processing, `None` is returned. + /// + /// # Assumptions + /// + /// `demodulate` expects that the timestamp counter value is equal + /// to 0 when the ADC samples its first input in a batch. This can + /// be achieved by configuring the timestamp counter to overflow + /// at the end of the ADC batch sampling period. + pub fn demodulate( + &mut self, + adc_samples: [i16; ADC_SAMPLE_BUFFER_SIZE], + timestamps: [u16; TIMESTAMP_BUFFER_SIZE], + valid_timestamps: u16, + ) -> Option<([f32; ADC_SAMPLE_BUFFER_SIZE], [f32; ADC_SAMPLE_BUFFER_SIZE])> + { + // update old timestamps for new ADC batch + let sample_period = self.sample_period as i32; + self.timestamps.iter_mut().for_each(|t| match *t { + Some(i) => { + *t = Some(i - ADC_SAMPLE_BUFFER_SIZE as i32 * sample_period); + } + None => (), + }); + + // record new timestamps + timestamps + .iter() + .take(valid_timestamps as usize) + .rev() + .take(2) + .rev() + .for_each(|t| self.timestamps.push(Some(*t as i32))); + + // return prematurely if there aren't enough timestamps for + // processing + if self.timestamps.iter().filter(|t| t.is_some()).count() < 2 { + return None; + } + + // compute ADC sample phases, sines/cosines and demodulate + let reference_period = + self.timestamps[0].unwrap() - self.timestamps[1].unwrap(); + let mut in_phase = [0f32; ADC_SAMPLE_BUFFER_SIZE]; + let mut quadrature = [0f32; ADC_SAMPLE_BUFFER_SIZE]; + in_phase + .iter_mut() + .zip(quadrature.iter_mut()) + .zip(adc_samples.iter()) + .enumerate() + .for_each(|(n, ((i, q), sample))| { + let integer_phase: i32 = (n as i32 * self.sample_period as i32 + - self.timestamps[0].unwrap()) + * self.harmonic as i32; + let phase = self.phase_offset + + 2. * PI * integer_phase as f32 / reference_period as f32; + let (sine, cosine) = libm::sincosf(phase); + let sample = *sample as f32; + *i = sine * sample; + *q = cosine * sample; + }); + + Some((in_phase, quadrature)) + } + + /// Filter the in-phase and quadrature signals using the supplied + /// biquad IIR. The signal arrays are modified in place. + /// + /// # Arguments + /// + /// * `in_phase` - In-phase signal. + /// * `quadrature` - Quadrature signal. + pub fn filter(&mut self, in_phase: &mut [f32], quadrature: &mut [f32]) { + in_phase + .iter_mut() + .zip(quadrature.iter_mut()) + .for_each(|(i, q)| { + *i = self.iir[0].update(&mut self.iirstate[0], *i); + *q = self.iir[1].update(&mut self.iirstate[1], *q); + }); + } +} + +/// Decimate the in-phase and quadrature signals to +/// `DECIMATED_BUFFER_SIZE`. The ratio of `ADC_SAMPLE_BUFFER_SIZE` to +/// `DECIMATED_BUFFER_SIZE` must be a power of 2. +/// +/// # Arguments +/// +/// * `in_phase` - In-phase signal. +/// * `quadrature` - Quadrature signal. +/// +/// # Returns +/// +/// The decimated in-phase and quadrature signals. +pub fn decimate( + in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE], + quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE], +) -> ([f32; DECIMATED_BUFFER_SIZE], [f32; DECIMATED_BUFFER_SIZE]) { + let n_k = ADC_SAMPLE_BUFFER_SIZE / DECIMATED_BUFFER_SIZE; + debug_assert!( + ADC_SAMPLE_BUFFER_SIZE == DECIMATED_BUFFER_SIZE || n_k % 2 == 0 + ); + + let mut in_phase_decimated = [0f32; DECIMATED_BUFFER_SIZE]; + let mut quadrature_decimated = [0f32; DECIMATED_BUFFER_SIZE]; + + in_phase_decimated + .iter_mut() + .zip(quadrature_decimated.iter_mut()) + .zip(in_phase.iter().step_by(n_k)) + .zip(quadrature.iter().step_by(n_k)) + .for_each(|(((i_decimated, q_decimated), i_original), q_original)| { + *i_decimated = *i_original; + *q_decimated = *q_original; + }); + + (in_phase_decimated, quadrature_decimated) +} + +/// Compute the magnitude and phase from the in-phase and quadrature +/// signals. The in-phase and quadrature arrays are modified in place. +/// +/// # Arguments +/// +/// * `in_phase` - In-phase signal. +/// * `quadrature` - Quadrature signal. +pub fn magnitude_phase(in_phase: &mut [f32], quadrature: &mut [f32]) { + in_phase + .iter_mut() + .zip(quadrature.iter_mut()) + .for_each(|(i, q)| { + let new_i = libm::sqrtf([*i, *q].iter().map(|i| i * i).sum()); + let new_q = libm::atan2f(*q, *i); + *i = new_i; + *q = new_q; + }); +} + +/// Treat the 2-element array as a FIFO. This allows new elements to +/// be pushed into the array, existing elements to shift back in the +/// array, and the last element to fall off the array. +trait Fifo2 { + fn push(&mut self, new_element: Option); +} + +impl Fifo2 for [Option; 2] { + /// Push a new element into the array. The existing elements move + /// backward in the array by one location, and the current last + /// element is discarded. + /// + /// # Arguments + /// + /// * `new_element` - New element pushed into the front of the + /// array. + fn push(&mut self, new_element: Option) { + // For array sizes greater than 2 it would be preferable to + // use a rotating index to avoid unnecessary data + // copying. However, this would somewhat complicate the use of + // iterators and for 2 elements, shifting is inexpensive. + self[1] = self[0]; + self[0] = new_element; + } +} + +#[cfg(test)] +mod tests { + use super::*; + extern crate std; + + fn f32_is_close(a: f32, b: f32) -> bool { + (a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON + } + + fn f32_array_is_close(a: &[f32], b: &[f32]) -> bool { + let mut result: bool = true; + a.iter().zip(b.iter()).for_each(|(i, j)| { + result &= f32_is_close(*i, *j); + }); + result + } + + fn within_tolerance( + a: f32, + b: f32, + relative_tolerance: f32, + fixed_tolerance: f32, + ) -> bool { + (a - b).abs() + <= a.abs().max(b.abs()) * relative_tolerance + fixed_tolerance + } + + fn array_within_tolerance( + a: &[f32], + b: &[f32], + relative_tolerance: f32, + fixed_tolerance: f32, + ) -> bool { + let mut result: bool = true; + a.iter().zip(b.iter()).for_each(|(i, j)| { + result &= + within_tolerance(*i, *j, relative_tolerance, fixed_tolerance); + }); + result + } + + #[test] + fn array_push() { + let mut arr: [Option; 2] = [None, None]; + arr.push(Some(1)); + assert_eq!(arr, [Some(1), None]); + arr.push(Some(2)); + assert_eq!(arr, [Some(2), Some(1)]); + arr.push(Some(10)); + assert_eq!(arr, [Some(10), Some(2)]); + } + + #[test] + fn magnitude_phase_length_1_quadrant_1() { + let mut in_phase: [f32; 1] = [1.]; + let mut quadrature: [f32; 1] = [1.]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[2_f32.sqrt()])); + assert!(f32_array_is_close(&quadrature, &[PI / 4.])); + + in_phase = [3_f32.sqrt() / 2.]; + quadrature = [1. / 2.]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[1_f32])); + assert!(f32_array_is_close(&quadrature, &[PI / 6.])); + } + + #[test] + fn magnitude_phase_length_1_quadrant_2() { + let mut in_phase: [f32; 1] = [-1.]; + let mut quadrature: [f32; 1] = [1.]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[2_f32.sqrt()])); + assert!(f32_array_is_close(&quadrature, &[3. * PI / 4.])); + + in_phase = [-1. / 2.]; + quadrature = [3_f32.sqrt() / 2.]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[1_f32])); + assert!(f32_array_is_close(&quadrature, &[2. * PI / 3.])); + } + + #[test] + fn magnitude_phase_length_1_quadrant_3() { + let mut in_phase: [f32; 1] = [-1. / 2_f32.sqrt()]; + let mut quadrature: [f32; 1] = [-1. / 2_f32.sqrt()]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[1_f32.sqrt()])); + assert!(f32_array_is_close(&quadrature, &[-3. * PI / 4.])); + + in_phase = [-1. / 2.]; + quadrature = [-2_f32.sqrt()]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[(3. / 2.) as f32])); + assert!(f32_array_is_close(&quadrature, &[-1.91063323625 as f32])); + } + + #[test] + fn magnitude_phase_length_1_quadrant_4() { + let mut in_phase: [f32; 1] = [1. / 2_f32.sqrt()]; + let mut quadrature: [f32; 1] = [-1. / 2_f32.sqrt()]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[1_f32.sqrt()])); + assert!(f32_array_is_close(&quadrature, &[-1. * PI / 4.])); + + in_phase = [3_f32.sqrt() / 2.]; + quadrature = [-1. / 2.]; + magnitude_phase(&mut in_phase, &mut quadrature); + assert!(f32_array_is_close(&in_phase, &[1_f32])); + assert!(f32_array_is_close(&quadrature, &[-PI / 6.])); + } + + #[test] + fn decimate_sample_16_decimated_1() { + let in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE] = [ + 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2, + 1.3, 1.4, 1.5, + ]; + let quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE] = [ + 1.6, 1.7, 1.8, 1.9, 2.0, 2.1, 2.2, 2.3, 2.4, 2.5, 2.6, 2.7, 2.8, + 2.9, 3.0, 3.1, + ]; + assert_eq!(decimate(in_phase, quadrature), ([0.0], [1.6])); + } + + #[test] + fn lockin_demodulate_valid_0() { + let mut lockin = Lockin::new( + 0., + 200, + 1, + IIR { + ba: [0_f32; 5], + y_offset: 0., + y_min: -(1 << 15) as f32, + y_max: (1 << 15) as f32 - 1., + }, + ); + assert_eq!( + lockin.demodulate( + [0; ADC_SAMPLE_BUFFER_SIZE], + [0; TIMESTAMP_BUFFER_SIZE], + 0 + ), + None + ); + } + + #[test] + fn lockin_demodulate_valid_1() { + let mut lockin = Lockin::new( + 0., + 200, + 1, + IIR { + ba: [0_f32; 5], + y_offset: 0., + y_min: -(1 << 15) as f32, + y_max: (1 << 15) as f32 - 1., + }, + ); + assert_eq!( + lockin.demodulate( + [0; ADC_SAMPLE_BUFFER_SIZE], + [0; TIMESTAMP_BUFFER_SIZE], + 1 + ), + None + ); + } + + #[test] + fn lockin_demodulate_valid_2() { + let adc_period: u32 = 200; + let mut lockin = Lockin::new( + 0., + adc_period, + 1, + IIR { + ba: [0_f32; 5], + y_offset: 0., + y_min: -(1 << 15) as f32, + y_max: (1 << 15) as f32 - 1., + }, + ); + let adc_samples: [i16; ADC_SAMPLE_BUFFER_SIZE] = + [-8, 7, -7, 6, -6, 5, -5, 4, -4, 3, -3, 2, -2, -1, 1, 0]; + let reference_period: u16 = 2800; + let initial_phase_integer: u16 = 200; + let timestamps: [u16; TIMESTAMP_BUFFER_SIZE] = [ + initial_phase_integer, + initial_phase_integer + reference_period, + 0, + 0, + 0, + 0, + 0, + 0, + ]; + let initial_phase: f32 = + -(initial_phase_integer as f32) / reference_period as f32 * 2. * PI; + let phase_increment: f32 = + adc_period as f32 / reference_period as f32 * 2. * PI; + let mut in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE] = + [0.; ADC_SAMPLE_BUFFER_SIZE]; + let mut quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE] = + [0.; ADC_SAMPLE_BUFFER_SIZE]; + for (n, (i, q)) in + in_phase.iter_mut().zip(quadrature.iter_mut()).enumerate() + { + let adc_phase = initial_phase + n as f32 * phase_increment; + let sine = adc_phase.sin(); + let cosine = adc_phase.cos(); + *i = sine * adc_samples[n] as f32; + *q = cosine * adc_samples[n] as f32; + } + let (result_in_phase, result_quadrature) = + lockin.demodulate(adc_samples, timestamps, 2).unwrap(); + assert!( + array_within_tolerance(&result_in_phase, &in_phase, 0., 1e-5), + "\nin_phase computed: {:?},\nin_phase expected: {:?}", + result_in_phase, + in_phase + ); + assert!( + array_within_tolerance(&result_quadrature, &quadrature, 0., 1e-5), + "\nquadrature computed: {:?},\nquadrature expected: {:?}", + result_quadrature, + quadrature + ); + } +} From 8ae20009d70f455d50b77c8ddcacc5edda5775f3 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sun, 22 Nov 2020 14:35:23 -0800 Subject: [PATCH 46/71] add lock-in low-pass integration tests --- dsp/tests/lockin_low_pass.rs | 1130 ++++++++++++++++++++++++++++++++++ 1 file changed, 1130 insertions(+) create mode 100644 dsp/tests/lockin_low_pass.rs diff --git a/dsp/tests/lockin_low_pass.rs b/dsp/tests/lockin_low_pass.rs new file mode 100644 index 0000000..e138c6f --- /dev/null +++ b/dsp/tests/lockin_low_pass.rs @@ -0,0 +1,1130 @@ +use dsp::iir::IIR; +use dsp::lockin::{ + decimate, magnitude_phase, Lockin, ADC_SAMPLE_BUFFER_SIZE, + DECIMATED_BUFFER_SIZE, TIMESTAMP_BUFFER_SIZE, +}; + +use std::f64::consts::PI; +use std::vec::Vec; + +const ADC_MAX: f64 = 1.; +const ADC_MAX_COUNT: f64 = (1 << 15) as f64; + +/// Single-frequency sinusoid. +#[derive(Copy, Clone)] +struct PureSine { + // Frequency (in Hz). + frequency: f64, + // Amplitude in dBFS (decibels relative to full-scale). A 16-bit + // ADC has a minimum dBFS for each sample of -90. + amplitude_dbfs: f64, + // Phase offset (in radians). + phase_offset: f64, +} + +/// Convert a dBFS voltage ratio to a linear ratio. +/// +/// # Arguments +/// +/// * `dbfs` - dB ratio relative to full scale. +/// +/// # Returns +/// +/// Linear value. +fn linear(dbfs: f64) -> f64 { + let base = 10_f64; + ADC_MAX * base.powf(dbfs / 20.) +} + +/// Convert a linear voltage ratio to a dBFS ratio. +/// +/// # Arguments +/// +/// * `linear` - Linear voltage ratio. +/// +/// # Returns +/// +/// dBFS value. +fn dbfs(linear: f64) -> f64 { + 20. * (linear / ADC_MAX).log10() +} + +/// Convert a real ADC input value in the range `-ADC_MAX` to +/// `+ADC_MAX` to an equivalent 16-bit ADC sampled value. This models +/// the ideal ADC transfer function. +/// +/// # Arguments +/// +/// * `x` - Real ADC input value. +/// +/// # Returns +/// +/// Sampled ADC value. +fn real_to_adc_sample(x: f64) -> i16 { + let max: i32 = i16::MAX as i32; + let min: i32 = i16::MIN as i32; + + let xi: i32 = (x / ADC_MAX * ADC_MAX_COUNT) as i32; + + // It's difficult to characterize the correct output result when + // the inputs are clipped, so panic instead. + if xi > max { + panic!("Input clipped to maximum, result is unlikely to be correct."); + } else if xi < min { + panic!("Input clipped to minimum, result is unlikely to be correct."); + } + + xi as i16 +} + +/// Generate `ADC_SAMPLE_BUFFER_SIZE` values of an ADC-sampled signal +/// starting at `timestamp_start`. +/// +/// # Arguments +/// +/// * `pure_signals` - Pure sinusoidal components of the ADC-sampled +/// signal. +/// * `timestamp_start` - Starting time of ADC-sampled signal in terms +/// of the internal clock count. +/// * `internal_frequency` - Internal clock frequency (in Hz). +/// * `adc_frequency` - ADC sampling frequency (in Hz). +/// +/// # Returns +/// +/// The sampled signal at the ADC input. +fn adc_sampled_signal( + pure_signals: &Vec, + timestamp_start: u64, + internal_frequency: f64, + adc_frequency: f64, +) -> [i16; ADC_SAMPLE_BUFFER_SIZE] { + // amplitude of each pure signal + let mut amplitude: Vec = Vec::::new(); + // initial phase value for each pure signal + let mut initial_phase: Vec = Vec::::new(); + // phase increment at each ADC sample for each pure signal + let mut phase_increment: Vec = Vec::::new(); + let adc_period = internal_frequency / adc_frequency; + + // For each pure sinusoid, compute the amplitude, phase + // corresponding to the first ADC sample, and phase increment for + // each subsequent ADC sample. + for pure_signal in pure_signals.iter() { + let signal_period = internal_frequency / pure_signal.frequency; + let phase_offset_count = + pure_signal.phase_offset / (2. * PI) * signal_period; + let initial_phase_count = + (phase_offset_count + timestamp_start as f64) % signal_period; + + amplitude.push(linear(pure_signal.amplitude_dbfs)); + initial_phase.push(2. * PI * initial_phase_count / signal_period); + phase_increment.push(2. * PI * adc_period / signal_period); + } + + // Compute the input signal corresponding to each ADC sample by + // summing the contributions from each pure sinusoid. + let mut signal: [i16; ADC_SAMPLE_BUFFER_SIZE] = [0; ADC_SAMPLE_BUFFER_SIZE]; + signal.iter_mut().enumerate().for_each(|(n, s)| { + *s = real_to_adc_sample( + amplitude + .iter() + .zip(initial_phase.iter()) + .zip(phase_increment.iter()) + .fold(0., |acc, ((a, phi), theta)| { + acc + a * (phi + theta * n as f64).sin() + }), + ); + }); + + signal +} + +/// Reference clock timestamp values in one ADC batch period starting +/// at `timestamp_start`. Also returns the number of valid timestamps. +/// +/// # Arguments +/// +/// * `reference_frequency` - External reference signal frequency (in +/// Hz). +/// * `timestamp_start` - Start time in terms of the internal clock +/// count. This is the start time of the current processing sequence +/// (i.e., for the current `ADC_SAMPLE_BUFFER_SIZE` ADC samples). +/// * `timestamp_stop` - Stop time in terms of the internal clock +/// count. +/// * `internal_frequency` - Internal clock frequency (in Hz). +/// +/// # Returns +/// +/// Tuple consisting of the number of valid timestamps in the ADC +/// batch period, followed by an array of the timestamp values. +fn adc_batch_timestamps( + reference_frequency: f64, + timestamp_start: u64, + timestamp_stop: u64, + internal_frequency: f64, +) -> (usize, [u16; TIMESTAMP_BUFFER_SIZE]) { + let reference_period = internal_frequency / reference_frequency; + let start_count = timestamp_start as f64 % reference_period; + let mut valid_timestamps: usize = 0; + let mut timestamps: [u16; TIMESTAMP_BUFFER_SIZE] = + [0; TIMESTAMP_BUFFER_SIZE]; + + let mut timestamp = (reference_period - start_count) % reference_period; + while timestamp < (timestamp_stop - timestamp_start) as f64 { + timestamps[valid_timestamps] = timestamp as u16; + timestamp += reference_period; + valid_timestamps += 1; + } + + (valid_timestamps, timestamps) +} + +/// Lowpass biquad filter using cutoff and sampling frequencies. +/// Taken from: +/// https://webaudio.github.io/Audio-EQ-Cookbook/audio-eq-cookbook.html +/// +/// # Arguments +/// +/// * `corner_frequency` - Corner frequency, or 3dB cutoff frequency +/// (in Hz). +/// * `sampling_frequency` - Sampling frequency (in Hz). +/// +/// # Returns +/// +/// 2nd-order IIR filter coefficients in the form [b0,b1,b2,a1,a2]. a0 +/// is set to -1. +fn lowpass_iir_coefficients( + corner_frequency: f64, + sampling_frequency: f64, +) -> [f32; 5] { + let normalized_angular_frequency: f64 = + 2. * PI * corner_frequency / sampling_frequency; + let quality_factor: f64 = 1. / 2f64.sqrt(); + let alpha: f64 = normalized_angular_frequency.sin() / (2. * quality_factor); + // All b coefficients have been multiplied by a factor of 2 in + // comparison with the link above in order to set the passband + // gain to 2. + let mut b0: f64 = 1. - normalized_angular_frequency.cos(); + let mut b1: f64 = 2. * (1. - normalized_angular_frequency.cos()); + let mut b2: f64 = b0; + let a0: f64 = 1. + alpha; + let mut a1: f64 = -2. * normalized_angular_frequency.cos(); + let mut a2: f64 = 1. - alpha; + b0 /= a0; + b1 /= a0; + b2 /= a0; + a1 /= -a0; + a2 /= -a0; + + [b0 as f32, b1 as f32, b2 as f32, a1 as f32, a2 as f32] +} + +/// Check that a measured value is within some tolerance of the actual +/// value. This allows setting both fixed and relative tolerances. +/// +/// # Arguments +/// +/// * `actual` - Actual value with respect to which the magnitude of +/// the relative tolerance is computed. +/// * `computed` - Computed value. This is compared with the actual +/// value, `actual`. +/// * `fixed_tolerance` - Fixed tolerance. +/// * `relative_tolerance` - Relative tolerance. +/// `relative_tolerance`*`actual` gives the total contribution of the +/// relative tolerance. +/// +/// # Returns +/// +/// `true` if the `actual` and `computed` values are within the +/// specified tolerance of one another, and `false` otherwise. +fn tolerance_check( + actual: f32, + computed: f32, + fixed_tolerance: f32, + relative_tolerance: f32, +) -> bool { + (actual - computed).abs() + < max_error(actual, fixed_tolerance, relative_tolerance) +} + +/// Maximum acceptable error from an actual value given fixed and +/// relative tolerances. +/// +/// # Arguments +/// +/// * `actual` - Actual value with respect to which the magnitude of the +/// relative tolerance is computed. +/// * `fixed_tolerance` - Fixed tolerance. +/// * `relative_tolerance` - Relative tolerance. +/// `relative_tolerance`*`actual` gives the total contribution of the +/// relative tolerance. +/// +/// # Returns +/// +/// Maximum acceptable error. +fn max_error( + actual: f32, + fixed_tolerance: f32, + relative_tolerance: f32, +) -> f32 { + relative_tolerance * actual.abs() + fixed_tolerance +} + +/// Total noise amplitude of the input signal after sampling by the +/// ADC. This computes an upper bound of the total noise amplitude, +/// rather than its actual value. +/// +/// # Arguments +/// +/// * `noise_inputs` - Noise sources at the ADC input. +/// * `demodulation_frequency` - Frequency of the demodulation signal +/// (in Hz). +/// * `corner_frequency` - Low-pass filter 3dB corner (cutoff) +/// frequency. +/// +/// # Returns +/// +/// Upper bound of the total amplitude of all noise sources. +fn sampled_noise_amplitude( + noise_inputs: &Vec, + demodulation_frequency: f64, + corner_frequency: f64, +) -> f64 { + // There is not a simple way to compute the amplitude of a + // superpostition of sinusoids with different frequencies and + // phases. Although we can compute the amplitude in special cases + // (e.g., two signals whose periods have a common multiple), these + // do not help us in the general case. However, we can say that + // the total amplitude will not be greater than the sum of the + // amplitudes of the individual noise sources. We treat this as an + // upper bound, and use it as an approximation of the actual + // amplitude. + + let mut noise: f64 = noise_inputs + .iter() + .map(|n| { + // Noise inputs create an oscillation at the output, where the + // oscillation magnitude is determined by the strength of the + // noise and its attenuation (attenuation is determined by its + // proximity to the demodulation frequency and filter + // rolloff). + let octaves = ((n.frequency - demodulation_frequency).abs() + / corner_frequency) + .log2(); + // 2nd-order filter. Approximately 12dB/octave rolloff. + let attenuation = -2. * 20. * 2_f64.log10() * octaves; + linear(n.amplitude_dbfs + attenuation) + }) + .sum(); + + // Add in 1/2 LSB for the maximum amplitude deviation resulting + // from quantization. + noise += 1. / ADC_MAX_COUNT / 2.; + + noise +} + +/// Compute the maximum effect of input noise on the lock-in magnitude +/// computation. +/// +/// The maximum effect of noise on the magnitude computation is given +/// by: +/// +/// | sqrt((I+n*sin(x))**2 + (Q+n*cos(x))**2) - sqrt(I**2 + Q**2) | +/// +/// * I is the in-phase component of the part of the input signal we +/// care about (component of the input signal with the same frequency +/// as the demodulation signal). +/// * Q is the quadrature component. +/// * n is the total noise amplitude (from all contributions, after +/// attenuation from filtering). +/// * x is the phase of the demodulation signal and can be chosen to +/// be anywhere in the range [0, 2pi) to maximize this expression. +/// +/// We need to find the demodulation phase (x) that maximizes this +/// expression. We could compute this, because we know I, Q, and n, +/// but that's a fairly expensive computation and probably +/// overkill. Instead, we can employ the heuristic that when |I|>>|Q|, +/// sin(x)=+-1 (+- denotes plus or minus) will maximize the error, +/// when |Q|>>|I|, cos(x)=+-1 will maximize the error and when +/// |I|~|Q|, max,min(sin(x)+cos(x)) will maximize the error (this +/// occurs when sin(x)=cos(x)=+-1/sqrt(2)). Whether a positive or +/// negative noise term maximizes the error depends on the values and +/// signs of I and Q (for instance, when I,Q>0, negative noise terms +/// will maximize the error since the sqrt function is concave down), +/// but the difference should be modest in each case so we should be +/// able to get a reasonably good approximation by using the positive +/// noise case. We can use the maximum of all 3 cases as a rough +/// approximation of the real maximum. +/// +/// # Arguments +/// +/// * `total_noise_amplitude` - Combined amplitude of all noise +/// sources sampled by the ADC. +/// * `in_phase_actual` - Value of the in-phase component if no noise +/// were present at the ADC input. +/// * `quadrature_actual` - Value of the quadrature component if no +/// noise were present at the ADC input. +/// * `desired_input_amplitude` - Amplitude of the desired input +/// signal. That is, the input signal component with the same +/// frequency as the demodulation signal. +/// +/// # Returns +/// +/// Approximation of the maximum effect on the magnitude computation +/// due to noise sources at the ADC input. +fn magnitude_noise( + total_noise_amplitude: f64, + in_phase_actual: f64, + quadrature_actual: f64, + desired_input_amplitude: f64, +) -> f64 { + // See function documentation for explanation. + let noise = |in_phase_delta: f64, quadrature_delta: f64| -> f64 { + (((in_phase_actual + in_phase_delta).powf(2.) + + (quadrature_actual + quadrature_delta).powf(2.)) + .sqrt() + - desired_input_amplitude) + .abs() + }; + + let mut max_noise: f64 = 0.; + for (in_phase_delta, quadrature_delta) in [ + (total_noise_amplitude, 0.), + (0., total_noise_amplitude), + ( + total_noise_amplitude / 2_f64.sqrt(), + total_noise_amplitude / 2_f64.sqrt(), + ), + ] + .iter() + { + max_noise = max_noise.max(noise(*in_phase_delta, *quadrature_delta)); + } + + max_noise +} + +/// Compute the maximum phase deviation from the correct value due to +/// the input noise sources. +/// +/// The maximum effect of noise on the phase computation is given by: +/// +/// | atan2(Q+n*cos(x), I+n*sin(x)) - atan2(Q, I) | +/// +/// See `magnitude_noise` for an explanation of the terms in this +/// mathematical expression. +/// +/// Similar to the heuristic used when computing the error in +/// `magnitude_noise`, we can use (sin(x)=+-1,cos(x)=0), +/// (sin(x)=0,cos(x)=+-1), and the value of x that maximizes +/// |sin(x)-cos(x)| (when sin(x)=1/sqrt(2) and cos(x)=-1/sqrt(2), or +/// when the signs are flipped) as cases to test as an approximation +/// for the actual maximum value of this expression. +/// +/// # Arguments +/// +/// * `total_noise_amplitude` - Total amplitude of all input noise +/// sources. +/// * `in_phase_actual` - Value of the in-phase component if no noise +/// were present at the input. +/// * `quadrature_actual` - Value of the quadrature component if no +/// noise were present at the input. +/// +/// # Returns +/// +/// Approximation of the maximum effect on the phase computation due +/// to noise sources at the ADC input. +fn phase_noise( + total_noise_amplitude: f64, + in_phase_actual: f64, + quadrature_actual: f64, +) -> f64 { + // See function documentation for explanation. + let noise = |in_phase_delta: f64, quadrature_delta: f64| -> f64 { + ((quadrature_actual + quadrature_delta) + .atan2(in_phase_actual + in_phase_delta) + - quadrature_actual.atan2(in_phase_actual)) + .abs() + }; + + let mut max_noise: f64 = 0.; + for (in_phase_delta, quadrature_delta) in [ + ( + total_noise_amplitude / 2_f64.sqrt(), + total_noise_amplitude / -2_f64.sqrt(), + ), + ( + total_noise_amplitude / -2_f64.sqrt(), + total_noise_amplitude / 2_f64.sqrt(), + ), + (total_noise_amplitude, 0.), + (-total_noise_amplitude, 0.), + (0., total_noise_amplitude), + (0., -total_noise_amplitude), + ] + .iter() + { + max_noise = max_noise.max(noise(*in_phase_delta, *quadrature_delta)); + } + + max_noise +} + +/// Lowpass filter test for in-phase/quadrature and magnitude/phase +/// computations. +/// +/// This attempts to "intelligently" model acceptable tolerance ranges +/// for the measured in-phase, quadrature, magnitude and phase results +/// of lock-in processing for a typical low-pass filter +/// application. So, instead of testing whether the lock-in processing +/// extracts the true magnitude and phase (or in-phase and quadrature +/// components) of the input signal, it attempts to calculate what the +/// lock-in processing should compute given any set of input noise +/// sources. For example, if a noise source of sufficient strength +/// differs in frequency by 1kHz from the reference frequency and the +/// filter cutoff frequency is also 1kHz, testing if the lock-in +/// amplifier extracts the amplitude and phase of the input signal +/// whose frequency is equal to the demodulation frequency is doomed +/// to failure. Instead, this function tests whether the lock-in +/// correctly adheres to its actual transfer function, whether or not +/// it was given reasonable inputs. The logic for computing acceptable +/// tolerance ranges is performed in `sampled_noise_amplitude`, +/// `magnitude_noise`, and `phase_noise`. +/// +/// # Arguments +/// +/// * `internal_frequency` - Internal clock frequency (Hz). The +/// internal clock increments timestamp counter values used to +/// record the edges of the external reference. +/// * `adc_frequency` - ADC sampling frequency (in Hz). +/// * `reference_frequency` - External reference frequency (in Hz). +/// * `demodulation_phase_offset` - Phase offset applied to the +/// in-phase and quadrature demodulation signals. +/// * `harmonic` - Scaling factor for the demodulation +/// frequency. E.g., 2 would demodulate with the first harmonic of the +/// reference frequency. +/// * `corner_frequency` - Lowpass filter 3dB cutoff frequency. +/// * `desired_input` - `PureSine` giving the frequency, amplitude and +/// phase of the desired result. +/// * `noise_inputs` - Vector of `PureSine` for any noise inputs on top +/// of `desired_input`. +/// * `time_constant_factor` - Number of time constants after which +/// the output is considered valid. +/// * `tolerance` - Acceptable relative tolerance for the magnitude +/// and angle outputs. The outputs must remain within this tolerance +/// between `time_constant_factor` and `time_constant_factor+1` time +/// constants. +fn lowpass_test( + internal_frequency: f64, + adc_frequency: f64, + reference_frequency: f64, + demodulation_phase_offset: f64, + harmonic: u32, + corner_frequency: f64, + desired_input: PureSine, + noise_inputs: &mut Vec, + time_constant_factor: f64, + tolerance: f32, +) { + let mut lockin = Lockin::new( + demodulation_phase_offset as f32, + (internal_frequency / adc_frequency) as u32, + harmonic, + IIR { + ba: lowpass_iir_coefficients(corner_frequency, adc_frequency), + y_offset: 0., + y_min: -ADC_MAX_COUNT as f32, + y_max: (ADC_MAX_COUNT - 1.) as f32, + }, + ); + + let mut timestamp_start: u64 = 0; + let time_constant: f64 = 1. / (2. * PI * corner_frequency); + let samples = + (time_constant_factor * time_constant * adc_frequency) as usize; + // Ensure the result remains within tolerance for 1 time constant + // after `time_constant_factor` time constants. + let extra_samples = (time_constant * adc_frequency) as usize; + let sample_count: u64 = (internal_frequency / adc_frequency) as u64 + * ADC_SAMPLE_BUFFER_SIZE as u64; + + let effective_phase_offset = + desired_input.phase_offset - demodulation_phase_offset; + let in_phase_actual = + linear(desired_input.amplitude_dbfs) * effective_phase_offset.cos(); + let quadrature_actual = + linear(desired_input.amplitude_dbfs) * effective_phase_offset.sin(); + + let total_noise_amplitude = sampled_noise_amplitude( + noise_inputs, + reference_frequency * harmonic as f64, + corner_frequency, + ); + let total_magnitude_noise = magnitude_noise( + total_noise_amplitude, + in_phase_actual, + quadrature_actual, + linear(desired_input.amplitude_dbfs), + ); + let total_phase_noise = + phase_noise(total_noise_amplitude, in_phase_actual, quadrature_actual); + + let pure_signals = noise_inputs; + pure_signals.push(desired_input); + + for n in 0..(samples + extra_samples) { + let signal: [i16; ADC_SAMPLE_BUFFER_SIZE] = adc_sampled_signal( + &pure_signals, + timestamp_start, + internal_frequency, + adc_frequency, + ); + let (valid_timestamps, timestamps) = adc_batch_timestamps( + reference_frequency, + timestamp_start, + timestamp_start + sample_count - 1, + internal_frequency, + ); + + let mut in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE]; + let mut quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE]; + let lockin_demodulate = + lockin.demodulate(signal, timestamps, valid_timestamps as u16); + match lockin_demodulate { + Some(i) => { + in_phase = i.0; + quadrature = i.1; + } + None => { + continue; + } + } + + lockin.filter(&mut in_phase, &mut quadrature); + let (in_phase_decimated, quadrature_decimated) = + decimate(in_phase, quadrature); + + let mut magnitude_decimated = in_phase_decimated.clone(); + let mut phase_decimated = quadrature_decimated.clone(); + + magnitude_phase(&mut magnitude_decimated, &mut phase_decimated); + + // Ensure stable within tolerance for 1 time constant after + // `time_constant_factor`. + if n >= samples { + for k in 0..DECIMATED_BUFFER_SIZE { + let amplitude_normalized: f32 = + magnitude_decimated[k] / ADC_MAX_COUNT as f32; + assert!( + tolerance_check(linear(desired_input.amplitude_dbfs) as f32, amplitude_normalized, total_magnitude_noise as f32, tolerance), + "magnitude actual: {:.4} ({:.2} dBFS), magnitude computed: {:.4} ({:.2} dBFS), tolerance: {:.4}", + linear(desired_input.amplitude_dbfs), + desired_input.amplitude_dbfs, + amplitude_normalized, + dbfs(amplitude_normalized as f64), + max_error(linear(desired_input.amplitude_dbfs) as f32, total_magnitude_noise as f32, tolerance) + ); + assert!( + tolerance_check( + effective_phase_offset as f32, + phase_decimated[k], + total_phase_noise as f32, + tolerance + ), + "phase actual: {:.4}, phase computed: {:.4}, tolerance: {:.4}", + effective_phase_offset as f32, + phase_decimated[k], + max_error( + effective_phase_offset as f32, + total_phase_noise as f32, + tolerance + ) + ); + + let in_phase_normalized: f32 = + in_phase_decimated[k] / ADC_MAX_COUNT as f32; + let quadrature_normalized: f32 = + quadrature_decimated[k] / ADC_MAX_COUNT as f32; + assert!( + tolerance_check( + in_phase_actual as f32, + in_phase_normalized, + total_noise_amplitude as f32, + tolerance + ), + "in-phase actual: {:.4}, in-phase computed: {:.3}, tolerance: {:.4}", + in_phase_actual, + in_phase_normalized, + max_error( + in_phase_actual as f32, + total_noise_amplitude as f32, + tolerance + ) + ); + assert!( + tolerance_check( + quadrature_actual as f32, + quadrature_normalized, + total_noise_amplitude as f32, + tolerance + ), + "quadrature actual: {:.4}, quadrature computed: {:.4}, tolerance: {:.4}", + quadrature_actual, + quadrature_normalized, + max_error( + quadrature_actual as f32, + total_noise_amplitude as f32, + tolerance + ) + ); + } + } + + timestamp_start += sample_count; + } +} + +#[test] +fn lowpass() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_demodulation_phase_offset_pi_2() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = PI / 2.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_phase_offset_pi_2() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: PI / 2., + }, + &mut vec![ + PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_fundamental_111e3_phase_offset_pi_4() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 111e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: PI / 4., + }, + &mut vec![ + PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.9 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_first_harmonic() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 2; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_second_harmonic() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 3; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_third_harmonic() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 4; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_first_harmonic_phase_shift() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 50e3; + let harmonic: u32 = 2; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: PI / 4., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_adc_frequency_1e6() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 1e6; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_internal_frequency_125e6() { + let internal_frequency: f64 = 125e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 100e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![ + PureSine { + frequency: 1.2 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + PureSine { + frequency: 0.8 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }, + ], + time_constant_factor, + tolerance, + ); +} + +#[test] +fn lowpass_low_signal_frequency() { + let internal_frequency: f64 = 100e6; + let adc_frequency: f64 = 500e3; + let signal_frequency: f64 = 10e3; + let harmonic: u32 = 1; + let corner_frequency: f64 = 1e3; + let demodulation_frequency: f64 = harmonic as f64 * signal_frequency; + let demodulation_phase_offset: f64 = 0.; + let time_constant_factor: f64 = 5.; + let tolerance: f32 = 1e-2; + + lowpass_test( + internal_frequency, + adc_frequency, + signal_frequency, + demodulation_phase_offset, + harmonic, + corner_frequency, + PureSine { + frequency: demodulation_frequency, + amplitude_dbfs: -30., + phase_offset: 0., + }, + &mut vec![PureSine { + frequency: 1.1 * demodulation_frequency, + amplitude_dbfs: -20., + phase_offset: 0., + }], + time_constant_factor, + tolerance, + ); +} From 8806feb4237464c7733db6d67bb699e1129cddbc Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Mon, 23 Nov 2020 16:16:21 -0800 Subject: [PATCH 47/71] lockin_low_pass: compute magnitude noise analytically --- dsp/tests/lockin_low_pass.rs | 87 ++++++++++++++++++++---------------- 1 file changed, 48 insertions(+), 39 deletions(-) diff --git a/dsp/tests/lockin_low_pass.rs b/dsp/tests/lockin_low_pass.rs index e138c6f..0cc32fd 100644 --- a/dsp/tests/lockin_low_pass.rs +++ b/dsp/tests/lockin_low_pass.rs @@ -332,30 +332,30 @@ fn sampled_noise_amplitude( /// /// | sqrt((I+n*sin(x))**2 + (Q+n*cos(x))**2) - sqrt(I**2 + Q**2) | /// -/// * I is the in-phase component of the part of the input signal we -/// care about (component of the input signal with the same frequency -/// as the demodulation signal). +/// * I is the in-phase component of the portion of the input signal +/// with the same frequency as the demodulation signal. /// * Q is the quadrature component. /// * n is the total noise amplitude (from all contributions, after /// attenuation from filtering). -/// * x is the phase of the demodulation signal and can be chosen to -/// be anywhere in the range [0, 2pi) to maximize this expression. +/// * x is the phase of the demodulation signal. /// /// We need to find the demodulation phase (x) that maximizes this -/// expression. We could compute this, because we know I, Q, and n, -/// but that's a fairly expensive computation and probably -/// overkill. Instead, we can employ the heuristic that when |I|>>|Q|, -/// sin(x)=+-1 (+- denotes plus or minus) will maximize the error, -/// when |Q|>>|I|, cos(x)=+-1 will maximize the error and when -/// |I|~|Q|, max,min(sin(x)+cos(x)) will maximize the error (this -/// occurs when sin(x)=cos(x)=+-1/sqrt(2)). Whether a positive or -/// negative noise term maximizes the error depends on the values and -/// signs of I and Q (for instance, when I,Q>0, negative noise terms -/// will maximize the error since the sqrt function is concave down), -/// but the difference should be modest in each case so we should be -/// able to get a reasonably good approximation by using the positive -/// noise case. We can use the maximum of all 3 cases as a rough -/// approximation of the real maximum. +/// expression. We can ignore the absolute value operation by also +/// considering the expression minimum. The locations of the minimum +/// and maximum can be computed analytically by finding the value of x +/// when the derivative of this expression with respect to x is +/// 0. When we solve this equation, we find: +/// +/// x = atan(I/Q) +/// +/// It's worth noting that this solution is technically only valid +/// when cos(x)!=0 (i.e., x!=pi/2,-pi/2). However, this is not a +/// problem because we only get these values when Q=0. Rust correctly +/// computes atan(inf)=pi/2, which is precisely what we want because +/// x=pi/2 maximizes sin(x) and therefore also the noise effect. +/// +/// The other maximum or minimum is pi radians away from this +/// value. /// /// # Arguments /// @@ -388,21 +388,17 @@ fn magnitude_noise( .abs() }; - let mut max_noise: f64 = 0.; - for (in_phase_delta, quadrature_delta) in [ - (total_noise_amplitude, 0.), - (0., total_noise_amplitude), - ( - total_noise_amplitude / 2_f64.sqrt(), - total_noise_amplitude / 2_f64.sqrt(), - ), - ] - .iter() - { - max_noise = max_noise.max(noise(*in_phase_delta, *quadrature_delta)); - } + let phase = (in_phase_actual / quadrature_actual).atan(); + let max_noise_1 = noise( + total_noise_amplitude * phase.sin(), + total_noise_amplitude * phase.cos(), + ); + let max_noise_2 = noise( + total_noise_amplitude * (phase + PI).sin(), + total_noise_amplitude * (phase + PI).cos(), + ); - max_noise + max_noise_1.max(max_noise_2) } /// Compute the maximum phase deviation from the correct value due to @@ -415,12 +411,25 @@ fn magnitude_noise( /// See `magnitude_noise` for an explanation of the terms in this /// mathematical expression. /// -/// Similar to the heuristic used when computing the error in -/// `magnitude_noise`, we can use (sin(x)=+-1,cos(x)=0), -/// (sin(x)=0,cos(x)=+-1), and the value of x that maximizes -/// |sin(x)-cos(x)| (when sin(x)=1/sqrt(2) and cos(x)=-1/sqrt(2), or -/// when the signs are flipped) as cases to test as an approximation -/// for the actual maximum value of this expression. +/// This expression is harder to compute analytically than the +/// expression in `magnitude_noise`. We could compute it numerically, +/// but that's expensive. However, we can use heuristics to try to +/// guess the values of x that will maximize the noise +/// effect. Intuitively, the difference will be largest when the +/// Y-argument of the atan2 function (Q+n*cos(x)) is pushed in the +/// opposite direction of the noise effect on the X-argument (i.e., +/// cos(x) and sin(x) have different signs). We can use: +/// +/// * sin(x)=+-1 (+- denotes plus or minus), cos(x)=0, +/// * sin(x)=0, cos(x)=+-1, and +/// * the value of x that maximizes |sin(x)-cos(x)| (when +/// sin(x)=1/sqrt(2) and cos(x)=-1/sqrt(2), or when the signs are +/// flipped) +/// +/// The first choice addresses cases in which |I|>>|Q|, the second +/// choice addresses cases in which |Q|>>|I|, and the third choice +/// addresses cases in which |I|~|Q|. We can test all of these cases +/// as an approximation for the real maximum. /// /// # Arguments /// From 3c4e83bf0ffecf0ae93dcc21987280f76ab8ff0b Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 24 Nov 2020 23:30:57 -0800 Subject: [PATCH 48/71] lockin: move fifo trait before use This clarifies what it means to "push" to an array. --- dsp/src/lockin.rs | 52 +++++++++++++++++++++++------------------------ 1 file changed, 26 insertions(+), 26 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 3645754..8b7e51d 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -74,6 +74,32 @@ pub const TIMESTAMP_BUFFER_SIZE: usize = ADC_SAMPLE_BUFFER_SIZE / 2; /// The number of outputs sent to the DAC for each ADC batch. pub const DECIMATED_BUFFER_SIZE: usize = 1; +/// Treat the 2-element array as a FIFO. This allows new elements to +/// be pushed into the array, existing elements to shift back in the +/// array, and the last element to fall off the array. +trait Fifo2 { + fn push(&mut self, new_element: Option); +} + +impl Fifo2 for [Option; 2] { + /// Push a new element into the array. The existing elements move + /// backward in the array by one location, and the current last + /// element is discarded. + /// + /// # Arguments + /// + /// * `new_element` - New element pushed into the front of the + /// array. + fn push(&mut self, new_element: Option) { + // For array sizes greater than 2 it would be preferable to + // use a rotating index to avoid unnecessary data + // copying. However, this would somewhat complicate the use of + // iterators and for 2 elements, shifting is inexpensive. + self[1] = self[0]; + self[0] = new_element; + } +} + /// Performs lock-in amplifier processing of a signal. pub struct Lockin { phase_offset: f32, @@ -274,32 +300,6 @@ pub fn magnitude_phase(in_phase: &mut [f32], quadrature: &mut [f32]) { }); } -/// Treat the 2-element array as a FIFO. This allows new elements to -/// be pushed into the array, existing elements to shift back in the -/// array, and the last element to fall off the array. -trait Fifo2 { - fn push(&mut self, new_element: Option); -} - -impl Fifo2 for [Option; 2] { - /// Push a new element into the array. The existing elements move - /// backward in the array by one location, and the current last - /// element is discarded. - /// - /// # Arguments - /// - /// * `new_element` - New element pushed into the front of the - /// array. - fn push(&mut self, new_element: Option) { - // For array sizes greater than 2 it would be preferable to - // use a rotating index to avoid unnecessary data - // copying. However, this would somewhat complicate the use of - // iterators and for 2 elements, shifting is inexpensive. - self[1] = self[0]; - self[0] = new_element; - } -} - #[cfg(test)] mod tests { use super::*; From da4430e912bda5c9fba3973679aa2a48e1d64330 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 24 Nov 2020 23:43:21 -0800 Subject: [PATCH 49/71] lockin: add documentation explaining timestamp decrement --- dsp/src/lockin.rs | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 8b7e51d..3aabf33 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -180,8 +180,12 @@ impl Lockin { // update old timestamps for new ADC batch let sample_period = self.sample_period as i32; self.timestamps.iter_mut().for_each(|t| match *t { - Some(i) => { - *t = Some(i - ADC_SAMPLE_BUFFER_SIZE as i32 * sample_period); + Some(timestamp) => { + // Existing timestamps have aged by one ADC batch + // period since the last ADC batch. + *t = Some( + timestamp - ADC_SAMPLE_BUFFER_SIZE as i32 * sample_period, + ); } None => (), }); From 4edda09d86a3560a3adaa2676f27abcf4f9e73b3 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 24 Nov 2020 23:44:39 -0800 Subject: [PATCH 50/71] lockin: change demodulate to return result instead of option --- dsp/src/lockin.rs | 14 ++++++++------ dsp/tests/lockin_low_pass.rs | 8 ++++---- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 3aabf33..e185c31 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -175,8 +175,10 @@ impl Lockin { adc_samples: [i16; ADC_SAMPLE_BUFFER_SIZE], timestamps: [u16; TIMESTAMP_BUFFER_SIZE], valid_timestamps: u16, - ) -> Option<([f32; ADC_SAMPLE_BUFFER_SIZE], [f32; ADC_SAMPLE_BUFFER_SIZE])> - { + ) -> Result< + ([f32; ADC_SAMPLE_BUFFER_SIZE], [f32; ADC_SAMPLE_BUFFER_SIZE]), + &str, + > { // update old timestamps for new ADC batch let sample_period = self.sample_period as i32; self.timestamps.iter_mut().for_each(|t| match *t { @@ -202,7 +204,7 @@ impl Lockin { // return prematurely if there aren't enough timestamps for // processing if self.timestamps.iter().filter(|t| t.is_some()).count() < 2 { - return None; + return Err("insufficient timestamps"); } // compute ADC sample phases, sines/cosines and demodulate @@ -227,7 +229,7 @@ impl Lockin { *q = cosine * sample; }); - Some((in_phase, quadrature)) + Ok((in_phase, quadrature)) } /// Filter the in-phase and quadrature signals using the supplied @@ -448,7 +450,7 @@ mod tests { [0; TIMESTAMP_BUFFER_SIZE], 0 ), - None + Err("insufficient timestamps") ); } @@ -471,7 +473,7 @@ mod tests { [0; TIMESTAMP_BUFFER_SIZE], 1 ), - None + Err("insufficient timestamps") ); } diff --git a/dsp/tests/lockin_low_pass.rs b/dsp/tests/lockin_low_pass.rs index 0cc32fd..1f8bed2 100644 --- a/dsp/tests/lockin_low_pass.rs +++ b/dsp/tests/lockin_low_pass.rs @@ -601,11 +601,11 @@ fn lowpass_test( let lockin_demodulate = lockin.demodulate(signal, timestamps, valid_timestamps as u16); match lockin_demodulate { - Some(i) => { - in_phase = i.0; - quadrature = i.1; + Ok((i, q)) => { + in_phase = i; + quadrature = q; } - None => { + Err(_) => { continue; } } From 9592bb74a72ed723433a4189b074630bf83a4ffe Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 24 Nov 2020 23:49:49 -0800 Subject: [PATCH 51/71] lockin: change zip order in decimate for clarity --- dsp/src/lockin.rs | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index e185c31..7f420d2 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -277,9 +277,13 @@ pub fn decimate( in_phase_decimated .iter_mut() .zip(quadrature_decimated.iter_mut()) - .zip(in_phase.iter().step_by(n_k)) - .zip(quadrature.iter().step_by(n_k)) - .for_each(|(((i_decimated, q_decimated), i_original), q_original)| { + .zip( + in_phase + .iter() + .step_by(n_k) + .zip(quadrature.iter().step_by(n_k)), + ) + .for_each(|((i_decimated, q_decimated), (i_original, q_original))| { *i_decimated = *i_original; *q_decimated = *q_original; }); From f259d6cf65ba8e0a3dbcc7e7e18a1390b7418624 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Tue, 24 Nov 2020 23:56:36 -0800 Subject: [PATCH 52/71] lockin: minor variable name changes --- dsp/src/lockin.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 7f420d2..858cd77 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -283,9 +283,9 @@ pub fn decimate( .step_by(n_k) .zip(quadrature.iter().step_by(n_k)), ) - .for_each(|((i_decimated, q_decimated), (i_original, q_original))| { - *i_decimated = *i_original; - *q_decimated = *q_original; + .for_each(|((i_d, q_d), (i, q))| { + *i_d = *i; + *q_d = *q; }); (in_phase_decimated, quadrature_decimated) From 90ef9f1e6a54e5fae2bd222438f12f7dce91d965 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sat, 28 Nov 2020 13:20:42 -0800 Subject: [PATCH 53/71] lockin: borrow adc samples and timestamps as slices --- dsp/src/lockin.rs | 32 +++++++------------------------- dsp/tests/lockin_low_pass.rs | 17 ++++++++--------- 2 files changed, 15 insertions(+), 34 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 858cd77..30d2f8e 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -154,9 +154,6 @@ impl Lockin { /// * `timestamps` - Counter values corresponding to the edges of /// an external reference signal. The counter is incremented by a /// fast internal clock. - /// * `valid_timestamps` - The number of valid timestamps in - /// `timestamps`. Only `×tamps[..valid_timestamps]` are used; - /// every other value in the `timestamps` array is ignored. /// /// # Returns /// @@ -172,9 +169,8 @@ impl Lockin { /// at the end of the ADC batch sampling period. pub fn demodulate( &mut self, - adc_samples: [i16; ADC_SAMPLE_BUFFER_SIZE], - timestamps: [u16; TIMESTAMP_BUFFER_SIZE], - valid_timestamps: u16, + adc_samples: &[i16], + timestamps: &[u16], ) -> Result< ([f32; ADC_SAMPLE_BUFFER_SIZE], [f32; ADC_SAMPLE_BUFFER_SIZE]), &str, @@ -195,7 +191,7 @@ impl Lockin { // record new timestamps timestamps .iter() - .take(valid_timestamps as usize) + .take(timestamps.len()) .rev() .take(2) .rev() @@ -449,11 +445,7 @@ mod tests { }, ); assert_eq!( - lockin.demodulate( - [0; ADC_SAMPLE_BUFFER_SIZE], - [0; TIMESTAMP_BUFFER_SIZE], - 0 - ), + lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[],), Err("insufficient timestamps") ); } @@ -472,11 +464,7 @@ mod tests { }, ); assert_eq!( - lockin.demodulate( - [0; ADC_SAMPLE_BUFFER_SIZE], - [0; TIMESTAMP_BUFFER_SIZE], - 1 - ), + lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[0],), Err("insufficient timestamps") ); } @@ -499,15 +487,9 @@ mod tests { [-8, 7, -7, 6, -6, 5, -5, 4, -4, 3, -3, 2, -2, -1, 1, 0]; let reference_period: u16 = 2800; let initial_phase_integer: u16 = 200; - let timestamps: [u16; TIMESTAMP_BUFFER_SIZE] = [ + let timestamps: &[u16] = &[ initial_phase_integer, initial_phase_integer + reference_period, - 0, - 0, - 0, - 0, - 0, - 0, ]; let initial_phase: f32 = -(initial_phase_integer as f32) / reference_period as f32 * 2. * PI; @@ -527,7 +509,7 @@ mod tests { *q = cosine * adc_samples[n] as f32; } let (result_in_phase, result_quadrature) = - lockin.demodulate(adc_samples, timestamps, 2).unwrap(); + lockin.demodulate(&adc_samples, timestamps).unwrap(); assert!( array_within_tolerance(&result_in_phase, &in_phase, 0., 1e-5), "\nin_phase computed: {:?},\nin_phase expected: {:?}", diff --git a/dsp/tests/lockin_low_pass.rs b/dsp/tests/lockin_low_pass.rs index 1f8bed2..af64651 100644 --- a/dsp/tests/lockin_low_pass.rs +++ b/dsp/tests/lockin_low_pass.rs @@ -1,7 +1,7 @@ use dsp::iir::IIR; use dsp::lockin::{ decimate, magnitude_phase, Lockin, ADC_SAMPLE_BUFFER_SIZE, - DECIMATED_BUFFER_SIZE, TIMESTAMP_BUFFER_SIZE, + DECIMATED_BUFFER_SIZE, }; use std::f64::consts::PI; @@ -159,15 +159,14 @@ fn adc_sampled_signal( /// batch period, followed by an array of the timestamp values. fn adc_batch_timestamps( reference_frequency: f64, + timestamps: &mut [u16], timestamp_start: u64, timestamp_stop: u64, internal_frequency: f64, -) -> (usize, [u16; TIMESTAMP_BUFFER_SIZE]) { +) -> &[u16] { let reference_period = internal_frequency / reference_frequency; let start_count = timestamp_start as f64 % reference_period; let mut valid_timestamps: usize = 0; - let mut timestamps: [u16; TIMESTAMP_BUFFER_SIZE] = - [0; TIMESTAMP_BUFFER_SIZE]; let mut timestamp = (reference_period - start_count) % reference_period; while timestamp < (timestamp_stop - timestamp_start) as f64 { @@ -176,7 +175,7 @@ fn adc_batch_timestamps( valid_timestamps += 1; } - (valid_timestamps, timestamps) + ×tamps[..valid_timestamps] } /// Lowpass biquad filter using cutoff and sampling frequencies. @@ -589,8 +588,10 @@ fn lowpass_test( internal_frequency, adc_frequency, ); - let (valid_timestamps, timestamps) = adc_batch_timestamps( + let mut timestamps_array = [0_u16; ADC_SAMPLE_BUFFER_SIZE / 2]; + let timestamps = adc_batch_timestamps( reference_frequency, + &mut timestamps_array, timestamp_start, timestamp_start + sample_count - 1, internal_frequency, @@ -598,9 +599,7 @@ fn lowpass_test( let mut in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE]; let mut quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE]; - let lockin_demodulate = - lockin.demodulate(signal, timestamps, valid_timestamps as u16); - match lockin_demodulate { + match lockin.demodulate(&signal, timestamps) { Ok((i, q)) => { in_phase = i; quadrature = q; From 785c98f93dd2a964ef74285cde70843d060061e7 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sat, 28 Nov 2020 13:25:49 -0800 Subject: [PATCH 54/71] lockin: remove TIMESTAMP_BUFFER_SIZE constant --- dsp/src/lockin.rs | 4 ---- 1 file changed, 4 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 30d2f8e..a8bcce4 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -67,10 +67,6 @@ use core::f32::consts::PI; /// The number of ADC samples in one batch. pub const ADC_SAMPLE_BUFFER_SIZE: usize = 16; -/// The maximum number of timestamps in the period for one ADC -/// batch. Each timestamp corresponds to the time of an external -/// reference clock edge. -pub const TIMESTAMP_BUFFER_SIZE: usize = ADC_SAMPLE_BUFFER_SIZE / 2; /// The number of outputs sent to the DAC for each ADC batch. pub const DECIMATED_BUFFER_SIZE: usize = 1; From fcdfcb0be7e6ac7d9347ddbd235a5e84974e80f4 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sat, 28 Nov 2020 14:21:48 -0800 Subject: [PATCH 55/71] lockin: use single iir instance for both in-phase and quadrature signals --- dsp/src/lockin.rs | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index a8bcce4..321aceb 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -102,7 +102,7 @@ pub struct Lockin { sample_period: u32, harmonic: u32, timestamps: [Option; 2], - iir: [IIR; 2], + iir: IIR, iirstate: [IIRState; 2], } @@ -118,9 +118,7 @@ impl Lockin { /// * `harmonic` - Integer scaling factor used to adjust the /// demodulation frequency. E.g., 2 would demodulate with the /// first harmonic. - /// * `iir` - IIR biquad filter. Two identical copies of this IIR - /// filter are used: one for the in-phase signal and the other for - /// the quadrature signal. + /// * `iir` - IIR biquad filter. /// /// # Returns /// @@ -136,7 +134,7 @@ impl Lockin { sample_period: sample_period, harmonic: harmonic, timestamps: [None, None], - iir: [iir, iir], + iir: iir, iirstate: [[0.; 5]; 2], } } @@ -236,8 +234,8 @@ impl Lockin { .iter_mut() .zip(quadrature.iter_mut()) .for_each(|(i, q)| { - *i = self.iir[0].update(&mut self.iirstate[0], *i); - *q = self.iir[1].update(&mut self.iirstate[1], *q); + *i = self.iir.update(&mut self.iirstate[0], *i); + *q = self.iir.update(&mut self.iirstate[1], *q); }); } } From d1b7efad48e4925b18f3439fe68840b434051413 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sat, 28 Nov 2020 16:04:22 -0800 Subject: [PATCH 56/71] dsp: replace in_phase and quadrature with Complex --- dsp/src/complex.rs | 21 +++ dsp/src/lib.rs | 1 + dsp/src/lockin.rs | 333 +++++++++++++++++------------------ dsp/tests/lockin_low_pass.rs | 35 ++-- 4 files changed, 201 insertions(+), 189 deletions(-) create mode 100644 dsp/src/complex.rs diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs new file mode 100644 index 0000000..3fb998f --- /dev/null +++ b/dsp/src/complex.rs @@ -0,0 +1,21 @@ +use core::cmp::PartialEq; + +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct Complex { + pub re: f32, + pub im: f32, +} + +impl Complex { + pub fn new(re: f32, im: f32) -> Self { + Complex { re: re, im: im } + } + + pub fn arg(&self) -> f32 { + libm::atan2f(self.im, self.re) + } + + pub fn abs(&self) -> f32 { + libm::sqrtf([self.re, self.im].iter().map(|i| i * i).sum()) + } +} diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index ffb021c..41e8a52 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -1,6 +1,7 @@ #![cfg_attr(not(test), no_std)] #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] +pub mod complex; pub mod iir; pub mod lockin; pub mod pll; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 321aceb..4dcfe69 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -2,25 +2,15 @@ //! //! Lock-in processing is performed through a combination of the //! following modular processing blocks: demodulation, filtering, -//! decimation and computing the magnitude and phase from the in-phase -//! and quadrature signals. These processing blocks are mutually -//! independent. +//! decimation and computing the magnitude and phase from a complex +//! signal. These processing blocks are mutually independent. //! //! # Terminology //! //! * _demodulation signal_ - A copy of the reference signal that is -//! optionally frequency scaled and phase shifted. There are two -//! copies of this signal. The first copy is in-phase with the -//! reference signal (before any optional phase shifting). The second -//! is 90 degrees out of phase (in quadrature) with the first -//! copy. The demodulation signals are used to demodulate the ADC +//! optionally frequency scaled and phase shifted. This is a complex +//! signal. The demodulation signals are used to demodulate the ADC //! sampled signal. -//! * _in-phase_ and _quadrature_ - These terms are used to delineate -//! between the two components of the demodulation signal and the -//! resulting two signals at any step downstream of the demodulation -//! step. The in-phase signal is in-phase with the reference signal -//! prior to any phase shifts. The quadrature signal is 90 degrees out -//! of phase with the in-phase signal. //! * _internal clock_ - A fast internal clock used to increment a //! counter for determining the 0-phase points of a reference signal. //! * _reference signal_ - A constant-frequency signal used to derive @@ -44,24 +34,25 @@ //! //! * `demodulate` - Computes the phase of the demodulation signal //! corresponding to each ADC sample, uses this phase to compute the -//! in-phase and quadrature demodulation signals, and multiplies these -//! demodulation signals by the ADC-sampled signal. This is a method -//! of `Lockin` since it requires information about how to modify the -//! reference signal for demodulation. -//! * `filter` - Performs IIR biquad filtering of in-phase and -//! quadrature signals. This is commonly performed on the in-phase and -//! quadrature components provided by the demodulation step, but can -//! be performed at any other point in the processing chain or omitted -//! entirely. `filter` is a method of `Lockin` since it must hold onto -//! the filter configuration and state. -//! * `decimate` - This decimates the in-phase and quadrature signals -//! to reduce the load on the DAC output. It does not require any -//! state information and is therefore a normal function. +//! demodulation signal, and multiplies this demodulation signal by +//! the ADC-sampled signal. This is a method of `Lockin` since it +//! requires information about how to modify the reference signal for +//! demodulation. +//! * `filter` - Performs IIR biquad filtering of a complex +//! signals. This is commonly performed on the signal provided by the +//! demodulation step, but can be performed at any other point in the +//! processing chain or omitted entirely. `filter` is a method of +//! `Lockin` since it must hold onto the filter configuration and +//! state. +//! * `decimate` - This decimates a signal to reduce the load on the +//! DAC output. It does not require any state information and is +//! therefore a normal function. //! * `magnitude_phase` - Computes the magnitude and phase of the //! component of the ADC-sampled signal whose frequency is equal to //! the demodulation frequency. This does not require any state //! information and is therefore a normal function. +use super::complex::Complex; use super::iir::{IIRState, IIR}; use core::f32::consts::PI; @@ -139,8 +130,7 @@ impl Lockin { } } - /// Demodulate an input signal with in-phase and quadrature - /// reference signals. + /// Demodulate an input signal with the complex reference signal. /// /// # Arguments /// @@ -151,9 +141,9 @@ impl Lockin { /// /// # Returns /// - /// The demodulated in-phase and quadrature signals as an - /// `Option`. When there are an insufficient number of timestamps - /// to perform processing, `None` is returned. + /// The demodulated complex signal as a `Result`. When there are + /// an insufficient number of timestamps to perform processing, + /// `Err` is returned. /// /// # Assumptions /// @@ -165,10 +155,7 @@ impl Lockin { &mut self, adc_samples: &[i16], timestamps: &[u16], - ) -> Result< - ([f32; ADC_SAMPLE_BUFFER_SIZE], [f32; ADC_SAMPLE_BUFFER_SIZE]), - &str, - > { + ) -> Result<[Complex; ADC_SAMPLE_BUFFER_SIZE], &str> { // update old timestamps for new ADC batch let sample_period = self.sample_period as i32; self.timestamps.iter_mut().for_each(|t| match *t { @@ -200,14 +187,12 @@ impl Lockin { // compute ADC sample phases, sines/cosines and demodulate let reference_period = self.timestamps[0].unwrap() - self.timestamps[1].unwrap(); - let mut in_phase = [0f32; ADC_SAMPLE_BUFFER_SIZE]; - let mut quadrature = [0f32; ADC_SAMPLE_BUFFER_SIZE]; - in_phase + let mut signal = [Complex::new(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; + signal .iter_mut() - .zip(quadrature.iter_mut()) .zip(adc_samples.iter()) .enumerate() - .for_each(|(n, ((i, q), sample))| { + .for_each(|(n, (s, sample))| { let integer_phase: i32 = (n as i32 * self.sample_period as i32 - self.timestamps[0].unwrap()) * self.harmonic as i32; @@ -215,104 +200,90 @@ impl Lockin { + 2. * PI * integer_phase as f32 / reference_period as f32; let (sine, cosine) = libm::sincosf(phase); let sample = *sample as f32; - *i = sine * sample; - *q = cosine * sample; + s.re = sine * sample; + s.im = cosine * sample; }); - Ok((in_phase, quadrature)) + Ok(signal) } - /// Filter the in-phase and quadrature signals using the supplied - /// biquad IIR. The signal arrays are modified in place. + /// Filter the complex signal using the supplied biquad IIR. The + /// signal array is modified in place. /// /// # Arguments /// - /// * `in_phase` - In-phase signal. - /// * `quadrature` - Quadrature signal. - pub fn filter(&mut self, in_phase: &mut [f32], quadrature: &mut [f32]) { - in_phase - .iter_mut() - .zip(quadrature.iter_mut()) - .for_each(|(i, q)| { - *i = self.iir.update(&mut self.iirstate[0], *i); - *q = self.iir.update(&mut self.iirstate[1], *q); - }); + /// * `signal` - Complex signal to filter. + pub fn filter(&mut self, signal: &mut [Complex]) { + signal.iter_mut().for_each(|s| { + s.re = self.iir.update(&mut self.iirstate[0], s.re); + s.im = self.iir.update(&mut self.iirstate[1], s.im); + }); } } -/// Decimate the in-phase and quadrature signals to -/// `DECIMATED_BUFFER_SIZE`. The ratio of `ADC_SAMPLE_BUFFER_SIZE` to -/// `DECIMATED_BUFFER_SIZE` must be a power of 2. +/// Decimate the complex signal to `DECIMATED_BUFFER_SIZE`. The ratio +/// of `ADC_SAMPLE_BUFFER_SIZE` to `DECIMATED_BUFFER_SIZE` must be a +/// power of 2. /// /// # Arguments /// -/// * `in_phase` - In-phase signal. -/// * `quadrature` - Quadrature signal. +/// * `signal` - Complex signal to decimate. /// /// # Returns /// -/// The decimated in-phase and quadrature signals. +/// The decimated signal. pub fn decimate( - in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE], - quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE], -) -> ([f32; DECIMATED_BUFFER_SIZE], [f32; DECIMATED_BUFFER_SIZE]) { + signal: [Complex; ADC_SAMPLE_BUFFER_SIZE], +) -> [Complex; DECIMATED_BUFFER_SIZE] { let n_k = ADC_SAMPLE_BUFFER_SIZE / DECIMATED_BUFFER_SIZE; debug_assert!( ADC_SAMPLE_BUFFER_SIZE == DECIMATED_BUFFER_SIZE || n_k % 2 == 0 ); - let mut in_phase_decimated = [0f32; DECIMATED_BUFFER_SIZE]; - let mut quadrature_decimated = [0f32; DECIMATED_BUFFER_SIZE]; + let mut signal_decimated = [Complex::new(0., 0.); DECIMATED_BUFFER_SIZE]; - in_phase_decimated + signal_decimated .iter_mut() - .zip(quadrature_decimated.iter_mut()) - .zip( - in_phase - .iter() - .step_by(n_k) - .zip(quadrature.iter().step_by(n_k)), - ) - .for_each(|((i_d, q_d), (i, q))| { - *i_d = *i; - *q_d = *q; + .zip(signal.iter().step_by(n_k)) + .for_each(|(s_d, s)| { + s_d.re = s.re; + s_d.im = s.im; }); - (in_phase_decimated, quadrature_decimated) + signal_decimated } -/// Compute the magnitude and phase from the in-phase and quadrature -/// signals. The in-phase and quadrature arrays are modified in place. +/// Compute the magnitude and phase from the complex signal. The +/// signal array is modified in place. /// /// # Arguments /// -/// * `in_phase` - In-phase signal. -/// * `quadrature` - Quadrature signal. -pub fn magnitude_phase(in_phase: &mut [f32], quadrature: &mut [f32]) { - in_phase - .iter_mut() - .zip(quadrature.iter_mut()) - .for_each(|(i, q)| { - let new_i = libm::sqrtf([*i, *q].iter().map(|i| i * i).sum()); - let new_q = libm::atan2f(*q, *i); - *i = new_i; - *q = new_q; - }); +/// * `signal` - Complex signal to decimate. +pub fn magnitude_phase(signal: &mut [Complex]) { + signal.iter_mut().for_each(|s| { + let new_i = s.abs(); + let new_q = s.arg(); + s.re = new_i; + s.im = new_q; + }); } #[cfg(test)] mod tests { use super::*; - extern crate std; fn f32_is_close(a: f32, b: f32) -> bool { (a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON } - fn f32_array_is_close(a: &[f32], b: &[f32]) -> bool { + fn complex_is_close(a: Complex, b: Complex) -> bool { + f32_is_close(a.re, b.re) && f32_is_close(a.im, b.im) + } + + fn complex_array_is_close(a: &[Complex], b: &[Complex]) -> bool { let mut result: bool = true; a.iter().zip(b.iter()).for_each(|(i, j)| { - result &= f32_is_close(*i, *j); + result &= complex_is_close(*i, *j); }); result } @@ -327,16 +298,30 @@ mod tests { <= a.abs().max(b.abs()) * relative_tolerance + fixed_tolerance } - fn array_within_tolerance( - a: &[f32], - b: &[f32], + fn complex_within_tolerance( + a: Complex, + b: Complex, + relative_tolerance: f32, + fixed_tolerance: f32, + ) -> bool { + within_tolerance(a.re, b.re, relative_tolerance, fixed_tolerance) + && within_tolerance(a.im, b.im, relative_tolerance, fixed_tolerance) + } + + fn complex_array_within_tolerance( + a: &[Complex], + b: &[Complex], relative_tolerance: f32, fixed_tolerance: f32, ) -> bool { let mut result: bool = true; a.iter().zip(b.iter()).for_each(|(i, j)| { - result &= - within_tolerance(*i, *j, relative_tolerance, fixed_tolerance); + result &= complex_within_tolerance( + *i, + *j, + relative_tolerance, + fixed_tolerance, + ); }); result } @@ -354,75 +339,93 @@ mod tests { #[test] fn magnitude_phase_length_1_quadrant_1() { - let mut in_phase: [f32; 1] = [1.]; - let mut quadrature: [f32; 1] = [1.]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[2_f32.sqrt()])); - assert!(f32_array_is_close(&quadrature, &[PI / 4.])); + let mut signal: [Complex; 1] = [Complex::new(1., 1.)]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new(2_f32.sqrt(), PI / 4.)] + )); - in_phase = [3_f32.sqrt() / 2.]; - quadrature = [1. / 2.]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[1_f32])); - assert!(f32_array_is_close(&quadrature, &[PI / 6.])); + signal = [Complex::new(3_f32.sqrt() / 2., 1. / 2.)]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new(1., PI / 6.)] + )); } #[test] fn magnitude_phase_length_1_quadrant_2() { - let mut in_phase: [f32; 1] = [-1.]; - let mut quadrature: [f32; 1] = [1.]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[2_f32.sqrt()])); - assert!(f32_array_is_close(&quadrature, &[3. * PI / 4.])); + let mut signal = [Complex::new(-1., 1.)]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new(2_f32.sqrt(), 3. * PI / 4.)] + )); - in_phase = [-1. / 2.]; - quadrature = [3_f32.sqrt() / 2.]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[1_f32])); - assert!(f32_array_is_close(&quadrature, &[2. * PI / 3.])); + signal = [Complex::new(-1. / 2., 3_f32.sqrt() / 2.)]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new(1_f32, 2. * PI / 3.)] + )); } #[test] fn magnitude_phase_length_1_quadrant_3() { - let mut in_phase: [f32; 1] = [-1. / 2_f32.sqrt()]; - let mut quadrature: [f32; 1] = [-1. / 2_f32.sqrt()]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[1_f32.sqrt()])); - assert!(f32_array_is_close(&quadrature, &[-3. * PI / 4.])); + let mut signal = [Complex::new(-1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new(1_f32.sqrt(), -3. * PI / 4.)] + )); - in_phase = [-1. / 2.]; - quadrature = [-2_f32.sqrt()]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[(3. / 2.) as f32])); - assert!(f32_array_is_close(&quadrature, &[-1.91063323625 as f32])); + signal = [Complex::new(-1. / 2., -2_f32.sqrt())]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new((3. / 2.) as f32, -1.91063323625 as f32)] + )); } #[test] fn magnitude_phase_length_1_quadrant_4() { - let mut in_phase: [f32; 1] = [1. / 2_f32.sqrt()]; - let mut quadrature: [f32; 1] = [-1. / 2_f32.sqrt()]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[1_f32.sqrt()])); - assert!(f32_array_is_close(&quadrature, &[-1. * PI / 4.])); + let mut signal = [Complex::new(1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new(1_f32.sqrt(), -1. * PI / 4.)] + )); - in_phase = [3_f32.sqrt() / 2.]; - quadrature = [-1. / 2.]; - magnitude_phase(&mut in_phase, &mut quadrature); - assert!(f32_array_is_close(&in_phase, &[1_f32])); - assert!(f32_array_is_close(&quadrature, &[-PI / 6.])); + signal = [Complex::new(3_f32.sqrt() / 2., -1. / 2.)]; + magnitude_phase(&mut signal); + assert!(complex_array_is_close( + &signal, + &[Complex::new(1_f32, -PI / 6.)] + )); } #[test] fn decimate_sample_16_decimated_1() { - let in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE] = [ - 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2, - 1.3, 1.4, 1.5, + let signal: [Complex; ADC_SAMPLE_BUFFER_SIZE] = [ + Complex::new(0.0, 1.6), + Complex::new(0.1, 1.7), + Complex::new(0.2, 1.8), + Complex::new(0.3, 1.9), + Complex::new(0.4, 2.0), + Complex::new(0.5, 2.1), + Complex::new(0.6, 2.2), + Complex::new(0.7, 2.3), + Complex::new(0.8, 2.4), + Complex::new(0.9, 2.5), + Complex::new(1.0, 2.6), + Complex::new(1.1, 2.7), + Complex::new(1.2, 2.8), + Complex::new(1.3, 2.9), + Complex::new(1.4, 3.0), + Complex::new(1.5, 3.1), ]; - let quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE] = [ - 1.6, 1.7, 1.8, 1.9, 2.0, 2.1, 2.2, 2.3, 2.4, 2.5, 2.6, 2.7, 2.8, - 2.9, 3.0, 3.1, - ]; - assert_eq!(decimate(in_phase, quadrature), ([0.0], [1.6])); + assert_eq!(decimate(signal), [Complex::new(0.0, 1.6)]); } #[test] @@ -439,7 +442,7 @@ mod tests { }, ); assert_eq!( - lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[],), + lockin.demodulate(&[0; ADC_SAMPLE_BUFFER_SIZE], &[]), Err("insufficient timestamps") ); } @@ -489,32 +492,20 @@ mod tests { -(initial_phase_integer as f32) / reference_period as f32 * 2. * PI; let phase_increment: f32 = adc_period as f32 / reference_period as f32 * 2. * PI; - let mut in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE] = - [0.; ADC_SAMPLE_BUFFER_SIZE]; - let mut quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE] = - [0.; ADC_SAMPLE_BUFFER_SIZE]; - for (n, (i, q)) in - in_phase.iter_mut().zip(quadrature.iter_mut()).enumerate() - { + let mut signal = [Complex::new(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; + for (n, s) in signal.iter_mut().enumerate() { let adc_phase = initial_phase + n as f32 * phase_increment; let sine = adc_phase.sin(); let cosine = adc_phase.cos(); - *i = sine * adc_samples[n] as f32; - *q = cosine * adc_samples[n] as f32; + s.re = sine * adc_samples[n] as f32; + s.im = cosine * adc_samples[n] as f32; } - let (result_in_phase, result_quadrature) = - lockin.demodulate(&adc_samples, timestamps).unwrap(); + let result = lockin.demodulate(&adc_samples, timestamps).unwrap(); assert!( - array_within_tolerance(&result_in_phase, &in_phase, 0., 1e-5), - "\nin_phase computed: {:?},\nin_phase expected: {:?}", - result_in_phase, - in_phase - ); - assert!( - array_within_tolerance(&result_quadrature, &quadrature, 0., 1e-5), - "\nquadrature computed: {:?},\nquadrature expected: {:?}", - result_quadrature, - quadrature + complex_array_within_tolerance(&result, &signal, 0., 1e-5), + "\nsignal computed: {:?},\nsignal expected: {:?}", + result, + signal ); } } diff --git a/dsp/tests/lockin_low_pass.rs b/dsp/tests/lockin_low_pass.rs index af64651..21ace3e 100644 --- a/dsp/tests/lockin_low_pass.rs +++ b/dsp/tests/lockin_low_pass.rs @@ -1,3 +1,4 @@ +use dsp::complex::Complex; use dsp::iir::IIR; use dsp::lockin::{ decimate, magnitude_phase, Lockin, ADC_SAMPLE_BUFFER_SIZE, @@ -582,7 +583,7 @@ fn lowpass_test( pure_signals.push(desired_input); for n in 0..(samples + extra_samples) { - let signal: [i16; ADC_SAMPLE_BUFFER_SIZE] = adc_sampled_signal( + let adc_signal: [i16; ADC_SAMPLE_BUFFER_SIZE] = adc_sampled_signal( &pure_signals, timestamp_start, internal_frequency, @@ -597,33 +598,31 @@ fn lowpass_test( internal_frequency, ); - let mut in_phase: [f32; ADC_SAMPLE_BUFFER_SIZE]; - let mut quadrature: [f32; ADC_SAMPLE_BUFFER_SIZE]; - match lockin.demodulate(&signal, timestamps) { - Ok((i, q)) => { - in_phase = i; - quadrature = q; + let mut signal: [Complex; ADC_SAMPLE_BUFFER_SIZE]; + match lockin.demodulate(&adc_signal, timestamps) { + Ok(s) => { + signal = s; } Err(_) => { continue; } } - lockin.filter(&mut in_phase, &mut quadrature); - let (in_phase_decimated, quadrature_decimated) = - decimate(in_phase, quadrature); + lockin.filter(&mut signal); + let signal_decimated = decimate(signal); - let mut magnitude_decimated = in_phase_decimated.clone(); - let mut phase_decimated = quadrature_decimated.clone(); + let mut magnitude_phase_decimated = signal.clone(); + // let mut magnitude_decimated = in_phase_decimated.clone(); + // let mut phase_decimated = quadrature_decimated.clone(); - magnitude_phase(&mut magnitude_decimated, &mut phase_decimated); + magnitude_phase(&mut magnitude_phase_decimated); // Ensure stable within tolerance for 1 time constant after // `time_constant_factor`. if n >= samples { for k in 0..DECIMATED_BUFFER_SIZE { let amplitude_normalized: f32 = - magnitude_decimated[k] / ADC_MAX_COUNT as f32; + magnitude_phase_decimated[k].re / ADC_MAX_COUNT as f32; assert!( tolerance_check(linear(desired_input.amplitude_dbfs) as f32, amplitude_normalized, total_magnitude_noise as f32, tolerance), "magnitude actual: {:.4} ({:.2} dBFS), magnitude computed: {:.4} ({:.2} dBFS), tolerance: {:.4}", @@ -636,13 +635,13 @@ fn lowpass_test( assert!( tolerance_check( effective_phase_offset as f32, - phase_decimated[k], + magnitude_phase_decimated[k].im, total_phase_noise as f32, tolerance ), "phase actual: {:.4}, phase computed: {:.4}, tolerance: {:.4}", effective_phase_offset as f32, - phase_decimated[k], + magnitude_phase_decimated[k].im, max_error( effective_phase_offset as f32, total_phase_noise as f32, @@ -651,9 +650,9 @@ fn lowpass_test( ); let in_phase_normalized: f32 = - in_phase_decimated[k] / ADC_MAX_COUNT as f32; + signal_decimated[k].re / ADC_MAX_COUNT as f32; let quadrature_normalized: f32 = - quadrature_decimated[k] / ADC_MAX_COUNT as f32; + signal_decimated[k].im / ADC_MAX_COUNT as f32; assert!( tolerance_check( in_phase_actual as f32, From 260206e4f0d2d1be8e5fabe253bc81df3699e999 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sat, 28 Nov 2020 16:21:08 -0800 Subject: [PATCH 57/71] dsp: implement Complex as type alias for tuple --- dsp/src/complex.rs | 21 ------ dsp/src/lib.rs | 2 +- dsp/src/lockin.rs | 140 ++++++++++++++++------------------- dsp/tests/lockin_low_pass.rs | 14 ++-- 4 files changed, 72 insertions(+), 105 deletions(-) delete mode 100644 dsp/src/complex.rs diff --git a/dsp/src/complex.rs b/dsp/src/complex.rs deleted file mode 100644 index 3fb998f..0000000 --- a/dsp/src/complex.rs +++ /dev/null @@ -1,21 +0,0 @@ -use core::cmp::PartialEq; - -#[derive(Copy, Clone, Debug, PartialEq)] -pub struct Complex { - pub re: f32, - pub im: f32, -} - -impl Complex { - pub fn new(re: f32, im: f32) -> Self { - Complex { re: re, im: im } - } - - pub fn arg(&self) -> f32 { - libm::atan2f(self.im, self.re) - } - - pub fn abs(&self) -> f32 { - libm::sqrtf([self.re, self.im].iter().map(|i| i * i).sum()) - } -} diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 41e8a52..ca1daec 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -1,7 +1,7 @@ #![cfg_attr(not(test), no_std)] #![cfg_attr(feature = "nightly", feature(asm, core_intrinsics))] -pub mod complex; +pub type Complex = (T, T); pub mod iir; pub mod lockin; pub mod pll; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 4dcfe69..66b3a9a 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -52,8 +52,8 @@ //! the demodulation frequency. This does not require any state //! information and is therefore a normal function. -use super::complex::Complex; use super::iir::{IIRState, IIR}; +use super::Complex; use core::f32::consts::PI; /// The number of ADC samples in one batch. @@ -155,7 +155,7 @@ impl Lockin { &mut self, adc_samples: &[i16], timestamps: &[u16], - ) -> Result<[Complex; ADC_SAMPLE_BUFFER_SIZE], &str> { + ) -> Result<[Complex; ADC_SAMPLE_BUFFER_SIZE], &str> { // update old timestamps for new ADC batch let sample_period = self.sample_period as i32; self.timestamps.iter_mut().for_each(|t| match *t { @@ -187,7 +187,7 @@ impl Lockin { // compute ADC sample phases, sines/cosines and demodulate let reference_period = self.timestamps[0].unwrap() - self.timestamps[1].unwrap(); - let mut signal = [Complex::new(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; + let mut signal = [(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; signal .iter_mut() .zip(adc_samples.iter()) @@ -200,8 +200,8 @@ impl Lockin { + 2. * PI * integer_phase as f32 / reference_period as f32; let (sine, cosine) = libm::sincosf(phase); let sample = *sample as f32; - s.re = sine * sample; - s.im = cosine * sample; + s.0 = sine * sample; + s.1 = cosine * sample; }); Ok(signal) @@ -213,10 +213,10 @@ impl Lockin { /// # Arguments /// /// * `signal` - Complex signal to filter. - pub fn filter(&mut self, signal: &mut [Complex]) { + pub fn filter(&mut self, signal: &mut [Complex]) { signal.iter_mut().for_each(|s| { - s.re = self.iir.update(&mut self.iirstate[0], s.re); - s.im = self.iir.update(&mut self.iirstate[1], s.im); + s.0 = self.iir.update(&mut self.iirstate[0], s.0); + s.1 = self.iir.update(&mut self.iirstate[1], s.1); }); } } @@ -233,21 +233,21 @@ impl Lockin { /// /// The decimated signal. pub fn decimate( - signal: [Complex; ADC_SAMPLE_BUFFER_SIZE], -) -> [Complex; DECIMATED_BUFFER_SIZE] { + signal: [Complex; ADC_SAMPLE_BUFFER_SIZE], +) -> [Complex; DECIMATED_BUFFER_SIZE] { let n_k = ADC_SAMPLE_BUFFER_SIZE / DECIMATED_BUFFER_SIZE; debug_assert!( ADC_SAMPLE_BUFFER_SIZE == DECIMATED_BUFFER_SIZE || n_k % 2 == 0 ); - let mut signal_decimated = [Complex::new(0., 0.); DECIMATED_BUFFER_SIZE]; + let mut signal_decimated = [(0_f32, 0_f32); DECIMATED_BUFFER_SIZE]; signal_decimated .iter_mut() .zip(signal.iter().step_by(n_k)) .for_each(|(s_d, s)| { - s_d.re = s.re; - s_d.im = s.im; + s_d.0 = s.0; + s_d.1 = s.1; }); signal_decimated @@ -259,12 +259,12 @@ pub fn decimate( /// # Arguments /// /// * `signal` - Complex signal to decimate. -pub fn magnitude_phase(signal: &mut [Complex]) { +pub fn magnitude_phase(signal: &mut [Complex]) { signal.iter_mut().for_each(|s| { - let new_i = s.abs(); - let new_q = s.arg(); - s.re = new_i; - s.im = new_q; + let new_i = libm::sqrtf([s.0, s.1].iter().map(|i| i * i).sum()); + let new_q = libm::atan2f(s.1, s.0); + s.0 = new_i; + s.1 = new_q; }); } @@ -276,11 +276,11 @@ mod tests { (a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON } - fn complex_is_close(a: Complex, b: Complex) -> bool { - f32_is_close(a.re, b.re) && f32_is_close(a.im, b.im) + fn complex_is_close(a: Complex, b: Complex) -> bool { + f32_is_close(a.0, b.0) && f32_is_close(a.1, b.1) } - fn complex_array_is_close(a: &[Complex], b: &[Complex]) -> bool { + fn complex_array_is_close(a: &[Complex], b: &[Complex]) -> bool { let mut result: bool = true; a.iter().zip(b.iter()).for_each(|(i, j)| { result &= complex_is_close(*i, *j); @@ -299,18 +299,18 @@ mod tests { } fn complex_within_tolerance( - a: Complex, - b: Complex, + a: Complex, + b: Complex, relative_tolerance: f32, fixed_tolerance: f32, ) -> bool { - within_tolerance(a.re, b.re, relative_tolerance, fixed_tolerance) - && within_tolerance(a.im, b.im, relative_tolerance, fixed_tolerance) + within_tolerance(a.0, b.0, relative_tolerance, fixed_tolerance) + && within_tolerance(a.1, b.1, relative_tolerance, fixed_tolerance) } fn complex_array_within_tolerance( - a: &[Complex], - b: &[Complex], + a: &[Complex], + b: &[Complex], relative_tolerance: f32, fixed_tolerance: f32, ) -> bool { @@ -339,93 +339,81 @@ mod tests { #[test] fn magnitude_phase_length_1_quadrant_1() { - let mut signal: [Complex; 1] = [Complex::new(1., 1.)]; + let mut signal: [Complex; 1] = [(1., 1.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close( - &signal, - &[Complex::new(2_f32.sqrt(), PI / 4.)] - )); + assert!(complex_array_is_close(&signal, &[(2_f32.sqrt(), PI / 4.)])); - signal = [Complex::new(3_f32.sqrt() / 2., 1. / 2.)]; + signal = [(3_f32.sqrt() / 2., 1. / 2.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close( - &signal, - &[Complex::new(1., PI / 6.)] - )); + assert!(complex_array_is_close(&signal, &[(1., PI / 6.)])); } #[test] fn magnitude_phase_length_1_quadrant_2() { - let mut signal = [Complex::new(-1., 1.)]; + let mut signal = [(-1., 1.)]; magnitude_phase(&mut signal); assert!(complex_array_is_close( &signal, - &[Complex::new(2_f32.sqrt(), 3. * PI / 4.)] + &[(2_f32.sqrt(), 3. * PI / 4.)] )); - signal = [Complex::new(-1. / 2., 3_f32.sqrt() / 2.)]; + signal = [(-1. / 2., 3_f32.sqrt() / 2.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close( - &signal, - &[Complex::new(1_f32, 2. * PI / 3.)] - )); + assert!(complex_array_is_close(&signal, &[(1_f32, 2. * PI / 3.)])); } #[test] fn magnitude_phase_length_1_quadrant_3() { - let mut signal = [Complex::new(-1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; + let mut signal = [(-1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; magnitude_phase(&mut signal); assert!(complex_array_is_close( &signal, - &[Complex::new(1_f32.sqrt(), -3. * PI / 4.)] + &[(1_f32.sqrt(), -3. * PI / 4.)] )); - signal = [Complex::new(-1. / 2., -2_f32.sqrt())]; + signal = [(-1. / 2., -2_f32.sqrt())]; magnitude_phase(&mut signal); assert!(complex_array_is_close( &signal, - &[Complex::new((3. / 2.) as f32, -1.91063323625 as f32)] + &[((3. / 2.) as f32, -1.91063323625 as f32)] )); } #[test] fn magnitude_phase_length_1_quadrant_4() { - let mut signal = [Complex::new(1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; + let mut signal = [(1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; magnitude_phase(&mut signal); assert!(complex_array_is_close( &signal, - &[Complex::new(1_f32.sqrt(), -1. * PI / 4.)] + &[(1_f32.sqrt(), -1. * PI / 4.)] )); - signal = [Complex::new(3_f32.sqrt() / 2., -1. / 2.)]; + signal = [(3_f32.sqrt() / 2., -1. / 2.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close( - &signal, - &[Complex::new(1_f32, -PI / 6.)] - )); + assert!(complex_array_is_close(&signal, &[(1_f32, -PI / 6.)])); } #[test] fn decimate_sample_16_decimated_1() { - let signal: [Complex; ADC_SAMPLE_BUFFER_SIZE] = [ - Complex::new(0.0, 1.6), - Complex::new(0.1, 1.7), - Complex::new(0.2, 1.8), - Complex::new(0.3, 1.9), - Complex::new(0.4, 2.0), - Complex::new(0.5, 2.1), - Complex::new(0.6, 2.2), - Complex::new(0.7, 2.3), - Complex::new(0.8, 2.4), - Complex::new(0.9, 2.5), - Complex::new(1.0, 2.6), - Complex::new(1.1, 2.7), - Complex::new(1.2, 2.8), - Complex::new(1.3, 2.9), - Complex::new(1.4, 3.0), - Complex::new(1.5, 3.1), + let signal: [Complex; ADC_SAMPLE_BUFFER_SIZE] = [ + (0.0, 1.6), + (0.1, 1.7), + (0.2, 1.8), + (0.3, 1.9), + (0.4, 2.0), + (0.5, 2.1), + (0.6, 2.2), + (0.7, 2.3), + (0.8, 2.4), + (0.9, 2.5), + (1.0, 2.6), + (1.1, 2.7), + (1.2, 2.8), + (1.3, 2.9), + (1.4, 3.0), + (1.5, 3.1), ]; - assert_eq!(decimate(signal), [Complex::new(0.0, 1.6)]); + assert_eq!(decimate(signal), [(0.0, 1.6)]); } #[test] @@ -492,13 +480,13 @@ mod tests { -(initial_phase_integer as f32) / reference_period as f32 * 2. * PI; let phase_increment: f32 = adc_period as f32 / reference_period as f32 * 2. * PI; - let mut signal = [Complex::new(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; + let mut signal = [(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; for (n, s) in signal.iter_mut().enumerate() { let adc_phase = initial_phase + n as f32 * phase_increment; let sine = adc_phase.sin(); let cosine = adc_phase.cos(); - s.re = sine * adc_samples[n] as f32; - s.im = cosine * adc_samples[n] as f32; + s.0 = sine * adc_samples[n] as f32; + s.1 = cosine * adc_samples[n] as f32; } let result = lockin.demodulate(&adc_samples, timestamps).unwrap(); assert!( diff --git a/dsp/tests/lockin_low_pass.rs b/dsp/tests/lockin_low_pass.rs index 21ace3e..37ab2b0 100644 --- a/dsp/tests/lockin_low_pass.rs +++ b/dsp/tests/lockin_low_pass.rs @@ -1,9 +1,9 @@ -use dsp::complex::Complex; use dsp::iir::IIR; use dsp::lockin::{ decimate, magnitude_phase, Lockin, ADC_SAMPLE_BUFFER_SIZE, DECIMATED_BUFFER_SIZE, }; +use dsp::Complex; use std::f64::consts::PI; use std::vec::Vec; @@ -598,7 +598,7 @@ fn lowpass_test( internal_frequency, ); - let mut signal: [Complex; ADC_SAMPLE_BUFFER_SIZE]; + let mut signal: [Complex; ADC_SAMPLE_BUFFER_SIZE]; match lockin.demodulate(&adc_signal, timestamps) { Ok(s) => { signal = s; @@ -622,7 +622,7 @@ fn lowpass_test( if n >= samples { for k in 0..DECIMATED_BUFFER_SIZE { let amplitude_normalized: f32 = - magnitude_phase_decimated[k].re / ADC_MAX_COUNT as f32; + magnitude_phase_decimated[k].0 / ADC_MAX_COUNT as f32; assert!( tolerance_check(linear(desired_input.amplitude_dbfs) as f32, amplitude_normalized, total_magnitude_noise as f32, tolerance), "magnitude actual: {:.4} ({:.2} dBFS), magnitude computed: {:.4} ({:.2} dBFS), tolerance: {:.4}", @@ -635,13 +635,13 @@ fn lowpass_test( assert!( tolerance_check( effective_phase_offset as f32, - magnitude_phase_decimated[k].im, + magnitude_phase_decimated[k].1, total_phase_noise as f32, tolerance ), "phase actual: {:.4}, phase computed: {:.4}, tolerance: {:.4}", effective_phase_offset as f32, - magnitude_phase_decimated[k].im, + magnitude_phase_decimated[k].1, max_error( effective_phase_offset as f32, total_phase_noise as f32, @@ -650,9 +650,9 @@ fn lowpass_test( ); let in_phase_normalized: f32 = - signal_decimated[k].re / ADC_MAX_COUNT as f32; + signal_decimated[k].0 / ADC_MAX_COUNT as f32; let quadrature_normalized: f32 = - signal_decimated[k].im / ADC_MAX_COUNT as f32; + signal_decimated[k].1 / ADC_MAX_COUNT as f32; assert!( tolerance_check( in_phase_actual as f32, From 277a5d2d812157e1d71b5cb96afe3a1db978a898 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Sat, 28 Nov 2020 16:41:16 -0800 Subject: [PATCH 58/71] dsp: move common test code to testing.rs file --- dsp/src/lib.rs | 3 +++ dsp/src/lockin.rs | 57 +++------------------------------------------- dsp/src/testing.rs | 54 +++++++++++++++++++++++++++++++++++++++++++ 3 files changed, 60 insertions(+), 54 deletions(-) create mode 100644 dsp/src/testing.rs diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index ca1daec..389bda5 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -5,3 +5,6 @@ pub type Complex = (T, T); pub mod iir; pub mod lockin; pub mod pll; + +#[cfg(test)] +mod testing; diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 66b3a9a..6b6017b 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -271,60 +271,9 @@ pub fn magnitude_phase(signal: &mut [Complex]) { #[cfg(test)] mod tests { use super::*; - - fn f32_is_close(a: f32, b: f32) -> bool { - (a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON - } - - fn complex_is_close(a: Complex, b: Complex) -> bool { - f32_is_close(a.0, b.0) && f32_is_close(a.1, b.1) - } - - fn complex_array_is_close(a: &[Complex], b: &[Complex]) -> bool { - let mut result: bool = true; - a.iter().zip(b.iter()).for_each(|(i, j)| { - result &= complex_is_close(*i, *j); - }); - result - } - - fn within_tolerance( - a: f32, - b: f32, - relative_tolerance: f32, - fixed_tolerance: f32, - ) -> bool { - (a - b).abs() - <= a.abs().max(b.abs()) * relative_tolerance + fixed_tolerance - } - - fn complex_within_tolerance( - a: Complex, - b: Complex, - relative_tolerance: f32, - fixed_tolerance: f32, - ) -> bool { - within_tolerance(a.0, b.0, relative_tolerance, fixed_tolerance) - && within_tolerance(a.1, b.1, relative_tolerance, fixed_tolerance) - } - - fn complex_array_within_tolerance( - a: &[Complex], - b: &[Complex], - relative_tolerance: f32, - fixed_tolerance: f32, - ) -> bool { - let mut result: bool = true; - a.iter().zip(b.iter()).for_each(|(i, j)| { - result &= complex_within_tolerance( - *i, - *j, - relative_tolerance, - fixed_tolerance, - ); - }); - result - } + use crate::testing::{ + complex_array_is_close, complex_array_within_tolerance, + }; #[test] fn array_push() { diff --git a/dsp/src/testing.rs b/dsp/src/testing.rs new file mode 100644 index 0000000..0fe9e4f --- /dev/null +++ b/dsp/src/testing.rs @@ -0,0 +1,54 @@ +use super::Complex; + +pub fn f32_is_close(a: f32, b: f32) -> bool { + (a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON +} + +pub fn complex_is_close(a: Complex, b: Complex) -> bool { + f32_is_close(a.0, b.0) && f32_is_close(a.1, b.1) +} + +pub fn complex_array_is_close(a: &[Complex], b: &[Complex]) -> bool { + let mut result: bool = true; + a.iter().zip(b.iter()).for_each(|(i, j)| { + result &= complex_is_close(*i, *j); + }); + result +} + +pub fn within_tolerance( + a: f32, + b: f32, + relative_tolerance: f32, + fixed_tolerance: f32, +) -> bool { + (a - b).abs() <= a.abs().max(b.abs()) * relative_tolerance + fixed_tolerance +} + +pub fn complex_within_tolerance( + a: Complex, + b: Complex, + relative_tolerance: f32, + fixed_tolerance: f32, +) -> bool { + within_tolerance(a.0, b.0, relative_tolerance, fixed_tolerance) + && within_tolerance(a.1, b.1, relative_tolerance, fixed_tolerance) +} + +pub fn complex_array_within_tolerance( + a: &[Complex], + b: &[Complex], + relative_tolerance: f32, + fixed_tolerance: f32, +) -> bool { + let mut result: bool = true; + a.iter().zip(b.iter()).for_each(|(i, j)| { + result &= complex_within_tolerance( + *i, + *j, + relative_tolerance, + fixed_tolerance, + ); + }); + result +} From 0cca4589fde0bed505113219f44a9c5723ecae90 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Mon, 30 Nov 2020 11:37:59 -0800 Subject: [PATCH 59/71] lockin: compute reference period from the closest 2 timestamps to the ADC sample --- dsp/src/lockin.rs | 78 ++++++++++++++++++++++++++++++++++++----------- 1 file changed, 61 insertions(+), 17 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 6b6017b..03e7334 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -156,8 +156,8 @@ impl Lockin { adc_samples: &[i16], timestamps: &[u16], ) -> Result<[Complex; ADC_SAMPLE_BUFFER_SIZE], &str> { - // update old timestamps for new ADC batch let sample_period = self.sample_period as i32; + // update old timestamps for new ADC batch self.timestamps.iter_mut().for_each(|t| match *t { Some(timestamp) => { // Existing timestamps have aged by one ADC batch @@ -169,32 +169,66 @@ impl Lockin { None => (), }); - // record new timestamps - timestamps - .iter() - .take(timestamps.len()) - .rev() - .take(2) - .rev() - .for_each(|t| self.timestamps.push(Some(*t as i32))); - // return prematurely if there aren't enough timestamps for // processing - if self.timestamps.iter().filter(|t| t.is_some()).count() < 2 { + let old_timestamp_count = + self.timestamps.iter().filter(|t| t.is_some()).count(); + if old_timestamp_count + timestamps.len() < 2 { return Err("insufficient timestamps"); } - // compute ADC sample phases, sines/cosines and demodulate - let reference_period = - self.timestamps[0].unwrap() - self.timestamps[1].unwrap(); let mut signal = [(0., 0.); ADC_SAMPLE_BUFFER_SIZE]; + // if we have not yet recorded any timestamps, the first + // reference period must be computed from the first and + // second timestamps in the array + let mut timestamp_index: usize = + if old_timestamp_count == 0 { 1 } else { 0 }; + + // compute ADC sample phases, sines/cosines and demodulate signal .iter_mut() .zip(adc_samples.iter()) .enumerate() - .for_each(|(n, (s, sample))| { - let integer_phase: i32 = (n as i32 * self.sample_period as i32 - - self.timestamps[0].unwrap()) + .for_each(|(i, (s, sample))| { + let adc_sample_count = i as i32 * sample_period; + // index of the closest timestamp that occurred after + // the current ADC sample + let closest_timestamp_after_index: i32 = if timestamps.len() > 0 + { + // Linear search is fast because both the timestamps + // and ADC sample counts are sorted. Because of this, + // we only need to check timestamps that were also + // greater than the last ADC sample count. + while timestamp_index < timestamps.len() - 1 + && (timestamps[timestamp_index] as i32) + < adc_sample_count + { + timestamp_index += 1; + } + timestamp_index as i32 + } else { + -1 + }; + + // closest timestamp that occurred before to the + // current ADC sample + let closest_timestamp_before: i32; + let reference_period = if closest_timestamp_after_index < 0 { + closest_timestamp_before = self.timestamps[0].unwrap(); + closest_timestamp_before - self.timestamps[1].unwrap() + } else if closest_timestamp_after_index == 0 { + closest_timestamp_before = self.timestamps[0].unwrap(); + timestamps[0] as i32 - closest_timestamp_before + } else { + closest_timestamp_before = timestamps + [(closest_timestamp_after_index - 1) as usize] + as i32; + timestamps[closest_timestamp_after_index as usize] as i32 + - closest_timestamp_before + }; + + let integer_phase: i32 = (adc_sample_count + - closest_timestamp_before) * self.harmonic as i32; let phase = self.phase_offset + 2. * PI * integer_phase as f32 / reference_period as f32; @@ -204,6 +238,16 @@ impl Lockin { s.1 = cosine * sample; }); + // record new timestamps + let start_index: usize = if timestamps.len() < 2 { + 0 + } else { + timestamps.len() - 2 + }; + timestamps[start_index..] + .iter() + .for_each(|t| self.timestamps.push(Some(*t as i32))); + Ok(signal) } From 55e7f1f0db74761626eb6c57501407773dd4a9e7 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Mon, 30 Nov 2020 12:34:20 -0800 Subject: [PATCH 60/71] dsp: fix small comment grammar error --- dsp/src/lockin.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index 03e7334..d4e635d 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -210,8 +210,8 @@ impl Lockin { -1 }; - // closest timestamp that occurred before to the - // current ADC sample + // closest timestamp that occurred before the current + // ADC sample let closest_timestamp_before: i32; let reference_period = if closest_timestamp_after_index < 0 { closest_timestamp_before = self.timestamps[0].unwrap(); From 1b02f558f660cbe4d9df0ae144b08907e06e9333 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Fri, 4 Dec 2020 08:13:02 -0800 Subject: [PATCH 61/71] CI: specify test location using --package dsp --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 253a307..6c7b0e9 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -94,7 +94,7 @@ jobs: uses: actions-rs/cargo@v1 with: command: test - args: --manifest-path dsp/Cargo.toml --target=x86_64-unknown-linux-gnu + args: --package dsp --target=x86_64-unknown-linux-gnu # Tell bors about it # https://github.com/rtic-rs/cortex-m-rtic/blob/8a4f9c6b8ae91bebeea0791680f89375a78bffc6/.github/workflows/build.yml#L566-L603 From 43a760e57a6b65d615de51513a4d9c92a50b8638 Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Fri, 4 Dec 2020 09:09:23 -0800 Subject: [PATCH 62/71] dsp testing: improve api for tolerance checking --- dsp/src/lockin.rs | 58 +++++++++++++++++++++++++++++++++------------- dsp/src/testing.rs | 47 ++++++++----------------------------- 2 files changed, 52 insertions(+), 53 deletions(-) diff --git a/dsp/src/lockin.rs b/dsp/src/lockin.rs index d4e635d..bb649ad 100644 --- a/dsp/src/lockin.rs +++ b/dsp/src/lockin.rs @@ -315,9 +315,7 @@ pub fn magnitude_phase(signal: &mut [Complex]) { #[cfg(test)] mod tests { use super::*; - use crate::testing::{ - complex_array_is_close, complex_array_within_tolerance, - }; + use crate::testing::complex_allclose; #[test] fn array_push() { @@ -334,41 +332,62 @@ mod tests { fn magnitude_phase_length_1_quadrant_1() { let mut signal: [Complex; 1] = [(1., 1.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close(&signal, &[(2_f32.sqrt(), PI / 4.)])); + assert!(complex_allclose( + &signal, + &[(2_f32.sqrt(), PI / 4.)], + f32::EPSILON, + 0. + )); signal = [(3_f32.sqrt() / 2., 1. / 2.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close(&signal, &[(1., PI / 6.)])); + assert!(complex_allclose( + &signal, + &[(1., PI / 6.)], + f32::EPSILON, + 0. + )); } #[test] fn magnitude_phase_length_1_quadrant_2() { let mut signal = [(-1., 1.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close( + assert!(complex_allclose( &signal, - &[(2_f32.sqrt(), 3. * PI / 4.)] + &[(2_f32.sqrt(), 3. * PI / 4.)], + f32::EPSILON, + 0. )); signal = [(-1. / 2., 3_f32.sqrt() / 2.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close(&signal, &[(1_f32, 2. * PI / 3.)])); + assert!(complex_allclose( + &signal, + &[(1_f32, 2. * PI / 3.)], + f32::EPSILON, + 0. + )); } #[test] fn magnitude_phase_length_1_quadrant_3() { let mut signal = [(-1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; magnitude_phase(&mut signal); - assert!(complex_array_is_close( + assert!(complex_allclose( &signal, - &[(1_f32.sqrt(), -3. * PI / 4.)] + &[(1_f32.sqrt(), -3. * PI / 4.)], + f32::EPSILON, + 0. )); signal = [(-1. / 2., -2_f32.sqrt())]; magnitude_phase(&mut signal); - assert!(complex_array_is_close( + assert!(complex_allclose( &signal, - &[((3. / 2.) as f32, -1.91063323625 as f32)] + &[((3. / 2.) as f32, -1.91063323625 as f32)], + f32::EPSILON, + 0. )); } @@ -376,14 +395,21 @@ mod tests { fn magnitude_phase_length_1_quadrant_4() { let mut signal = [(1. / 2_f32.sqrt(), -1. / 2_f32.sqrt())]; magnitude_phase(&mut signal); - assert!(complex_array_is_close( + assert!(complex_allclose( &signal, - &[(1_f32.sqrt(), -1. * PI / 4.)] + &[(1_f32.sqrt(), -1. * PI / 4.)], + f32::EPSILON, + 0. )); signal = [(3_f32.sqrt() / 2., -1. / 2.)]; magnitude_phase(&mut signal); - assert!(complex_array_is_close(&signal, &[(1_f32, -PI / 6.)])); + assert!(complex_allclose( + &signal, + &[(1_f32, -PI / 6.)], + f32::EPSILON, + 0. + )); } #[test] @@ -483,7 +509,7 @@ mod tests { } let result = lockin.demodulate(&adc_samples, timestamps).unwrap(); assert!( - complex_array_within_tolerance(&result, &signal, 0., 1e-5), + complex_allclose(&result, &signal, 0., 1e-5), "\nsignal computed: {:?},\nsignal expected: {:?}", result, signal diff --git a/dsp/src/testing.rs b/dsp/src/testing.rs index 0fe9e4f..1a8e109 100644 --- a/dsp/src/testing.rs +++ b/dsp/src/testing.rs @@ -1,54 +1,27 @@ use super::Complex; -pub fn f32_is_close(a: f32, b: f32) -> bool { - (a - b).abs() <= a.abs().max(b.abs()) * f32::EPSILON +pub fn isclose(a: f32, b: f32, rtol: f32, atol: f32) -> bool { + (a - b).abs() <= a.abs().max(b.abs()) * rtol + atol } -pub fn complex_is_close(a: Complex, b: Complex) -> bool { - f32_is_close(a.0, b.0) && f32_is_close(a.1, b.1) -} - -pub fn complex_array_is_close(a: &[Complex], b: &[Complex]) -> bool { - let mut result: bool = true; - a.iter().zip(b.iter()).for_each(|(i, j)| { - result &= complex_is_close(*i, *j); - }); - result -} - -pub fn within_tolerance( - a: f32, - b: f32, - relative_tolerance: f32, - fixed_tolerance: f32, -) -> bool { - (a - b).abs() <= a.abs().max(b.abs()) * relative_tolerance + fixed_tolerance -} - -pub fn complex_within_tolerance( +pub fn complex_isclose( a: Complex, b: Complex, - relative_tolerance: f32, - fixed_tolerance: f32, + rtol: f32, + atol: f32, ) -> bool { - within_tolerance(a.0, b.0, relative_tolerance, fixed_tolerance) - && within_tolerance(a.1, b.1, relative_tolerance, fixed_tolerance) + isclose(a.0, b.0, rtol, atol) && isclose(a.1, b.1, rtol, atol) } -pub fn complex_array_within_tolerance( +pub fn complex_allclose( a: &[Complex], b: &[Complex], - relative_tolerance: f32, - fixed_tolerance: f32, + rtol: f32, + atol: f32, ) -> bool { let mut result: bool = true; a.iter().zip(b.iter()).for_each(|(i, j)| { - result &= complex_within_tolerance( - *i, - *j, - relative_tolerance, - fixed_tolerance, - ); + result &= complex_isclose(*i, *j, rtol, atol); }); result } From b23d5fa0dc6e4d0a98673f1ea2ab50698c209131 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 4 Dec 2020 18:22:38 +0100 Subject: [PATCH 63/71] main.rs: whitespace --- src/main.rs | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main.rs b/src/main.rs index f2c3baa..632b58a 100644 --- a/src/main.rs +++ b/src/main.rs @@ -783,7 +783,6 @@ const APP: () = { } #[idle(resources=[net_interface, pounder, mac_addr, eth_mac, iir_state, iir_ch, afes])] - fn idle(mut c: idle::Context) -> ! { let mut socket_set_entries: [_; 8] = Default::default(); let mut sockets = From 4f8bdb971bb84846bfc26ce89db075e1f524871b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 4 Dec 2020 18:22:53 +0100 Subject: [PATCH 64/71] main.rs: style --- src/main.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main.rs b/src/main.rs index 632b58a..1ee6415 100644 --- a/src/main.rs +++ b/src/main.rs @@ -214,7 +214,7 @@ const APP: () = { pounder: Option>, // Format: iir_state[ch][cascade-no][coeff] - #[init([[[0.; 5]; IIR_CASCADE_LENGTH];2])] + #[init([[[0.; 5]; IIR_CASCADE_LENGTH]; 2])] iir_state: [[iir::IIRState; IIR_CASCADE_LENGTH]; 2], #[init([[iir::IIR { ba: [1., 0., 0., 0., 0.], y_offset: 0., y_min: -SCALE - 1., y_max: SCALE }; IIR_CASCADE_LENGTH]; 2])] iir_ch: [[iir::IIR; IIR_CASCADE_LENGTH]; 2], From 0072cda85fe5579fd9d4d90b5cc3407b616787ac Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Fri, 4 Dec 2020 23:05:04 +0100 Subject: [PATCH 65/71] pll: add convergence test --- dsp/src/pll.rs | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/dsp/src/pll.rs b/dsp/src/pll.rs index 8cef99b..4156d43 100644 --- a/dsp/src/pll.rs +++ b/dsp/src/pll.rs @@ -74,4 +74,23 @@ mod tests { assert_eq!(y, 0xc2); assert_eq!(f, y); } + + #[test] + fn converge() { + let mut p = PLLState::default(); + let f0 = 0x71f63049_i32; + let shift = 10; + let n = 31 << shift + 2; + let mut x = 0i32; + for i in 0..n { + x = x.wrapping_add(f0); + let (y, f) = p.update(x, shift); + if i > n / 4 { + assert_eq!(f.wrapping_sub(f0).abs() <= 1, true); + } + if i > n / 2 { + assert_eq!(y.wrapping_sub(x).abs() < 1 << 18, true); + } + } + } } From 628845d3566b6c575a253efde48c36ff59e2b858 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sat, 5 Dec 2020 08:12:07 +0100 Subject: [PATCH 66/71] pll: rename to just PLL --- dsp/src/pll.rs | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/dsp/src/pll.rs b/dsp/src/pll.rs index 4156d43..c27cf4a 100644 --- a/dsp/src/pll.rs +++ b/dsp/src/pll.rs @@ -29,7 +29,7 @@ use serde::{Deserialize, Serialize}; /// The extension to I^3,I^2,I behavior to track chirps phase-accurately or to i64 data to /// increase resolution for extremely narrowband applications is obvious. #[derive(Copy, Clone, Default, Deserialize, Serialize)] -pub struct PLLState { +pub struct PLL { // last input phase x: i32, // filtered frequency @@ -38,7 +38,7 @@ pub struct PLLState { y: i32, } -impl PLLState { +impl PLL { /// Update the PLL with a new phase sample. /// /// Args: @@ -69,7 +69,7 @@ mod tests { use super::*; #[test] fn mini() { - let mut p = PLLState::default(); + let mut p = PLL::default(); let (y, f) = p.update(0x10000, 10); assert_eq!(y, 0xc2); assert_eq!(f, y); @@ -77,7 +77,7 @@ mod tests { #[test] fn converge() { - let mut p = PLLState::default(); + let mut p = PLL::default(); let f0 = 0x71f63049_i32; let shift = 10; let n = 31 << shift + 2; From 2179560a3c1351cfae81868e6dd5026ba37aede9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sat, 5 Dec 2020 09:56:41 +0100 Subject: [PATCH 67/71] unwrap: add phase unwrapping tools --- dsp/src/lib.rs | 1 + dsp/src/unwrap.rs | 73 +++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 74 insertions(+) create mode 100644 dsp/src/unwrap.rs diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 9b9e966..33464f5 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -3,3 +3,4 @@ pub mod iir; pub mod pll; +pub mod unwrap; diff --git a/dsp/src/unwrap.rs b/dsp/src/unwrap.rs new file mode 100644 index 0000000..8bc5ffd --- /dev/null +++ b/dsp/src/unwrap.rs @@ -0,0 +1,73 @@ +use serde::{Deserialize, Serialize}; + +/// Get phase wrap from x to y. +/// +/// Phases are modulo integer overflow. +/// +/// Args: +/// * `x`: Old phase sample +/// * `y`: New phase sample +/// +/// Returns: +/// A tuple containg the (wrapped) phase difference and +/// one times the direction of the wrap. +pub fn get_wrap(x: i32, y: i32) -> (i32, i8) { + let delta = y.wrapping_sub(x); + let wrap = (delta >= 0) as i8 - (y >= x) as i8; + (delta, wrap) +} + +/// Phase unwrapper. +#[derive(Copy, Clone, Default, Deserialize, Serialize)] +pub struct Unwrapper { + // last input + x: i32, + // last wraps + v: i32, +} + +impl Unwrapper { + /// Unwrap a new sample from a phase sequence and update the + /// unwrapper state. + /// + /// Args: + /// * `x`: New phase sample + /// + /// Returns: + /// A tuple containing the (wrapped) phase difference + /// and the signed number of phase wraps corresponding to the new sample. + pub fn update(&mut self, x: i32) -> (i32, i32) { + let (dx, v) = get_wrap(self.x, x); + self.x = x; + self.v = self.v.wrapping_add(v as i32); + (dx, self.v) + } +} + +#[cfg(test)] +mod tests { + use super::*; + #[test] + fn mini() { + for (x0, x1, v) in [ + (0i32, 0i32, 0i8), + (1, 1, 0), + (-1, -1, 0), + (1, -1, 0), + (-1, 1, 0), + (0, 0x7fff_ffff, 0), + (-1, 0x7fff_ffff, -1), + (0, -0x8000_0000, 0), + (1, -0x8000_0000, 1), + (-0x6000_0000, 0x6000_0000, -1), + (0x6000_0000, -0x6000_0000, 1), + (0x6000_0000, -0x6000_0000, 1), + (0x6000_0000, -0x6000_0000, 1), + ] + .iter() + { + let (_dx, w) = get_wrap(*x0, *x1); + assert_eq!(*v, w, " = get_wrap({:#x}, {:#x})", *x0, *x1); + } + } +} From 941a94bbf6d37c042750e42ccb6b588611a789d3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sat, 5 Dec 2020 11:44:09 +0100 Subject: [PATCH 68/71] dsp/pll: style --- dsp/src/lib.rs | 2 +- dsp/src/pll.rs | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/dsp/src/lib.rs b/dsp/src/lib.rs index 748b4b8..10cfcaa 100644 --- a/dsp/src/lib.rs +++ b/dsp/src/lib.rs @@ -8,4 +8,4 @@ pub mod pll; pub mod unwrap; #[cfg(test)] -mod testing; \ No newline at end of file +mod testing; diff --git a/dsp/src/pll.rs b/dsp/src/pll.rs index c27cf4a..8f060c2 100644 --- a/dsp/src/pll.rs +++ b/dsp/src/pll.rs @@ -49,7 +49,7 @@ impl PLL { /// Returns: /// A tuple of instantaneous phase and frequency (the current phase increment). pub fn update(&mut self, x: i32, shift: u8) -> (i32, i32) { - debug_assert!(shift >= 1 && shift <= 31); + debug_assert!((1..=30).contains(&shift)); let bias = 1i32 << shift; let e = x.wrapping_sub(self.f); self.f = self.f.wrapping_add( @@ -57,7 +57,7 @@ impl PLL { ); self.x = x; let f = self.f.wrapping_add( - bias.wrapping_add(e).wrapping_sub(self.y) >> shift - 1, + bias.wrapping_add(e).wrapping_sub(self.y) >> (shift - 1), ); self.y = self.y.wrapping_add(f); (self.y, f) From 526fea8e23cd960f0a118c7ddcf810707d5d78ed Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sat, 5 Dec 2020 11:58:59 +0100 Subject: [PATCH 69/71] unwrap: more tests --- dsp/src/unwrap.rs | 33 ++++++++++++++++++++------------- 1 file changed, 20 insertions(+), 13 deletions(-) diff --git a/dsp/src/unwrap.rs b/dsp/src/unwrap.rs index 8bc5ffd..0c20852 100644 --- a/dsp/src/unwrap.rs +++ b/dsp/src/unwrap.rs @@ -2,15 +2,16 @@ use serde::{Deserialize, Serialize}; /// Get phase wrap from x to y. /// -/// Phases are modulo integer overflow. +/// Phases are modulo integer overflow. The wraps have the same bias as +/// the base data type itself. /// /// Args: /// * `x`: Old phase sample /// * `y`: New phase sample /// /// Returns: -/// A tuple containg the (wrapped) phase difference and -/// one times the direction of the wrap. +/// A tuple containg the (wrapped) phase difference and the signum of the wrap. +#[inline(always)] pub fn get_wrap(x: i32, y: i32) -> (i32, i8) { let delta = y.wrapping_sub(x); let wrap = (delta >= 0) as i8 - (y >= x) as i8; @@ -27,15 +28,15 @@ pub struct Unwrapper { } impl Unwrapper { - /// Unwrap a new sample from a phase sequence and update the - /// unwrapper state. + /// Unwrap a new sample from a phase sequence and update the unwrapper + /// state. /// /// Args: /// * `x`: New phase sample /// /// Returns: - /// A tuple containing the (wrapped) phase difference - /// and the signed number of phase wraps corresponding to the new sample. + /// A tuple containing the (wrapped) phase difference and the signed + /// number of phase wraps accumulated to the new sample. pub fn update(&mut self, x: i32) -> (i32, i32) { let (dx, v) = get_wrap(self.x, x); self.x = x; @@ -51,18 +52,24 @@ mod tests { fn mini() { for (x0, x1, v) in [ (0i32, 0i32, 0i8), - (1, 1, 0), - (-1, -1, 0), - (1, -1, 0), - (-1, 1, 0), + (0, 1, 0), + (0, -1, 0), + (1, 0, 0), + (-1, 0, 0), (0, 0x7fff_ffff, 0), (-1, 0x7fff_ffff, -1), + (-2, 0x7fff_ffff, -1), + (-1, -0x8000_0000, 0), (0, -0x8000_0000, 0), (1, -0x8000_0000, 1), (-0x6000_0000, 0x6000_0000, -1), (0x6000_0000, -0x6000_0000, 1), - (0x6000_0000, -0x6000_0000, 1), - (0x6000_0000, -0x6000_0000, 1), + (-0x4000_0000, 0x3fff_ffff, 0), + (-0x4000_0000, 0x4000_0000, -1), + (-0x4000_0000, 0x4000_0001, -1), + (0x4000_0000, -0x3fff_ffff, 0), + (0x4000_0000, -0x4000_0000, 0), + (0x4000_0000, -0x4000_0001, 1), ] .iter() { From 974fa6e220f18bd39ff67087c9836daae2cbca74 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sat, 5 Dec 2020 12:41:02 +0100 Subject: [PATCH 70/71] unwrap: clean up docs and names --- dsp/src/unwrap.rs | 41 ++++++++++++++++++++++------------------- 1 file changed, 22 insertions(+), 19 deletions(-) diff --git a/dsp/src/unwrap.rs b/dsp/src/unwrap.rs index 0c20852..c89599f 100644 --- a/dsp/src/unwrap.rs +++ b/dsp/src/unwrap.rs @@ -1,24 +1,25 @@ use serde::{Deserialize, Serialize}; -/// Get phase wrap from x to y. +/// Subtract `y - x` with signed overflow. /// -/// Phases are modulo integer overflow. The wraps have the same bias as -/// the base data type itself. -/// -/// Args: -/// * `x`: Old phase sample -/// * `y`: New phase sample +/// This is very similar to `i32::overflowing_sub(y, x)` except that the +/// overflow indicator is not a boolean but the signum of the overflow. +/// Additionally it's typically faster. /// /// Returns: -/// A tuple containg the (wrapped) phase difference and the signum of the wrap. +/// A tuple containg the (wrapped) difference `y - x` and the signum of the +/// overflow. #[inline(always)] -pub fn get_wrap(x: i32, y: i32) -> (i32, i8) { +pub fn overflowing_sub(y: i32, x: i32) -> (i32, i8) { let delta = y.wrapping_sub(x); let wrap = (delta >= 0) as i8 - (y >= x) as i8; (delta, wrap) } -/// Phase unwrapper. +/// Overflow unwrapper. +/// +/// This is unwrapping as in the phase unwrapping context, not unwrapping as +/// in the `Result`/`Option` context. #[derive(Copy, Clone, Default, Deserialize, Serialize)] pub struct Unwrapper { // last input @@ -28,19 +29,18 @@ pub struct Unwrapper { } impl Unwrapper { - /// Unwrap a new sample from a phase sequence and update the unwrapper - /// state. + /// Unwrap a new sample from a sequence and update the unwrapper state. /// /// Args: - /// * `x`: New phase sample + /// * `x`: New sample /// /// Returns: - /// A tuple containing the (wrapped) phase difference and the signed - /// number of phase wraps accumulated to the new sample. + /// A tuple containing the (wrapped) difference `x - x_old` and the signed + /// number of wraps accumulated by `x`. pub fn update(&mut self, x: i32) -> (i32, i32) { - let (dx, v) = get_wrap(self.x, x); + let (dx, v) = overflowing_sub(x, self.x); self.x = x; - self.v = self.v.wrapping_add(v as i32); + self.v = self.v.saturating_add(v as i32); (dx, self.v) } } @@ -73,8 +73,11 @@ mod tests { ] .iter() { - let (_dx, w) = get_wrap(*x0, *x1); - assert_eq!(*v, w, " = get_wrap({:#x}, {:#x})", *x0, *x1); + let (dx, w) = overflowing_sub(*x1, *x0); + assert_eq!(*v, w, " = overflowing_sub({:#x}, {:#x})", *x0, *x1); + let (dx0, w0) = x1.overflowing_sub(*x0); + assert_eq!(w0, w != 0); + assert_eq!(dx, dx0); } } } From 4cfe6ba41634d4c4e6046855271529277a3c8161 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Robert=20J=C3=B6rdens?= Date: Sat, 5 Dec 2020 13:05:59 +0100 Subject: [PATCH 71/71] pll: add note on dithering --- dsp/src/pll.rs | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/dsp/src/pll.rs b/dsp/src/pll.rs index 8f060c2..5ee054d 100644 --- a/dsp/src/pll.rs +++ b/dsp/src/pll.rs @@ -10,9 +10,8 @@ use serde::{Deserialize, Serialize}; /// stable for any gain (1 <= shift <= 30). It has a single parameter that determines the loop /// bandwidth in octave steps. The gain can be changed freely between updates. /// -/// The frequency settling time constant for an (any) frequency jump is `1 << shift` update cycles. -/// The phase settling time in response to a frequency jump is about twice that. The loop bandwidth -/// is about `1/(2*pi*(1 << shift))` in units of the sample rate. +/// The frequency and phase settling time constants for an (any) frequency jump are `1 << shift` +/// update cycles. The loop bandwidth is about `1/(2*pi*(1 << shift))` in units of the sample rate. /// /// All math is naturally wrapping 32 bit integer. Phase and frequency are understood modulo that /// overflow in the first Nyquist zone. Expressing the IIR equations in other ways (e.g. single @@ -20,7 +19,8 @@ use serde::{Deserialize, Serialize}; /// /// There are no floating point rounding errors here. But there is integer quantization/truncation /// error of the `shift` lowest bits leading to a phase offset for very low gains. Truncation -/// bias is applied. Rounding is "half up". +/// bias is applied. Rounding is "half up". The phase truncation error can be removed very +/// efficiently by dithering. /// /// This PLL does not unwrap phase slips during lock acquisition. This can and should be /// implemented elsewhere by (down) scaling and then unwrapping the input phase and (up) scaling @@ -89,6 +89,7 @@ mod tests { assert_eq!(f.wrapping_sub(f0).abs() <= 1, true); } if i > n / 2 { + // The remaining error is removed by dithering. assert_eq!(y.wrapping_sub(x).abs() < 1 << 18, true); } }