diff --git a/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/EliteADC.h b/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/EliteADC.h
index 189326e0a..3b695668b 100644
--- a/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/EliteADC.h
+++ b/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/EliteADC.h
@@ -704,16 +704,27 @@ static int32_t read_LPTIA_Iin(){
 }
 
 static uint32_t read_HSTIA_Iin(){
-    static uint32_t RealCurrent, RealCurrentRMS, correctedDFT;
+    static uint32_t originalDFT, RealCurrent, RealCurrentRMS, correctedDFT, freq, cutoff;
 
     instru.real = neg_18bit(ReadRealZ());
     instru.imag = neg_18bit(ReadImagZ());
 
-    RealCurrent = sqrt(instru.imag * instru.imag + instru.real * instru.real);
+    originalDFT = sqrt(instru.imag * instru.imag + instru.real * instru.real);
 
-//    correctedDFT = ((100 + (User2Freq(instru.fset) * User2Freq(instru.fset)) / (Cutoff_Freq * Cutoff_Freq)) * sqrt(instru.imag * instru.imag + instru.real * instru.real)) * 10;
+    if (instru.fset > 1000) {
+        freq = (instru.fset + 500) / 1000;
+        cutoff = Cutoff_Freq / 100000;
+        correctedDFT = (10000 + (User2Freq(freq) * User2Freq(freq)) / (cutoff * cutoff)) * originalDFT / 10000;
+    } else {
+        correctedDFT = (1000000 + (User2Freq(instru.fset) * User2Freq(instru.fset)) / (Cutoff_Freq * Cutoff_Freq)) * originalDFT / 1000000;
+    }
 //    RealCurrentRMS = (correctedDFT / HSRTIATable[instru.ADCGainLv] * 227 + 5) / 10; // Irms[nA]
 //    RealCurrent = (RealCurrentRMS * 7071 + 5000) / 10000;
+
+    InputNotify(NOTIFY_CURRENT, instru.imag);
+    InputNotify(NOTIFY_VOLT, instru.real);
+    InputNotify(NOTIFY_IMPEDANCE, correctedDFT);
+
     return RealCurrent;
 }
 
diff --git a/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/EliteInstruction.h b/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/EliteInstruction.h
index d86b1ec0e..4381b11c3 100644
--- a/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/EliteInstruction.h
+++ b/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/EliteInstruction.h
@@ -170,7 +170,7 @@ struct HEADSTAGE_INSTRUCTION {
 #define Sinc3OSR1333      11
 
 #define AD5940_SYS_CLOCK    16000000
-#define Cutoff_Freq            21000  // 210kHz
+#define Cutoff_Freq         21000000  // 210kHz
 
 ///* LPTIA gain Level */
 //#define LPRTIA200R         1        //Max 3mA
diff --git a/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/Elite_mode_ADC_DAC.h b/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/Elite_mode_ADC_DAC.h
index 76a5695bd..e0fb33841 100644
--- a/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/Elite_mode_ADC_DAC.h
+++ b/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/Elite_mode_ADC_DAC.h
@@ -156,56 +156,17 @@ static int16_t CalcPhase()
 
     correctPhase = (measPhase - (25 * (int64_t)instru.fset - 89078000) / 100000) / 10;
 
-    return measPhase;
+    return correctPhase;
 }
 
-static uint16_t FixPhase(uint16_t measPhase, int8_t cnt)//, int8_t cnt)
+static int32_t correctReal()
 {
-    static int16_t phase;
-
-    if (neg_18bit(ReadRealZ()) >= 0) {
-        phase = measPhase + cnt * 360;
-    } else if (neg_18bit(ReadRealZ()) <= 0) {
-        phase = measPhase + 180 + cnt * 360;
-    }
-
-//    fixedPhase = phase - 0.0015 * freq + 90;
-
-//    InputNotify(NOTIFY_CURRENT, realZ);
-//    InputNotify(NOTIFY_VOLT, imagZ);
-//    InputNotify(NOTIFY_IMPEDANCE, phase);
-
-    return phase; //(int16_t) fixedPhase;
+    return;
 }
 
-
-static uint32_t FixMag(uint32_t mag_0)
+static int32_t correctImag()
 {
-    static uint32_t freq;
-//    static uint64_t freq_c; //400kHz for 100kOhm Rload; 10.1 kOhm RTIA
-    static uint32_t mag;
-
-    freq = ReadFreq();
-    mag = mag_0 * (1 + (pow(freq / 100, 2) / pow(Cutoff_Freq, 2)));
-
-    return (uint16_t) mag;
-}
-
-static uint32_t CalcZ()
-{
-    static int32_t realZ;
-    static int32_t imagZ;
-    static uint32_t magC;
-    static uint32_t Z;
-    static uint16_t caliGain;
-
-    realZ = neg_18bit(ReadRealZ());
-    imagZ = neg_18bit(ReadImagZ());
-
-    magC = sqrt(imagZ * imagZ + realZ * realZ);
-    Z = GrabAmp() * 1000 / FixMag(magC) / 1.2;
-
-    return (uint32_t) Z;
+    return;
 }
 
 //////EIS PLOT RELATED FUNCTION END//////