From 4e2f389d214d28766d1b681f4d40ef683d09e6a2 Mon Sep 17 00:00:00 2001 From: Hubald Verzijl Date: Wed, 15 Oct 2025 18:16:26 +0200 Subject: [PATCH 01/11] Moved readout to impedance.rs, changed frequency type to u32. --- Cargo.lock | 49 +++++++------- Cargo.toml | 4 +- src/impedance.rs | 165 ++++++++++++++++++++++++++++++++++++++++++++--- src/main.rs | 148 +----------------------------------------- 4 files changed, 183 insertions(+), 183 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 34f7f66..ab27109 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -61,7 +61,7 @@ name = "bioz-firmware-rs" version = "0.1.0" dependencies = [ "bioz-icd-rs", - "bitflags 2.9.1", + "bitflags 2.9.4", "cortex-m", "cortex-m-rt", "defmt 1.0.1", @@ -117,9 +117,9 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] name = "bitflags" -version = "2.9.1" +version = "2.9.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1b8e56985ec62d17e9c1001dc89c88ecd7dc08e47eba5ec7c29c7b5eeecde967" +checksum = "2261d10cca569e4643e526d8dc2e62e433cc8aba21ab764233731f8d369bf394" [[package]] name = "block-device-driver" @@ -287,14 +287,14 @@ dependencies = [ [[package]] name = "embassy-embedded-hal" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8578db196d74db92efdd5ebc546736dac1685499ee245b22eff92fa5e4b57945" +checksum = "8c62a3bf127e03832fb97d8b01a058775e617653bc89e2a12c256485a7fb54c1" dependencies = [ - "defmt 1.0.1", + "defmt 0.3.100", + "embassy-embedded-hal 0.4.0", "embassy-futures", - "embassy-hal-internal 0.3.0", - "embassy-sync 0.7.0", + "embassy-sync 0.6.2", "embassy-time", "embedded-hal 0.2.7", "embedded-hal 1.0.0", @@ -313,7 +313,7 @@ dependencies = [ "defmt 1.0.1", "embassy-futures", "embassy-hal-internal 0.3.0", - "embassy-sync 0.7.0", + "embassy-sync 0.7.2", "embedded-hal 0.2.7", "embedded-hal 1.0.0", "embedded-hal-async", @@ -349,9 +349,9 @@ dependencies = [ [[package]] name = "embassy-futures" -version = "0.1.1" +version = "0.1.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "1f878075b9794c1e4ac788c95b728f26aa6366d32eeb10c7051389f898f7d067" +checksum = "dc2d050bdc5c21e0862a89256ed8029ae6c290a93aecefc73084b3002cdebb01" [[package]] name = "embassy-hal-internal" @@ -385,13 +385,13 @@ dependencies = [ [[package]] name = "embassy-net-driver-channel" -version = "0.3.1" +version = "0.3.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "25a567ab50319d866ad5e6c583ed665ba9b07865389644d3d82e45bf1497c934" +checksum = "b7b2739fbcf6cd206ae08779c7d709087b16577d255f2ea4a45bc4bbbf305b3f" dependencies = [ "embassy-futures", "embassy-net-driver", - "embassy-sync 0.7.0", + "embassy-sync 0.7.2", ] [[package]] @@ -402,7 +402,7 @@ checksum = "e1e0bb733acdddbc7097765a47ce80bde2385647cf1d8427331931e06cff9a87" dependencies = [ "aligned", "bit_field", - "bitflags 2.9.1", + "bitflags 2.9.4", "block-device-driver", "cfg-if", "cortex-m", @@ -410,7 +410,7 @@ dependencies = [ "critical-section", "defmt 0.3.100", "document-features", - "embassy-embedded-hal 0.3.1", + "embassy-embedded-hal 0.3.2", "embassy-futures", "embassy-hal-internal 0.2.0", "embassy-net-driver", @@ -459,15 +459,15 @@ dependencies = [ [[package]] name = "embassy-sync" -version = "0.7.0" +version = "0.7.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cef1a8a1ea892f9b656de0295532ac5d8067e9830d49ec75076291fd6066b136" +checksum = "73974a3edbd0bd286759b3d483540f0ebef705919a5f56f4fc7709066f71689b" dependencies = [ "cfg-if", "critical-section", "embedded-io-async", + "futures-core", "futures-sink", - "futures-util", "heapless 0.8.0", ] @@ -490,9 +490,9 @@ dependencies = [ [[package]] name = "embassy-time-driver" -version = "0.2.0" +version = "0.2.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8d45f5d833b6d98bd2aab0c2de70b18bfaa10faf661a1578fd8e5dfb15eb7eba" +checksum = "a0a244c7dc22c8d0289379c8d8830cae06bb93d8f990194d0de5efb3b5ae7ba6" dependencies = [ "document-features", ] @@ -525,11 +525,12 @@ dependencies = [ [[package]] name = "embassy-usb-driver" -version = "0.1.0" +version = "0.1.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4fc247028eae04174b6635104a35b1ed336aabef4654f5e87a8f32327d231970" +checksum = "340c5ce591ef58c6449e43f51d2c53efe1bf0bb6a40cbf80afa0d259c7d52c76" dependencies = [ - "defmt 0.3.100", + "defmt 1.0.1", + "embedded-io-async", ] [[package]] diff --git a/Cargo.toml b/Cargo.toml index 8a1d981..65c4e28 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -17,9 +17,9 @@ embassy-futures = { version = "0.1.0"} defmt = "1.0.1" defmt-rtt = "1.0.0" -bitflags = "2.9.1" +bitflags = "2.9.4" -postcard-rpc = {version = "0.11.13", features = ["embassy-usb-0_4-server", "defmt"]} +postcard-rpc = {version = "0.11.15", features = ["embassy-usb-0_4-server", "defmt"]} bioz-icd-rs = {path = "../bioz-icd-rs"} libm = { version = "0.2.15" } diff --git a/src/impedance.rs b/src/impedance.rs index 63c1bb4..1ae8835 100644 --- a/src/impedance.rs +++ b/src/impedance.rs @@ -1,20 +1,23 @@ use defmt::{info, error}; use embassy_stm32::spi::Error; +use embassy_stm32::exti::ExtiInput; use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; use embassy_sync::channel::Channel; use embassy_sync::mutex::Mutex; use static_cell::StaticCell; +use heapless::Vec; + use crate::ad5940::*; use crate::ad5940_registers::*; 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_SINGLE: Channel = Channel::new(); +pub static IMPEDANCE_CHANNEL_MULTI: Channel = Channel::new(); pub type ImpedanceSetupType = Mutex; pub static IMPEDANCE_SETUP: StaticCell = StaticCell::new(); @@ -191,14 +194,14 @@ impl ImpedanceSetup { 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, + f if f < 10 => DFTNUM::Num16384, + f if f < 100 => DFTNUM::Num16384, + f if f < 250 => DFTNUM::Num8192, + f if f < 1000 => DFTNUM::Num4096, + f if f < 2500 => DFTNUM::Num2048, + f if f < 10000 => DFTNUM::Num1024, + f if f < 25000 => DFTNUM::Num512, + f if f < 100000 => DFTNUM::Num256, _ => DFTNUM::Num128, }; @@ -290,3 +293,145 @@ impl ImpedanceSetup { self.ad5940.read_fifo(data).await } } + +// Task to wait for the interrupt from the AD5940 and read the FIFO +// Depending on the running mode, it will read either a single frequency or multiple frequencies +// Result is sent via channel to the communication task which then sends it via USB to the host +#[embassy_executor::task] +pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance_setup: &'static ImpedanceSetupType) { + loop { + // Wait untill sequence is done + pin.wait_for_rising_edge().await; + + // Lock the impedance setup + let mut impedance_setup = impedance_setup.lock().await; + + // Trigger the sequencer again + if impedance_setup.running_mode == RunningMode::SingleFrequency || impedance_setup.running_mode == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { + impedance_setup.start_measurement().await; + } + + // Read the FIFO count + let count = impedance_setup.get_fifo_count().await.unwrap() as usize; + // info!("FIFOCNTSTA: {}", count); + + match impedance_setup.running_mode { + RunningMode::None => { + continue; // Skip processing if not running + } + RunningMode::SingleFrequency => { + if count >= 4 { + let mut data: [u32; 4] = [0; 4]; + + impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap(); + + let result = calculate_impedance(data); + + // Log + // info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase); + + let data = SingleImpedanceOutput { + magnitude: result.magnitude, + phase: result.phase, + }; + + IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok(); + } + } + + 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 per frequency point + for chunk in data_slice.chunks(4) { + let result = calculate_impedance(chunk.try_into().unwrap()); + + 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(); + } + } + } + + IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok(); + } + continue; + } + } + } +} + +extern crate libm; + +#[derive(Debug, Clone, Copy)] +pub struct Complex { + real: i32, + imag: i32, +} + +#[derive(Debug)] +pub struct ImpedanceResult { + pub magnitude: f32, + pub phase: f32, +} + +// Example Rcal value (Ohms) +const RCAL_VAL: f32 = 1000.0; + +/// Convert raw 18-bit 2's complement value to signed i32 +fn sign_extend_18bit(val: u32) -> i32 { + ((val << 14) as i32) >> 14 +} + +/// Calculate magnitude and phase of Rz using Rcal reference +pub fn calculate_impedance(data: [u32; 4]) -> ImpedanceResult { + let mut signed_data = [0i32; 4]; + for (i, &val) in data.iter().enumerate() { + signed_data[i] = sign_extend_18bit(val); + } + + let dft_rcal = Complex { + real: signed_data[0], + imag: signed_data[1], + }; + + let dft_rz = Complex { + real: signed_data[2], + imag: signed_data[3], + }; + + let rcal_mag = libm::sqrtf((dft_rcal.real as f32) * (dft_rcal.real as f32) + + (dft_rcal.imag as f32) * (dft_rcal.imag as f32)); + + let rz_mag = libm::sqrtf((dft_rz.real as f32) * (dft_rz.real as f32) + + (dft_rz.imag as f32) * (dft_rz.imag as f32)); + + let rcal_phase = libm::atan2f(-(dft_rcal.imag as f32), dft_rcal.real as f32); + let rz_phase = libm::atan2f(-(dft_rz.imag as f32), dft_rz.real as f32); + + let magnitude = (rcal_mag / rz_mag) * RCAL_VAL; + let phase = rcal_phase - rz_phase; + + ImpedanceResult { magnitude, phase } +} diff --git a/src/main.rs b/src/main.rs index 2bb168f..a1303bf 100644 --- a/src/main.rs +++ b/src/main.rs @@ -11,15 +11,11 @@ use embassy_stm32::gpio::{Level, Output, Speed}; use embassy_stm32::{spi, Config}; use embassy_stm32::time::Hertz; -use heapless::Vec; - use {defmt_rtt as _, panic_probe as _}; // use crate::ad5940::*; // use crate::ad5940_registers::*; -use bioz_icd_rs::{MeasurementPointSet, MultiImpedanceOutput, SingleImpedanceOutput}; - mod ad5940; use ad5940::AD5940; @@ -37,10 +33,8 @@ use embassy_stm32::{bind_interrupts, peripherals, usb}; mod communication; use communication::{init_communication, LED_FREQUENCY_SIGNAL}; -use impedance::{IMPEDANCE_CHANNEL_SINGLE, IMPEDANCE_CHANNEL_MULTI}; - mod impedance; -use impedance::{ImpedanceSetup, ImpedanceSetupType, IMPEDANCE_SETUP}; +use impedance::{ImpedanceSetup, IMPEDANCE_SETUP, impedance_setup_readout_task}; mod icd_mapping; @@ -174,143 +168,3 @@ async fn green_led(mut led: Output<'static>) { led.toggle(); } } - -#[embassy_executor::task] -async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance_setup: &'static ImpedanceSetupType) { - loop { - // Wait untill sequence is done - pin.wait_for_rising_edge().await; - - // Lock the impedance setup - 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(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() as usize; - // info!("FIFOCNTSTA: {}", count); - - match impedance_setup.running_mode { - impedance::RunningMode::None => { - continue; // Skip processing if not running - } - impedance::RunningMode::SingleFrequency => { - if count >= 4 { - let mut data: [u32; 4] = [0; 4]; - - impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap(); - - let result = calculate_impedance(data); - - // Log - // info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase); - - let data = SingleImpedanceOutput { - magnitude: result.magnitude, - phase: result.phase, - }; - - IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok(); - } - } - - 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 per frequency point - for chunk in data_slice.chunks(4) { - let result = calculate_impedance(chunk.try_into().unwrap()); - - 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(); - } - } - } - - IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok(); - } - continue; - } - } - } -} - - -extern crate libm; - -#[derive(Debug, Clone, Copy)] -pub struct Complex { - real: i32, - imag: i32, -} - -#[derive(Debug)] -pub struct ImpedanceResult { - pub magnitude: f32, - pub phase: f32, -} - -// Example Rcal value (Ohms) -const RCAL_VAL: f32 = 1000.0; - -/// Convert raw 18-bit 2's complement value to signed i32 -fn sign_extend_18bit(val: u32) -> i32 { - ((val << 14) as i32) >> 14 -} - -/// Calculate magnitude and phase of Rz using Rcal reference -pub fn calculate_impedance(data: [u32; 4]) -> ImpedanceResult { - let mut signed_data = [0i32; 4]; - for (i, &val) in data.iter().enumerate() { - signed_data[i] = sign_extend_18bit(val); - } - - let dft_rcal = Complex { - real: signed_data[0], - imag: signed_data[1], - }; - - let dft_rz = Complex { - real: signed_data[2], - imag: signed_data[3], - }; - - let rcal_mag = libm::sqrtf((dft_rcal.real as f32) * (dft_rcal.real as f32) - + (dft_rcal.imag as f32) * (dft_rcal.imag as f32)); - - let rz_mag = libm::sqrtf((dft_rz.real as f32) * (dft_rz.real as f32) - + (dft_rz.imag as f32) * (dft_rz.imag as f32)); - - let rcal_phase = libm::atan2f(-(dft_rcal.imag as f32), dft_rcal.real as f32); - let rz_phase = libm::atan2f(-(dft_rz.imag as f32), dft_rz.real as f32); - - let magnitude = (rcal_mag / rz_mag) * RCAL_VAL; - let phase = rcal_phase - rz_phase; - - ImpedanceResult { magnitude, phase } -} From a17b25dba01248f2e9783ea07f204663a41a348b Mon Sep 17 00:00:00 2001 From: Hubald Verzijl Date: Fri, 17 Oct 2025 12:12:46 +0200 Subject: [PATCH 02/11] Add small delay to stabilize power. --- Cargo.lock | 4 ++-- Cargo.toml | 2 +- src/main.rs | 4 ++++ 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index ab27109..cccde7e 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -268,9 +268,9 @@ dependencies = [ [[package]] name = "defmt-rtt" -version = "1.0.0" +version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b2cac3b8a5644a9e02b75085ebad3b6deafdbdbdec04bb25086523828aa4dfd1" +checksum = "93d5a25c99d89c40f5676bec8cefe0614f17f0f40e916f98e345dae941807f9e" dependencies = [ "critical-section", "defmt 1.0.1", diff --git a/Cargo.toml b/Cargo.toml index 65c4e28..a40927a 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -15,7 +15,7 @@ embassy-usb = { version = "0.4.0", features = ["defmt"] } embassy-futures = { version = "0.1.0"} defmt = "1.0.1" -defmt-rtt = "1.0.0" +defmt-rtt = "1.1.0" bitflags = "2.9.4" diff --git a/src/main.rs b/src/main.rs index a1303bf..f3f88cd 100644 --- a/src/main.rs +++ b/src/main.rs @@ -67,6 +67,10 @@ async fn main(spawner: Spawner) { } let p = embassy_stm32::init(config); + + // Small delay to allow power to stabilize + Timer::after_millis(100).await; + info!("Hello World!"); // let mut led = Output::new(p.PA5, Level::High, Speed::Low); From 80957ce316d0c877a76232ffa912efc2b5460489 Mon Sep 17 00:00:00 2001 From: Hubald Verzijl Date: Sat, 18 Oct 2025 17:52:52 +0200 Subject: [PATCH 03/11] Add 2/4-lead option to GUI. --- src/communication.rs | 15 +++++++++------ src/impedance.rs | 12 ++++++++---- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/src/communication.rs b/src/communication.rs index 01c171b..c42b473 100644 --- a/src/communication.rs +++ b/src/communication.rs @@ -169,7 +169,10 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader info!("Start impedance measurement at {:?} Hz.", rqst.sinus_frequency); // Mark the impedance setup as running - context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency; + match rqst.lead_mode { + bioz_icd_rs::BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency2Lead, + bioz_icd_rs::BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency4Lead, + } // Init the sequencer let init_impedance_result = context.impedance_setup.lock().await.init_single_frequency_measurement(rqst.sinus_frequency, rqst.dft_number).await; @@ -217,10 +220,10 @@ 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(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { + if was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { STOP.signal(()); } - was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) + was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) } #[embassy_executor::task] @@ -305,9 +308,9 @@ 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(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { + let was_busy = context.impedance_setup.lock().await.running_mode; + if was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { STOP.signal(()); } - was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) + was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || 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 1ae8835..a5b9834 100644 --- a/src/impedance.rs +++ b/src/impedance.rs @@ -25,7 +25,8 @@ pub static IMPEDANCE_SETUP: StaticCell = StaticCell::new(); #[derive(PartialEq, Copy, Clone)] pub enum RunningMode { None, - SingleFrequency, + SingleFrequency2Lead, + SingleFrequency4Lead, MultiFrequency(MeasurementPointSet), } @@ -307,7 +308,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance let mut impedance_setup = impedance_setup.lock().await; // Trigger the sequencer again - if impedance_setup.running_mode == RunningMode::SingleFrequency || impedance_setup.running_mode == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { + if impedance_setup.running_mode == RunningMode::SingleFrequency2Lead || impedance_setup.running_mode == RunningMode::SingleFrequency4Lead || impedance_setup.running_mode == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { impedance_setup.start_measurement().await; } @@ -319,7 +320,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance RunningMode::None => { continue; // Skip processing if not running } - RunningMode::SingleFrequency => { + RunningMode::SingleFrequency2Lead => { if count >= 4 { let mut data: [u32; 4] = [0; 4]; @@ -338,7 +339,10 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok(); } } - + RunningMode::SingleFrequency4Lead => { + info!("TODO: SingleFrequency4Lead not implemented yet"); + continue; + } RunningMode::MultiFrequency(points) => { // Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz) let required_count = points.len() * 4; From 0859de24fb77dd474b0f375170f675635998ad23 Mon Sep 17 00:00:00 2001 From: Hubald Verzijl Date: Fri, 31 Oct 2025 10:23:25 +0100 Subject: [PATCH 04/11] Added num_complex instead of custom complex struct. --- Cargo.lock | 55 +++++++++++++++++++++++++++++++++++++++++++++++- Cargo.toml | 4 +++- src/impedance.rs | 43 ++++++++++++------------------------- 3 files changed, 71 insertions(+), 31 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index cccde7e..bb1b67c 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -75,7 +75,7 @@ dependencies = [ "embassy-usb", "embedded-hal 1.0.0", "heapless 0.9.1", - "libm", + "num", "panic-probe", "postcard-rpc", "static_cell", @@ -776,6 +776,58 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "8d5439c4ad607c3c23abf66de8c8bf57ba8adcd1f129e699851a6e43935d339d" +[[package]] +name = "num" +version = "0.4.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "35bd024e8b2ff75562e5f34e7f4905839deb4b22955ef5e73d2fea1b9813cb23" +dependencies = [ + "num-complex", + "num-integer", + "num-iter", + "num-rational", + "num-traits", +] + +[[package]] +name = "num-complex" +version = "0.4.6" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-integer" +version = "0.1.46" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f" +dependencies = [ + "num-traits", +] + +[[package]] +name = "num-iter" +version = "0.1.45" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "1429034a0490724d0075ebb2bc9e875d6503c3cf69e235a8941aa757d83ef5bf" +dependencies = [ + "autocfg", + "num-integer", + "num-traits", +] + +[[package]] +name = "num-rational" +version = "0.4.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" +dependencies = [ + "num-integer", + "num-traits", +] + [[package]] name = "num-traits" version = "0.2.19" @@ -783,6 +835,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "071dfc062690e90b734c0b2273ce72ad0ffa95f0c74596bc250dcfd960262841" dependencies = [ "autocfg", + "libm", ] [[package]] diff --git a/Cargo.toml b/Cargo.toml index a40927a..aa1d418 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -22,7 +22,9 @@ bitflags = "2.9.4" postcard-rpc = {version = "0.11.15", features = ["embassy-usb-0_4-server", "defmt"]} bioz-icd-rs = {path = "../bioz-icd-rs"} -libm = { version = "0.2.15" } +# libm = { version = "0.2.15" } + +num = {version = "0.4.3", default-features = false, features = ["libm"]} heapless = { version = "0.9.1" } diff --git a/src/impedance.rs b/src/impedance.rs index a5b9834..8e2c51d 100644 --- a/src/impedance.rs +++ b/src/impedance.rs @@ -10,6 +10,8 @@ use static_cell::StaticCell; use heapless::Vec; +use num::complex::Complex; + use crate::ad5940::*; use crate::ad5940_registers::*; @@ -386,14 +388,6 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance } } -extern crate libm; - -#[derive(Debug, Clone, Copy)] -pub struct Complex { - real: i32, - imag: i32, -} - #[derive(Debug)] pub struct ImpedanceResult { pub magnitude: f32, @@ -410,29 +404,20 @@ fn sign_extend_18bit(val: u32) -> i32 { /// Calculate magnitude and phase of Rz using Rcal reference pub fn calculate_impedance(data: [u32; 4]) -> ImpedanceResult { - let mut signed_data = [0i32; 4]; - for (i, &val) in data.iter().enumerate() { - signed_data[i] = sign_extend_18bit(val); - } + let dft_rcal = Complex::new( + sign_extend_18bit(data[0]) as f32, + sign_extend_18bit(data[1]) as f32 + ); + let dft_rz = Complex::new( + sign_extend_18bit(data[2]) as f32, + sign_extend_18bit(data[3]) as f32 + ); - let dft_rcal = Complex { - real: signed_data[0], - imag: signed_data[1], - }; + let rcal_mag = dft_rcal.norm(); + let rz_mag = dft_rz.norm(); - let dft_rz = Complex { - real: signed_data[2], - imag: signed_data[3], - }; - - let rcal_mag = libm::sqrtf((dft_rcal.real as f32) * (dft_rcal.real as f32) - + (dft_rcal.imag as f32) * (dft_rcal.imag as f32)); - - let rz_mag = libm::sqrtf((dft_rz.real as f32) * (dft_rz.real as f32) - + (dft_rz.imag as f32) * (dft_rz.imag as f32)); - - let rcal_phase = libm::atan2f(-(dft_rcal.imag as f32), dft_rcal.real as f32); - let rz_phase = libm::atan2f(-(dft_rz.imag as f32), dft_rz.real as f32); + let rcal_phase = dft_rcal.arg(); + let rz_phase = dft_rz.arg(); let magnitude = (rcal_mag / rz_mag) * RCAL_VAL; let phase = rcal_phase - rz_phase; From 6358882a4c8d6af605355b7361ecfe305b17de00 Mon Sep 17 00:00:00 2001 From: Hubald Verzijl Date: Mon, 3 Nov 2025 17:19:10 +0100 Subject: [PATCH 05/11] Added 4 lead-option for single frequency measurement. --- src/ad5940.rs | 11 ++ src/ad5940_registers.rs | 31 ++++- src/communication.rs | 5 +- src/impedance.rs | 277 ++++++++++++++++++++++++++++++++++++++-- 4 files changed, 307 insertions(+), 17 deletions(-) diff --git a/src/ad5940.rs b/src/ad5940.rs index 78a746d..b3fc588 100644 --- a/src/ad5940.rs +++ b/src/ad5940.rs @@ -71,6 +71,7 @@ impl SwitchConfig { #[allow(dead_code)] #[derive(Default)] pub struct DspConfig { + pub gnpgain: Option, muxseln: Option, muxselp: Option, ctiacon: Option, @@ -87,6 +88,11 @@ pub struct DspConfig { } impl DspConfig { + pub fn gnpgain(&mut self, gnpgain: GNPGAIN) -> &mut Self { + self.gnpgain = Some(gnpgain); + self + } + pub fn adc_mux_n(&mut self, muxseln: MUXSELN) -> &mut Self { self.muxseln = Some(muxseln); self @@ -592,6 +598,9 @@ impl AD5940 { // ADCCON let mut current = self.read_reg(Register::ADCCON).await?; + if let Some(gnpgain) = config.gnpgain { + current = GNPGAIN::apply(current, gnpgain as u32); + } if let Some(muxseln) = config.muxseln { current = MUXSELN::apply(current, muxseln as u32); } @@ -855,6 +864,8 @@ pub enum Register { OSCCON = 0x0000_0A10, // Oscillator Control Register SEQTIMEOUT = 0x0000_2068, // Sequencer Timeout Counter Register SEQCRC = 0x0000_2060, // Sequencer CRC Value Register + DFTREAL = 0x0000_2078, // DFT Result, Real Device Register + DFTIMAG = 0x0000_207C, // DFT Result, Imaginary Device Register DATAFIFOTHRES = 0x0000_21E0, // Data FIFO Threshold Register SYNCEXTDEVICE = 0x0000_2054, // Sync External Device Register GP0CON = 0x0000_0000, // GPIO Port 0 Configuration Register diff --git a/src/ad5940_registers.rs b/src/ad5940_registers.rs index 49ce3c2..103f1af 100644 --- a/src/ad5940_registers.rs +++ b/src/ad5940_registers.rs @@ -130,6 +130,25 @@ impl RegisterField for DMUXCON { const MASK: u32 = 0b1111; } +#[allow(dead_code)] +#[repr(u32)] +#[derive(Copy, Clone)] +pub enum GNPGAIN { + Gain1 = 0b0, + Gain1_5 = 0b1, + Gain2 = 0b10, + Gain4 = 0b11, + Gain9 = 0b100, +} + +impl RegisterField for GNPGAIN { + fn reset() -> Self { + GNPGAIN::Gain1 + } + const BIT_OFFSET: u32 = 16; + const MASK: u32 = 0b111; +} + #[allow(dead_code)] #[repr(u32)] #[derive(Copy, Clone)] @@ -138,7 +157,9 @@ pub enum MUXSELN Floating = 0b00000, HsTiaNeg = 0b00001, LpTiaNeg = 0b00010, - AIN1 = 0b00101, + AIN1 = 0b00101, + AIN3 = 0b00111, + ExciNNode= 0b10100, } impl RegisterField for MUXSELN { @@ -155,7 +176,11 @@ impl RegisterField for MUXSELN { pub enum MUXSELP { Floating = 0b00000, HsTiaPos = 0b00001, - AIN1 = 0b00101, + AIN1 = 0b00101, + AIN2 = 0b00110, + AIN3 = 0b00111, + CE0 = 0b011001, + ExciPNode= 0b100100, } impl RegisterField for MUXSELP { @@ -163,7 +188,7 @@ impl RegisterField for MUXSELP { MUXSELP::Floating } const BIT_OFFSET: u32 = 0; - const MASK: u32 = 0b11111; + const MASK: u32 = 0b111111; } #[allow(dead_code)] diff --git a/src/communication.rs b/src/communication.rs index c42b473..5f43b19 100644 --- a/src/communication.rs +++ b/src/communication.rs @@ -175,7 +175,10 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader } // Init the sequencer - let init_impedance_result = context.impedance_setup.lock().await.init_single_frequency_measurement(rqst.sinus_frequency, rqst.dft_number).await; + let init_impedance_result = match rqst.lead_mode { + bioz_icd_rs::BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.init_single_frequency_measurement(rqst.sinus_frequency, rqst.dft_number).await, + bioz_icd_rs::BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.init_single_frequency_measurement_4_lead(rqst.sinus_frequency, rqst.dft_number).await, + }; // Trigger the sequencer context.impedance_setup.lock().await.start_measurement().await; diff --git a/src/impedance.rs b/src/impedance.rs index 8e2c51d..786d66d 100644 --- a/src/impedance.rs +++ b/src/impedance.rs @@ -5,6 +5,7 @@ use embassy_stm32::exti::ExtiInput; use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex; use embassy_sync::channel::Channel; use embassy_sync::mutex::Mutex; +use embassy_time::Timer; use static_cell::StaticCell; @@ -24,6 +25,8 @@ pub static IMPEDANCE_CHANNEL_MULTI: Channel; pub static IMPEDANCE_SETUP: StaticCell = StaticCell::new(); +pub const RCAL: f32 = 1008.0; // Calibration resistor in Ohm + #[derive(PartialEq, Copy, Clone)] pub enum RunningMode { None, @@ -32,15 +35,21 @@ pub enum RunningMode { MultiFrequency(MeasurementPointSet), } +pub struct RtiaCalibrationResult { + pub magnitude: f32, + pub phase: f32, +} + pub struct ImpedanceSetup { ad5940: AD5940, dsp_config: Option, pub running_mode: RunningMode, + pub rtia_calibrated: Option, } impl ImpedanceSetup { pub fn new(ad5940: AD5940) -> Self { - ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None } + ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None, rtia_calibrated: None } } pub async fn init(&mut self) -> Result<(), Error> { @@ -68,6 +77,7 @@ impl ImpedanceSetup { // Set DSP configuration let mut dsp_config = DspConfig::default(); dsp_config + .gnpgain(GNPGAIN::Gain1) .adc_mux_n(MUXSELN::HsTiaNeg) .adc_mux_p(MUXSELP::HsTiaPos) .ctiacon(CTIACON::C32) @@ -100,6 +110,96 @@ impl ImpedanceSetup { Ok(()) } + pub async fn calibrate_rtia(&mut self, frequency: u32, wait_time: u32) -> Result { + /* CALIBRATION METHOD + 1) Measure the complex voltage V_Rcal across the calibration DUT (Rcal). + 2) Measure the complex voltage V_Rtia across Rtia [HSTIA_P (output) - HSTIA_N]. + 3) Note Rtia carries the same current as Rcal; I_Rtia = I_exc = I_Rcal + 4) Implement the equation: Rtia = V_Rtia / I_Rtia + --> Rtia = (V_Rtia / V_Rcal) * Rcal + */ + + // self.ad5940.hsr_calibrate_rtia(wait_time).await; + self.ad5940.wgfcw(frequency).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(); + + // Switches for Rcal + let switch_config = SwitchConfig::default() + .t9con(T9CON::T9Closed) + .tmuxcon(TMUXCON::TR1Closed) + .nmuxcon(NMUXCON::NR1Closed) + .pmuxcon(PMUXCON::PR0Closed) + .dmuxcon(DMUXCON::DR0Closed); + self.ad5940.apply_switch_config(switch_config).await.unwrap(); + + // Voltage measurement Rcal + self.dsp_config.as_mut().unwrap() + .adc_mux_n(MUXSELN::ExciNNode) + .adc_mux_p(MUXSELP::ExciPNode); + self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap(); + + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await; + // self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz + Timer::after_micros(10).await; + self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await; + // self.ad5940.sequencer_wait(wait_time).await; // Determined above + // self.ad5940.sequencer_wait(16*20).await; // 20 us based on SYSCLK = 16MHz + Timer::after_micros((wait_time/16) as u64).await; + Timer::after_micros(20).await; + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await; + + let real_raw = self.ad5940.read_reg(Register::DFTREAL).await.unwrap(); + let imag_raw = self.ad5940.read_reg(Register::DFTIMAG).await.unwrap(); + let mut rcal = Complex::new( + sign_extend_18bit(real_raw) as f32, + sign_extend_18bit(imag_raw) as f32 + ); + + // Voltage measurement Rtia + self.dsp_config.as_mut().unwrap() + .adc_mux_n(MUXSELN::HsTiaNeg) + .adc_mux_p(MUXSELP::HsTiaPos); + self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap(); + + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await; + // self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz + Timer::after_micros(10).await; + self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await; + // self.ad5940.sequencer_wait(wait_time).await; // Determined above + // self.ad5940.sequencer_wait(16*20).await; // 20 us based on SYSCLK = 16MHz + Timer::after_micros((wait_time/16) as u64).await; + Timer::after_micros(20).await; + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await; + + let real_raw = self.ad5940.read_reg(Register::DFTREAL).await.unwrap(); + let imag_raw = self.ad5940.read_reg(Register::DFTIMAG).await.unwrap(); + let mut rtia = Complex::new( + sign_extend_18bit(real_raw) as f32, + sign_extend_18bit(imag_raw) as f32 + ); + + // Current is measured inverted in rtia + rtia.re *= -1.0; + rtia.im *= -1.0; + + // Impedance imaginary part is measured inverted + rcal.im *= -1.0; + rtia.im *= -1.0; + + // Rtia = (V_Rtia / V_Rcal) * Rcal + let mut temp = rtia/rcal; + temp = temp.scale(RCAL); + + // Calibration result + let calibration_result = RtiaCalibrationResult { + magnitude: temp.norm(), // Magnitude in Ohm + phase: temp.arg(), // Phase in radians + }; + + Ok(calibration_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(); @@ -179,6 +279,103 @@ impl ImpedanceSetup { Ok(sinus_periods_per_dft) } + pub async fn init_single_frequency_measurement_4_lead(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result { + // 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 wait_time; + let sinus_periods_per_dft; + if let Some(dsp_config) = &self.dsp_config { + wait_time = self.ad5940.sequencer_calculate_wait_time(dsp_config).await.unwrap(); + sinus_periods_per_dft = wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32; + info!("Sinus periods per DFT: {}", sinus_periods_per_dft); + } else { + error!("DSP configuration not set, cannot calculate wait time"); + return Err(ImpedanceInitError::DSPNotSet); + } + + // Calibrate Rtia + match self.calibrate_rtia(frequency, wait_time).await { + Ok(calibration_result) => { + self.rtia_calibrated = Some(calibration_result); + }, + Err(e) => { + error!("Rtia calibration failed: {:?}", e); + } + } + + // Configure switches + let switch_config = SwitchConfig::default() + .t9con(T9CON::T9Closed) + .tmuxcon(TMUXCON::T2Closed) + .nmuxcon(NMUXCON::N2Closed) + .pmuxcon(PMUXCON::P11Closed) + .dmuxcon(DMUXCON::D5Closed); + self.ad5940.apply_switch_config(switch_config).await.unwrap(); + + // Configure sequencer + self.ad5940.sequencer_enable(true).await; + + self.ad5940.wgfcw(frequency).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(); + + // Voltage measurement + self.dsp_config.as_mut().unwrap() + .adc_mux_n(MUXSELN::AIN3) + .adc_mux_p(MUXSELP::AIN2); + // self.dsp_config.as_mut().unwrap() + // .adc_mux_n(MUXSELN::AIN1) + // .adc_mux_p(MUXSELP::CE0); + self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap(); + + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await; + self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await; + self.ad5940.sequencer_wait(wait_time).await; // Determined above + self.ad5940.sequencer_wait(16*20).await; // 20 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await; + + // Current measurement + self.dsp_config.as_mut().unwrap() + .adc_mux_n(MUXSELN::HsTiaNeg) + .adc_mux_p(MUXSELP::HsTiaPos); + self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap(); + + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await; + self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await; + self.ad5940.sequencer_wait(wait_time).await; // Determined above + self.ad5940.sequencer_wait(16*20).await; // 20 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await; + + // Toggle leds + self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b010).await.unwrap(); + self.ad5940.sequencer_wait(16 * 1_000).await; // 1ms based on SYSCLK = 16MHz + self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap(); + + self.ad5940.sequencer_enable(false).await; + + // Write sequence to SRAM + let start_address = 0; + self.ad5940.sequencer_cmd_write(start_address).await; + + self.ad5940.sequencer_info_configure(0, self.ad5940.seq_len, start_address).await; + + self.start_measurement().await; + + Ok(sinus_periods_per_dft) + } + 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(); @@ -342,8 +539,56 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance } } RunningMode::SingleFrequency4Lead => { - info!("TODO: SingleFrequency4Lead not implemented yet"); - continue; + if count >= 4 { + let mut data: [u32; 4] = [0; 4]; + + impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap(); + + let mut dft_volt = Complex::new( + sign_extend_18bit(data[0]) as f32, + sign_extend_18bit(data[1]) as f32 + ); + + let mut dft_curr = Complex::new( + sign_extend_18bit(data[2]) as f32, + sign_extend_18bit(data[3]) as f32 + ); + + // Current is measured inverted in rtia + dft_curr.re *= -1.0; + dft_curr.im *= -1.0; + + // Impedance imaginary part is measured inverted + dft_volt.im *= -1.0; + dft_curr.im *= -1.0; + + let (volt_mag, volt_phase) = dft_volt.to_polar(); + let (curr_mag, curr_phase) = dft_curr.to_polar(); + + // info!("Volt Phase = {} V, Volt Phase = {} rad", volt_phase, curr_phase); + + match impedance_setup.rtia_calibrated.as_ref() { + None => { + error!("Rtia not calibrated, cannot compute impedance"); + continue; + }, + Some(cal) => { + let rtia_mag = cal.magnitude; + let rtia_phase = cal.phase; + + // Calculate impedance using calibrated Rtia + let magnitude = volt_mag / curr_mag * rtia_mag; + let phase = volt_phase - curr_phase + rtia_phase; + + let data = SingleImpedanceOutput { + magnitude, + phase + }; + + IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok(); + } + } + } } RunningMode::MultiFrequency(points) => { // Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz) @@ -394,9 +639,6 @@ pub struct ImpedanceResult { pub phase: f32, } -// Example Rcal value (Ohms) -const RCAL_VAL: f32 = 1000.0; - /// Convert raw 18-bit 2's complement value to signed i32 fn sign_extend_18bit(val: u32) -> i32 { ((val << 14) as i32) >> 14 @@ -404,22 +646,31 @@ fn sign_extend_18bit(val: u32) -> i32 { /// Calculate magnitude and phase of Rz using Rcal reference pub fn calculate_impedance(data: [u32; 4]) -> ImpedanceResult { - let dft_rcal = Complex::new( + let mut dft_rcal = Complex::new( sign_extend_18bit(data[0]) as f32, sign_extend_18bit(data[1]) as f32 ); - let dft_rz = Complex::new( + let mut dft_rz = Complex::new( sign_extend_18bit(data[2]) as f32, sign_extend_18bit(data[3]) as f32 ); - let rcal_mag = dft_rcal.norm(); - let rz_mag = dft_rz.norm(); + // Current is measured inverted in rtia + // --> Will cancel out in the impedance calculation + // dft_rcal.re *= -1.0; + // dft_rcal.im *= -1.0; - let rcal_phase = dft_rcal.arg(); - let rz_phase = dft_rz.arg(); + // dft_rz.re *= -1.0; + // dft_rz.im *= -1.0; - let magnitude = (rcal_mag / rz_mag) * RCAL_VAL; + // Impedance imaginary part is measured inverted + dft_rcal.im *= -1.0; + dft_rz.im *= -1.0; + + let (rcal_mag, rcal_phase) = dft_rcal.to_polar(); + let (rz_mag, rz_phase) = dft_rz.to_polar(); + + let magnitude = (rcal_mag / rz_mag) * RCAL; let phase = rcal_phase - rz_phase; ImpedanceResult { magnitude, phase } From 324189ba72276a4aa3b8ab616a5e0f15a423d25e Mon Sep 17 00:00:00 2001 From: Hubald Verzijl Date: Mon, 3 Nov 2025 19:16:42 +0100 Subject: [PATCH 06/11] Added 4-lead multi calibration. --- src/communication.rs | 64 +++++++-- src/impedance.rs | 332 ++++++++++++++++++++++++++++++++++++------- 2 files changed, 332 insertions(+), 64 deletions(-) diff --git a/src/communication.rs b/src/communication.rs index 5f43b19..3df5a95 100644 --- a/src/communication.rs +++ b/src/communication.rs @@ -20,7 +20,7 @@ use postcard_rpc::{ }, }; -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 bioz_icd_rs::{GetUniqueIdEndpoint, MeasurementPointSet, MultiImpedanceOutputTopic, MultiImpedanceResult, MultiImpedanceStartRequest, PingEndpoint, SetGreenLedEndpoint, SingleImpedanceOutputTopic, SingleImpedanceStartRequest, StartMultiImpedanceEndpoint, StartSingleImpedanceEndpoint, StopMultiImpedanceEndpoint, StopSingleImpedanceEndpoint, BioImpedanceLeadMode, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST}; use crate::impedance::{ImpedanceSetupType, RunningMode, IMPEDANCE_CHANNEL_MULTI, IMPEDANCE_CHANNEL_SINGLE}; @@ -170,14 +170,14 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader // Mark the impedance setup as running match rqst.lead_mode { - bioz_icd_rs::BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency2Lead, - bioz_icd_rs::BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency4Lead, + BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency2Lead, + BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency4Lead, } // Init the sequencer let init_impedance_result = match rqst.lead_mode { - bioz_icd_rs::BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.init_single_frequency_measurement(rqst.sinus_frequency, rqst.dft_number).await, - bioz_icd_rs::BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.init_single_frequency_measurement_4_lead(rqst.sinus_frequency, rqst.dft_number).await, + BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.init_single_frequency_measurement_2_lead(rqst.sinus_frequency, rqst.dft_number).await, + BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.init_single_frequency_measurement_4_lead(rqst.sinus_frequency, rqst.dft_number).await, }; // Trigger the sequencer context.impedance_setup.lock().await.start_measurement().await; @@ -223,26 +223,27 @@ 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::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { + if was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) { STOP.signal(()); } - was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) + was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) } #[embassy_executor::task] 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(rqst.points); + // Init the sequencer - let response = match rqst.points { - MeasurementPointSet::Eight => { + let response = match (rqst.lead_mode, rqst.points) { + (BioImpedanceLeadMode::TwoLead, MeasurementPointSet::Eight) => { + context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency2Lead(rqst.points); const SIZE: usize = 8; context .impedance_setup .lock() .await - .init_multi_frequency_measurement::(rqst.points) + .init_multi_frequency_measurement_2_lead::(rqst.points) .await .map(|periods| MultiImpedanceResult { points: rqst.points, @@ -250,13 +251,46 @@ pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, periods_per_dft_18: Vec::new(), }) } - MeasurementPointSet::Eighteen => { + (BioImpedanceLeadMode::TwoLead, MeasurementPointSet::Eighteen) => { + context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency2Lead(rqst.points); const SIZE: usize = 18; context .impedance_setup .lock() .await - .init_multi_frequency_measurement::(rqst.points) + .init_multi_frequency_measurement_2_lead::(rqst.points) + .await + .map(|periods| MultiImpedanceResult { + points: rqst.points, + periods_per_dft_8: Vec::new(), + periods_per_dft_18: periods, + }) + } + (BioImpedanceLeadMode::FourLead, MeasurementPointSet::Eight) => { + info!("Start multi impedance 4 lead 8 points"); + context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency4Lead(rqst.points); + const SIZE: usize = 8; + context + .impedance_setup + .lock() + .await + .init_multi_frequency_measurement_4_lead::(rqst.points) + .await + .map(|periods| MultiImpedanceResult { + points: rqst.points, + periods_per_dft_8: periods, + periods_per_dft_18: Vec::new(), + }) + } + (BioImpedanceLeadMode::FourLead, MeasurementPointSet::Eighteen) => { + info!("Start multi impedance 4 lead 18 points"); + context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency4Lead(rqst.points); + const SIZE: usize = 18; + context + .impedance_setup + .lock() + .await + .init_multi_frequency_measurement_4_lead::(rqst.points) .await .map(|periods| MultiImpedanceResult { points: rqst.points, @@ -312,8 +346,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::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { + if was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) { STOP.signal(()); } - was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) + was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) } \ No newline at end of file diff --git a/src/impedance.rs b/src/impedance.rs index 786d66d..b2000bc 100644 --- a/src/impedance.rs +++ b/src/impedance.rs @@ -32,9 +32,18 @@ pub enum RunningMode { None, SingleFrequency2Lead, SingleFrequency4Lead, - MultiFrequency(MeasurementPointSet), + MultiFrequency2Lead(MeasurementPointSet), + MultiFrequency4Lead(MeasurementPointSet), } +pub enum RtiaCalibrated { + None, + Single(RtiaCalibrationResult), + Vec8(Vec), + Vec18(Vec), +} + +#[derive(Debug, Clone, Copy, PartialEq)] pub struct RtiaCalibrationResult { pub magnitude: f32, pub phase: f32, @@ -44,12 +53,12 @@ pub struct ImpedanceSetup { ad5940: AD5940, dsp_config: Option, pub running_mode: RunningMode, - pub rtia_calibrated: Option, + pub rtia_calibrated: RtiaCalibrated, } impl ImpedanceSetup { pub fn new(ad5940: AD5940) -> Self { - ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None, rtia_calibrated: None } + ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None, rtia_calibrated: RtiaCalibrated::None } } pub async fn init(&mut self) -> Result<(), Error> { @@ -200,7 +209,51 @@ impl ImpedanceSetup { Ok(calibration_result) } - pub async fn init_single_frequency_measurement(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result { + async fn calibrate_rtia_multiple(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { + let mut results = Vec::::new(); + + // 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 => DFTNUM::Num16384, + f if f < 100 => DFTNUM::Num16384, + f if f < 250 => DFTNUM::Num8192, + f if f < 1000 => DFTNUM::Num4096, + f if f < 2500 => DFTNUM::Num2048, + f if f < 10000 => DFTNUM::Num1024, + f if f < 25000 => DFTNUM::Num512, + f if f < 100000 => 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(); + + // Calibrate Rtia at the given frequency + let calibration_result = self.calibrate_rtia(frequency, wait_time).await.unwrap(); + + results.push(calibration_result).unwrap(); + } else { + error!("DSP configuration not set, cannot calculate wait time"); + return Err(ImpedanceInitError::DSPNotSet); + } + } + + Ok(results) + } + + pub async fn init_single_frequency_measurement_2_lead(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result { // Reset FIFO self.ad5940.clear_and_enable_fifo().await.unwrap(); @@ -306,7 +359,7 @@ impl ImpedanceSetup { // Calibrate Rtia match self.calibrate_rtia(frequency, wait_time).await { Ok(calibration_result) => { - self.rtia_calibrated = Some(calibration_result); + self.rtia_calibrated = RtiaCalibrated::Single(calibration_result); }, Err(e) => { error!("Rtia calibration failed: {:?}", e); @@ -376,7 +429,7 @@ impl ImpedanceSetup { Ok(sinus_periods_per_dft) } - pub async fn init_multi_frequency_measurement(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { + pub async fn init_multi_frequency_measurement_2_lead(&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(); @@ -481,6 +534,131 @@ impl ImpedanceSetup { Ok(periods_per_dft_vec) } + pub async fn init_multi_frequency_measurement_4_lead(&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(); + + // Calibrate Rtia + match number_of_points { + MeasurementPointSet::Eight => { + let results = self.calibrate_rtia_multiple::<8>(number_of_points).await.unwrap(); + self.rtia_calibrated = RtiaCalibrated::Vec8(results); + }, + MeasurementPointSet::Eighteen => { + let results = self.calibrate_rtia_multiple::<18>(number_of_points).await.unwrap(); + self.rtia_calibrated = RtiaCalibrated::Vec18(results); + }, + } + + // Reset FIFO + self.ad5940.clear_and_enable_fifo().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(); + + // Configure sequencer + self.ad5940.sequencer_enable(true).await; + + // 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 => DFTNUM::Num16384, + f if f < 100 => DFTNUM::Num16384, + f if f < 250 => DFTNUM::Num8192, + f if f < 1000 => DFTNUM::Num4096, + f if f < 2500 => DFTNUM::Num2048, + f if f < 10000 => DFTNUM::Num1024, + f if f < 25000 => DFTNUM::Num512, + f if f < 100000 => 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); + } + + // Configure switches + let switch_config = SwitchConfig::default() + .t9con(T9CON::T9Closed) + .tmuxcon(TMUXCON::T2Closed) + .nmuxcon(NMUXCON::N2Closed) + .pmuxcon(PMUXCON::P11Closed) + .dmuxcon(DMUXCON::D5Closed); + self.ad5940.apply_switch_config(switch_config).await.unwrap(); + + // 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(); + + // Voltage measurement + self.dsp_config.as_mut().unwrap() + .adc_mux_n(MUXSELN::AIN3) + .adc_mux_p(MUXSELP::AIN2); + // self.dsp_config.as_mut().unwrap() + // .adc_mux_n(MUXSELN::AIN1) + // .adc_mux_p(MUXSELP::CE0); + self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap(); + + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await; + self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await; + self.ad5940.sequencer_wait(wait_time).await; // Determined above + self.ad5940.sequencer_wait(16*20).await; // 20 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await; + + // Current measurement + self.dsp_config.as_mut().unwrap() + .adc_mux_n(MUXSELN::HsTiaNeg) + .adc_mux_p(MUXSELP::HsTiaPos); + self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap(); + + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await; + self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::ADCCONVEN | AFECON::DFTEN, true).await; + self.ad5940.sequencer_wait(wait_time).await; // Determined above + self.ad5940.sequencer_wait(16*20).await; // 20 us based on SYSCLK = 16MHz + self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await; + } + + // Toggle leds + self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b010).await.unwrap(); + self.ad5940.sequencer_wait(16 * 1_000).await; // 1ms based on SYSCLK = 16MHz + self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap(); + + self.ad5940.sequencer_enable(false).await; + + // Write sequence to SRAM + let start_address = 0; + self.ad5940.sequencer_cmd_write(start_address).await; + + 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) { self.ad5940.sequencer_trigger(0).await; } @@ -507,7 +685,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance let mut impedance_setup = impedance_setup.lock().await; // Trigger the sequencer again - if impedance_setup.running_mode == RunningMode::SingleFrequency2Lead || impedance_setup.running_mode == RunningMode::SingleFrequency4Lead || impedance_setup.running_mode == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) { + if impedance_setup.running_mode == RunningMode::SingleFrequency2Lead || impedance_setup.running_mode == RunningMode::SingleFrequency4Lead || impedance_setup.running_mode == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || impedance_setup.running_mode == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) { impedance_setup.start_measurement().await; } @@ -525,7 +703,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap(); - let result = calculate_impedance(data); + let result = calculate_impedance_2_lead(data); // Log // info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase); @@ -543,54 +721,25 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance let mut data: [u32; 4] = [0; 4]; impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap(); - - let mut dft_volt = Complex::new( - sign_extend_18bit(data[0]) as f32, - sign_extend_18bit(data[1]) as f32 - ); - - let mut dft_curr = Complex::new( - sign_extend_18bit(data[2]) as f32, - sign_extend_18bit(data[3]) as f32 - ); - - // Current is measured inverted in rtia - dft_curr.re *= -1.0; - dft_curr.im *= -1.0; - - // Impedance imaginary part is measured inverted - dft_volt.im *= -1.0; - dft_curr.im *= -1.0; - - let (volt_mag, volt_phase) = dft_volt.to_polar(); - let (curr_mag, curr_phase) = dft_curr.to_polar(); - - // info!("Volt Phase = {} V, Volt Phase = {} rad", volt_phase, curr_phase); - - match impedance_setup.rtia_calibrated.as_ref() { - None => { - error!("Rtia not calibrated, cannot compute impedance"); - continue; - }, - Some(cal) => { - let rtia_mag = cal.magnitude; - let rtia_phase = cal.phase; - - // Calculate impedance using calibrated Rtia - let magnitude = volt_mag / curr_mag * rtia_mag; - let phase = volt_phase - curr_phase + rtia_phase; + match impedance_setup.rtia_calibrated { + RtiaCalibrated::Single(ref cal) => { + let result = calculate_impedance_4_lead(data, cal); let data = SingleImpedanceOutput { - magnitude, - phase + magnitude: result.magnitude, + phase: result.phase }; IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok(); } + _ => { + error!("Rtia not (correctly) calibrated, cannot compute impedance"); + continue; + }, } } } - RunningMode::MultiFrequency(points) => { + RunningMode::MultiFrequency2Lead(points) => { // Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz) let required_count = points.len() * 4; @@ -611,7 +760,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance // Take 4 samples per frequency point for chunk in data_slice.chunks(4) { - let result = calculate_impedance(chunk.try_into().unwrap()); + let result = calculate_impedance_2_lead(chunk.try_into().unwrap()); match points { MeasurementPointSet::Eight => { @@ -629,6 +778,57 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance } continue; } + RunningMode::MultiFrequency4Lead(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(), + }; + + match (&points, &impedance_setup.rtia_calibrated) { + (MeasurementPointSet::Eight, RtiaCalibrated::Vec8(cal_vec)) => { + // Take 4 samples per frequency point + for (i, chunk) in data_slice.chunks(4).enumerate() { + let cal = &cal_vec[i]; + let result = calculate_impedance_4_lead(chunk.try_into().unwrap(), cal); + + impedance_output.magnitudes_8.push(result.magnitude).ok(); + impedance_output.phases_8.push(result.phase).ok(); + } + }, + (MeasurementPointSet::Eighteen, RtiaCalibrated::Vec18(cal_vec)) => { + // Take 4 samples per frequency point + for (i, chunk) in data_slice.chunks(4).enumerate() { + let cal = &cal_vec[i]; + let result = calculate_impedance_4_lead(chunk.try_into().unwrap(), cal); + + impedance_output.magnitudes_18.push(result.magnitude).ok(); + impedance_output.phases_18.push(result.phase).ok(); + } + }, + _ => { + error!("Rtia not (correctly) calibrated, cannot compute impedance"); + continue; + } + } + + IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok(); + } + continue; + + } } } } @@ -645,7 +845,7 @@ fn sign_extend_18bit(val: u32) -> i32 { } /// Calculate magnitude and phase of Rz using Rcal reference -pub fn calculate_impedance(data: [u32; 4]) -> ImpedanceResult { +pub fn calculate_impedance_2_lead(data: [u32; 4]) -> ImpedanceResult { let mut dft_rcal = Complex::new( sign_extend_18bit(data[0]) as f32, sign_extend_18bit(data[1]) as f32 @@ -675,3 +875,37 @@ pub fn calculate_impedance(data: [u32; 4]) -> ImpedanceResult { ImpedanceResult { magnitude, phase } } + +/// Calculate magnitude and phase of Rz using the calibrated Rtia +pub fn calculate_impedance_4_lead(data: [u32; 4], rtia: &RtiaCalibrationResult) -> ImpedanceResult { + let mut dft_volt = Complex::new( + sign_extend_18bit(data[0]) as f32, + sign_extend_18bit(data[1]) as f32 + ); + let mut dft_curr = Complex::new( + sign_extend_18bit(data[2]) as f32, + sign_extend_18bit(data[3]) as f32 + ); + + // Current is measured inverted in rtia + dft_curr.re *= -1.0; + dft_curr.im *= -1.0; + + // Impedance imaginary part is measured inverted + dft_volt.im *= -1.0; + dft_curr.im *= -1.0; + + let (volt_mag, volt_phase) = dft_volt.to_polar(); + let (curr_mag, curr_phase) = dft_curr.to_polar(); + + // Calculate impedance using calibrated Rtia + let magnitude = volt_mag / curr_mag * rtia.magnitude; + let phase = volt_phase - curr_phase + rtia.phase; + + let data = ImpedanceResult { + magnitude, + phase + }; + + data +} From 3716d7367ec4a0043c38b4c7e935be0ebed93c33 Mon Sep 17 00:00:00 2001 From: Hubald Verzijl Date: Thu, 6 Nov 2025 11:33:49 +0100 Subject: [PATCH 07/11] Implemented LP DAC for enabling common-mode during 4-lead measurement. --- src/ad5940.rs | 107 ++++++++++++++++++++++++++++++++++++++++ src/ad5940_registers.rs | 103 ++++++++++++++++++++++++++++++++++++++ src/impedance.rs | 41 +++++++++++++++ 3 files changed, 251 insertions(+) diff --git a/src/ad5940.rs b/src/ad5940.rs index b3fc588..8ba8a06 100644 --- a/src/ad5940.rs +++ b/src/ad5940.rs @@ -68,6 +68,55 @@ impl SwitchConfig { } } +#[allow(dead_code)] +#[derive(Default)] +pub struct LpConfig { + data_reset: Option, + power_enable: Option, + data_6bit: Option, + common_mode_switch: Option, + tia_switches_enabled: Option, + tia_enable: Option, + filter_resistor: Option, +} + +impl LpConfig { + pub fn data_reset(&mut self, rsten: bool) -> &mut Self { + self.data_reset = Some(rsten); + self + } + + pub fn power_enable(&mut self, pwden: bool) -> &mut Self { + self.power_enable = Some(pwden); + self + } + + pub fn data_6bit(&mut self, data: u8) -> &mut Self { + self.data_6bit = Some(data & 0x3F); + self + } + + pub fn common_mode_enable(&mut self, enable: bool) -> &mut Self { + self.common_mode_switch = Some(enable); + self + } + + pub fn tia_switches_enabled(&mut self, enable: u16) -> &mut Self { + self.tia_switches_enabled = Some(enable); + self + } + + pub fn tia_enable(&mut self, enable: bool) -> &mut Self { + self.tia_enable = Some(enable); + self + } + + pub fn filter_resistor(&mut self, tiarf: TIARF) -> &mut Self { + self.filter_resistor = Some(tiarf); + self + } +} + #[allow(dead_code)] #[derive(Default)] pub struct DspConfig { @@ -594,6 +643,59 @@ impl AD5940 { Ok(()) } + pub async fn apply_lp_config(&mut self, config: &LpConfig) -> Result<(), Error> { + // LPDACCON0 + let mut current = self.read_reg(Register::LPDACCON0).await?; + + if let Some(data_reset) = config.data_reset { + current = RSTEN::apply(current, data_reset as u32); + } + + if let Some(power_enable) = config.power_enable { + current = PWDEN::apply(current, !power_enable as u32); + } + + self.write_reg(Register::LPDACCON0, current).await?; + + // LPDACDAT0 + let mut current = self.read_reg(Register::LPDACDAT0).await?; + + if let Some(data) = config.data_6bit { + current = DACIN6::apply(current, data as u32); + } + + self.write_reg(Register::LPDACDAT0, current).await?; + + // SWMUX + let mut current = self.read_reg(Register::SWMUX).await?; + + if let Some(common_mode_switch) = config.common_mode_switch { + current = SWMUX::apply(current, common_mode_switch as u32); + } + + self.write_reg(Register::SWMUX, current).await?; + + // LPTIASW0 + if let Some(tia_switches_enabled) = config.tia_switches_enabled { + self.write_reg(Register::LPTIASW0, tia_switches_enabled as u32).await?; + } + + // LPTIACON0 + let mut current = self.read_reg(Register::LPTIACON0).await?; + + if let Some(tia_enable) = config.tia_enable { + current = TIAPDEN::apply(current, !tia_enable as u32); + } + + if let Some(tiarf) = config.filter_resistor { + current = TIARF::apply(current, tiarf as u32); + } + + self.write_reg(Register::LPTIACON0, current).await?; + + Ok(()) + } + pub async fn apply_dsp_config(&mut self, config: &DspConfig) -> Result<(), Error> { // ADCCON let mut current = self.read_reg(Register::ADCCON).await?; @@ -849,6 +951,10 @@ pub enum Register { CMDFIFOWRITE = 0x0000_2070, // Command FIFO Write Register TEMPSENSDAT = 0x0000_2084, // Temperature Sensor Result Register ADCDAT = 0x0000_2074, // ADC Raw Result Register + LPTIASW0 = 0x0000_20E4, // Low power TIA switch configuration + LPTIACON0 = 0x0000_20EC, // Low power TIA control bits, Channel 0 + LPDACDAT0 = 0x0000_2120, // Low power DAC data output Register + LPDACCON0 = 0x0000_2128, // Low power DAC configuration register TEMPSENS = 0x0000_2174, // Temperature Sensor Configuration Register ADCCON = 0x0000_21A8, // ADC Configuration Register SEQ0INFO = 0x0000_21CC, // Sequence 0 Information Register @@ -859,6 +965,7 @@ pub enum Register { AFEGENINTSTA = 0x0000_209C, // Analog Generation Interrupt Register CMDFIFOWADDR = 0x0000_21D4, // Command FIFO Write Address Register PMBW = 0x0000_22F0, // Power Mode Configuration Register + SWMUX = 0x0000_235C, // Common-mode switch mux select register INTCFLAG0 = 0x0000_3010, // Interrupt Control Flag 0 Register INTCFLAG1 = 0x0000_3014, // Interrupt Control Flag 1 Register OSCCON = 0x0000_0A10, // Oscillator Control Register diff --git a/src/ad5940_registers.rs b/src/ad5940_registers.rs index 103f1af..4aaac62 100644 --- a/src/ad5940_registers.rs +++ b/src/ad5940_registers.rs @@ -130,6 +130,107 @@ impl RegisterField for DMUXCON { const MASK: u32 = 0b1111; } +#[allow(dead_code)] +#[repr(u32)] +#[derive(Copy, Clone)] +pub enum RSTEN { + Disabled = 0b0, + Enabled = 0b1, +} + +impl RegisterField for RSTEN { + fn reset() -> Self { + RSTEN::Disabled + } + const BIT_OFFSET: u32 = 0; + const MASK: u32 = 0b1; +} + +#[allow(dead_code)] +#[repr(u32)] +#[derive(Copy, Clone)] +pub enum PWDEN { + DACPoweredOn = 0b0, + DACPoweredOff = 0b1, +} + +impl RegisterField for PWDEN { + fn reset() -> Self { + PWDEN::DACPoweredOff + } + const BIT_OFFSET: u32 = 1; + const MASK: u32 = 0b1; +} + +#[allow(dead_code)] +#[repr(u32)] +#[derive(Copy, Clone)] +pub enum DACIN6 { + Default = 0b0, +} + +impl RegisterField for DACIN6 { + fn reset() -> Self { + DACIN6::Default + } + const BIT_OFFSET: u32 = 12; + const MASK: u32 = 0b11_1111; +} + +#[allow(dead_code)] +#[repr(u32)] +#[derive(Copy, Clone)] +pub enum SWMUX { + CommonModeOff = 0b0, + COmmonModeOn = 0b1, +} + +impl RegisterField for SWMUX { + fn reset() -> Self { + SWMUX::CommonModeOff + } + const BIT_OFFSET: u32 = 3; + const MASK: u32 = 0b1; +} + +#[allow(dead_code)] +#[repr(u32)] +#[derive(Copy, Clone)] +pub enum TIAPDEN { + PowerUp = 0b0, + PowerDown = 0b1, +} + +impl RegisterField for TIAPDEN { + fn reset() -> Self { + TIAPDEN::PowerDown + } + const BIT_OFFSET: u32 = 0; + const MASK: u32 = 0b1; +} + +#[allow(dead_code)] +#[repr(u32)] +#[derive(Copy, Clone)] +pub enum TIARF { + Disconnected = 0b0, + R0 = 0b1, + R20k = 0b10, + R100k = 0b11, + R200k = 0b100, + R400k = 0b101, + R600k = 0b110, + R1M = 0b111, +} + +impl RegisterField for TIARF { + fn reset() -> Self { + TIARF::Disconnected + } + const BIT_OFFSET: u32 = 13; + const MASK: u32 = 0b111; +} + #[allow(dead_code)] #[repr(u32)] #[derive(Copy, Clone)] @@ -158,7 +259,9 @@ pub enum MUXSELN HsTiaNeg = 0b00001, LpTiaNeg = 0b00010, AIN1 = 0b00101, + AIN2 = 0b00110, AIN3 = 0b00111, + VBIAS_CAP= 0b01000, ExciNNode= 0b10100, } diff --git a/src/impedance.rs b/src/impedance.rs index b2000bc..807f547 100644 --- a/src/impedance.rs +++ b/src/impedance.rs @@ -83,6 +83,9 @@ impl ImpedanceSetup { self.ad5940.apply_clk_config(&clk_config).await.unwrap(); + // Configure LP DAC and TIA + self.common_mode_output_enable(false).await.unwrap(); + // Set DSP configuration let mut dsp_config = DspConfig::default(); dsp_config @@ -253,7 +256,36 @@ impl ImpedanceSetup { Ok(results) } + pub async fn common_mode_output_enable(&mut self, enable: bool) -> Result<(), Error> { + // Configure LP DAC and TIA + let mut lp_config = LpConfig::default(); + + if enable { + lp_config + .data_reset(true) + .power_enable(true) + .data_6bit(31) // Mid-scale for 6-bit DAC + .common_mode_enable(true) + .tia_switches_enabled(1 << 5 | 1 << 7 | 1 << 9 | 1 << 13) + .tia_enable(true) + .filter_resistor(TIARF::R20k); + + } else { + lp_config + .power_enable(false) + .common_mode_enable(false) + .tia_enable(false); + } + + self.ad5940.apply_lp_config(&lp_config).await.unwrap(); + + Ok(()) + } + pub async fn init_single_frequency_measurement_2_lead(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result { + // Configure LP DAC and TIA + self.common_mode_output_enable(false).await.unwrap(); + // Reset FIFO self.ad5940.clear_and_enable_fifo().await.unwrap(); @@ -333,6 +365,9 @@ impl ImpedanceSetup { } pub async fn init_single_frequency_measurement_4_lead(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result { + // Configure LP DAC and TIA + self.common_mode_output_enable(true).await.unwrap(); + // Reset FIFO self.ad5940.clear_and_enable_fifo().await.unwrap(); @@ -430,6 +465,9 @@ impl ImpedanceSetup { } pub async fn init_multi_frequency_measurement_2_lead(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { + // Configure LP DAC and TIA + self.common_mode_output_enable(false).await.unwrap(); + // Create vector to store the periods per DFT for each frequency let mut periods_per_dft_vec = heapless::Vec::::new(); @@ -535,6 +573,9 @@ impl ImpedanceSetup { } pub async fn init_multi_frequency_measurement_4_lead(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { + // Configure LP DAC and TIA + self.common_mode_output_enable(true).await.unwrap(); + // Create vector to store the periods per DFT for each frequency let mut periods_per_dft_vec = heapless::Vec::::new(); From f6a88b6d66da302b953ba0fdd217aabd80ce4ed7 Mon Sep 17 00:00:00 2001 From: Hubald Verzijl Date: Thu, 6 Nov 2025 12:13:52 +0100 Subject: [PATCH 08/11] Change multi to sweep. --- src/communication.rs | 70 ++++++++++++++++++-------------------------- src/impedance.rs | 35 +++++++++++----------- 2 files changed, 46 insertions(+), 59 deletions(-) diff --git a/src/communication.rs b/src/communication.rs index 3df5a95..fd49592 100644 --- a/src/communication.rs +++ b/src/communication.rs @@ -20,9 +20,9 @@ use postcard_rpc::{ }, }; -use bioz_icd_rs::{GetUniqueIdEndpoint, MeasurementPointSet, MultiImpedanceOutputTopic, MultiImpedanceResult, MultiImpedanceStartRequest, PingEndpoint, SetGreenLedEndpoint, SingleImpedanceOutputTopic, SingleImpedanceStartRequest, StartMultiImpedanceEndpoint, StartSingleImpedanceEndpoint, StopMultiImpedanceEndpoint, StopSingleImpedanceEndpoint, BioImpedanceLeadMode, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST}; +use bioz_icd_rs::{GetUniqueIdEndpoint, MeasurementPointSet, SweepImpedanceOutputTopic, SweepImpedanceResult, SweepImpedanceStartRequest, PingEndpoint, SetGreenLedEndpoint, SingleImpedanceOutputTopic, SingleImpedanceStartRequest, StartSweepImpedanceEndpoint, StartSingleImpedanceEndpoint, StopImpedanceEndpoint, BioImpedanceLeadMode, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST}; -use crate::impedance::{ImpedanceSetupType, RunningMode, IMPEDANCE_CHANNEL_MULTI, IMPEDANCE_CHANNEL_SINGLE}; +use crate::impedance::{ImpedanceSetupType, RunningMode, IMPEDANCE_CHANNEL_SWEEP, IMPEDANCE_CHANNEL_SINGLE}; // Postcard RPC types type AppDriver = usb::Driver<'static, peripherals::USB>; @@ -70,9 +70,8 @@ define_dispatch! { | GetUniqueIdEndpoint | blocking | get_unique_id_handler | | SetGreenLedEndpoint | async | set_green_led_handler | | StartSingleImpedanceEndpoint | spawn | start_single_impedance_handler | - | StopSingleImpedanceEndpoint | async | stop_single_impedance_handler | - | StartMultiImpedanceEndpoint | spawn | start_multi_impedance_handler | - | StopMultiImpedanceEndpoint | async | stop_multi_impedance_handler | + | StartSweepImpedanceEndpoint | spawn | start_sweep_impedance_handler | + | StopImpedanceEndpoint | async | stop_impedance_handler | }; topics_in: { list: TOPICS_IN_LIST; @@ -198,7 +197,7 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader match select(stop_fut, recv_fut).await { Either::First(_) => { - info!("Stop signal received."); + // info!("Stop signal received."); break; } Either::Second(msg) => { @@ -220,79 +219,70 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader STOP.reset(); } -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::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) { - STOP.signal(()); - } - was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) -} - #[embassy_executor::task] -pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: MultiImpedanceStartRequest, sender: Sender) { +pub async fn start_sweep_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: SweepImpedanceStartRequest, sender: Sender) { // Mark the impedance setup as running // Init the sequencer let response = match (rqst.lead_mode, rqst.points) { (BioImpedanceLeadMode::TwoLead, MeasurementPointSet::Eight) => { - context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency2Lead(rqst.points); + context.impedance_setup.lock().await.running_mode = RunningMode::SweepFrequency2Lead(rqst.points); const SIZE: usize = 8; context .impedance_setup .lock() .await - .init_multi_frequency_measurement_2_lead::(rqst.points) + .init_sweep_frequency_measurement_2_lead::(rqst.points) .await - .map(|periods| MultiImpedanceResult { + .map(|periods| SweepImpedanceResult { points: rqst.points, periods_per_dft_8: periods, periods_per_dft_18: Vec::new(), }) } (BioImpedanceLeadMode::TwoLead, MeasurementPointSet::Eighteen) => { - context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency2Lead(rqst.points); + context.impedance_setup.lock().await.running_mode = RunningMode::SweepFrequency2Lead(rqst.points); const SIZE: usize = 18; context .impedance_setup .lock() .await - .init_multi_frequency_measurement_2_lead::(rqst.points) + .init_sweep_frequency_measurement_2_lead::(rqst.points) .await - .map(|periods| MultiImpedanceResult { + .map(|periods| SweepImpedanceResult { points: rqst.points, periods_per_dft_8: Vec::new(), periods_per_dft_18: periods, }) } (BioImpedanceLeadMode::FourLead, MeasurementPointSet::Eight) => { - info!("Start multi impedance 4 lead 8 points"); - context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency4Lead(rqst.points); + info!("Start impedance spectroscopy 4-lead with 8 points"); + context.impedance_setup.lock().await.running_mode = RunningMode::SweepFrequency4Lead(rqst.points); const SIZE: usize = 8; context .impedance_setup .lock() .await - .init_multi_frequency_measurement_4_lead::(rqst.points) + .init_sweep_frequency_measurement_4_lead::(rqst.points) .await - .map(|periods| MultiImpedanceResult { + .map(|periods| SweepImpedanceResult { points: rqst.points, periods_per_dft_8: periods, periods_per_dft_18: Vec::new(), }) } (BioImpedanceLeadMode::FourLead, MeasurementPointSet::Eighteen) => { - info!("Start multi impedance 4 lead 18 points"); - context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency4Lead(rqst.points); + info!("Start impedance spectroscopy 4-lead with 18 points"); + context.impedance_setup.lock().await.running_mode = RunningMode::SweepFrequency4Lead(rqst.points); const SIZE: usize = 18; context .impedance_setup .lock() .await - .init_multi_frequency_measurement_4_lead::(rqst.points) + .init_sweep_frequency_measurement_4_lead::(rqst.points) .await - .map(|periods| MultiImpedanceResult { + .map(|periods| SweepImpedanceResult { points: rqst.points, periods_per_dft_8: Vec::new(), periods_per_dft_18: periods, @@ -304,7 +294,7 @@ pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, context.impedance_setup.lock().await.start_measurement().await; if sender - .reply::(header.seq_no, &response) + .reply::(header.seq_no, &response) .await .is_err() { @@ -312,21 +302,19 @@ pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, return; } - info!("Start multi impedance measurement."); - let mut seq: u8 = 0; loop { let stop_fut = STOP.wait(); - let recv_fut = IMPEDANCE_CHANNEL_MULTI.receive(); + let recv_fut = IMPEDANCE_CHANNEL_SWEEP.receive(); match select(stop_fut, recv_fut).await { Either::First(_) => { - info!("Stop signal received."); + // info!("Stop signal received."); break; } Either::Second(msg) => { if sender - .publish::(seq.into(), &msg) + .publish::(seq.into(), &msg) .await .is_err() { @@ -339,15 +327,15 @@ pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, } context.impedance_setup.lock().await.running_mode = RunningMode::None; - info!("Impedance measurement stopped."); + info!("Impedance spectroscopy measurement stopped."); STOP.reset(); } -pub async fn stop_multi_impedance_handler(context: &mut Context, _header: VarHeader, _rqst: ()) -> bool { +pub async fn stop_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::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) { + if was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::SweepFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::SweepFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::SweepFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::SweepFrequency4Lead(MeasurementPointSet::Eighteen) { STOP.signal(()); - } - was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) + } + was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::SweepFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::SweepFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::SweepFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::SweepFrequency4Lead(MeasurementPointSet::Eighteen) } \ No newline at end of file diff --git a/src/impedance.rs b/src/impedance.rs index 807f547..6518e57 100644 --- a/src/impedance.rs +++ b/src/impedance.rs @@ -16,11 +16,11 @@ use num::complex::Complex; use crate::ad5940::*; use crate::ad5940_registers::*; -use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, ImpedanceInitError, MeasurementPointSet, MultiImpedanceOutput}; +use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, ImpedanceInitError, MeasurementPointSet, SweepImpedanceOutput}; 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_SWEEP: Channel = Channel::new(); pub type ImpedanceSetupType = Mutex; pub static IMPEDANCE_SETUP: StaticCell = StaticCell::new(); @@ -32,8 +32,8 @@ pub enum RunningMode { None, SingleFrequency2Lead, SingleFrequency4Lead, - MultiFrequency2Lead(MeasurementPointSet), - MultiFrequency4Lead(MeasurementPointSet), + SweepFrequency2Lead(MeasurementPointSet), + SweepFrequency4Lead(MeasurementPointSet), } pub enum RtiaCalibrated { @@ -303,7 +303,7 @@ impl ImpedanceSetup { if let Some(dsp_config) = &self.dsp_config { wait_time = self.ad5940.sequencer_calculate_wait_time(dsp_config).await.unwrap(); sinus_periods_per_dft = wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32; - info!("Sinus periods per DFT: {}", sinus_periods_per_dft); + // info!("Sinus periods per DFT: {}", sinus_periods_per_dft); } else { error!("DSP configuration not set, cannot calculate wait time"); return Err(ImpedanceInitError::DSPNotSet); @@ -385,7 +385,7 @@ impl ImpedanceSetup { if let Some(dsp_config) = &self.dsp_config { wait_time = self.ad5940.sequencer_calculate_wait_time(dsp_config).await.unwrap(); sinus_periods_per_dft = wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32; - info!("Sinus periods per DFT: {}", sinus_periods_per_dft); + // info!("Sinus periods per DFT: {}", sinus_periods_per_dft); } else { error!("DSP configuration not set, cannot calculate wait time"); return Err(ImpedanceInitError::DSPNotSet); @@ -464,7 +464,7 @@ impl ImpedanceSetup { Ok(sinus_periods_per_dft) } - pub async fn init_multi_frequency_measurement_2_lead(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { + pub async fn init_sweep_frequency_measurement_2_lead(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { // Configure LP DAC and TIA self.common_mode_output_enable(false).await.unwrap(); @@ -511,7 +511,7 @@ impl ImpedanceSetup { 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); + // info!("{}Hz: Sinus periods per DFT: {}", frequency, periods_per_dft); } else { error!("DSP configuration not set, cannot calculate wait time"); @@ -572,7 +572,7 @@ impl ImpedanceSetup { Ok(periods_per_dft_vec) } - pub async fn init_multi_frequency_measurement_4_lead(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { + pub async fn init_sweep_frequency_measurement_4_lead(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { // Configure LP DAC and TIA self.common_mode_output_enable(true).await.unwrap(); @@ -631,8 +631,7 @@ impl ImpedanceSetup { 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); - + // info!("{}Hz: Sinus periods per DFT: {}", frequency, periods_per_dft); } else { error!("DSP configuration not set, cannot calculate wait time"); return Err(ImpedanceInitError::DSPNotSet); @@ -726,7 +725,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance let mut impedance_setup = impedance_setup.lock().await; // Trigger the sequencer again - if impedance_setup.running_mode == RunningMode::SingleFrequency2Lead || impedance_setup.running_mode == RunningMode::SingleFrequency4Lead || impedance_setup.running_mode == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || impedance_setup.running_mode == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) { + if impedance_setup.running_mode == RunningMode::SingleFrequency2Lead || impedance_setup.running_mode == RunningMode::SingleFrequency4Lead || impedance_setup.running_mode == RunningMode::SweepFrequency2Lead(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::SweepFrequency2Lead(MeasurementPointSet::Eighteen) || impedance_setup.running_mode == RunningMode::SweepFrequency4Lead(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::SweepFrequency4Lead(MeasurementPointSet::Eighteen) { impedance_setup.start_measurement().await; } @@ -780,7 +779,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance } } } - RunningMode::MultiFrequency2Lead(points) => { + RunningMode::SweepFrequency2Lead(points) => { // Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz) let required_count = points.len() * 4; @@ -791,7 +790,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance impedance_setup.read_fifo(data_slice).await.unwrap(); // Output structure - let mut impedance_output = MultiImpedanceOutput { + let mut impedance_output = SweepImpedanceOutput { points, magnitudes_8: Vec::new(), phases_8: Vec::new(), @@ -815,11 +814,11 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance } } - IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok(); + IMPEDANCE_CHANNEL_SWEEP.try_send(impedance_output).ok(); } continue; } - RunningMode::MultiFrequency4Lead(points) => { + RunningMode::SweepFrequency4Lead(points) => { // Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz) let required_count = points.len() * 4; @@ -830,7 +829,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance impedance_setup.read_fifo(data_slice).await.unwrap(); // Output structure - let mut impedance_output = MultiImpedanceOutput { + let mut impedance_output = SweepImpedanceOutput { points, magnitudes_8: Vec::new(), phases_8: Vec::new(), @@ -865,7 +864,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance } } - IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok(); + IMPEDANCE_CHANNEL_SWEEP.try_send(impedance_output).ok(); } continue; From 11dbf2a8d8075f64ec7f039f3dea8f5b64eb2edf Mon Sep 17 00:00:00 2001 From: Hubald Verzijl Date: Thu, 6 Nov 2025 13:10:59 +0100 Subject: [PATCH 09/11] Fix starting bug showing cal value. --- src/impedance.rs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/impedance.rs b/src/impedance.rs index 6518e57..0482b27 100644 --- a/src/impedance.rs +++ b/src/impedance.rs @@ -368,9 +368,6 @@ impl ImpedanceSetup { // Configure LP DAC and TIA self.common_mode_output_enable(true).await.unwrap(); - // 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(); @@ -401,6 +398,9 @@ impl ImpedanceSetup { } } + // Reset FIFO + self.ad5940.clear_and_enable_fifo().await.unwrap(); + // Configure switches let switch_config = SwitchConfig::default() .t9con(T9CON::T9Closed) From 04959ce9cb943604e8db9be74591e760d9757c22 Mon Sep 17 00:00:00 2001 From: Hubald Verzijl Date: Thu, 6 Nov 2025 14:09:11 +0100 Subject: [PATCH 10/11] Prevent phase wrap. --- src/impedance.rs | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/src/impedance.rs b/src/impedance.rs index 0482b27..dac2d4f 100644 --- a/src/impedance.rs +++ b/src/impedance.rs @@ -12,6 +12,7 @@ use static_cell::StaticCell; use heapless::Vec; use num::complex::Complex; +use core::f32::consts::PI; use crate::ad5940::*; use crate::ad5940_registers::*; @@ -911,7 +912,15 @@ pub fn calculate_impedance_2_lead(data: [u32; 4]) -> ImpedanceResult { let (rz_mag, rz_phase) = dft_rz.to_polar(); let magnitude = (rcal_mag / rz_mag) * RCAL; - let phase = rcal_phase - rz_phase; + let mut phase = rcal_phase - rz_phase; + + // Normalize phase to [-π, π] + if phase > PI { + phase -= 2.0 * PI; + } + else if phase < -PI { + phase += 2.0 * PI; + } ImpedanceResult { magnitude, phase } } @@ -940,8 +949,16 @@ pub fn calculate_impedance_4_lead(data: [u32; 4], rtia: &RtiaCalibrationResult) // Calculate impedance using calibrated Rtia let magnitude = volt_mag / curr_mag * rtia.magnitude; - let phase = volt_phase - curr_phase + rtia.phase; + let mut phase = volt_phase - curr_phase + rtia.phase; + // Normalize phase to [-π, π] + if phase > PI { + phase -= 2.0 * PI; + } + else if phase < -PI { + phase += 2.0 * PI; + } + let data = ImpedanceResult { magnitude, phase From de9f11e591e04da560470bf993dc6ca0a1d401ac Mon Sep 17 00:00:00 2001 From: Hubald Verzijl Date: Thu, 12 Feb 2026 18:09:42 +0100 Subject: [PATCH 11/11] Updated toml crate versions, added electrodes to impedance structure. --- Cargo.lock | 184 +++++++++++++++++++++---------------------- Cargo.toml | 14 ++-- src/adg2128.rs | 6 +- src/communication.rs | 6 +- src/electrodes.rs | 20 +++-- src/impedance.rs | 10 ++- src/main.rs | 46 ++++++----- 7 files changed, 150 insertions(+), 136 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index bb1b67c..4330a9f 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -16,9 +16,9 @@ dependencies = [ [[package]] name = "aligned" -version = "0.4.2" +version = "0.4.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "377e4c0ba83e4431b10df45c1d4666f178ea9c552cac93e60c3a88bf32785923" +checksum = "ee4508988c62edf04abd8d92897fca0c2995d907ce1dfeaf369dac3716a40685" dependencies = [ "as-slice", ] @@ -61,17 +61,17 @@ name = "bioz-firmware-rs" version = "0.1.0" dependencies = [ "bioz-icd-rs", - "bitflags 2.9.4", + "bitflags 2.10.0", "cortex-m", "cortex-m-rt", "defmt 1.0.1", "defmt-rtt", - "embassy-embedded-hal 0.4.0", + "embassy-embedded-hal", "embassy-executor", "embassy-futures", "embassy-stm32", - "embassy-sync 0.6.2", - "embassy-time", + "embassy-sync", + "embassy-time 0.4.0", "embassy-usb", "embedded-hal 1.0.0", "heapless 0.9.1", @@ -117,9 +117,9 @@ checksum = "bef38d45163c2f1dde094a7dfd33ccf595c92905c8f8f4fdc18d06fb1037718a" [[package]] name = "bitflags" -version = "2.9.4" +version = "2.10.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2261d10cca569e4643e526d8dc2e62e433cc8aba21ab764233731f8d369bf394" +checksum = "812e12b5285cc515a9c72a5c1d3b6d46a19dac5acfef5265968c166106e31dd3" [[package]] name = "block-device-driver" @@ -287,33 +287,15 @@ dependencies = [ [[package]] name = "embassy-embedded-hal" -version = "0.3.2" +version = "0.5.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8c62a3bf127e03832fb97d8b01a058775e617653bc89e2a12c256485a7fb54c1" -dependencies = [ - "defmt 0.3.100", - "embassy-embedded-hal 0.4.0", - "embassy-futures", - "embassy-sync 0.6.2", - "embassy-time", - "embedded-hal 0.2.7", - "embedded-hal 1.0.0", - "embedded-hal-async", - "embedded-storage", - "embedded-storage-async", - "nb 1.1.0", -] - -[[package]] -name = "embassy-embedded-hal" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "d1611b7a7ab5d1fbed84c338df26d56fd9bded58006ebb029075112ed2c5e039" +checksum = "554e3e840696f54b4c9afcf28a0f24da431c927f4151040020416e7393d6d0d8" dependencies = [ "defmt 1.0.1", "embassy-futures", - "embassy-hal-internal 0.3.0", - "embassy-sync 0.7.2", + "embassy-hal-internal", + "embassy-sync", + "embassy-time 0.5.0", "embedded-hal 0.2.7", "embedded-hal 1.0.0", "embedded-hal-async", @@ -324,22 +306,23 @@ dependencies = [ [[package]] name = "embassy-executor" -version = "0.7.0" +version = "0.9.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "90327bcc66333a507f89ecc4e2d911b265c45f5c9bc241f98eee076752d35ac6" +checksum = "06070468370195e0e86f241c8e5004356d696590a678d47d6676795b2e439c6b" dependencies = [ "cortex-m", "critical-section", - "defmt 0.3.100", + "defmt 1.0.1", "document-features", "embassy-executor-macros", + "embassy-executor-timer-queue", ] [[package]] name = "embassy-executor-macros" -version = "0.6.2" +version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3577b1e9446f61381179a330fc5324b01d511624c55f25e3c66c9e3c626dbecf" +checksum = "dfdddc3a04226828316bf31393b6903ee162238576b1584ee2669af215d55472" dependencies = [ "darling", "proc-macro2", @@ -347,30 +330,27 @@ dependencies = [ "syn 2.0.104", ] +[[package]] +name = "embassy-executor-timer-queue" +version = "0.1.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "2fc328bf943af66b80b98755db9106bf7e7471b0cf47dc8559cd9a6be504cc9c" + [[package]] name = "embassy-futures" version = "0.1.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "dc2d050bdc5c21e0862a89256ed8029ae6c290a93aecefc73084b3002cdebb01" -[[package]] -name = "embassy-hal-internal" -version = "0.2.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "0ef3bac31ec146321248a169e9c7b5799f1e0b3829c7a9b324cb4600a7438f59" -dependencies = [ - "cortex-m", - "critical-section", - "defmt 0.3.100", - "num-traits", -] - [[package]] name = "embassy-hal-internal" version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "95285007a91b619dc9f26ea8f55452aa6c60f7115a4edc05085cd2bd3127cd7a" dependencies = [ + "cortex-m", + "critical-section", + "defmt 1.0.1", "num-traits", ] @@ -391,31 +371,31 @@ checksum = "b7b2739fbcf6cd206ae08779c7d709087b16577d255f2ea4a45bc4bbbf305b3f" dependencies = [ "embassy-futures", "embassy-net-driver", - "embassy-sync 0.7.2", + "embassy-sync", ] [[package]] name = "embassy-stm32" -version = "0.2.0" +version = "0.4.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e1e0bb733acdddbc7097765a47ce80bde2385647cf1d8427331931e06cff9a87" +checksum = "0d972eab325cc96afee98f80a91ca6b00249b6356dc0fdbff68b70c200df9fae" dependencies = [ "aligned", "bit_field", - "bitflags 2.9.4", + "bitflags 2.10.0", "block-device-driver", "cfg-if", "cortex-m", "cortex-m-rt", "critical-section", - "defmt 0.3.100", + "defmt 1.0.1", "document-features", - "embassy-embedded-hal 0.3.2", + "embassy-embedded-hal", "embassy-futures", - "embassy-hal-internal 0.2.0", + "embassy-hal-internal", "embassy-net-driver", - "embassy-sync 0.6.2", - "embassy-time", + "embassy-sync", + "embassy-time 0.5.0", "embassy-time-driver", "embassy-time-queue-utils", "embassy-usb-driver", @@ -433,7 +413,8 @@ dependencies = [ "nb 1.1.0", "proc-macro2", "quote", - "rand_core", + "rand_core 0.6.4", + "rand_core 0.9.5", "sdio-host", "static_assertions", "stm32-fmc", @@ -442,21 +423,6 @@ dependencies = [ "volatile-register", ] -[[package]] -name = "embassy-sync" -version = "0.6.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8d2c8cdff05a7a51ba0087489ea44b0b1d97a296ca6b1d6d1a33ea7423d34049" -dependencies = [ - "cfg-if", - "critical-section", - "defmt 0.3.100", - "embedded-io-async", - "futures-sink", - "futures-util", - "heapless 0.8.0", -] - [[package]] name = "embassy-sync" version = "0.7.2" @@ -465,6 +431,7 @@ checksum = "73974a3edbd0bd286759b3d483540f0ebef705919a5f56f4fc7709066f71689b" dependencies = [ "cfg-if", "critical-section", + "defmt 1.0.1", "embedded-io-async", "futures-core", "futures-sink", @@ -488,6 +455,23 @@ dependencies = [ "futures-util", ] +[[package]] +name = "embassy-time" +version = "0.5.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "f4fa65b9284d974dad7a23bb72835c4ec85c0b540d86af7fc4098c88cff51d65" +dependencies = [ + "cfg-if", + "critical-section", + "defmt 1.0.1", + "document-features", + "embassy-time-driver", + "embedded-hal 0.2.7", + "embedded-hal 1.0.0", + "embedded-hal-async", + "futures-core", +] + [[package]] name = "embassy-time-driver" version = "0.2.1" @@ -499,25 +483,26 @@ dependencies = [ [[package]] name = "embassy-time-queue-utils" -version = "0.1.0" +version = "0.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc55c748d16908a65b166d09ce976575fb8852cf60ccd06174092b41064d8f83" +checksum = "80e2ee86063bd028a420a5fb5898c18c87a8898026da1d4c852af2c443d0a454" dependencies = [ - "embassy-executor", + "embassy-executor-timer-queue", "heapless 0.8.0", ] [[package]] name = "embassy-usb" -version = "0.4.0" +version = "0.5.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "6e651b9b7b47b514e6e6d1940a6e2e300891a2c33641917130643602a0cb6386" +checksum = "dc4462e48b19a4f401a11901bdd981aab80c6a826608016a0bdc73cbbab31954" dependencies = [ - "defmt 0.3.100", + "defmt 1.0.1", "embassy-futures", "embassy-net-driver-channel", - "embassy-sync 0.6.2", + "embassy-sync", "embassy-usb-driver", + "embedded-io-async", "heapless 0.8.0", "ssmarshal", "usbd-hid", @@ -525,9 +510,9 @@ dependencies = [ [[package]] name = "embassy-usb-driver" -version = "0.1.1" +version = "0.2.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "340c5ce591ef58c6449e43f51d2c53efe1bf0bb6a40cbf80afa0d259c7d52c76" +checksum = "17119855ccc2d1f7470a39756b12068454ae27a3eabb037d940b5c03d9c77b7a" dependencies = [ "defmt 1.0.1", "embedded-io-async", @@ -535,13 +520,13 @@ dependencies = [ [[package]] name = "embassy-usb-synopsys-otg" -version = "0.2.0" +version = "0.3.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "08e753b23799329780c7ac434264026d0422044d6649ed70a73441b14a6436d7" +checksum = "288751f8eaa44a5cf2613f13cee0ca8e06e6638cb96e897e6834702c79084b23" dependencies = [ "critical-section", - "defmt 0.3.100", - "embassy-sync 0.6.2", + "defmt 1.0.1", + "embassy-sync", "embassy-usb-driver", ] @@ -896,18 +881,18 @@ dependencies = [ [[package]] name = "postcard-rpc" -version = "0.11.15" +version = "0.12.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c7e1944dfb9859e440511700c442edce3eacd5862f90f5a9997d004bd3553f3b" +checksum = "d1b0f9cc8bd8f38f5893fb7f07626b1c8722b3166ead66cfb14b5c5d5352822c" dependencies = [ - "defmt 0.3.100", + "defmt 1.0.1", "embassy-executor", "embassy-futures", - "embassy-sync 0.6.2", - "embassy-time", + "embassy-sync", + "embassy-time 0.5.0", "embassy-usb", "embassy-usb-driver", - "heapless 0.8.0", + "heapless 0.9.1", "portable-atomic", "postcard", "postcard-schema", @@ -973,6 +958,12 @@ version = "0.6.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ec0be4795e2f6a28069bec0b5ff3e2ac9bafc99e6a9a7dc3547996c5c816922c" +[[package]] +name = "rand_core" +version = "0.9.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "76afc826de14238e6e8c374ddcc1fa19e374fd8dd986b0d2af0d02377261d83c" + [[package]] name = "rustc_version" version = "0.2.3" @@ -999,9 +990,9 @@ checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49" [[package]] name = "sdio-host" -version = "0.5.0" +version = "0.9.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f93c025f9cfe4c388c328ece47d11a54a823da3b5ad0370b22d95ad47137f85a" +checksum = "b328e2cb950eeccd55b7f55c3a963691455dcd044cfb5354f0c5e68d2c2d6ee2" [[package]] name = "semver" @@ -1105,12 +1096,13 @@ dependencies = [ [[package]] name = "stm32-metapac" -version = "16.0.0" +version = "18.0.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "dc520f60f6653a32479a95b9180b33908f0cbbdf106609465ee7dea98f4f5b37" +checksum = "6fd8ec3a292a0d9fc4798416a61b21da5ae50341b2e7b8d12e662bf305366097" dependencies = [ "cortex-m", "cortex-m-rt", + "defmt 0.3.100", ] [[package]] diff --git a/Cargo.toml b/Cargo.toml index aa1d418..57f4e7e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -5,21 +5,21 @@ edition = "2021" [dependencies] # Change stm32h563zi to your chip name, if necessary. -embassy-stm32 = { version = "0.2.0", features = ["defmt", "stm32h533re", "memory-x", "time-driver-any", "exti", "unstable-pac"] } -embassy-sync = { version = "0.6.0", features = ["defmt"] } -embassy-executor = { version = "0.7.0", features = ["arch-cortex-m", "executor-thread", "task-arena-size-32768", "defmt"] } +embassy-stm32 = { version = "0.4.0", features = ["defmt", "stm32h533re", "memory-x", "time-driver-any", "exti", "unstable-pac"] } +embassy-sync = { version = "0.7.2", features = ["defmt"] } +embassy-executor = { version = "0.9.1", features = ["arch-cortex-m", "executor-thread", "defmt"] } embassy-time = { version = "0.4.0", features = ["defmt", "defmt-timestamp-uptime", "tick-hz-32_768"] } -embassy-embedded-hal = { version = "0.4.0", features = ["defmt"]} +embassy-embedded-hal = { version = "0.5.0", features = ["defmt"]} # embassy-net = { version = "0.7.0", features = ["defmt", "tcp", "dhcpv4", "medium-ethernet", "proto-ipv6"] } -embassy-usb = { version = "0.4.0", features = ["defmt"] } +embassy-usb = { version = "0.5.1", features = ["defmt"] } embassy-futures = { version = "0.1.0"} defmt = "1.0.1" defmt-rtt = "1.1.0" -bitflags = "2.9.4" +bitflags = "2.10.0" -postcard-rpc = {version = "0.11.15", features = ["embassy-usb-0_4-server", "defmt"]} +postcard-rpc = {version = "0.12.1", features = ["embassy-usb-0_5-server", "defmt"]} bioz-icd-rs = {path = "../bioz-icd-rs"} # libm = { version = "0.2.15" } diff --git a/src/adg2128.rs b/src/adg2128.rs index e8d098d..bb219fc 100644 --- a/src/adg2128.rs +++ b/src/adg2128.rs @@ -1,5 +1,5 @@ use embassy_embedded_hal::shared_bus::blocking::i2c::I2cDevice; -use embassy_stm32::{i2c::I2c, mode::Blocking}; +use embassy_stm32::{i2c::I2c, mode::Blocking, i2c::Master}; use embassy_sync::blocking_mutex::raw::NoopRawMutex; use embedded_hal_1::i2c::I2c as _; @@ -99,13 +99,13 @@ enum LDSW { } pub struct ADG2128 { - i2c: I2cDevice<'static, NoopRawMutex, I2c<'static, Blocking>>, + i2c: I2cDevice<'static, NoopRawMutex, I2c<'static, Blocking, Master>>, address: u8, states_xy: [u8; 12] } impl ADG2128 { - pub fn new(i2c: I2cDevice<'static, NoopRawMutex, I2c<'static, Blocking>>, address: u8) -> Self { + pub fn new(i2c: I2cDevice<'static, NoopRawMutex, I2c<'static, Blocking, Master>>, address: u8) -> Self { ADG2128 { i2c, address, states_xy: [0u8; 12]} } diff --git a/src/communication.rs b/src/communication.rs index fd49592..4a36559 100644 --- a/src/communication.rs +++ b/src/communication.rs @@ -12,9 +12,9 @@ use postcard_rpc::{ define_dispatch, header::VarHeader, server::{ - impls::embassy_usb_v0_4::{ + impls::embassy_usb_v0_5::{ dispatch_impl::{spawn_fn, WireRxBuf, WireRxImpl, WireSpawnImpl, WireStorage, WireTxImpl}, - PacketBuffers, + PacketBuffers, USB_FS_MAX_PACKET_SIZE, }, Dispatch, Server, Sender, SpawnContext, }, @@ -127,7 +127,7 @@ pub fn init_communication(usb_driver: Driver<'static, peripherals::USB>, impedan impedance_setup: impedance_setup, }; - let (device, tx_impl, rx_impl) = STORAGE.init(usb_driver, config, pbufs.tx_buf.as_mut_slice()); + let (device, tx_impl, rx_impl) = STORAGE.init(usb_driver, config, pbufs.tx_buf.as_mut_slice(), USB_FS_MAX_PACKET_SIZE); let dispatcher = MyApp::new(context, spawner.into()); let vkk = dispatcher.min_key_len(); let server: AppServer = Server::new( diff --git a/src/electrodes.rs b/src/electrodes.rs index 3fde9cf..276e4b4 100644 --- a/src/electrodes.rs +++ b/src/electrodes.rs @@ -1,12 +1,12 @@ use embassy_embedded_hal::shared_bus::blocking::i2c::I2cDevice; -use embassy_stm32::{i2c, mode::Blocking}; +use embassy_stm32::{i2c, mode::Blocking, i2c::Master}; use embassy_sync::blocking_mutex::NoopMutex; use static_cell::StaticCell; use core::cell::RefCell; use crate::{ad5940, adg2128::{GetX, SetX, SetY, State, ADG2128}}; -static I2C_BUS: StaticCell>>> = StaticCell::new(); +static I2C_BUS: StaticCell>>> = StaticCell::new(); #[derive(Copy, Clone)] #[allow(dead_code)] @@ -43,15 +43,21 @@ pub struct Electrodes { mux_2: ADG2128, } +const MUX_1_ADDRESS: u8 = 0b1110_000; +const MUX_2_ADDRESS: u8 = 0b1110_001; + impl Electrodes { - pub fn new(i2c: i2c::I2c<'static, Blocking>) -> Self { + pub fn new(mut i2c: i2c::I2c<'static, Blocking, Master>) -> Result { + if i2c.blocking_write(MUX_1_ADDRESS, &[0x00]).is_err() { return Err("Failed to communicate with MUX 1"); } + if i2c.blocking_write(MUX_2_ADDRESS, &[0x00]).is_err() { return Err("Failed to communicate with MUX 2"); } + let i2c_bus = NoopMutex::new(RefCell::new(i2c)); let i2c_bus = I2C_BUS.init(i2c_bus); - Electrodes { - mux_1: ADG2128::new(I2cDevice::new(i2c_bus), 0b1110_000), - mux_2: ADG2128::new(I2cDevice::new(i2c_bus), 0b1110_001), - } + Ok(Electrodes { + mux_1: ADG2128::new(I2cDevice::new(i2c_bus), MUX_1_ADDRESS), + mux_2: ADG2128::new(I2cDevice::new(i2c_bus), MUX_2_ADDRESS), + }) } pub fn set(&mut self, electrode: Electrode, ad5940_pin: AD5940Pin, state: State) { diff --git a/src/impedance.rs b/src/impedance.rs index dac2d4f..c919acb 100644 --- a/src/impedance.rs +++ b/src/impedance.rs @@ -17,6 +17,8 @@ use core::f32::consts::PI; use crate::ad5940::*; use crate::ad5940_registers::*; +use crate::electrodes::Electrodes; + use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, ImpedanceInitError, MeasurementPointSet, SweepImpedanceOutput}; use crate::icd_mapping::IntoDftnum; @@ -55,11 +57,12 @@ pub struct ImpedanceSetup { dsp_config: Option, pub running_mode: RunningMode, pub rtia_calibrated: RtiaCalibrated, + pub electrodes: Option, } impl ImpedanceSetup { pub fn new(ad5940: AD5940) -> Self { - ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None, rtia_calibrated: RtiaCalibrated::None } + ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None, rtia_calibrated: RtiaCalibrated::None, electrodes: None } } pub async fn init(&mut self) -> Result<(), Error> { @@ -120,6 +123,11 @@ impl ImpedanceSetup { let config_wgcon = WGCON::TYPESEL_SIN.bits(); self.ad5940.write_reg(Register::WGCON, config_wgcon).await.unwrap(); + // Configure GPIOs + // Make sure that GP0 (INT), GP1 (MULTIPLEXER CONTROL) and GP2 (LED) are set as output and are high + self.ad5940.write_reg(Register::GP0CON, 0b10 << 4 | 0b10 << 2 | 0b10).await.unwrap(); + self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap(); + Ok(()) } diff --git a/src/main.rs b/src/main.rs index f3f88cd..b662c50 100644 --- a/src/main.rs +++ b/src/main.rs @@ -8,7 +8,7 @@ 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::{spi, Config}; +use embassy_stm32::{i2c, spi, Config}; use embassy_stm32::time::Hertz; use {defmt_rtt as _, panic_probe as _}; @@ -19,11 +19,11 @@ use {defmt_rtt as _, panic_probe as _}; mod ad5940; use ad5940::AD5940; -// mod adg2128; -// use adg2128::State; +mod adg2128; +use adg2128::State; -// mod electrodes; -// use electrodes::{Electrodes, Electrode, AD5940Pin}; +mod electrodes; +use electrodes::{Electrodes, Electrode, AD5940Pin}; mod ad5940_registers; @@ -100,21 +100,29 @@ async fn main(spawner: Spawner) { // impedance_setup.lock().await.init_single_frequency_measurement().await; - // // Set up I2C for ADG2128 - // let i2c = i2c::I2c::new_blocking( - // p.I2C1, - // p.PB6, - // p.PB7, - // Hertz(400_000), - // i2c::Config::default() - // ); + // Set up I2C for ADG2128 + let mut i2c_config = i2c::Config::default(); + i2c_config.frequency = Hertz(400_000); + let i2c = i2c::I2c::new_blocking( + p.I2C1, + p.PB6, + p.PB7, + i2c_config + ); - // // Initialize electrodes - // let mut electrodes = Electrodes::new(i2c); - // electrodes.reset_all(); - // electrodes.set(Electrode::E1, AD5940Pin::CE0, State::ENABLED); - // electrodes.set(Electrode::E3, AD5940Pin::AIN1, State::ENABLED); - // // electrodes.set(Electrode::E12, AD5940Pin::RE0, State::ENABLED); + // Initialize electrodes + match Electrodes::new(i2c) { + Ok(mut e) => { + e.reset_all(); + e.set(Electrode::E1, AD5940Pin::CE0, State::ENABLED); + e.set(Electrode::E23, AD5940Pin::AIN1, State::ENABLED); + impedance_setup.lock().await.electrodes = Some(e); + }, + Err(e) => { + info!("Error initializing electrodes: {}", e); + impedance_setup.lock().await.electrodes = None; + } + }; // Turn on the green LED // ad5940.write_reg_raw(0x0000_0004, 1 << 1).await.unwrap();