FUCKING VICTORY LAP KRONIG-KRAMER HAS BEEN DEFEATED WOOT WOOT

Impedance.c

@@ -65,6 +65,52 @@ AppIMPCfg_Type AppIMPCfg =
   .StopRequired = bFALSE,
 };
 
+/* ----------------------------------------------------------------------- */
+/* Helper Functions                                                        */
+/* ----------------------------------------------------------------------- */
+
+void AppIMPCleanup(void) {
+    // Ensure chip is awake before sending commands
+    if(AD5940_WakeUp(10) > 10) return;
+
+    // 1. Stop Sequencer and Wakeup Timer
+    AD5940_WUPTCtrl(bFALSE);
+    AD5940_SEQCtrlS(bFALSE);
+    
+    // 2. Stop any active conversions/WG, but KEEP Reference/LDOs ON
+    AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT|AFECTRL_WG, bFALSE);
+
+    // 3. Reset FIFO
+    FIFOCfg_Type fifo_cfg;
+    fifo_cfg.FIFOEn = bFALSE;           
+    AD5940_FIFOCfg(&fifo_cfg);          
+    
+    fifo_cfg.FIFOEn = bTRUE;            
+    fifo_cfg.FIFOMode = FIFOMODE_FIFO;
+    fifo_cfg.FIFOSize = FIFOSIZE_4KB;   
+    fifo_cfg.FIFOSrc = FIFOSRC_DFT;     
+    fifo_cfg.FIFOThresh = 6;            
+    AD5940_FIFOCfg(&fifo_cfg);
+    
+    // 4. Clear Interrupts
+    AD5940_INTCClrFlag(AFEINTSRC_ALLINT);
+}
+
+void AppIMPCalibrateLFO(void) {
+    LFOSCMeasure_Type cal_cfg;
+    cal_cfg.CalDuration = 1000.0; // 1000ms for high accuracy
+    cal_cfg.CalSeqAddr = 0;       // Use start of SRAM for calibration sequence
+    cal_cfg.SystemClkFreq = 16000000.0;
+    
+    float freq;
+    // Use SDK function to measure LFOSC
+    if(AD5940_LFOSCMeasure(&cal_cfg, &freq) == AD5940ERR_OK) {
+        AppIMPCfg.WuptClkFreq = freq;
+    }
+}
+
+/* ----------------------------------------------------------------------- */
+
 int32_t AppIMPGetCfg(void *pCfg)
 {
   if(pCfg)
@@ -288,7 +334,6 @@ static AD5940Err AppIMPSeqMeasureGen(void)
                 AFECTRL_WG|AFECTRL_DACREFPWR|AFECTRL_HSDACPWR|\
                 AFECTRL_SINC2NOTCH, bTRUE);
   AD5940_AFECtrlS(AFECTRL_WG|AFECTRL_ADCPWR, bTRUE); 
-  // Increased settling time to 2000us (16*2000) to ensure stability
   AD5940_SEQGenInsert(SEQ_WAIT(16*2000)); 
   AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT, bTRUE); 
   
@@ -311,7 +356,6 @@ static AD5940Err AppIMPSeqMeasureGen(void)
   AD5940_ADCMuxCfgS(ADCMUXP_HSTIA_P, ADCMUXN_HSTIA_N);
 
   AD5940_AFECtrlS(AFECTRL_ADCPWR|AFECTRL_WG, bTRUE); 
-  // Increased settling time to 2000us
   AD5940_SEQGenInsert(SEQ_WAIT(16*2000)); 
   AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT, bTRUE); 
   
@@ -330,7 +374,6 @@ static AD5940Err AppIMPSeqMeasureGen(void)
   AD5940_ADCMuxCfgS(ADCMUXP_AIN2, ADCMUXN_AIN3);
 
   AD5940_AFECtrlS(AFECTRL_ADCPWR|AFECTRL_WG, bTRUE); 
-  // Increased settling time to 2000us
   AD5940_SEQGenInsert(SEQ_WAIT(16*2000)); 
   AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT, bTRUE); 
   
@@ -344,10 +387,6 @@ static AD5940Err AppIMPSeqMeasureGen(void)
                 AFECTRL_SINC2NOTCH, bFALSE);
   AD5940_SEQGpioCtrlS(0); 
 
