Included bode plot measurements, including different number of points.

src/impedance.rs

@@ -1,5 +1,3 @@
-use bioz_icd_rs::MultiImpedanceOutput28;
-use bioz_icd_rs::NumberOfPoints;
 use defmt::{info, error};
 
 use embassy_stm32::spi::Error;
@@ -12,11 +10,11 @@ use static_cell::StaticCell;
 use crate::ad5940::*;
 use crate::ad5940_registers::*;
 
-use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, InitImpedanceError};
+use bioz_icd_rs::{SingleImpedanceOutput, IcdDftNum, ImpedanceInitError, MeasurementPointSet, MultiImpedanceOutput};
 use crate::icd_mapping::IntoDftnum;
 
 pub static IMPEDANCE_CHANNEL_SINGLE: Channel<ThreadModeRawMutex, SingleImpedanceOutput, 2000> = Channel::new();
-pub static IMPEDANCE_CHANNEL_MULTI: Channel<ThreadModeRawMutex, MultiImpedanceOutput28, 50> = 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();
@@ -25,7 +23,7 @@ pub static IMPEDANCE_SETUP: StaticCell<ImpedanceSetupType> = StaticCell::new();
 pub enum RunningMode {
     None,
     SingleFrequency,
-    MultiFrequency,
+    MultiFrequency(MeasurementPointSet),
 }
 
 pub struct ImpedanceSetup {
@@ -96,7 +94,7 @@ impl ImpedanceSetup {
         Ok(())
     }
 
-    pub async fn init_single_frequency_measurement(&mut self, frequency: u32, dft_number: IcdDftNum) -> Result<f32, InitImpedanceError> {
+    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();
 
@@ -117,7 +115,7 @@ impl ImpedanceSetup {
             info!("Sinus periods per DFT: {}", sinus_periods_per_dft);
         } else {
             error!("DSP configuration not set, cannot calculate wait time");
-            return Err(InitImpedanceError::DSPNotSet);
+            return Err(ImpedanceInitError::DSPNotSet);
         }
 
         // Configure sequencer
@@ -175,31 +173,58 @@ impl ImpedanceSetup {
         Ok(sinus_periods_per_dft)
     }
 
-    pub async fn init_multi_frequency_measurement(&mut self, dft_number: IcdDftNum, number_of_points: NumberOfPoints) {
+    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();
 
-        // 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 mut wait_time = 0;
-        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);
-        } else {
-            error!("DSP configuration not set, cannot calculate wait time");
-        }
-
         // Configure sequencer
         self.ad5940.sequencer_enable(true).await;
 
-        for &frequency in number_of_points.values() {        
+        // 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();
@@ -249,6 +274,8 @@ impl ImpedanceSetup {
         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) {