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

Merge pull request #1 from hubaldv/multi

Browse Source 
Included multi tab for creating bode plots realtime.
This commit is contained in:
Hubald
2025-10-08 15:45:55 +02:00
committed by GitHub
parent b53e58ec31 a9bb854a2e
commit 3df0f4b098
7 changed files with 431 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
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",
]
@@ -959,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",

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"

55
src/ad5940.rs
View File

@@ -87,57 +87,57 @@ pub struct DspConfig {
}
impl DspConfig {
pub fn adc_mux_n(mut self, muxseln: MUXSELN) -> Self {
pub fn adc_mux_n(&mut self, muxseln: MUXSELN) -> &mut Self {
self.muxseln = Some(muxseln);
self
}
pub fn adc_mux_p(mut self, muxselp: MUXSELP) -> Self {
pub fn adc_mux_p(&mut self, muxselp: MUXSELP) -> &mut Self {
self.muxselp = Some(muxselp);
self
}
pub fn ctiacon(mut self, ctiacon: CTIACON) -> Self {
pub fn ctiacon(&mut self, ctiacon: CTIACON) -> &mut Self {
self.ctiacon = Some(ctiacon);
self
}
pub fn rtiacon(mut self, rtiacon: RTIACON) -> Self {
pub fn rtiacon(&mut self, rtiacon: RTIACON) -> &mut Self {
self.rtiacon = Some(rtiacon);
self
}
pub fn sinc3osr(mut self, sinc3osr: SINC3OSR) -> Self {
pub fn sinc3osr(&mut self, sinc3osr: SINC3OSR) -> &mut Self {
self.sinc3osr = Some(sinc3osr);
self
}
pub fn sinc2osr(mut self, sinc2osr: SINC2OSR) -> Self {
pub fn sinc2osr(&mut self, sinc2osr: SINC2OSR) -> &mut Self {
self.sinc2osr = Some(sinc2osr);
self
}
pub fn adcsamplerate(mut self, adcsamplerate: ADCSAMPLERATE) -> Self {
pub fn adcsamplerate(&mut self, adcsamplerate: ADCSAMPLERATE) -> &mut Self {
self.adcsamplerate = Some(adcsamplerate);
self
}
pub fn dftin_sel(mut self, dftin: DFTINSEL) -> Self {
pub fn dftin_sel(&mut self, dftin: DFTINSEL) -> &mut Self {
self.dftin = Some(dftin);
self
}
pub fn dftnum(mut self, dftnum: DFTNUM) -> Self {
pub fn dftnum(&mut self, dftnum: DFTNUM) -> &mut Self {
self.dftnum = Some(dftnum);
self
}
pub fn hanning(mut self, hanning: bool) -> Self {
pub fn hanning(&mut self, hanning: bool) -> &mut Self {
self.hanning = Some(hanning);
self
}
pub fn set_clks(mut self, fsys: u32, fadc: u32) -> Self {
pub fn set_clks(&mut self, fsys: u32, fadc: u32) -> &mut Self {
self.fsys = Some(fsys);
self.fadc = Some(fadc);
self.ratio_sys2adc_clk = Some(fsys as f32 / fadc as f32);
@@ -156,27 +156,27 @@ pub struct SramConfig {
}
impl SramConfig {
pub fn datafifosrcsel(mut self, datafifosrcsel: DATAFIFOSRCSEL) -> Self {
pub fn datafifosrcsel(&mut self, datafifosrcsel: DATAFIFOSRCSEL) -> &mut Self {
self.datafifosrcsel = Some(datafifosrcsel);
self
}
pub fn datafifoen(mut self, datafifoen: DATAFIFOEN) -> Self {
pub fn datafifoen(&mut self, datafifoen: DATAFIFOEN) -> &mut Self {
self.datafifoen = Some(datafifoen);
self
}
pub fn data_size(mut self, data_mem_size: DATA_MEM_SEL) -> Self {
pub fn data_size(&mut self, data_mem_size: DATA_MEM_SEL) -> &mut Self {
self.data_mem_size = Some(data_mem_size);
self
}
pub fn cmd_mode(mut self, cmd_mem_mode: CMDMEMMDE) -> Self {
pub fn cmd_mode(&mut self, cmd_mem_mode: CMDMEMMDE) -> &mut Self {
self.cmd_mem_mode = Some(cmd_mem_mode);
self
}
pub fn cmd_size(mut self, cmd_mem_size: CMD_MEM_SEL) -> Self {
pub fn cmd_size(&mut self, cmd_mem_size: CMD_MEM_SEL) -> &mut Self {
self.cmd_mem_size = Some(cmd_mem_size);
self
}
@@ -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(),
}
}
@@ -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
};
@@ -765,6 +765,17 @@ impl AD5940 {
Ok((data >> 16) & 0x7FF)
}
pub async fn clear_and_enable_fifo(&mut self) -> Result<(), Error> {
let mut fifocon = self.read_reg(Register::FIFOCON).await?;
fifocon = DATAFIFOEN::apply(fifocon, DATAFIFOEN::FIFOisReset as u32); // Disable FIFO
self.write_reg(Register::FIFOCON, fifocon).await?;
fifocon = DATAFIFOEN::apply(fifocon, DATAFIFOEN::Normal as u32); // Enable FIFO
self.write_reg(Register::FIFOCON, fifocon).await?;
Ok(())
}
pub async fn init_waveform(&mut self) -> Result<(), Error> {
// Set frequency
let freq: u32 = 1000;

164
src/communication.rs
View File

@@ -1,12 +1,13 @@
use defmt::info;
use embassy_executor::Spawner;
use embassy_sync::mutex::Mutex;
use embassy_sync::blocking_mutex::raw::{ThreadModeRawMutex, CriticalSectionRawMutex};
use embassy_sync::signal::Signal;
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::{PingEndpoint, GetUniqueIdEndpoint, SetGreenLedEndpoint, StartImpedanceEndpoint, StopImpedanceEndpoint, StartImpedance, ImpedanceOutputTopic, 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};
use crate::impedance::{ImpedanceSetupType, RunningMode, IMPEDANCE_CHANNEL_MULTI, IMPEDANCE_CHANNEL_SINGLE};
// Postcard RPC types
type AppDriver = usb::Driver<'static, peripherals::USB>;
@@ -63,13 +64,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;
@@ -159,22 +162,22 @@ pub async fn set_green_led_handler(_context: &mut Context, _header: VarHeader, r
LED_FREQUENCY_SIGNAL.signal(rqst);
}
static STOP: Mutex<ThreadModeRawMutex, bool> = Mutex::new(false);
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: SingleImpedanceStartRequest, 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).await;
let init_impedance_result = context.impedance_setup.lock().await.init_single_frequency_measurement(rqst.sinus_frequency, rqst.dft_number).await;
// Trigger the sequencer
context.impedance_setup.lock().await.start_measurement().await;
if sender
.reply::<StartImpedanceEndpoint>(header.seq_no, &())
.reply::<StartSingleImpedanceEndpoint>(header.seq_no, &init_impedance_result)
.await
.is_err()
{
@@ -183,31 +186,128 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
}
let mut seq: u8 = 0;
while !*STOP.lock().await {
loop {
let stop_fut = STOP.wait();
let recv_fut = IMPEDANCE_CHANNEL_SINGLE.receive();
let msg = IMPEDANCE_CHANNEL.receive().await;
if sender
.publish::<ImpedanceOutputTopic>(seq.into(), &msg)
.await
.is_err()
{
defmt::error!("Topic send error!");
break;
match select(stop_fut, recv_fut).await {
Either::First(_) => {
info!("Stop signal received.");
break;
}
Either::Second(msg) => {
if sender
.publish::<SingleImpedanceOutputTopic>(seq.into(), &msg)
.await
.is_err()
{
defmt::error!("Topic send error!");
break;
}
seq = seq.wrapping_add(1);
}
}
seq = seq.wrapping_add(1);
}
context.impedance_setup.lock().await.running = false;
context.impedance_setup.lock().await.running_mode = RunningMode::None;
info!("Impedance measurement stopped.");
*STOP.lock().await = false;
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 {
*STOP.lock().await = true;
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) {
STOP.signal(());
}
was_busy
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: MultiImpedanceStartRequest, sender: Sender<AppTx>) {
// 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 => {
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, &response)
.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::<MultiImpedanceOutputTopic>(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(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)
}

27
src/icd_mapping.rs Normal file
View File

@@ -0,0 +1,27 @@
use crate::ad5940_registers::DFTNUM;
use bioz_icd_rs::IcdDftNum;
// Map ICD types to register types
pub trait IntoDftnum {
fn into_dftnum(self) -> DFTNUM;
}
impl IntoDftnum for IcdDftNum {
fn into_dftnum(self) -> DFTNUM {
match self {
IcdDftNum::Num4 => DFTNUM::Num4,
IcdDftNum::Num8 => DFTNUM::Num8,
IcdDftNum::Num16 => DFTNUM::Num16,
IcdDftNum::Num32 => DFTNUM::Num32,
IcdDftNum::Num64 => DFTNUM::Num64,
IcdDftNum::Num128 => DFTNUM::Num128,
IcdDftNum::Num256 => DFTNUM::Num256,
IcdDftNum::Num512 => DFTNUM::Num512,
IcdDftNum::Num1024 => DFTNUM::Num1024,
IcdDftNum::Num2048 => DFTNUM::Num2048,
IcdDftNum::Num4096 => DFTNUM::Num4096,
IcdDftNum::Num8192 => DFTNUM::Num8192,
IcdDftNum::Num16384 => DFTNUM::Num16384,
}
}
}

148
src/impedance.rs
View File

@@ -10,22 +10,31 @@ use static_cell::StaticCell;
use crate::ad5940::*;
use crate::ad5940_registers::*;
use bioz_icd_rs::ImpedanceOutput;
use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, ImpedanceInitError, MeasurementPointSet, MultiImpedanceOutput};
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, MultiImpedanceOutput, 10> = 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(MeasurementPointSet),
}
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> {
@@ -51,7 +60,8 @@ impl ImpedanceSetup {
self.ad5940.apply_clk_config(&clk_config).await.unwrap();
// Set DSP configuration
let dsp_config = DspConfig::default()
let mut dsp_config = DspConfig::default();
dsp_config
.adc_mux_n(MUXSELN::HsTiaNeg)
.adc_mux_p(MUXSELP::HsTiaPos)
.ctiacon(CTIACON::C32)
@@ -68,7 +78,8 @@ impl ImpedanceSetup {
self.dsp_config = Some(dsp_config);
// Set SRAM configuration (cmd and data sram)
let sram_config = SramConfig::default()
let mut sram_config = SramConfig::default();
sram_config
.datafifosrcsel(DATAFIFOSRCSEL::DFT)
.datafifoen(DATAFIFOEN::Normal)
.data_size(DATA_MEM_SEL::Size2kB)
@@ -83,18 +94,28 @@ impl ImpedanceSetup {
Ok(())
}
pub async fn init_single_frequency_measurement(&mut self, frequency: u32) {
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();
// 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::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;
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();
info!("Sinus periods per DFT: {}", wait_time as f32 / dsp_config.fsys.unwrap() as f32 * frequency as f32);
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);
}
// Configure sequencer
@@ -148,6 +169,113 @@ impl ImpedanceSetup {
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<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();
// 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.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();
// 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;
Ok(periods_per_dft_vec)
}
pub async fn start_measurement(&mut self) {

84
src/main.rs
View File

@@ -8,15 +8,17 @@ 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;
use {defmt_rtt as _, panic_probe as _};
// use crate::ad5940::*;
// use crate::ad5940_registers::*;
use bioz_icd_rs::ImpedanceOutput;
use bioz_icd_rs::{MeasurementPointSet, MultiImpedanceOutput, SingleImpedanceOutput};
mod ad5940;
use ad5940::AD5940;
@@ -35,11 +37,13 @@ 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};
mod icd_mapping;
bind_interrupts!(struct Irqs {
USB_DRD_FS => usb::InterruptHandler<peripherals::USB>;
});
@@ -181,31 +185,77 @@ 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(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);
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);
// Log
// info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase);
let data = SingleImpedanceOutput {
magnitude: result.magnitude,
phase: result.phase,
};
let data = ImpedanceOutput {
magnitude: result.magnitude,
phase: result.phase,
};
IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok();
}
}
IMPEDANCE_CHANNEL.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;
}
}
}
}