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

Included bode plot measurements, including different number of points.

Browse Source
This commit is contained in:
Hubald Verzijl
2025-10-08 15:40:17 +02:00
parent ef0cecced7
commit a9bb854a2e
5 changed files with 145 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
Cargo.lock generated
View File

@@ -972,18 +972,28 @@ checksum = "388a1df253eca08550bef6c72392cfe7c30914bf41df5269b68cbd6ff8f570a3"
[[package]]
name = "serde"
version = "1.0.219"
version = "1.0.228"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "5f0e2c6ed6606019b4e29e69dbaba95b11854410e5347d525002456dbbb786b6"
checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e"
dependencies = [
"serde_core",
"serde_derive",
]
[[package]]
name = "serde_core"
version = "1.0.228"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "41d385c7d4ca58e59fc732af25c3983b67ac852c1a25000afe1175de458b67ad"
dependencies = [
"serde_derive",
]
[[package]]
name = "serde_derive"
version = "1.0.219"
version = "1.0.228"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "5b0276cf7f2c73365f7157c8123c21cd9a50fbbd844757af28ca1f5925fc2a00"
checksum = "d540f220d3187173da220f885ab66608367b6574e925011a9353e4badda91d79"
dependencies = [
"proc-macro2",
"quote",

8
src/ad5940.rs
View File

@@ -473,7 +473,7 @@ impl AD5940 {
}
pub async fn sequencer_calculate_wait_time(&mut self, config: &DspConfig) -> Option<u32> {
let mut wait_time = 0;
let mut wait_time = 1;
let sinc3_table = [5, 4, 2];
let sinc2_table = [22, 44, 89, 178, 267, 533, 640, 667, 800, 889, 1067, 1333];
@@ -481,20 +481,20 @@ impl AD5940 {
match config.dftin {
Some(DFTINSEL::Sinc2) => {
if let Some(sinc3osr) = config.sinc3osr {
wait_time += sinc3_table[sinc3osr as usize];
wait_time *= sinc3_table[sinc3osr as usize];
} else {
return None; // Sinc2 requires sinc3osr to be set
};
if let Some(sinc2osr) = config.sinc2osr {
wait_time += sinc2_table[sinc2osr as usize];
wait_time *= sinc2_table[sinc2osr as usize];
} else {
return None; // Sinc2 requires sinc2osr to be set
};
}
Some(DFTINSEL::GainOffset) => {
if let Some(sinc3osr) = config.sinc3osr {
wait_time += sinc3_table[sinc3osr as usize];
wait_time *= sinc3_table[sinc3osr as usize];
} else {
return None; // Sinc2 requires sinc3osr to be set
};

55
src/communication.rs
View File

@@ -7,6 +7,7 @@ use embassy_futures::select::{select, Either};
use embassy_stm32::usb::Driver;
use embassy_stm32::{peripherals, uid, usb};
use heapless::Vec;
use postcard_rpc::{
define_dispatch,
header::VarHeader,
@@ -19,9 +20,9 @@ use postcard_rpc::{
},
};
use bioz_icd_rs::{GetUniqueIdEndpoint, PingEndpoint, SetGreenLedEndpoint, SingleImpedanceOutput, SingleImpedanceOutputTopic, StartMultiImpedance, StartMultiImpedanceEndpoint, StartSingleImpedance, StartSingleImpedanceEndpoint, StopMultiImpedanceEndpoint, StopSingleImpedanceEndpoint, MultiImpedanceOutputTopic28, MultiImpedanceOutput28, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST};
use bioz_icd_rs::{GetUniqueIdEndpoint, MeasurementPointSet, MultiImpedanceOutputTopic, MultiImpedanceResult, MultiImpedanceStartRequest, PingEndpoint, SetGreenLedEndpoint, SingleImpedanceOutputTopic, SingleImpedanceStartRequest, StartMultiImpedanceEndpoint, StartSingleImpedanceEndpoint, StopMultiImpedanceEndpoint, StopSingleImpedanceEndpoint, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST};
use crate::impedance::{ImpedanceSetupType, IMPEDANCE_CHANNEL_SINGLE, IMPEDANCE_CHANNEL_MULTI, RunningMode};
use crate::impedance::{ImpedanceSetupType, RunningMode, IMPEDANCE_CHANNEL_MULTI, IMPEDANCE_CHANNEL_SINGLE};
// Postcard RPC types
type AppDriver = usb::Driver<'static, peripherals::USB>;
@@ -164,7 +165,7 @@ pub async fn set_green_led_handler(_context: &mut Context, _header: VarHeader, r
static STOP: Signal<CriticalSectionRawMutex, ()> = Signal::new();
#[embassy_executor::task]
pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: StartSingleImpedance, sender: Sender<AppTx>) {
pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: SingleImpedanceStartRequest, sender: Sender<AppTx>) {
info!("Start impedance measurement at {:?} Hz.", rqst.sinus_frequency);
// Mark the impedance setup as running
@@ -216,24 +217,54 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
pub async fn stop_single_impedance_handler(context: &mut Context, _header: VarHeader, _rqst: ()) -> bool {
info!("Stop impedance measurement");
let was_busy = context.impedance_setup.lock().await.running_mode;
if was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency {
if was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) {
STOP.signal(());
}
was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency
was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen)
}
#[embassy_executor::task]
pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: StartMultiImpedance, sender: Sender<AppTx>) {
pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: MultiImpedanceStartRequest, sender: Sender<AppTx>) {
// Mark the impedance setup as running
context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency;
context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency(rqst.points);
// Init the sequencer
context.impedance_setup.lock().await.init_multi_frequency_measurement(rqst.dft_number, rqst.number_of_points).await;
let response = match rqst.points {
MeasurementPointSet::Eight => {
const SIZE: usize = 8;
context
.impedance_setup
.lock()
.await
.init_multi_frequency_measurement::<SIZE>(rqst.points)
.await
.map(|periods| MultiImpedanceResult {
points: rqst.points,
periods_per_dft_8: periods,
periods_per_dft_18: Vec::new(),
})
}
MeasurementPointSet::Eighteen => {
const SIZE: usize = 18;
context
.impedance_setup
.lock()
.await
.init_multi_frequency_measurement::<SIZE>(rqst.points)
.await
.map(|periods| MultiImpedanceResult {
points: rqst.points,
periods_per_dft_8: Vec::new(),
periods_per_dft_18: periods,
})
}
};
// Trigger the sequencer
context.impedance_setup.lock().await.start_measurement().await;
if sender
.reply::<StartMultiImpedanceEndpoint>(header.seq_no, &())
.reply::<StartMultiImpedanceEndpoint>(header.seq_no, &response)
.await
.is_err()
{
@@ -255,7 +286,7 @@ pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader,
}
Either::Second(msg) => {
if sender
.publish::<MultiImpedanceOutputTopic28>(seq.into(), &msg)
.publish::<MultiImpedanceOutputTopic>(seq.into(), &msg)
.await
.is_err()
{
@@ -275,8 +306,8 @@ pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader,
pub async fn stop_multi_impedance_handler(context: &mut Context, _header: VarHeader, _rqst: ()) -> bool {
info!("Stop impedance measurement");
let was_busy = context.impedance_setup.lock().await. running_mode;
if was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency {
if was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) {
STOP.signal(());
}
was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency
was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen)
}

71
src/impedance.rs
View File

@@ -1,5 +1,3 @@
use bioz_icd_rs::MultiImpedanceOutput28;
use bioz_icd_rs::NumberOfPoints;
use defmt::{info, error};
use embassy_stm32::spi::Error;
@@ -12,11 +10,11 @@ use static_cell::StaticCell;
use crate::ad5940::*;
use crate::ad5940_registers::*;
use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, InitImpedanceError};
use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, ImpedanceInitError, MeasurementPointSet, MultiImpedanceOutput};
use crate::icd_mapping::IntoDftnum;
pub static IMPEDANCE_CHANNEL_SINGLE: Channel<ThreadModeRawMutex, SingleImpedanceOutput, 2000> = Channel::new();
pub static IMPEDANCE_CHANNEL_MULTI: Channel<ThreadModeRawMutex, MultiImpedanceOutput28, 50> = Channel::new();
pub static IMPEDANCE_CHANNEL_MULTI: Channel<ThreadModeRawMutex, MultiImpedanceOutput, 10> = Channel::new();
pub type ImpedanceSetupType = Mutex<ThreadModeRawMutex, ImpedanceSetup>;
pub static IMPEDANCE_SETUP: StaticCell<ImpedanceSetupType> = StaticCell::new();
@@ -25,7 +23,7 @@ pub static IMPEDANCE_SETUP: StaticCell<ImpedanceSetupType> = StaticCell::new();
pub enum RunningMode {
None,
SingleFrequency,
MultiFrequency,
MultiFrequency(MeasurementPointSet),
}
pub struct ImpedanceSetup {
@@ -96,7 +94,7 @@ impl ImpedanceSetup {
Ok(())
}
pub async fn init_single_frequency_measurement(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result<f32, InitImpedanceError> {
pub async fn init_single_frequency_measurement(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result<f32, ImpedanceInitError> {
// Reset FIFO
self.ad5940.clear_and_enable_fifo().await.unwrap();
@@ -117,7 +115,7 @@ impl ImpedanceSetup {
info!("Sinus periods per DFT: {}", sinus_periods_per_dft);
} else {
error!("DSP configuration not set, cannot calculate wait time");
return Err(InitImpedanceError::DSPNotSet);
return Err(ImpedanceInitError::DSPNotSet);
}
// Configure sequencer
@@ -175,31 +173,58 @@ impl ImpedanceSetup {
Ok(sinus_periods_per_dft)
}
pub async fn init_multi_frequency_measurement(&mut self, dft_number: IcdDftNum, number_of_points: NumberOfPoints) {
pub async fn init_multi_frequency_measurement<const N: usize>(&mut self, number_of_points: MeasurementPointSet) -> Result<heapless::Vec<f32, N>, ImpedanceInitError> {
// Create vector to store the periods per DFT for each frequency
let mut periods_per_dft_vec = heapless::Vec::<f32, N>::new();
// Reset FIFO
self.ad5940.clear_and_enable_fifo().await.unwrap();
// Set DFT number
self.dsp_config.as_mut().unwrap().dftnum(dft_number.into_dftnum());
self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap();
// Configure GPIOs
self.ad5940.write_reg(Register::GP0CON, 0b10 << 4 | 0b10 << 2 | 0b10).await.unwrap();
self.ad5940.write_reg(Register::SYNCEXTDEVICE, 0b111).await.unwrap();
// Calculate wait time between measurement start and stop
let mut wait_time = 0;
if let Some(dsp_config) = &self.dsp_config {
wait_time = self.ad5940.sequencer_calculate_wait_time(dsp_config).await.unwrap();
// info!("Sinus periods per DFT: {}", wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32);
} else {
error!("DSP configuration not set, cannot calculate wait time");
}
// Configure sequencer
self.ad5940.sequencer_enable(true).await;
for &frequency in number_of_points.values() {
// Set DFT number based on the frequency
for &frequency in number_of_points.values() {
// Determine wait time
let selected_dft_num = match frequency {
f if f < 10.0 => DFTNUM::Num16384,
f if f < 100.0 => DFTNUM::Num16384,
f if f < 250.0 => DFTNUM::Num8192,
f if f < 1_000.0 => DFTNUM::Num4096,
f if f < 2_500.0 => DFTNUM::Num2048,
f if f < 10_000.0 => DFTNUM::Num1024,
f if f < 25_000.0 => DFTNUM::Num512,
f if f < 100_000.0 => DFTNUM::Num256,
_ => DFTNUM::Num128,
};
let wait_time: u32;
if let Some(dsp_config) = &mut self.dsp_config {
dsp_config
.dftnum(selected_dft_num);
// Update DFTNUM
let mut current = self.ad5940.read_reg(Register::DFTCON).await.unwrap();
current = DFTNUM::apply(current, selected_dft_num as u32);
self.ad5940.write_reg(Register::DFTCON, current).await.unwrap();
wait_time = self.ad5940.sequencer_calculate_wait_time(&dsp_config).await.unwrap();
let periods_per_dft = wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32;
periods_per_dft_vec.push(periods_per_dft).unwrap();
info!("{}Hz: Sinus periods per DFT: {}", frequency, periods_per_dft);
} else {
error!("DSP configuration not set, cannot calculate wait time");
return Err(ImpedanceInitError::DSPNotSet);
}
// Set frequency
self.ad5940.wgfcw(frequency as u32).await;
let wg_amplitude = 2047; // 2047 is the maximum amplitude for a 12-bit DAC --> 1.62V peak-to-peak
self.ad5940.write_reg(Register::WGAMPLITUDE, wg_amplitude).await.unwrap();
@@ -249,6 +274,8 @@ impl ImpedanceSetup {
self.ad5940.sequencer_info_configure(0, self.ad5940.seq_len, start_address).await;
self.start_measurement().await;
Ok(periods_per_dft_vec)
}
pub async fn start_measurement(&mut self) {

62
src/main.rs
View File

@@ -1,14 +1,14 @@
#![no_std]
#![no_main]
use defmt::{info, warn};
use defmt::info;
use embassy_executor::Spawner;
use embassy_stm32::exti::ExtiInput;
use embassy_sync::mutex::Mutex;
use embassy_time::{Timer, Duration};
use embassy_futures::{select::select, select::Either};
use embassy_stm32::gpio::{Level, Output, Speed};
use embassy_stm32::{i2c, spi, Config};
use embassy_stm32::{spi, Config};
use embassy_stm32::time::Hertz;
use heapless::Vec;
@@ -18,7 +18,7 @@ use {defmt_rtt as _, panic_probe as _};
// use crate::ad5940::*;
// use crate::ad5940_registers::*;
use bioz_icd_rs::{MultiImpedanceOutput28, SingleImpedanceOutput};
use bioz_icd_rs::{MeasurementPointSet, MultiImpedanceOutput, SingleImpedanceOutput};
mod ad5940;
use ad5940::AD5940;
@@ -185,12 +185,12 @@ async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance_set
let mut impedance_setup = impedance_setup.lock().await;
// Trigger the sequencer again
if impedance_setup.running_mode == impedance::RunningMode::SingleFrequency || impedance_setup.running_mode == impedance::RunningMode::MultiFrequency {
if impedance_setup.running_mode == impedance::RunningMode::SingleFrequency || impedance_setup.running_mode == impedance::RunningMode::MultiFrequency(MeasurementPointSet::Eight) || impedance_setup.running_mode == impedance::RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) {
impedance_setup.start_measurement().await;
}
// Read the FIFO count
let count = impedance_setup.get_fifo_count().await.unwrap();
let count = impedance_setup.get_fifo_count().await.unwrap() as usize;
// info!("FIFOCNTSTA: {}", count);
match impedance_setup.running_mode {
@@ -216,34 +216,42 @@ async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance_set
IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok();
}
}
impedance::RunningMode::MultiFrequency => {
if count >= 112 {
let mut data = [0u32; 112];
impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap();
let mut impedance_output = MultiImpedanceOutput28 {
magnitudes: Vec::new(),
phases: Vec::new(),
impedance::RunningMode::MultiFrequency(points) => {
// Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz)
let required_count = points.len() * 4;
if count >= required_count {
// Use stack-allocated array
let mut data: [u32; 72] = [0; 72]; // max needed size (18*4=72)
let data_slice = &mut data[..required_count];
impedance_setup.read_fifo(data_slice).await.unwrap();
// Output structure
let mut impedance_output = MultiImpedanceOutput {
points,
magnitudes_8: Vec::new(),
phases_8: Vec::new(),
magnitudes_18: Vec::new(),
phases_18: Vec::new(),
};
// Take 4 samples for each frequency point
for chunk in data.chunks(4) {
// Take 4 samples per frequency point
for chunk in data_slice.chunks(4) {
let result = calculate_impedance(chunk.try_into().unwrap());
// Store the results
impedance_output.magnitudes.push(result.magnitude).ok();
impedance_output.phases.push(result.phase).ok();
match points {
MeasurementPointSet::Eight => {
impedance_output.magnitudes_8.push(result.magnitude).ok();
impedance_output.phases_8.push(result.phase).ok();
}
MeasurementPointSet::Eighteen => {
impedance_output.magnitudes_18.push(result.magnitude).ok();
impedance_output.phases_18.push(result.phase).ok();
}
}
}
// Log
// for mags in impedance_output.magnitudes.iter() {
// info!("Mag: {}", mags);
// }
// for phases in impedance_output.phases.iter() {
// info!("Phase: {}", phases);
// }
IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok();
}
continue;