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

Added 4 lead-option for single frequency measurement.

Browse Source
This commit is contained in:
Hubald Verzijl
2025-11-03 17:19:10 +01:00
parent 0859de24fb
commit 6358882a4c
4 changed files with 307 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
src/ad5940.rs
View File

@@ -71,6 +71,7 @@ impl SwitchConfig {
#[allow(dead_code)]
#[derive(Default)]
pub struct DspConfig {
pub gnpgain: Option<GNPGAIN>,
muxseln: Option<MUXSELN>,
muxselp: Option<MUXSELP>,
ctiacon: Option<CTIACON>,
@@ -87,6 +88,11 @@ pub struct DspConfig {
}
impl DspConfig {
pub fn gnpgain(&mut self, gnpgain: GNPGAIN) -> &mut Self {
self.gnpgain = Some(gnpgain);
self
}
pub fn adc_mux_n(&mut self, muxseln: MUXSELN) -> &mut Self {
self.muxseln = Some(muxseln);
self
@@ -592,6 +598,9 @@ impl AD5940 {
// ADCCON
let mut current = self.read_reg(Register::ADCCON).await?;
if let Some(gnpgain) = config.gnpgain {
current = GNPGAIN::apply(current, gnpgain as u32);
}
if let Some(muxseln) = config.muxseln {
current = MUXSELN::apply(current, muxseln as u32);
}
@@ -855,6 +864,8 @@ pub enum Register {
OSCCON = 0x0000_0A10, // Oscillator Control Register
SEQTIMEOUT = 0x0000_2068, // Sequencer Timeout Counter Register
SEQCRC = 0x0000_2060, // Sequencer CRC Value Register
DFTREAL = 0x0000_2078, // DFT Result, Real Device Register
DFTIMAG = 0x0000_207C, // DFT Result, Imaginary Device Register
DATAFIFOTHRES = 0x0000_21E0, // Data FIFO Threshold Register
SYNCEXTDEVICE = 0x0000_2054, // Sync External Device Register
GP0CON = 0x0000_0000, // GPIO Port 0 Configuration Register

27
src/ad5940_registers.rs
View File

@@ -130,6 +130,25 @@ impl RegisterField for DMUXCON {
const MASK: u32 = 0b1111;
}
#[allow(dead_code)]
#[repr(u32)]
#[derive(Copy, Clone)]
pub enum GNPGAIN {
Gain1 = 0b0,
Gain1_5 = 0b1,
Gain2 = 0b10,
Gain4 = 0b11,
Gain9 = 0b100,
}
impl RegisterField for GNPGAIN {
fn reset() -> Self {
GNPGAIN::Gain1
}
const BIT_OFFSET: u32 = 16;
const MASK: u32 = 0b111;
}
#[allow(dead_code)]
#[repr(u32)]
#[derive(Copy, Clone)]
@@ -139,6 +158,8 @@ pub enum MUXSELN
HsTiaNeg = 0b00001,
LpTiaNeg = 0b00010,
AIN1 = 0b00101,
AIN3 = 0b00111,
ExciNNode= 0b10100,
}
impl RegisterField for MUXSELN {
@@ -156,6 +177,10 @@ pub enum MUXSELP {
Floating = 0b00000,
HsTiaPos = 0b00001,
AIN1 = 0b00101,
AIN2 = 0b00110,
AIN3 = 0b00111,
CE0 = 0b011001,
ExciPNode= 0b100100,
}
impl RegisterField for MUXSELP {
@@ -163,7 +188,7 @@ impl RegisterField for MUXSELP {
MUXSELP::Floating
}
const BIT_OFFSET: u32 = 0;
const MASK: u32 = 0b11111;
const MASK: u32 = 0b111111;
}
#[allow(dead_code)]

5
src/communication.rs
View File

@@ -175,7 +175,10 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
}
// Init the sequencer
let init_impedance_result = context.impedance_setup.lock().await.init_single_frequency_measurement(rqst.sinus_frequency, rqst.dft_number).await;
let init_impedance_result = match rqst.lead_mode {
bioz_icd_rs::BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.init_single_frequency_measurement(rqst.sinus_frequency, rqst.dft_number).await,
bioz_icd_rs::BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.init_single_frequency_measurement_4_lead(rqst.sinus_frequency, rqst.dft_number).await,
};
// Trigger the sequencer
context.impedance_setup.lock().await.start_measurement().await;

275
src/impedance.rs
View File

@@ -5,6 +5,7 @@ use embassy_stm32::exti::ExtiInput;
use embassy_sync::blocking_mutex::raw::ThreadModeRawMutex;
use embassy_sync::channel::Channel;
use embassy_sync::mutex::Mutex;
use embassy_time::Timer;
use static_cell::StaticCell;
@@ -24,6 +25,8 @@ pub static IMPEDANCE_CHANNEL_MULTI: Channel<ThreadModeRawMutex, MultiImpedanceOu
pub type ImpedanceSetupType = Mutex<ThreadModeRawMutex, ImpedanceSetup>;
pub static IMPEDANCE_SETUP: StaticCell<ImpedanceSetupType> = StaticCell::new();
pub const RCAL: f32 = 1008.0; // Calibration resistor in Ohm
#[derive(PartialEq, Copy, Clone)]
pub enum RunningMode {
None,
@@ -32,15 +35,21 @@ pub enum RunningMode {
MultiFrequency(MeasurementPointSet),
}
pub struct RtiaCalibrationResult {
pub magnitude: f32,
pub phase: f32,
}
pub struct ImpedanceSetup {
ad5940: AD5940,
dsp_config: Option<DspConfig>,
pub running_mode: RunningMode,
pub rtia_calibrated: Option<RtiaCalibrationResult>,
}
impl ImpedanceSetup {
pub fn new(ad5940: AD5940) -> Self {
ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None }
ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None, rtia_calibrated: None }
}
pub async fn init(&mut self) -> Result<(), Error> {
@@ -68,6 +77,7 @@ impl ImpedanceSetup {
// Set DSP configuration
let mut dsp_config = DspConfig::default();
dsp_config
.gnpgain(GNPGAIN::Gain1)
.adc_mux_n(MUXSELN::HsTiaNeg)
.adc_mux_p(MUXSELP::HsTiaPos)
.ctiacon(CTIACON::C32)
@@ -100,6 +110,96 @@ impl ImpedanceSetup {
Ok(())
}
pub async fn calibrate_rtia(&mut self, frequency: u32, wait_time: u32) -> Result<RtiaCalibrationResult, Error> {
/* CALIBRATION METHOD
1) Measure the complex voltage V_Rcal across the calibration DUT (Rcal).
2) Measure the complex voltage V_Rtia across Rtia [HSTIA_P (output) - HSTIA_N].
3) Note Rtia carries the same current as Rcal; I_Rtia = I_exc = I_Rcal
4) Implement the equation: Rtia = V_Rtia / I_Rtia
--> Rtia = (V_Rtia / V_Rcal) * Rcal
*/
// self.ad5940.hsr_calibrate_rtia(wait_time).await;
self.ad5940.wgfcw(frequency).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();
// Switches for 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();
// Voltage measurement Rcal
self.dsp_config.as_mut().unwrap()
.adc_mux_n(MUXSELN::ExciNNode)
.adc_mux_p(MUXSELP::ExciPNode);
self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap();
self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await;
// self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz
Timer::after_micros(10).await;
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
Timer::after_micros((wait_time/16) as u64).await;
Timer::after_micros(20).await;
self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await;
let real_raw = self.ad5940.read_reg(Register::DFTREAL).await.unwrap();
let imag_raw = self.ad5940.read_reg(Register::DFTIMAG).await.unwrap();
let mut rcal = Complex::new(
sign_extend_18bit(real_raw) as f32,
sign_extend_18bit(imag_raw) as f32
);
// Voltage measurement Rtia
self.dsp_config.as_mut().unwrap()
.adc_mux_n(MUXSELN::HsTiaNeg)
.adc_mux_p(MUXSELP::HsTiaPos);
self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).await.unwrap();
self.ad5940.afecon(AFECON::WAVEGENEN | AFECON::ADCEN, true).await;
// self.ad5940.sequencer_wait(16*10).await; // 10 us based on SYSCLK = 16MHz
Timer::after_micros(10).await;
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
Timer::after_micros((wait_time/16) as u64).await;
Timer::after_micros(20).await;
self.ad5940.afecon(AFECON::WAVEGENEN | AFECON:: ADCEN | AFECON::ADCCONVEN | AFECON::DFTEN, false).await;
let real_raw = self.ad5940.read_reg(Register::DFTREAL).await.unwrap();
let imag_raw = self.ad5940.read_reg(Register::DFTIMAG).await.unwrap();
let mut rtia = Complex::new(
sign_extend_18bit(real_raw) as f32,
sign_extend_18bit(imag_raw) as f32
);
// Current is measured inverted in rtia
rtia.re *= -1.0;
rtia.im *= -1.0;
// Impedance imaginary part is measured inverted
rcal.im *= -1.0;
rtia.im *= -1.0;
// Rtia = (V_Rtia / V_Rcal) * Rcal
let mut temp = rtia/rcal;
temp = temp.scale(RCAL);
// Calibration result
let calibration_result = RtiaCalibrationResult {
magnitude: temp.norm(), // Magnitude in Ohm
phase: temp.arg(), // Phase in radians
};
Ok(calibration_result)
}
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();
@@ -179,6 +279,103 @@ impl ImpedanceSetup {
Ok(sinus_periods_per_dft)
}
pub async fn init_single_frequency_measurement_4_lead(&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::SYNCEXTDEVICE, 0b111).await.unwrap();
// Calculate wait time between measurement start and stop
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();
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);
}
// Calibrate Rtia
match self.calibrate_rtia(frequency, wait_time).await {
Ok(calibration_result) => {
self.rtia_calibrated = Some(calibration_result);
},
Err(e) => {
error!("Rtia calibration failed: {:?}", e);
}
}
// Configure switches
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();
// Configure sequencer
self.ad5940.sequencer_enable(true).await;
self.ad5940.wgfcw(frequency).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();
// Voltage measurement
self.dsp_config.as_mut().unwrap()
.adc_mux_n(MUXSELN::AIN3)
.adc_mux_p(MUXSELP::AIN2);
// self.dsp_config.as_mut().unwrap()
// .adc_mux_n(MUXSELN::AIN1)
// .adc_mux_p(MUXSELP::CE0);
self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).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;
// Current measurement
self.dsp_config.as_mut().unwrap()
.adc_mux_n(MUXSELN::HsTiaNeg)
.adc_mux_p(MUXSELP::HsTiaPos);
self.ad5940.apply_dsp_config(self.dsp_config.as_ref().unwrap()).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(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();
@@ -342,8 +539,56 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance
}
}
RunningMode::SingleFrequency4Lead => {
info!("TODO: SingleFrequency4Lead not implemented yet");
if count >= 4 {
let mut data: [u32; 4] = [0; 4];
impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap();
let mut dft_volt = Complex::new(
sign_extend_18bit(data[0]) as f32,
sign_extend_18bit(data[1]) as f32
);
let mut dft_curr = Complex::new(
sign_extend_18bit(data[2]) as f32,
sign_extend_18bit(data[3]) as f32
);
// Current is measured inverted in rtia
dft_curr.re *= -1.0;
dft_curr.im *= -1.0;
// Impedance imaginary part is measured inverted
dft_volt.im *= -1.0;
dft_curr.im *= -1.0;
let (volt_mag, volt_phase) = dft_volt.to_polar();
let (curr_mag, curr_phase) = dft_curr.to_polar();
// info!("Volt Phase = {} V, Volt Phase = {} rad", volt_phase, curr_phase);
match impedance_setup.rtia_calibrated.as_ref() {
None => {
error!("Rtia not calibrated, cannot compute impedance");
continue;
},
Some(cal) => {
let rtia_mag = cal.magnitude;
let rtia_phase = cal.phase;
// Calculate impedance using calibrated Rtia
let magnitude = volt_mag / curr_mag * rtia_mag;
let phase = volt_phase - curr_phase + rtia_phase;
let data = SingleImpedanceOutput {
magnitude,
phase
};
IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok();
}
}
}
}
RunningMode::MultiFrequency(points) => {
// Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz)
@@ -394,9 +639,6 @@ pub struct ImpedanceResult {
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
@@ -404,22 +646,31 @@ fn sign_extend_18bit(val: u32) -> i32 {
/// Calculate magnitude and phase of Rz using Rcal reference
pub fn calculate_impedance(data: [u32; 4]) -> ImpedanceResult {
let dft_rcal = Complex::new(
let mut dft_rcal = Complex::new(
sign_extend_18bit(data[0]) as f32,
sign_extend_18bit(data[1]) as f32
);
let dft_rz = Complex::new(
let mut dft_rz = Complex::new(
sign_extend_18bit(data[2]) as f32,
sign_extend_18bit(data[3]) as f32
);
let rcal_mag = dft_rcal.norm();
let rz_mag = dft_rz.norm();
// Current is measured inverted in rtia
// --> Will cancel out in the impedance calculation
// dft_rcal.re *= -1.0;
// dft_rcal.im *= -1.0;
let rcal_phase = dft_rcal.arg();
let rz_phase = dft_rz.arg();
// dft_rz.re *= -1.0;
// dft_rz.im *= -1.0;
let magnitude = (rcal_mag / rz_mag) * RCAL_VAL;
// Impedance imaginary part is measured inverted
dft_rcal.im *= -1.0;
dft_rz.im *= -1.0;
let (rcal_mag, rcal_phase) = dft_rcal.to_polar();
let (rz_mag, rz_phase) = dft_rz.to_polar();
let magnitude = (rcal_mag / rz_mag) * RCAL;
let phase = rcal_phase - rz_phase;
ImpedanceResult { magnitude, phase }