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

Added multi frequency measurement mode.

Browse Source
This commit is contained in:
Hubald Verzijl
2025-10-07 10:19:26 +02:00
parent d150c70700
commit 50e1894edf
6 changed files with 235 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
Cargo.lock generated
View File

@@ -74,7 +74,7 @@ dependencies = [
"embassy-time",
"embassy-usb",
"embedded-hal 1.0.0",
"heapless 0.8.0",
"heapless 0.9.1",
"libm",
"panic-probe",
"postcard-rpc",
@@ -85,6 +85,7 @@ dependencies = [
name = "bioz-icd-rs"
version = "0.1.0"
dependencies = [
"heapless 0.9.1",
"postcard-rpc",
"postcard-schema",
"serde",
@@ -714,6 +715,17 @@ dependencies = [
"stable_deref_trait",
]
[[package]]
name = "heapless"
version = "0.9.1"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "b1edcd5a338e64688fbdcb7531a846cfd3476a54784dcb918a0844682bc7ada5"
dependencies = [
"hash32 0.3.1",
"serde",
"stable_deref_trait",
]
[[package]]
name = "ident_case"
version = "1.0.1"
@@ -830,9 +842,9 @@ dependencies = [
[[package]]
name = "postcard-rpc"
version = "0.11.13"
version = "0.11.15"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "e1af23d87c9a8308bbfaae655ac770ec10517e6448fa5e0b50838a36e5d860b9"
checksum = "c7e1944dfb9859e440511700c442edce3eacd5862f90f5a9997d004bd3553f3b"
dependencies = [
"defmt 0.3.100",
"embassy-executor",
@@ -852,10 +864,11 @@ dependencies = [
[[package]]
name = "postcard-schema"
version = "0.2.4"
version = "0.2.5"
source = "registry+https://github.com/rust-lang/crates.io-index"
checksum = "f5a13d8b1f8b3473b45c2c779b97c18c260ac6458eb045d4be75df8087784400"
checksum = "9475666d89f42231a0a57da32d5f6ca7f9b5cd4c335ea1fe8f3278215b7a21ff"
dependencies = [
"heapless 0.9.1",
"postcard-derive",
"serde",
]

4
Cargo.toml
View File

@@ -7,7 +7,7 @@ edition = "2021"
# 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", "defmt"] }
embassy-executor = { version = "0.7.0", features = ["arch-cortex-m", "executor-thread", "task-arena-size-32768", "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-net = { version = "0.7.0", features = ["defmt", "tcp", "dhcpv4", "medium-ethernet", "proto-ipv6"] }
@@ -24,7 +24,7 @@ bioz-icd-rs = {path = "../bioz-icd-rs"}
libm = { version = "0.2.15" }
heapless = { version = "0.8.0" }
heapless = { version = "0.9.1" }
cortex-m = { version = "0.7.7", features = ["inline-asm", "critical-section-single-core"] }
cortex-m-rt = "0.7.5"

4
src/ad5940.rs
View File

@@ -204,7 +204,7 @@ pub struct AD5940 {
rst: Output<'static>,
seq_enabled: bool,
pub seq_len: usize,
pub seq_buffer: [u32; 25],
pub seq_buffer: [u32; 500],
pub seq_gen_db: LinearMap<Register, u32, 16>,
}
@@ -216,7 +216,7 @@ impl AD5940 {
rst,
seq_enabled: false,
seq_len: 0,
seq_buffer: [0; 25],
seq_buffer: [0; 500],
seq_gen_db: LinearMap::new(),
}
}

97
src/communication.rs
View File

@@ -19,9 +19,9 @@ use postcard_rpc::{
},
};
use bioz_icd_rs::{PingEndpoint, GetUniqueIdEndpoint, SetGreenLedEndpoint, StartImpedanceEndpoint, StopImpedanceEndpoint, StartImpedance, ImpedanceOutputTopic, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST};
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 crate::impedance::{ImpedanceSetupType, IMPEDANCE_CHANNEL};
use crate::impedance::{ImpedanceSetupType, IMPEDANCE_CHANNEL_SINGLE, IMPEDANCE_CHANNEL_MULTI, RunningMode};
// Postcard RPC types
type AppDriver = usb::Driver<'static, peripherals::USB>;
@@ -63,13 +63,15 @@ define_dispatch! {
endpoints: {
list: ENDPOINT_LIST;
| EndpointTy | kind | handler |
| ---------- | ---- | ------- |
| PingEndpoint | blocking | ping_handler |
| GetUniqueIdEndpoint | blocking | get_unique_id_handler |
| SetGreenLedEndpoint | async | set_green_led_handler |
| StartImpedanceEndpoint | spawn | start_single_impedance_handler |
| StopImpedanceEndpoint | async | stop_single_impedance_handler |
| EndpointTy | kind | handler |
| ---------- | ---- | ------- |
| PingEndpoint | blocking | ping_handler |
| 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 |
};
topics_in: {
list: TOPICS_IN_LIST;
@@ -162,11 +164,11 @@ 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: StartImpedance, sender: Sender<AppTx>) {
pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: StartSingleImpedance, sender: Sender<AppTx>) {
info!("Start impedance measurement at {:?} Hz.", rqst.sinus_frequency);
// Mark the impedance setup as running
context.impedance_setup.lock().await.running = true;
context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency;
// Init the sequencer
context.impedance_setup.lock().await.init_single_frequency_measurement(rqst.sinus_frequency, rqst.dft_number).await;
@@ -174,7 +176,7 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
context.impedance_setup.lock().await.start_measurement().await;
if sender
.reply::<StartImpedanceEndpoint>(header.seq_no, &())
.reply::<StartSingleImpedanceEndpoint>(header.seq_no, &())
.await
.is_err()
{
@@ -185,7 +187,7 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
let mut seq: u8 = 0;
loop {
let stop_fut = STOP.wait();
let recv_fut = IMPEDANCE_CHANNEL.receive();
let recv_fut = IMPEDANCE_CHANNEL_SINGLE.receive();
match select(stop_fut, recv_fut).await {
Either::First(_) => {
@@ -194,7 +196,7 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
}
Either::Second(msg) => {
if sender
.publish::<ImpedanceOutputTopic>(seq.into(), &msg)
.publish::<SingleImpedanceOutputTopic>(seq.into(), &msg)
.await
.is_err()
{
@@ -206,16 +208,75 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
}
}
context.impedance_setup.lock().await.running = false;
context.impedance_setup.lock().await.running_mode = RunningMode::None;
info!("Impedance measurement stopped.");
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;
if was_busy {
let was_busy = context.impedance_setup.lock().await.running_mode;
if was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency {
STOP.signal(());
}
was_busy
was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency
}
#[embassy_executor::task]
pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader, rqst: StartMultiImpedance, sender: Sender<AppTx>) {
// Mark the impedance setup as running
context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency;
// Init the sequencer
context.impedance_setup.lock().await.init_multi_frequency_measurement(rqst.dft_number, rqst.number_of_points).await;
// Trigger the sequencer
context.impedance_setup.lock().await.start_measurement().await;
if sender
.reply::<StartMultiImpedanceEndpoint>(header.seq_no, &())
.await
.is_err()
{
defmt::error!("Failed to reply, stopping accel");
return;
}
info!("Start multi impedance measurement.");
let mut seq: u8 = 0;
loop {
let stop_fut = STOP.wait();
let recv_fut = IMPEDANCE_CHANNEL_MULTI.receive();
match select(stop_fut, recv_fut).await {
Either::First(_) => {
info!("Stop signal received.");
break;
}
Either::Second(msg) => {
if sender
.publish::<MultiImpedanceOutputTopic28>(seq.into(), &msg)
.await
.is_err()
{
defmt::error!("Topic send error!");
break;
}
seq = seq.wrapping_add(1);
}
}
}
context.impedance_setup.lock().await.running_mode = RunningMode::None;
info!("Impedance measurement stopped.");
STOP.reset();
}
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 {
STOP.signal(());
}
was_busy == RunningMode::SingleFrequency || was_busy == RunningMode::MultiFrequency
}

89
src/impedance.rs
View File

@@ -1,3 +1,5 @@
use bioz_icd_rs::MultiImpedanceOutput28;
use bioz_icd_rs::NumberOfPoints;
use defmt::{info, error};
use embassy_stm32::spi::Error;
@@ -10,23 +12,31 @@ use static_cell::StaticCell;
use crate::ad5940::*;
use crate::ad5940_registers::*;
use bioz_icd_rs::{ImpedanceOutput, IcdDftNum};
use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum};
use crate::icd_mapping::IntoDftnum;
pub static IMPEDANCE_CHANNEL: Channel<ThreadModeRawMutex, ImpedanceOutput, 2000> = Channel::new();
pub static IMPEDANCE_CHANNEL_SINGLE: Channel<ThreadModeRawMutex, SingleImpedanceOutput, 2000> = Channel::new();
pub static IMPEDANCE_CHANNEL_MULTI: Channel<ThreadModeRawMutex, MultiImpedanceOutput28, 50> = Channel::new();
pub type ImpedanceSetupType = Mutex<ThreadModeRawMutex, ImpedanceSetup>;
pub static IMPEDANCE_SETUP: StaticCell<ImpedanceSetupType> = StaticCell::new();
#[derive(PartialEq, Copy, Clone)]
pub enum RunningMode {
None,
SingleFrequency,
MultiFrequency,
}
pub struct ImpedanceSetup {
ad5940: AD5940,
dsp_config: Option<DspConfig>,
pub running: bool,
pub running_mode: RunningMode,
}
impl ImpedanceSetup {
pub fn new(ad5940: AD5940) -> Self {
ImpedanceSetup { ad5940, dsp_config: None, running: false }
ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None }
}
pub async fn init(&mut self) -> Result<(), Error> {
@@ -157,7 +167,78 @@ impl ImpedanceSetup {
self.start_measurement().await;
}
pub async fn init_multi_frequency_measurement(&mut self, dft_number: IcdDftNum, number_of_points: NumberOfPoints) {
// 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() {
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();
// 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();
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;
// Rz
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();
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;
}
pub async fn start_measurement(&mut self) {
self.ad5940.sequencer_trigger(0).await;

64
src/main.rs
View File

@@ -11,12 +11,14 @@ use embassy_stm32::gpio::{Level, Output, Speed};
use embassy_stm32::{i2c, 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::ImpedanceOutput;
use bioz_icd_rs::{MultiImpedanceOutput28, SingleImpedanceOutput};
mod ad5940;
use ad5940::AD5940;
@@ -35,7 +37,7 @@ use embassy_stm32::{bind_interrupts, peripherals, usb};
mod communication;
use communication::{init_communication, LED_FREQUENCY_SIGNAL};
use impedance::IMPEDANCE_CHANNEL;
use impedance::{IMPEDANCE_CHANNEL_SINGLE, IMPEDANCE_CHANNEL_MULTI};
mod impedance;
use impedance::{ImpedanceSetup, ImpedanceSetupType, IMPEDANCE_SETUP};
@@ -183,31 +185,63 @@ 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 {
if impedance_setup.running_mode == impedance::RunningMode::SingleFrequency || impedance_setup.running_mode == impedance::RunningMode::MultiFrequency {
impedance_setup.start_measurement().await;
}
// Read the FIFO count
let count = impedance_setup.get_fifo_count().await.unwrap();
// info!("FIFOCNTSTA: {}", count);
info!("FIFOCNTSTA: {}", count);
if count >= 4 {
let mut data: [u32; 4] = [0; 4];
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();
impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap();
let result = calculate_impedance(data);
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();
}
}
// Log
// info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase);
impedance::RunningMode::MultiFrequency => {
if count >= 112 {
let mut data = [0u32; 112];
impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap();
let data = ImpedanceOutput {
magnitude: result.magnitude,
phase: result.phase,
};
let mut impedance_output = MultiImpedanceOutput28 {
magnitudes: Vec::new(),
phases: Vec::new(),
};
IMPEDANCE_CHANNEL.try_send(data).ok();
// Take 4 samples for each frequency point
for chunk in data.chunks(4) {
let result = calculate_impedance([chunk[0], chunk[1], chunk[2], chunk[3]]);
// Store the results
impedance_output.magnitudes.push(result.magnitude).ok();
impedance_output.phases.push(result.phase).ok();
}
// Log
// info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase);
IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok();
}
continue;
}
}
}
}