-  // REMOVED: AD5940_EnterSleepS(); 
-  // Keeping AFE active during sweep to prevent wake-up instability.
-
-  // CRITICAL FIX: Must stop sequencer to generate ENDSEQ interrupt
   AD5940_SEQGenInsert(SEQ_STOP()); 
 
   error = AD5940_SEQGenFetchSeq(&pSeqCmd, &SeqLen);
@@ -378,42 +417,23 @@ AD5940Err AppIMPCheckFreq(float freq)
   
   freq_params = AD5940_GetFreqParameters(freq);
   
-  // Only switch modes if necessary to avoid glitching the sequencer
+  // Update ADC Filter and DFT settings based on frequency
-  if(freq < 80000)
-  {
-    if (AppIMPCfg.SysClkFreq != 16000000.0) {
-        filter_cfg.ADCRate = ADCRATE_800KHZ;
-        AppIMPCfg.AdcClkFreq = 16e6;
-        AppIMPCfg.SysClkFreq = 16000000.0;
-        AD5940_HPModeEn(bFALSE);
-        // Allow clock to settle before further SPI writes
-        AD5940_Delay10us(50); 
-    }
-  }
-  else
-  {
-    if (AppIMPCfg.SysClkFreq != 32000000.0) {
-        filter_cfg.ADCRate = ADCRATE_1P6MHZ;
-        AppIMPCfg.AdcClkFreq = 32e6;
-        AppIMPCfg.SysClkFreq = 32000000.0;
-        AD5940_HPModeEn(bTRUE);
-        // Allow clock to settle before further SPI writes
-        AD5940_Delay10us(50);
-    }
-  }
-  
   filter_cfg.ADCAvgNum = ADCAVGNUM_16; 
   filter_cfg.ADCSinc2Osr = freq_params.ADCSinc2Osr;
   filter_cfg.ADCSinc3Osr = freq_params.ADCSinc3Osr;
   filter_cfg.BpSinc3 = bFALSE;
   filter_cfg.BpNotch = bTRUE;
   filter_cfg.Sinc2NotchEnable = bTRUE;
+  filter_cfg.ADCRate = ADCRATE_800KHZ; // Fixed ADC Rate for stability
+  
   dft_cfg.DftNum = freq_params.DftNum;
   dft_cfg.DftSrc = freq_params.DftSrc;
   dft_cfg.HanWinEn = AppIMPCfg.HanWinEn;
+  
   AD5940_ADCFilterCfgS(&filter_cfg);
   AD5940_DFTCfgS(&dft_cfg);
   
+  // Calculate new WaitClks for the sequence
   clks_cal.DataType = DATATYPE_DFT;
   clks_cal.DftSrc = freq_params.DftSrc;
   clks_cal.DataCount = 1L<<(freq_params.DftNum+2); 
@@ -422,19 +442,18 @@ AD5940Err AppIMPCheckFreq(float freq)
   clks_cal.ADCAvgNum = 0;
   clks_cal.RatioSys2AdcClk = AppIMPCfg.SysClkFreq/AppIMPCfg.AdcClkFreq;
   AD5940_ClksCalculate(&clks_cal, &WaitClks);		
+  
+  // Add safety margin
+  WaitClks += 200;
 	
