From a9bb854a2e2774f4ae028591fb007f56287fefa3 Mon Sep 17 00:00:00 2001 From: Hubald Verzijl Date: Wed, 8 Oct 2025 15:40:17 +0200 Subject: [PATCH] Included bode plot measurements, including different number of points. --- Cargo.lock | 18 ++++++++--- src/ad5940.rs | 8 ++--- src/communication.rs | 55 ++++++++++++++++++++++++++-------- src/impedance.rs | 71 ++++++++++++++++++++++++++++++-------------- src/main.rs | 62 +++++++++++++++++++++----------------- 5 files changed, 145 insertions(+), 69 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index bd9c1c8..34f7f66 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -972,18 +972,28 @@ checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3" [[package]] name = "serde" -version = "1.0.219" +version = "1.0.228" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5f0e2c6ed6606019b4e29e69dbaba95b11854410e5347d525002456dbbb786b6" +checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e" +dependencies = [ + "serde_core", + "serde_derive", +] + +[[package]] +name = "serde_core" +version = "1.0.228" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "41d385c7d4ca58e59fc732af25c3983b67ac852c1a25000afe1175de458b67ad" dependencies = [ "serde_derive", ] [[package]] name = "serde_derive" -version = "1.0.219" +version = "1.0.228" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "5b0276cf7f2c73365f7157c8123c21cd9a50fbbd844757af28ca1f5925fc2a00" +checksum = "d540f220d3187173da220f885ab66608367b6574e925011a9353e4badda91d79" dependencies = [ "proc-macro2", "quote", diff --git a/src/ad5940.rs b/src/ad5940.rs index 6f8f74a..78a746d 100644 --- a/src/ad5940.rs +++ b/src/ad5940.rs @@ -473,7 +473,7 @@ impl AD5940 { } pub async fn sequencer_calculate_wait_time(&mut self, config: &DspConfig) -> Option { - let mut wait_time = 0; + let mut wait_time = 1; let sinc3_table = [5, 4, 2]; let sinc2_table = [22, 44, 89, 178, 267, 533, 640, 667, 800, 889, 1067, 1333]; @@ -481,20 +481,20 @@ impl AD5940 { match config.dftin { Some(DFTINSEL::Sinc2) => { if let Some(sinc3osr) = config.sinc3osr { - wait_time += sinc3_table[sinc3osr as usize]; + wait_time *= sinc3_table[sinc3osr as usize]; } else { return None; // Sinc2 requires sinc3osr to be set }; if let Some(sinc2osr) = config.sinc2osr { - wait_time += sinc2_table[sinc2osr as usize]; + wait_time *= sinc2_table[sinc2osr as usize]; } else { return None; // Sinc2 requires sinc2osr to be set }; } Some(DFTINSEL::GainOffset) => { if let Some(sinc3osr) = config.sinc3osr { - wait_time += sinc3_table[sinc3osr as usize]; + wait_time *= sinc3_table[sinc3osr as usize]; } else { return None; // Sinc2 requires sinc3osr to be set }; diff --git a/src/communication.rs b/src/communication.rs index 8077934..01c171b 100644 --- a/src/communication.rs +++ b/src/communication.rs @@ -7,6 +7,7 @@ use embassy_futures::select::{select, Either}; use embassy_stm32::usb::Driver; use embassy_stm32::{peripherals, uid, usb}; +use heapless::Vec; use postcard_rpc::{ define_dispatch, header::VarHeader, @@ -19,9 +20,9 @@ use postcard_rpc::{ }, }; -use bioz_icd_rs::{GetUniqueIdEndpoint, PingEndpoint, SetGreenLedEndpoint, SingleImpedanceOutput, SingleImpedanceOutputTopic, StartMultiImpedance, StartMultiImpedanceEndpoint, StartSingleImpedance, StartSingleImpedanceEndpoint, StopMultiImpedanceEndpoint, StopSingleImpedanceEndpoint, MultiImpedanceOutputTopic28, MultiImpedanceOutput28, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST}; +use bioz_icd_rs::{GetUniqueIdEndpoint, MeasurementPointSet, MultiImpedanceOutputTopic, MultiImpedanceResult, MultiImpedanceStartRequest, PingEndpoint, SetGreenLedEndpoint, SingleImpedanceOutputTopic, SingleImpedanceStartRequest, StartMultiImpedanceEndpoint, StartSingleImpedanceEndpoint, StopMultiImpedanceEndpoint, StopSingleImpedanceEndpoint, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST}; -use crate::impedance::{ImpedanceSetupType, IMPEDANCE_CHANNEL_SINGLE, IMPEDANCE_CHANNEL_MULTI, RunningMode}; +use crate::impedance::{ImpedanceSetupType, RunningMode, IMPEDANCE_CHANNEL_MULTI, IMPEDANCE_CHANNEL_SINGLE}; // Postcard RPC types type AppDriver = usb::Driver<'static, peripherals::USB>; @@ -164,7 +165,7 @@ pub async fn set_green_led_handler(_context: &mut Context, _header: VarHeader, r static STOP: Signal = Signal::new(); #[embassy_executor::task] -pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: StartSingleImpedance, sender: Sender) { +pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: SingleImpedanceStartRequest, sender: Sender) { info!("Start impedance measurement at {:?} Hz.", rqst.sinus_frequency); // Mark the impedance setup as running @@ -216,24 +217,54 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader pub async fn stop_single_impedance_handler(context: &mut Context, _header: VarHeader, _rqst: ()) -> bool { info!("Stop impedance measurement"); let was_busy = context.impedance_setup.lock().await.running_mode; - if was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency { + if was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { STOP.signal(()); } - was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency + was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) } #[embassy_executor::task] -pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: StartMultiImpedance, sender: Sender) { +pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: MultiImpedanceStartRequest, sender: Sender) { // Mark the impedance setup as running - context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency; + context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency(rqst.points); // Init the sequencer - context.impedance_setup.lock().await.init_multi_frequency_measurement(rqst.dft_number, rqst.number_of_points).await; + let response = match rqst.points { + MeasurementPointSet::Eight => { + const SIZE: usize = 8; + context + .impedance_setup + .lock() + .await + .init_multi_frequency_measurement::(rqst.points) + .await + .map(|periods| MultiImpedanceResult { + points: rqst.points, + periods_per_dft_8: periods, + periods_per_dft_18: Vec::new(), + }) + } + MeasurementPointSet::Eighteen => { + const SIZE: usize = 18; + context + .impedance_setup + .lock() + .await + .init_multi_frequency_measurement::(rqst.points) + .await + .map(|periods| MultiImpedanceResult { + points: rqst.points, + periods_per_dft_8: Vec::new(), + periods_per_dft_18: periods, + }) + } + }; + // Trigger the sequencer context.impedance_setup.lock().await.start_measurement().await; if sender - .reply::(header.seq_no, &()) + .reply::(header.seq_no, &response) .await .is_err() { @@ -255,7 +286,7 @@ pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, } Either::Second(msg) => { if sender - .publish::(seq.into(), &msg) + .publish::(seq.into(), &msg) .await .is_err() { @@ -275,8 +306,8 @@ pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, pub async fn stop_multi_impedance_handler(context: &mut Context, _header: VarHeader, _rqst: ()) -> bool { info!("Stop impedance measurement"); let was_busy = context.impedance_setup.lock().await. running_mode; - if was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency { + if was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { STOP.signal(()); } - was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency + was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) } \ No newline at end of file diff --git a/src/impedance.rs b/src/impedance.rs index 66c2da6..63c1bb4 100644 --- a/src/impedance.rs +++ b/src/impedance.rs @@ -1,5 +1,3 @@ -use bioz_icd_rs::MultiImpedanceOutput28; -use bioz_icd_rs::NumberOfPoints; use defmt::{info, error}; use embassy_stm32::spi::Error; @@ -12,11 +10,11 @@ use static_cell::StaticCell; use crate::ad5940::*; use crate::ad5940_registers::*; -use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, InitImpedanceError}; +use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, ImpedanceInitError, MeasurementPointSet, MultiImpedanceOutput}; use crate::icd_mapping::IntoDftnum; pub static IMPEDANCE_CHANNEL_SINGLE: Channel = Channel::new(); -pub static IMPEDANCE_CHANNEL_MULTI: Channel = Channel::new(); +pub static IMPEDANCE_CHANNEL_MULTI: Channel = Channel::new(); pub type ImpedanceSetupType = Mutex; pub static IMPEDANCE_SETUP: StaticCell = StaticCell::new(); @@ -25,7 +23,7 @@ pub static IMPEDANCE_SETUP: StaticCell = StaticCell::new(); pub enum RunningMode { None, SingleFrequency, - MultiFrequency, + MultiFrequency(MeasurementPointSet), } pub struct ImpedanceSetup { @@ -96,7 +94,7 @@ impl ImpedanceSetup { Ok(()) } - pub async fn init_single_frequency_measurement(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result { + pub async fn init_single_frequency_measurement(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result { // Reset FIFO self.ad5940.clear_and_enable_fifo().await.unwrap(); @@ -117,7 +115,7 @@ impl ImpedanceSetup { info!("Sinus periods per DFT: {}", sinus_periods_per_dft); } else { error!("DSP configuration not set, cannot calculate wait time"); - return Err(InitImpedanceError::DSPNotSet); + return Err(ImpedanceInitError::DSPNotSet); } // Configure sequencer @@ -175,31 +173,58 @@ impl ImpedanceSetup { Ok(sinus_periods_per_dft) } - pub async fn init_multi_frequency_measurement(&mut self, dft_number: IcdDftNum, number_of_points: NumberOfPoints) { + pub async fn init_multi_frequency_measurement(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { + // Create vector to store the periods per DFT for each frequency + let mut periods_per_dft_vec = heapless::Vec::::new(); + // Reset FIFO self.ad5940.clear_and_enable_fifo().await.unwrap(); - // Set DFT number - self.dsp_config.as_mut().unwrap().dftnum(dft_number.into_dftnum()); - self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap(); - // Configure GPIOs self.ad5940.write_reg(Register::GP0CON, 0b10 << 4 | 0b10 << 2 | 0b10).await.unwrap(); self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap(); - // Calculate wait time between measurement start and stop - let mut wait_time = 0; - if let Some(dsp_config) = &self.dsp_config { - wait_time = self.ad5940.sequencer_calculate_wait_time(dsp_config).await.unwrap(); - // info!("Sinus periods per DFT: {}", wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32); - } else { - error!("DSP configuration not set, cannot calculate wait time"); - } - // Configure sequencer self.ad5940.sequencer_enable(true).await; - for &frequency in number_of_points.values() { + // Set DFT number based on the frequency + for &frequency in number_of_points.values() { + // Determine wait time + let selected_dft_num = match frequency { + f if f < 10.0 => DFTNUM::Num16384, + f if f < 100.0 => DFTNUM::Num16384, + f if f < 250.0 => DFTNUM::Num8192, + f if f < 1_000.0 => DFTNUM::Num4096, + f if f < 2_500.0 => DFTNUM::Num2048, + f if f < 10_000.0 => DFTNUM::Num1024, + f if f < 25_000.0 => DFTNUM::Num512, + f if f < 100_000.0 => DFTNUM::Num256, + _ => DFTNUM::Num128, + }; + + let wait_time: u32; + + if let Some(dsp_config) = &mut self.dsp_config { + dsp_config + .dftnum(selected_dft_num); + + // Update DFTNUM + let mut current = self.ad5940.read_reg(Register::DFTCON).await.unwrap(); + current = DFTNUM::apply(current, selected_dft_num as u32); + self.ad5940.write_reg(Register::DFTCON, current).await.unwrap(); + + wait_time = self.ad5940.sequencer_calculate_wait_time(&dsp_config).await.unwrap(); + let periods_per_dft = wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32; + + periods_per_dft_vec.push(periods_per_dft).unwrap(); + info!("{}Hz: Sinus periods per DFT: {}", frequency, periods_per_dft); + + } else { + error!("DSP configuration not set, cannot calculate wait time"); + return Err(ImpedanceInitError::DSPNotSet); + } + + // Set frequency self.ad5940.wgfcw(frequency as u32).await; let wg_amplitude = 2047; // 2047 is the maximum amplitude for a 12-bit DAC --> 1.62V peak-to-peak self.ad5940.write_reg(Register::WGAMPLITUDE, wg_amplitude).await.unwrap(); @@ -249,6 +274,8 @@ impl ImpedanceSetup { self.ad5940.sequencer_info_configure(0, self.ad5940.seq_len, start_address).await; self.start_measurement().await; + + Ok(periods_per_dft_vec) } pub async fn start_measurement(&mut self) { diff --git a/src/main.rs b/src/main.rs index 558168c..2bb168f 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,14 +1,14 @@ #![no_std] #![no_main] -use defmt::{info, warn}; +use defmt::info; use embassy_executor::Spawner; use embassy_stm32::exti::ExtiInput; use embassy_sync::mutex::Mutex; use embassy_time::{Timer, Duration}; use embassy_futures::{select::select, select::Either}; use embassy_stm32::gpio::{Level, Output, Speed}; -use embassy_stm32::{i2c, spi, Config}; +use embassy_stm32::{spi, Config}; use embassy_stm32::time::Hertz; use heapless::Vec; @@ -18,7 +18,7 @@ use {defmt_rtt as _, panic_probe as _}; // use crate::ad5940::*; // use crate::ad5940_registers::*; -use bioz_icd_rs::{MultiImpedanceOutput28, SingleImpedanceOutput}; +use bioz_icd_rs::{MeasurementPointSet, MultiImpedanceOutput, SingleImpedanceOutput}; mod ad5940; use ad5940::AD5940; @@ -185,12 +185,12 @@ async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance_set let mut impedance_setup = impedance_setup.lock().await; // Trigger the sequencer again - if impedance_setup.running_mode == impedance::RunningMode::SingleFrequency || impedance_setup.running_mode == impedance::RunningMode::MultiFrequency { + if impedance_setup.running_mode == impedance::RunningMode::SingleFrequency || impedance_setup.running_mode == impedance::RunningMode::MultiFrequency(MeasurementPointSet::Eight) || impedance_setup.running_mode == impedance::RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { impedance_setup.start_measurement().await; } // Read the FIFO count - let count = impedance_setup.get_fifo_count().await.unwrap(); + let count = impedance_setup.get_fifo_count().await.unwrap() as usize; // info!("FIFOCNTSTA: {}", count); match impedance_setup.running_mode { @@ -216,34 +216,42 @@ async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance_set IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok(); } } - - impedance::RunningMode::MultiFrequency => { - if count >= 112 { - let mut data = [0u32; 112]; - impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap(); - let mut impedance_output = MultiImpedanceOutput28 { - magnitudes: Vec::new(), - phases: Vec::new(), + impedance::RunningMode::MultiFrequency(points) => { + // Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz) + let required_count = points.len() * 4; + + if count >= required_count { + // Use stack-allocated array + let mut data: [u32; 72] = [0; 72]; // max needed size (18*4=72) + let data_slice = &mut data[..required_count]; + impedance_setup.read_fifo(data_slice).await.unwrap(); + + // Output structure + let mut impedance_output = MultiImpedanceOutput { + points, + magnitudes_8: Vec::new(), + phases_8: Vec::new(), + magnitudes_18: Vec::new(), + phases_18: Vec::new(), }; - // Take 4 samples for each frequency point - for chunk in data.chunks(4) { + // Take 4 samples per frequency point + for chunk in data_slice.chunks(4) { let result = calculate_impedance(chunk.try_into().unwrap()); - // Store the results - impedance_output.magnitudes.push(result.magnitude).ok(); - impedance_output.phases.push(result.phase).ok(); + + match points { + MeasurementPointSet::Eight => { + impedance_output.magnitudes_8.push(result.magnitude).ok(); + impedance_output.phases_8.push(result.phase).ok(); + } + MeasurementPointSet::Eighteen => { + impedance_output.magnitudes_18.push(result.magnitude).ok(); + impedance_output.phases_18.push(result.phase).ok(); + } + } } - // Log - // for mags in impedance_output.magnitudes.iter() { - // info!("Mag: {}", mags); - // } - - // for phases in impedance_output.phases.iter() { - // info!("Phase: {}", phases); - // } - IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok(); } continue;