From e599977983dd097f9854c4f553a67ba3b55c207a Mon Sep 17 00:00:00 2001 From: Matt Huszagh Date: Wed, 13 Jan 2021 08:53:06 -0800 Subject: [PATCH] revert changes in main.rs and server.rs --- src/main.rs | 109 ++++++++++++-------------------------------------- src/server.rs | 12 +++--- 2 files changed, 32 insertions(+), 89 deletions(-) diff --git a/src/main.rs b/src/main.rs index 6de262c..c5a9698 100644 --- a/src/main.rs +++ b/src/main.rs @@ -60,26 +60,14 @@ use heapless::{consts::*, String}; // The number of ticks in the ADC sampling timer. The timer runs at 100MHz, so the step size is // equal to 10ns per tick. // Currently, the sample rate is equal to: Fsample = 100/256 MHz = 390.625 KHz -const ADC_SAMPLE_TICKS_LOG2: u16 = 8; -const ADC_SAMPLE_TICKS: u16 = 1 << ADC_SAMPLE_TICKS_LOG2; +const ADC_SAMPLE_TICKS: u16 = 256; // The desired ADC sample processing buffer size. -const SAMPLE_BUFFER_SIZE_LOG2: usize = 3; -const SAMPLE_BUFFER_SIZE: usize = 1 << SAMPLE_BUFFER_SIZE_LOG2; - -// The number of ADC batches in one timer overflow period. -pub const ADC_BATCHES_LOG2: usize = - 32 - SAMPLE_BUFFER_SIZE_LOG2 - ADC_SAMPLE_TICKS_LOG2 as usize; -pub const ADC_BATCHES: usize = 1 << ADC_BATCHES_LOG2; +const SAMPLE_BUFFER_SIZE: usize = 8; // The number of cascaded IIR biquads per channel. Select 1 or 2! const IIR_CASCADE_LENGTH: usize = 1; -// Frequency scaling factor for lock-in harmonic demodulation. -const HARMONIC: u32 = 1; -// Phase offset applied to the lock-in demodulation signal. -const PHASE_OFFSET: u32 = 0; - #[link_section = ".sram3.eth"] static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new(); @@ -96,13 +84,7 @@ mod timers; use adc::{Adc0Input, Adc1Input}; use dac::{Dac0Output, Dac1Output}; -use dsp::{ - divide_round, - iir_int::{IIRState, IIR}, - pll::PLL, - shift_round, - trig::{atan2, cossin}, -}; +use dsp::iir; use pounder::DdsOutput; #[cfg(not(feature = "semihosting"))] @@ -146,6 +128,8 @@ static mut NET_STORE: NetStorage = NetStorage { routes_storage: [None; 1], }; +const SCALE: f32 = ((1 << 15) - 1) as f32; + // static ETHERNET_PENDING: AtomicBool = AtomicBool::new(true); const TCP_RX_BUFFER_SIZE: usize = 8192; @@ -221,9 +205,6 @@ const APP: () = { adcs: (Adc0Input, Adc1Input), dacs: (Dac0Output, Dac1Output), input_stamper: digital_input_stamper::InputStamper, - timestamp_handler: TimestampHandler, - iir_lockin: IIR, - iir_state_lockin: [IIRState; 2], eeprom_i2c: hal::i2c::I2c, @@ -245,10 +226,10 @@ const APP: () = { pounder_stamper: Option, // Format: iir_state[ch][cascade-no][coeff] - #[init([[[0; 5]; IIR_CASCADE_LENGTH]; 2])] - iir_state: [[IIRState; IIR_CASCADE_LENGTH]; 2], - #[init([[IIR { ba: [1, 0, 0, 0, 0] }; IIR_CASCADE_LENGTH]; 2])] - iir_ch: [[IIR; 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], } #[init] @@ -946,12 +927,6 @@ const APP: () = { #[cfg(not(feature = "pounder_v1_1"))] let pounder_stamper = None; - let timestamp_handler = TimestampHandler::new(4, 3); - let iir_lockin = IIR { - ba: [1, 0, 0, 0, 0], - }; - let iir_state_lockin = [[0; 5]; 2]; - // Start sampling ADCs. sampling_timer.start(); timestamp_timer.start(); @@ -967,10 +942,6 @@ const APP: () = { pounder: pounder_devices, pounder_stamper, - timestamp_handler, - iir_lockin, - iir_state_lockin, - eeprom_i2c, net_interface: network_interface, eth_mac, @@ -978,7 +949,7 @@ const APP: () = { } } - #[task(binds=DMA1_STR4, resources=[pounder_stamper, adcs, dacs, iir_state, iir_ch, dds_output, input_stamper, timestamp_handler, iir_lockin, iir_state_lockin], priority=2)] + #[task(binds=DMA1_STR4, resources=[pounder_stamper, adcs, dacs, iir_state, iir_ch, dds_output, input_stamper], priority=2)] fn process(c: process::Context) { if let Some(stamper) = c.resources.pounder_stamper { let pounder_timestamps = stamper.acquire_buffer(); @@ -994,52 +965,23 @@ const APP: () = { c.resources.dacs.1.acquire_buffer(), ]; - let (demodulation_initial_phase, demodulation_frequency) = c - .resources - .timestamp_handler - .update(c.resources.input_stamper.latest_timestamp()); + let _timestamp = c.resources.input_stamper.latest_timestamp(); - let [dac0, dac1] = dac_samples; - let iir_lockin = c.resources.iir_lockin; - let iir_state_lockin = c.resources.iir_state_lockin; - let iir_ch = c.resources.iir_ch; - let iir_state = c.resources.iir_state; - - dac0.iter_mut().zip(dac1.iter_mut()).enumerate().for_each( - |(i, (d0, d1))| { - let sample_phase = (HARMONIC.wrapping_mul( - (demodulation_frequency.wrapping_mul(i as u32)) - .wrapping_add(demodulation_initial_phase), - )) - .wrapping_add(PHASE_OFFSET); - let (cos, sin) = cossin(sample_phase as i32); - - let mut signal = (0_i32, 0_i32); - - // shift cos/sin before multiplying to avoid i64 multiplication - signal.0 = - adc_samples[0][i] as i16 as i32 * shift_round(cos, 16); - signal.1 = - adc_samples[0][i] as i16 as i32 * shift_round(sin, 16); - - signal.0 = - iir_lockin.update(&mut iir_state_lockin[0], signal.0); - signal.1 = - iir_lockin.update(&mut iir_state_lockin[1], signal.1); - - let mut magnitude = signal.0 * signal.0 + signal.1 * signal.1; - let mut phase = atan2(signal.1, signal.0); - - for j in 0..iir_state[0].len() { - magnitude = - iir_ch[0][j].update(&mut iir_state[0][j], magnitude); - phase = iir_ch[1][j].update(&mut iir_state[1][j], phase); + 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 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); } - - *d0 = shift_round(magnitude, 16) as i16 as u16; - *d1 = shift_round(phase, 16) as i16 as u16; - }, - ); + // 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::() }; + // Convert to DAC code + dac_samples[channel][sample] = y as u16 ^ 0x8000; + } + } if let Some(dds_output) = c.resources.dds_output { let builder = dds_output.builder().update_channels( @@ -1052,6 +994,7 @@ const APP: () = { builder.write_profile(); } + let [dac0, dac1] = dac_samples; c.resources.dacs.0.release_buffer(dac0); c.resources.dacs.1.release_buffer(dac1); } diff --git a/src/server.rs b/src/server.rs index 72e9102..2803805 100644 --- a/src/server.rs +++ b/src/server.rs @@ -6,8 +6,8 @@ use serde::{Deserialize, Serialize}; use serde_json_core::{de::from_slice, ser::to_string}; +use super::iir; use super::net; -use super::IIR; #[derive(Deserialize, Serialize, Debug)] pub enum AccessRequest { @@ -25,7 +25,7 @@ pub struct Request<'a> { #[derive(Serialize, Deserialize)] pub struct IirRequest { pub channel: u8, - pub iir: IIR, + pub iir: iir::IIR, } #[derive(Serialize)] @@ -137,10 +137,10 @@ impl Response { #[derive(Serialize)] pub struct Status { pub t: u32, - pub x0: i32, - pub y0: i32, - pub x1: i32, - pub y1: i32, + pub x0: f32, + pub y0: f32, + pub x1: f32, + pub y1: f32, } pub fn json_reply(socket: &mut net::socket::TcpSocket, msg: &T) {