diff --git a/src/communication.rs b/src/communication.rs index 5f43b19..3df5a95 100644 --- a/src/communication.rs +++ b/src/communication.rs @@ -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) { // 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::(rqst.points) + .init_multi_frequency_measurement_2_lead::(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::(rqst.points) + .init_multi_frequency_measurement_2_lead::(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::(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::(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) } \ No newline at end of file diff --git a/src/impedance.rs b/src/impedance.rs index 786d66d..b2000bc 100644 --- a/src/impedance.rs +++ b/src/impedance.rs @@ -32,9 +32,18 @@ pub enum RunningMode { None, SingleFrequency2Lead, SingleFrequency4Lead, - MultiFrequency(MeasurementPointSet), + MultiFrequency2Lead(MeasurementPointSet), + MultiFrequency4Lead(MeasurementPointSet), } +pub enum RtiaCalibrated { + None, + Single(RtiaCalibrationResult), + Vec8(Vec), + Vec18(Vec), +} + +#[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, pub running_mode: RunningMode, - pub rtia_calibrated: Option, + 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 { + async fn calibrate_rtia_multiple(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { + let mut results = Vec::::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 { // 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(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { + pub async fn init_multi_frequency_measurement_2_lead(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { // Create vector to store the periods per DFT for each frequency let mut periods_per_dft_vec = heapless::Vec::::new(); @@ -481,6 +534,131 @@ impl ImpedanceSetup { Ok(periods_per_dft_vec) } + pub async fn init_multi_frequency_measurement_4_lead(&mut self, number_of_points: MeasurementPointSet) -> Result, ImpedanceInitError> { + // Create vector to store the periods per DFT for each frequency + let mut periods_per_dft_vec = heapless::Vec::::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 +}