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 multi calibration.

Browse Source
This commit is contained in:
Hubald Verzijl
2025-11-03 19:16:42 +01:00
parent 6358882a4c
commit 324189ba72
2 changed files with 332 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files

64
src/communication.rs
View File

@@ -20,7 +20,7 @@ use postcard_rpc::{
},
};
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 bioz_icd_rs::{GetUniqueIdEndpoint, MeasurementPointSet, MultiImpedanceOutputTopic, MultiImpedanceResult, MultiImpedanceStartRequest, PingEndpoint, SetGreenLedEndpoint, SingleImpedanceOutputTopic, SingleImpedanceStartRequest, StartMultiImpedanceEndpoint, StartSingleImpedanceEndpoint, StopMultiImpedanceEndpoint, StopSingleImpedanceEndpoint, BioImpedanceLeadMode, ENDPOINT_LIST, TOPICS_IN_LIST, TOPICS_OUT_LIST};
use crate::impedance::{ImpedanceSetupType, RunningMode, IMPEDANCE_CHANNEL_MULTI, IMPEDANCE_CHANNEL_SINGLE};
@@ -170,14 +170,14 @@ pub async fn start_single_impedance_handler(context: SpawnCtx, header: VarHeader
// Mark the impedance setup as running
match rqst.lead_mode {
bioz_icd_rs::BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency2Lead,
bioz_icd_rs::BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency4Lead,
BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency2Lead,
BioImpedanceLeadMode::FourLead => context.impedance_setup.lock().await.running_mode = RunningMode::SingleFrequency4Lead,
}
// Init the sequencer
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,
BioImpedanceLeadMode::TwoLead => context.impedance_setup.lock().await.init_single_frequency_measurement_2_lead(rqst.sinus_frequency, rqst.dft_number).await,
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;
@@ -223,26 +223,27 @@ 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::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) {
if was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) {
STOP.signal(());
}
was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen)
was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(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 => {
let response = match (rqst.lead_mode, rqst.points) {
(BioImpedanceLeadMode::TwoLead, MeasurementPointSet::Eight) => {
context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency2Lead(rqst.points);
const SIZE: usize = 8;
context
.impedance_setup
.lock()
.await
.init_multi_frequency_measurement::<SIZE>(rqst.points)
.init_multi_frequency_measurement_2_lead::<SIZE>(rqst.points)
.await
.map(|periods| MultiImpedanceResult {
points: rqst.points,
@@ -250,13 +251,46 @@ pub async fn start_multi_impedance_handler(context: SpawnCtx, header: VarHeader,
periods_per_dft_18: Vec::new(),
})
}
MeasurementPointSet::Eighteen => {
(BioImpedanceLeadMode::TwoLead, MeasurementPointSet::Eighteen) => {
context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency2Lead(rqst.points);
const SIZE: usize = 18;
context
.impedance_setup
.lock()
.await
.init_multi_frequency_measurement::<SIZE>(rqst.points)
.init_multi_frequency_measurement_2_lead::<SIZE>(rqst.points)
.await
.map(|periods| MultiImpedanceResult {
points: rqst.points,
periods_per_dft_8: Vec::new(),
periods_per_dft_18: periods,
})
}
(BioImpedanceLeadMode::FourLead, MeasurementPointSet::Eight) => {
info!("Start multi impedance 4 lead 8 points");
context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency4Lead(rqst.points);
const SIZE: usize = 8;
context
.impedance_setup
.lock()
.await
.init_multi_frequency_measurement_4_lead::<SIZE>(rqst.points)
.await
.map(|periods| MultiImpedanceResult {
points: rqst.points,
periods_per_dft_8: periods,
periods_per_dft_18: Vec::new(),
})
}
(BioImpedanceLeadMode::FourLead, MeasurementPointSet::Eighteen) => {
info!("Start multi impedance 4 lead 18 points");
context.impedance_setup.lock().await.running_mode = RunningMode::MultiFrequency4Lead(rqst.points);
const SIZE: usize = 18;
context
.impedance_setup
.lock()
.await
.init_multi_frequency_measurement_4_lead::<SIZE>(rqst.points)
.await
.map(|periods| MultiImpedanceResult {
points: rqst.points,
@@ -312,8 +346,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::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) {
if was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) {
STOP.signal(());
}
was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen)
was_busy == RunningMode::SingleFrequency2Lead || was_busy == RunningMode::SingleFrequency4Lead || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || was_busy == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen)
}

332
src/impedance.rs
View File

@@ -32,9 +32,18 @@ pub enum RunningMode {
None,
SingleFrequency2Lead,
SingleFrequency4Lead,
MultiFrequency(MeasurementPointSet),
MultiFrequency2Lead(MeasurementPointSet),
MultiFrequency4Lead(MeasurementPointSet),
}
pub enum RtiaCalibrated {
None,
Single(RtiaCalibrationResult),
Vec8(Vec<RtiaCalibrationResult, 8>),
Vec18(Vec<RtiaCalibrationResult, 18>),
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct RtiaCalibrationResult {
pub magnitude: f32,
pub phase: f32,
@@ -44,12 +53,12 @@ pub struct ImpedanceSetup {
ad5940: AD5940,
dsp_config: Option<DspConfig>,
pub running_mode: RunningMode,
pub rtia_calibrated: Option<RtiaCalibrationResult>,
pub rtia_calibrated: RtiaCalibrated,
}
impl ImpedanceSetup {
pub fn new(ad5940: AD5940) -> Self {
ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None, rtia_calibrated: None }
ImpedanceSetup { ad5940, dsp_config: None, running_mode: RunningMode::None, rtia_calibrated: RtiaCalibrated::None }
}
pub async fn init(&mut self) -> Result<(), Error> {
@@ -200,7 +209,51 @@ impl ImpedanceSetup {
Ok(calibration_result)
}
pub async fn init_single_frequency_measurement(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result<f32, ImpedanceInitError> {
async fn calibrate_rtia_multiple<const N: usize>(&mut self, number_of_points: MeasurementPointSet) -> Result<Vec<RtiaCalibrationResult, N>, ImpedanceInitError> {
let mut results = Vec::<RtiaCalibrationResult, N>::new();
// 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 => 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,
};
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();
// Calibrate Rtia at the given frequency
let calibration_result = self.calibrate_rtia(frequency, wait_time).await.unwrap();
results.push(calibration_result).unwrap();
} else {
error!("DSP configuration not set, cannot calculate wait time");
return Err(ImpedanceInitError::DSPNotSet);
}
}
Ok(results)
}
pub async fn init_single_frequency_measurement_2_lead(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result<f32, ImpedanceInitError> {
// Reset FIFO
self.ad5940.clear_and_enable_fifo().await.unwrap();
@@ -306,7 +359,7 @@ impl ImpedanceSetup {
// Calibrate Rtia
match self.calibrate_rtia(frequency, wait_time).await {
Ok(calibration_result) => {
self.rtia_calibrated = Some(calibration_result);
self.rtia_calibrated = RtiaCalibrated::Single(calibration_result);
},
Err(e) => {
error!("Rtia calibration failed: {:?}", e);
@@ -376,7 +429,7 @@ impl ImpedanceSetup {
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> {
pub async fn init_multi_frequency_measurement_2_lead<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();
@@ -481,6 +534,131 @@ impl ImpedanceSetup {
Ok(periods_per_dft_vec)
}
pub async fn init_multi_frequency_measurement_4_lead<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();
// Calibrate Rtia
match number_of_points {
MeasurementPointSet::Eight => {
let results = self.calibrate_rtia_multiple::<8>(number_of_points).await.unwrap();
self.rtia_calibrated = RtiaCalibrated::Vec8(results);
},
MeasurementPointSet::Eighteen => {
let results = self.calibrate_rtia_multiple::<18>(number_of_points).await.unwrap();
self.rtia_calibrated = RtiaCalibrated::Vec18(results);
},
}
// 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 => 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,
};
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);
}
// 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();
// 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();
// 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(periods_per_dft_vec)
}
pub async fn start_measurement(&mut self) {
self.ad5940.sequencer_trigger(0).await;
}
@@ -507,7 +685,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance
let mut impedance_setup = impedance_setup.lock().await;
// Trigger the sequencer again
if impedance_setup.running_mode == RunningMode::SingleFrequency2Lead || impedance_setup.running_mode == RunningMode::SingleFrequency4Lead || impedance_setup.running_mode == RunningMode::MultiFrequency(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency(MeasurementPointSet::Eighteen) {
if impedance_setup.running_mode == RunningMode::SingleFrequency2Lead || impedance_setup.running_mode == RunningMode::SingleFrequency4Lead || impedance_setup.running_mode == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency2Lead(MeasurementPointSet::Eighteen) || impedance_setup.running_mode == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eight) || impedance_setup.running_mode == RunningMode::MultiFrequency4Lead(MeasurementPointSet::Eighteen) {
impedance_setup.start_measurement().await;
}
@@ -525,7 +703,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance
impedance_setup.read_fifo(data.as_mut_slice()).await.unwrap();
let result = calculate_impedance(data);
let result = calculate_impedance_2_lead(data);
// Log
// info!("Impedance: Magnitude = {} Ω, Phase = {} rad", result.magnitude, result.phase);
@@ -543,54 +721,25 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance
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;
match impedance_setup.rtia_calibrated {
RtiaCalibrated::Single(ref cal) => {
let result = calculate_impedance_4_lead(data, cal);
let data = SingleImpedanceOutput {
magnitude,
phase
magnitude: result.magnitude,
phase: result.phase
};
IMPEDANCE_CHANNEL_SINGLE.try_send(data).ok();
}
_ => {
error!("Rtia not (correctly) calibrated, cannot compute impedance");
continue;
},
}
}
}
RunningMode::MultiFrequency(points) => {
RunningMode::MultiFrequency2Lead(points) => {
// Each frequency point produces 4 samples (DFT real/imag for Rcal and Rz)
let required_count = points.len() * 4;
@@ -611,7 +760,7 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance
// Take 4 samples per frequency point
for chunk in data_slice.chunks(4) {
let result = calculate_impedance(chunk.try_into().unwrap());
let result = calculate_impedance_2_lead(chunk.try_into().unwrap());
match points {
MeasurementPointSet::Eight => {
@@ -629,6 +778,57 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance
}
continue;
}
RunningMode::MultiFrequency4Lead(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(),
};
match (&points, &impedance_setup.rtia_calibrated) {
(MeasurementPointSet::Eight, RtiaCalibrated::Vec8(cal_vec)) => {
// Take 4 samples per frequency point
for (i, chunk) in data_slice.chunks(4).enumerate() {
let cal = &cal_vec[i];
let result = calculate_impedance_4_lead(chunk.try_into().unwrap(), cal);
impedance_output.magnitudes_8.push(result.magnitude).ok();
impedance_output.phases_8.push(result.phase).ok();
}
},
(MeasurementPointSet::Eighteen, RtiaCalibrated::Vec18(cal_vec)) => {
// Take 4 samples per frequency point
for (i, chunk) in data_slice.chunks(4).enumerate() {
let cal = &cal_vec[i];
let result = calculate_impedance_4_lead(chunk.try_into().unwrap(), cal);
impedance_output.magnitudes_18.push(result.magnitude).ok();
impedance_output.phases_18.push(result.phase).ok();
}
},
_ => {
error!("Rtia not (correctly) calibrated, cannot compute impedance");
continue;
}
}
IMPEDANCE_CHANNEL_MULTI.try_send(impedance_output).ok();
}
continue;
}
}
}
}
@@ -645,7 +845,7 @@ fn sign_extend_18bit(val: u32) -> i32 {
}
/// Calculate magnitude and phase of Rz using Rcal reference
pub fn calculate_impedance(data: [u32; 4]) -> ImpedanceResult {
pub fn calculate_impedance_2_lead(data: [u32; 4]) -> ImpedanceResult {
let mut dft_rcal = Complex::new(
sign_extend_18bit(data[0]) as f32,
sign_extend_18bit(data[1]) as f32
@@ -675,3 +875,37 @@ pub fn calculate_impedance(data: [u32; 4]) -> ImpedanceResult {
ImpedanceResult { magnitude, phase }
}
/// Calculate magnitude and phase of Rz using the calibrated Rtia
pub fn calculate_impedance_4_lead(data: [u32; 4], rtia: &RtiaCalibrationResult) -> ImpedanceResult {
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();
// Calculate impedance using calibrated Rtia
let magnitude = volt_mag / curr_mag * rtia.magnitude;
let phase = volt_phase - curr_phase + rtia.phase;
let data = ImpedanceResult {
magnitude,
phase
};
data
}