diff --git a/src/ad5940.rs b/src/ad5940.rs index 78a746d..b3fc588 100644 --- a/src/ad5940.rs +++ b/src/ad5940.rs @@ -71,6 +71,7 @@ impl SwitchConfig { #[allow(dead_code)] #[derive(Default)] pub struct DspConfig { + pub gnpgain: Option, muxseln: Option, muxselp: Option, ctiacon: Option, @@ -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 diff --git a/src/ad5940_registers.rs b/src/ad5940_registers.rs index 49ce3c2..103f1af 100644 --- a/src/ad5940_registers.rs +++ b/src/ad5940_registers.rs @@ -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)] @@ -138,7 +157,9 @@ pub enum MUXSELN Floating = 0b00000, HsTiaNeg = 0b00001, LpTiaNeg = 0b00010, - AIN1 = 0b00101, + AIN1 = 0b00101, + AIN3 = 0b00111, + ExciNNode= 0b10100, } impl RegisterField for MUXSELN { @@ -155,7 +176,11 @@ impl RegisterField for MUXSELN { pub enum MUXSELP { Floating = 0b00000, HsTiaPos = 0b00001, - AIN1 = 0b00101, + 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)] diff --git a/src/communication.rs b/src/communication.rs index c42b473..5f43b19 100644 --- a/src/communication.rs +++ b/src/communication.rs @@ -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; diff --git a/src/impedance.rs b/src/impedance.rs index 8e2c51d..786d66d 100644 --- a/src/impedance.rs +++ b/src/impedance.rs @@ -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; pub static IMPEDANCE_SETUP: StaticCell = 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, pub running_mode: RunningMode, + pub rtia_calibrated: Option, } 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 { + /* 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 { // 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 { + // 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(&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(); @@ -342,8 +539,56 @@ pub async fn impedance_setup_readout_task(mut pin: ExtiInput<'static>, impedance } } RunningMode::SingleFrequency4Lead => { - info!("TODO: SingleFrequency4Lead not implemented yet"); - continue; + 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 }