From 4e2f389d214d28766d1b681f4d40ef683d09e6a2 Mon Sep 17 00:00:00 2001 From: Hubald Verzijl Date: Wed, 15 Oct 2025 18:16:26 +0200 Subject: [PATCH] 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 } -}