LUMZ-bioz/bioz-firmware-rs
1
0
Fork 0
mirror of https://github.com/hubaldv/bioz-firmware-rs.git synced 2025-12-06 05:01:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

Moved readout to impedance.rs, changed frequency type to u32.

Browse Source
This commit is contained in:
Hubald Verzijl
2025-10-15 18:16:26 +02:00
parent 3df0f4b098
commit 4e2f389d21
4 changed files with 183 additions and 183 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
Cargo.lock generated
View File

@@ -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]]

4
Cargo.toml
View File

@@ -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" }

165
src/impedance.rs
View File

@@ -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<ThreadModeRawMutex, SingleImpedanceOutput, 2000> = Channel::new();
pub static IMPEDANCE_CHANNEL_MULTI: Channel<ThreadModeRawMutex, MultiImpedanceOutput, 10> = Channel::new();
pub static IMPEDANCE_CHANNEL_SINGLE: Channel<ThreadModeRawMutex, SingleImpedanceOutput, 50> = Channel::new();
pub static IMPEDANCE_CHANNEL_MULTI: Channel<ThreadModeRawMutex, MultiImpedanceOutput, 5> = Channel::new();
pub type ImpedanceSetupType = Mutex<ThreadModeRawMutex, ImpedanceSetup>;
pub static IMPEDANCE_SETUP: StaticCell<ImpedanceSetupType> = 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 }
}

148
src/main.rs
View File

@@ -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 }
}