-  // Update Wait Times for all 3 measurements
+  // Update Wait Times in SRAM for all 3 measurements
   for (int i = 0; i < 3; i++) {
-      // Safety check: Ensure address is valid
       if (AppIMPCfg.SeqWaitAddr[i] == 0) continue;
 
       SRAMAddr = AppIMPCfg.MeasureSeqInfo.SeqRamAddr + AppIMPCfg.SeqWaitAddr[i];
       
-      // CRITICAL FIX: Double the wait time for High Power Mode (>=80kHz)
+      // Split wait time into two commands to handle large values
       uint32_t finalWait = WaitClks/2;
-      if (freq >= 80000) {
-          finalWait *= 2; 
-      }
       
       SeqCmdBuff[0] = SEQ_WAIT(finalWait);
       SeqCmdBuff[1] = SEQ_WAIT(finalWait);
@@ -494,14 +513,13 @@ int32_t AppIMPInit(uint32_t *pBuffer, uint32_t BufferSize)
   AD5940_SEQCfg(&seq_cfg); 
   AD5940_SEQMmrTrig(AppIMPCfg.InitSeqInfo.SeqId);
   
-  // CRITICAL FIX: Ensure ENDSEQ interrupt is enabled for Init Sequence
   AD5940_INTCCfg(AFEINTC_1, AFEINTSRC_ENDSEQ, bTRUE);
   
   // Safety timeout for Init Sequence
   int timeout = 100000;
   while(AD5940_INTCTestFlag(AFEINTC_1, AFEINTSRC_ENDSEQ) == bFALSE) {
       if(--timeout <= 0) {
-          return AD5940ERR_TIMEOUT; // Return error if init fails
+          return AD5940ERR_TIMEOUT; 
       }
   }
   AD5940_INTCClrFlag(AFEINTSRC_ALLINT);
@@ -509,6 +527,7 @@ int32_t AppIMPInit(uint32_t *pBuffer, uint32_t BufferSize)
   AppIMPCfg.MeasureSeqInfo.WriteSRAM = bFALSE;
   AD5940_SEQInfoCfg(&AppIMPCfg.MeasureSeqInfo);
   
+  // Set initial frequency parameters
   AppIMPCheckFreq(AppIMPCfg.FreqofData);
 
   seq_cfg.SeqEnable = bTRUE;
@@ -523,11 +542,11 @@ int32_t AppIMPRegModify(int32_t * const pData, uint32_t *pDataCount)
 {
   if(AppIMPCfg.NumOfData > 0)
   {
-    AppIMPCfg.FifoDataCount += *pDataCount/6; // 6 words per point
+    AppIMPCfg.FifoDataCount += *pDataCount/6; 
     if(AppIMPCfg.FifoDataCount >= AppIMPCfg.NumOfData)
     {
       AD5940_WUPTCtrl(bFALSE);
-      AD5940_SEQCtrlS(bFALSE); // Added safety
+      AD5940_SEQCtrlS(bFALSE); 
       return AD5940ERR_OK;
     }
   }
@@ -538,8 +557,11 @@ int32_t AppIMPRegModify(int32_t * const pData, uint32_t *pDataCount)
   }
   if(AppIMPCfg.SweepCfg.SweepEn) 
   {
-    AD5940_WGFreqCtrlS(AppIMPCfg.SweepNextFreq, AppIMPCfg.SysClkFreq);
+    // Update Filters and Wait Times in SRAM
     AppIMPCheckFreq(AppIMPCfg.SweepNextFreq);
+    
+    // Update WG Frequency
+    AD5940_WGFreqCtrlS(AppIMPCfg.SweepNextFreq, AppIMPCfg.SysClkFreq);
   }
   return AD5940ERR_OK;
 }
@@ -547,7 +569,7 @@ int32_t AppIMPRegModify(int32_t * const pData, uint32_t *pDataCount)
 int32_t AppIMPDataProcess(int32_t * const pData, uint32_t *pDataCount)
 {
   uint32_t DataCount = *pDataCount;
-  uint32_t ImpResCount = DataCount/6; // 3 measurements * 2 words
+  uint32_t ImpResCount = DataCount/6; 
 
   fImpPol_Type * const pOut = (fImpPol_Type*)pData;
   iImpCar_Type * pSrcData = (iImpCar_Type*)pData;
@@ -575,24 +597,19 @@ int32_t AppIMPDataProcess(int32_t * const pData, uint32_t *pDataCount)
     float MagV, PhaseV;
     float ZMag, ZPhase;
     
-    // Calculate Magnitude and Phase for Current (I)
     MagI = sqrt((float)pDftI->Real*pDftI->Real + (float)pDftI->Image*pDftI->Image);
     PhaseI = atan2(-pDftI->Image, pDftI->Real);
 
-    // Calculate Magnitude and Phase for Voltage (V)
     MagV = sqrt((float)pDftV->Real*pDftV->Real + (float)pDftV->Image*pDftV->Image);
     PhaseV = atan2(-pDftV->Image, pDftV->Real);
 
-    // Z = V / I * Rtia
+    if(MagI > 1e-9) 
-    if(MagI > 1e-9) // Avoid division by zero
         ZMag = (MagV / MagI) * AppIMPCfg.RtiaVal;
     else
         ZMag = 0;
 
-    // Correct Phase: TIA inverts current, so we add 180 degrees (PI)
     ZPhase = PhaseV - PhaseI + MATH_PI;
     
-    // Normalize Phase to -PI to +PI
     while(ZPhase > MATH_PI) ZPhase -= 2*MATH_PI;
     while(ZPhase < -MATH_PI) ZPhase += 2*MATH_PI;
 
@@ -623,44 +640,51 @@ int32_t AppIMPISR(void *pBuff, uint32_t *pCount)
   if(AD5940_WakeUp(10) > 10) return AD5940ERR_WAKEUP;
   AD5940_SleepKeyCtrlS(SLPKEY_LOCK); 
 
-  // Check Interrupt Source
   uint32_t IntFlag = AD5940_INTCGetFlag(AFEINTC_1);
 
-  // Unified ISR: Handle ENDSEQ for both Sweep and Measure modes
+  if (IntFlag & (AFEINTSRC_DATAFIFOOF | AFEINTSRC_DATAFIFOUF)) {
+      AD5940_INTCClrFlag(AFEINTSRC_DATAFIFOOF | AFEINTSRC_DATAFIFOUF);
+      FIFOCfg_Type fifo_cfg;
+      fifo_cfg.FIFOEn = bFALSE;
+      AD5940_FIFOCfg(&fifo_cfg);
+      fifo_cfg.FIFOEn = bTRUE;
+      fifo_cfg.FIFOMode = FIFOMODE_FIFO;
+      fifo_cfg.FIFOSize = FIFOSIZE_4KB;
+      fifo_cfg.FIFOSrc = FIFOSRC_DFT;
+      fifo_cfg.FIFOThresh = AppIMPCfg.FifoThresh;
+      AD5940_FIFOCfg(&fifo_cfg);
+      
+      AD5940_SleepKeyCtrlS(SLPKEY_UNLOCK);
+      return AD5940ERR_FIFO;
+  }
+
   if(IntFlag & AFEINTSRC_ENDSEQ)
   {
-      // FIFO Latency Wait: Wait for data to arrive.
+      int timeout = 200; 
-      // Use a real delay loop to ensure we don't timeout prematurely.
-      int timeout = 100; 
       while(AD5940_FIFOGetCnt() < 6 && timeout-- > 0)
       {
-          AD5940_Delay10us(10); // Wait 100us per check
+          AD5940_Delay10us(10); 
       }
 
-      // Read all available data
       FifoCnt = (AD5940_FIFOGetCnt()/6)*6;
       if(FifoCnt > BuffCount) FifoCnt = BuffCount;
       
       AD5940_FIFORd((uint32_t *)pBuff, FifoCnt);
       AD5940_INTCClrFlag(AFEINTSRC_ENDSEQ);
       
-      // CRITICAL: Only advance if we actually got data.
-      // If FifoCnt is 0, it means we missed the data or it hasn't arrived.
-      // Advancing the sweep in this case causes desynchronization.
       if (FifoCnt > 0) 
       {
-          // Update Frequency (Sweep) or Check Limits (Measure)
+          AD5940_SEQCtrlS(bFALSE);
+
           if (AppIMPRegModify(pBuff, &FifoCnt) == AD5940ERR_OK) 
           {
               if(AppIMPCfg.FifoDataCount < AppIMPCfg.NumOfData || AppIMPCfg.NumOfData == -1)
               {
                   if(AppIMPCfg.StopRequired == bFALSE)
                   {
-                      // SRAM Write Settling: AppIMPRegModify writes to SRAM in Sweep Mode.
                       AD5940_Delay10us(20); 
 
-                      // SEQ_STOP Side Effects: Toggle Sequencer Enable
+                      AD5940_WriteReg(REG_AFE_SEQCNT, 0);
-                      AD5940_SEQCtrlS(bFALSE);
                       AD5940_SEQCtrlS(bTRUE);
                       AD5940_SEQMmrTrig(SEQID_0);
                   }
@@ -673,15 +697,12 @@ int32_t AppIMPISR(void *pBuff, uint32_t *pCount)
       }
       else
       {
-          // Error Recovery: If we got ENDSEQ but no data, clear flags and unlock.
-          // We do NOT trigger the next point to avoid runaway errors.
           AD5940_SleepKeyCtrlS(SLPKEY_UNLOCK);
           *pCount = 0;
       }
       return 0;
   }
   
-  // Clear any other spurious flags
   AD5940_INTCClrFlag(AFEINTSRC_ALLINT);
   AD5940_SleepKeyCtrlS(SLPKEY_UNLOCK);
   return 0;

Impedance.h

@@ -6,6 +6,9 @@
 #include "string.h"
 #include "math.h"
 
+// Define custom error code for FIFO overflow
+#define AD5940ERR_FIFO 20
+
 typedef struct
 {
 /* Common configurations for all kinds of Application. */
@@ -69,4 +72,8 @@ int32_t AppIMPGetCfg(void *pCfg);
 int32_t AppIMPISR(void *pBuff, uint32_t *pCount);
 int32_t AppIMPCtrl(uint32_t Command, void *pPara);
 
+/* Helper Routines */
+void AppIMPCleanup(void);
+void AppIMPCalibrateLFO(void);
+
 #endif

main.c

@@ -56,7 +56,6 @@ void AD5940_MCUGpioCtrl(uint32_t pin, BoolFlag enable) { (void)pin; (void)enable
 // ---------------------------------------------------------------------------
 
 void setup_pins(void) {
-    // Increase SPI speed to 4MHz to prevent FIFO overflow during fast sweeps
     spi_init(spi0, 4000000); 
     gpio_set_function(PIN_MISO, GPIO_FUNC_SPI);
     gpio_set_function(PIN_SCK, GPIO_FUNC_SPI);
@@ -122,15 +121,17 @@ static int32_t AD5940PlatformCfg(void)
   AD5940_HWReset();
   AD5940_Initialize();
   
+  // Use HFOSC (16MHz) for stability across all frequencies
   clk_cfg.ADCClkDiv = ADCCLKDIV_1;
   clk_cfg.ADCCLkSrc = ADCCLKSRC_HFOSC;
   clk_cfg.SysClkDiv = SYSCLKDIV_1;
   clk_cfg.SysClkSrc = SYSCLKSRC_HFOSC;
   clk_cfg.HfOSC32MHzMode = bFALSE;
   clk_cfg.HFOSCEn = bTRUE;
-  clk_cfg.HFXTALEn = bFALSE;
+  clk_cfg.HFXTALEn = bFALSE; 
   clk_cfg.LFOSCEn = bTRUE;
   AD5940_CLKCfg(&clk_cfg);
+  printf("Clock Configured (HFOSC 16MHz).\n");
   
   fifo_cfg.FIFOEn = bFALSE;
   fifo_cfg.FIFOMode = FIFOMODE_FIFO;
@@ -165,7 +166,6 @@ void ImpedanceShowResult(uint32_t *pData, uint32_t DataCount)
 
   for(int i=0;i<DataCount;i++)
   {
-    // Convert Polar (Mag, Phase) back to Cartesian (Real, Imag) for display
     float mag = pImp[i].Magnitude;
     float phase = pImp[i].Phase;
     
@@ -182,6 +182,115 @@ void ImpedanceShowResult(uint32_t *pData, uint32_t DataCount)
   }
 }
 
+// ---------------------------------------------------------------------------
+// High-Level Routines
+// ---------------------------------------------------------------------------
+
+void Routine_CalibrateLFO(void) {
+    printf(">> Calibrating LFOSC...\n");
+    AppIMPCleanup();
+    AppIMPCalibrateLFO();
+    printf(">> LFOSC Calibrated.\n");
+}
+
+void Routine_Measure(float freq) {
+    AppIMPCfg_Type *pCfg;
+    AppIMPGetCfg(&pCfg);
+
+    AppIMPCleanup();
+    AppIMPCalibrateLFO(); 
+
+    pCfg->SweepCfg.SweepEn = bFALSE;
+    pCfg->SinFreq = freq;
+    pCfg->NumOfData = -1; 
+    pCfg->bParaChanged = bTRUE; 
+    
+    if(AppIMPInit(AppBuff, APPBUFF_SIZE) == AD5940ERR_OK) {
+        AppIMPCtrl(IMPCTRL_START, 0);
+    } else {
+        printf("ERROR: Init Failed\n");
+    }
+}
+
+void Routine_Sweep(float start, float end, int steps) {
+    AppIMPCfg_Type *pCfg;
+    AppIMPGetCfg(&pCfg);
+
+    AppIMPCleanup();
+    AppIMPCalibrateLFO(); 
+
+    pCfg->SweepCfg.SweepEn = bTRUE;
+    pCfg->SweepCfg.SweepStart = start;
+    pCfg->SweepCfg.SweepStop = end;
+    pCfg->SweepCfg.SweepPoints = steps;
+    pCfg->SweepCfg.SweepLog = bTRUE;
+    pCfg->NumOfData = steps; 
+    pCfg->bParaChanged = bTRUE; 
+    
+    if(AppIMPInit(AppBuff, APPBUFF_SIZE) == AD5940ERR_OK) {
+        AppIMPCtrl(IMPCTRL_START, 0);
+    } else {
+        printf("ERROR: Init Failed\n");
+    }
+}
+
+void Routine_CalibrateSystem(void) {
+    AppIMPCfg_Type *pCfg;
+    AppIMPGetCfg(&pCfg);
+
+    AppIMPCleanup();
+    
+    // 1. ADC Calibration
+    ADCPGACal_Type adcpga_cal;
+    adcpga_cal.AdcClkFreq = 16000000.0;
+    adcpga_cal.SysClkFreq = 16000000.0;
+    adcpga_cal.ADCSinc3Osr = ADCSINC3OSR_4;
+    adcpga_cal.ADCSinc2Osr = ADCSINC2OSR_22;
+    adcpga_cal.ADCPga = ADCPGA_1P5; 
+    adcpga_cal.PGACalType = PGACALTYPE_OFFSET;
+    adcpga_cal.TimeOut10us = 1000;
+    adcpga_cal.VRef1p11 = 1.11;
+    adcpga_cal.VRef1p82 = 1.82;
+    printf(">> Calibrating ADC Offset...\n");
+    AD5940_ADCPGACal(&adcpga_cal);
+
+    // 2. HSDAC Configuration
+    HSDACCfg_Type hsdac_cfg;
+    hsdac_cfg.ExcitBufGain = EXCITBUFGAIN_0P25;
+    hsdac_cfg.HsDacGain = HSDACGAIN_0P2;
+    hsdac_cfg.HsDacUpdateRate = 7;
+    AD5940_HSDacCfgS(&hsdac_cfg);
+
+    // 3. RTIA Calibration
+    HSRTIACal_Type hsrtia_cal;
+    fImpPol_Type Res;
+    memset(&hsrtia_cal, 0, sizeof(hsrtia_cal));
+
+    hsrtia_cal.fFreq = 1000.0f;
+    hsrtia_cal.AdcClkFreq = 16000000.0;
+    hsrtia_cal.SysClkFreq = 16000000.0;
+    hsrtia_cal.ADCSinc3Osr = ADCSINC3OSR_4;
+    hsrtia_cal.ADCSinc2Osr = ADCSINC2OSR_22;
+    hsrtia_cal.bPolarResult = bTRUE; 
+    hsrtia_cal.fRcal = 100.0;           
+    hsrtia_cal.HsTiaCfg.DiodeClose = bFALSE;
+    hsrtia_cal.HsTiaCfg.HstiaBias = HSTIABIAS_1P1;
+    hsrtia_cal.HsTiaCfg.HstiaCtia = 31;
+    hsrtia_cal.HsTiaCfg.HstiaRtiaSel = HSTIARTIA_200; 
+    hsrtia_cal.HsTiaCfg.HstiaDeRtia = HSTIADERTIA_OPEN;
+    hsrtia_cal.HsTiaCfg.HstiaDeRload = HSTIADERLOAD_OPEN;
+    hsrtia_cal.DftCfg.DftNum = DFTNUM_16384;
+    hsrtia_cal.DftCfg.DftSrc = DFTSRC_SINC3;
+
+    printf(">> Calibrating HSTIARTIA_200...\n");
+    if (AD5940_HSRtiaCal(&hsrtia_cal, &Res) == AD5940ERR_OK) {
+        printf("Calibrated Rtia: Mag = %f Ohm, Phase = %f\n", Res.Magnitude, Res.Phase);
+        pCfg->RtiaVal = Res.Magnitude; 
+    } else {
+        printf("Calibration Failed\n");
+    }
+}
+
 // ---------------------------------------------------------------------------
 // Main Loop
 // ---------------------------------------------------------------------------
@@ -189,156 +298,34 @@ void ImpedanceShowResult(uint32_t *pData, uint32_t DataCount)
 char input_buffer[64];
 int input_pos = 0;
 
-// Helper for robust state cleanup
-void AD5940_Main_Cleanup(void) {
-    // Ensure chip is awake before sending commands
-    AD5940_WakeUp(10);
-
-    // 1. Stop Sequencer and Wakeup Timer
-    AD5940_WUPTCtrl(bFALSE);
-    AD5940_SEQCtrlS(bFALSE);
-    
-    // 2. Stop any active conversions/WG, but KEEP Reference/LDOs ON
-    // This prevents settling issues on restart
-    AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT|AFECTRL_WG, bFALSE);
-
-    // 3. Reset FIFO
-    FIFOCfg_Type fifo_cfg;
-    fifo_cfg.FIFOEn = bFALSE;           
-    AD5940_FIFOCfg(&fifo_cfg);          
-    
-    fifo_cfg.FIFOEn = bTRUE;            
-    fifo_cfg.FIFOMode = FIFOMODE_FIFO;
-    fifo_cfg.FIFOSize = FIFOSIZE_4KB;   
-    fifo_cfg.FIFOSrc = FIFOSRC_DFT;     
-    fifo_cfg.FIFOThresh = 6;            
-    AD5940_FIFOCfg(&fifo_cfg);
-    
-    // 4. Clear Interrupts
-    AD5940_INTCClrFlag(AFEINTSRC_ALLINT);
-}
-
 void process_command() {
     char cmd = input_buffer[0];
-    AppIMPCfg_Type *pCfg;
-    AppIMPGetCfg(&pCfg);
 
-    // CRITICAL: Perform robust cleanup before any new operation
+    sleep_ms(10); 
-    AD5940_Main_Cleanup();
-    sleep_ms(10); // Give AFE time to settle
 
     if (cmd == 'v') {
         uint32_t id = AD5940_ReadReg(REG_AFECON_CHIPID);
         printf("CHIP_ID:0x%04X\n", id);
     } 
     else if (cmd == 'c') {
-        // Cleanup already done by AD5940_Main_Cleanup() above
+        Routine_CalibrateSystem();
-
-        // 5. Perform ADC Calibration (Offset)
-        ADCPGACal_Type adcpga_cal;
-        adcpga_cal.AdcClkFreq = 16000000.0;
-        adcpga_cal.SysClkFreq = 16000000.0;
-        adcpga_cal.ADCSinc3Osr = ADCSINC3OSR_4;
-        adcpga_cal.ADCSinc2Osr = ADCSINC2OSR_22;
-        adcpga_cal.ADCPga = ADCPGA_1P5; 
-        adcpga_cal.PGACalType = PGACALTYPE_OFFSET;
-        adcpga_cal.TimeOut10us = 1000;
-        adcpga_cal.VRef1p11 = 1.11;
-        adcpga_cal.VRef1p82 = 1.82;
-        printf(">> Calibrating ADC Offset...\n");
-        AD5940_ADCPGACal(&adcpga_cal);
-
-        // 6. Configure HSDAC to Low Gain explicitly
-        HSDACCfg_Type hsdac_cfg;
-        hsdac_cfg.ExcitBufGain = EXCITBUFGAIN_0P25;
-        hsdac_cfg.HsDacGain = HSDACGAIN_0P2;
-        hsdac_cfg.HsDacUpdateRate = 7;
-        AD5940_HSDacCfgS(&hsdac_cfg);
-
-        // 7. Run RTIA Calibration
-        HSRTIACal_Type hsrtia_cal;
-        fImpPol_Type Res;
-        
-        memset(&hsrtia_cal, 0, sizeof(hsrtia_cal));
-
-        hsrtia_cal.fFreq = 1000.0f;
-        hsrtia_cal.AdcClkFreq = 16000000.0;
-        hsrtia_cal.SysClkFreq = 16000000.0;
-        hsrtia_cal.ADCSinc3Osr = ADCSINC3OSR_4;
-        hsrtia_cal.ADCSinc2Osr = ADCSINC2OSR_22;
-        hsrtia_cal.bPolarResult = bTRUE; 
-        hsrtia_cal.fRcal = 100.0;           
-        hsrtia_cal.HsTiaCfg.DiodeClose = bFALSE;
-        hsrtia_cal.HsTiaCfg.HstiaBias = HSTIABIAS_1P1;
-        hsrtia_cal.HsTiaCfg.HstiaCtia = 31;
-        hsrtia_cal.HsTiaCfg.HstiaRtiaSel = HSTIARTIA_200; 
-        
-        // CRITICAL FIX: Explicitly OPEN the DE0 switches. 
-        // memset set them to 0 (50 Ohm / 0 Ohm), causing the 475 Ohm error.
-        hsrtia_cal.HsTiaCfg.HstiaDeRtia = HSTIADERTIA_OPEN;
-        hsrtia_cal.HsTiaCfg.HstiaDeRload = HSTIADERLOAD_OPEN;
-
-        hsrtia_cal.DftCfg.DftNum = DFTNUM_16384;
-        hsrtia_cal.DftCfg.DftSrc = DFTSRC_SINC3;
-
-        printf(">> Calibrating HSTIARTIA_200...\n");
-        if (AD5940_HSRtiaCal(&hsrtia_cal, &Res) == AD5940ERR_OK) {
-            printf("Calibrated Rtia: Mag = %f Ohm, Phase = %f\n", Res.Magnitude, Res.Phase);
-            // Update global config with actual calibrated value to fix scaling error
-            pCfg->RtiaVal = Res.Magnitude; 
-        } else {
-            printf("Calibration Failed\n");
-        }
     }
     else if (cmd == 'm') {
-        // Measure: Continuous monitoring at fixed frequency
         float freq = 1000.0f;
         if (strlen(input_buffer) > 2) freq = atof(input_buffer + 2);
-        
+        Routine_Measure(freq);
-        pCfg->SweepCfg.SweepEn = bFALSE;
-        pCfg->SinFreq = freq;
-        pCfg->NumOfData = -1; // Continuous
-        
-        // CRITICAL FIX: Force parameter change flag to TRUE.
-        // This ensures AppIMPInit calls AppIMPSeqCfgGen, which re-enables
-        // the analog blocks (DAC, TIA, etc.) that AD5940_Main_Cleanup turned off.
-        pCfg->bParaChanged = bTRUE; 
-        
-        if(AppIMPInit(AppBuff, APPBUFF_SIZE) == AD5940ERR_OK) {
-            AppIMPCtrl(IMPCTRL_START, 0);
-        } else {
-            printf("ERROR: Init Failed\n");
-        }
     }
     else if (cmd == 's') {
-        // Sweep: Run exactly 'steps' measurements then stop
         float start = 100.0f, end = 100000.0f;
         int steps = 50;
         if (strlen(input_buffer) > 2) sscanf(input_buffer + 2, "%f %f %d", &start, &end, &steps);
-        
+        Routine_Sweep(start, end, steps);
-        pCfg->SweepCfg.SweepEn = bTRUE;
-        pCfg->SweepCfg.SweepStart = start;
-        pCfg->SweepCfg.SweepStop = end;
-        pCfg->SweepCfg.SweepPoints = steps;
-        pCfg->SweepCfg.SweepLog = bTRUE;
-        pCfg->NumOfData = steps; // Stop after sweep
-        
-        // CRITICAL FIX: Force parameter change flag to TRUE.
-        pCfg->bParaChanged = bTRUE; 
-        
-        if(AppIMPInit(AppBuff, APPBUFF_SIZE) == AD5940ERR_OK) {
-            AppIMPCtrl(IMPCTRL_START, 0);
-        } else {
-            printf("ERROR: Init Failed\n");
-        }
     }
     else if (cmd == 'x') {
-        // Stop Measurement
+        AppIMPCleanup();
-        // Cleanup already done by AD5940_Main_Cleanup() at start of function
         printf("STOPPED\n");
     }
     else if (cmd == 'z') {
-        // Full System Reset
         watchdog_reboot(0, 0, 0);
     }
 }
@@ -367,8 +354,12 @@ int main() {
 
         if (gpio_get(PIN_INT) == 0) {
             uint32_t temp = APPBUFF_SIZE;
-            AppIMPISR(AppBuff, &temp);
+            int32_t status = AppIMPISR(AppBuff, &temp);
-            if(temp > 0) {
+            
+            if (status == AD5940ERR_FIFO) {
+                printf("ERROR: FIFO Overflow/Underflow. Stopping.\n");
+                AppIMPCleanup();
+            } else if(temp > 0) {
                 ImpedanceShowResult(AppBuff, temp);
             }
         }