diff --git a/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/headstage.h b/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/headstage.h
index f7495ded9..0bb0e82d9 100644
--- a/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/headstage.h
+++ b/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/headstage.h
@@ -663,8 +663,8 @@ static uint8_t *spi_transact_rec_instruction();
  */
 // static void spi_DAC_transact_instruction();
 static void ADC_write(uint8_t ADCin);
-static void ADC_read();
-static void DecodeADC(uint8_t *ADCrx);
+static void ADC_read(uint8_t ADCin);
+static void DecodeADC(uint8_t ADCin);
 
 /**
  * update the instruction of ADC control register
@@ -895,21 +895,24 @@ static void update_ZM_instruction(uint8 *ins) {
                         //  V1 = AIN0 voltage
                         // V1 / 1000 = Iin (mA)
                         case 0 :{
-                            ADC_write(0xC1);
+                            ADCin = 0xC1;
+                            ADC_write(ADCin);
                             break;
                         }
 
                         // AINp is AIN1; AINn is GND
                         // AIN1 = 25 * AIN0
                         case 1 :{
-                            ADC_write(0xD1);
+                            ADCin = 0xD1;
+                            ADC_write(ADCin);
                             break;
                         }
 
                         // AINp is AIN2; AINn is GND
                         // AIN2 = 25 * AIN1 = 625 * AIN0
                         case 2 :{
-                            ADC_write(0xE1);
+                            ADCin = 0xE1;
+                            ADC_write(ADCin);
                             break;
                         }
 
@@ -917,7 +920,8 @@ static void update_ZM_instruction(uint8 *ins) {
                         // AIN3 = (-0.1) * Vin (V)
                         // Vo = GNDS - Vin/10 (to GNDA)
                         case 3 :{
-                            ADC_write(0xF1);
+                            ADCin = 0xF1;
+                            ADC_write(ADCin);
                             break;
                         }
                         default :{
@@ -926,7 +930,7 @@ static void update_ZM_instruction(uint8 *ins) {
                     }
 
                     CPUdelay(16000);
-                    ADC_read();
+                    ADC_read(ADCin);
                     break;
                 }
                 default:{
diff --git a/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/impedance_meter.h b/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/impedance_meter.h
index b6aa4e04f..d947dea25 100644
--- a/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/impedance_meter.h
+++ b/simplelink/ble_sdk_2_02_02_25/src/examples/simple_peripheral/cc26xx/app/headstage/impedance_meter.h
@@ -169,7 +169,7 @@ static void ADC_write(uint8_t ADCin) {
     PIN_setOutputValue(pin_handle, ADC_CS, 1);  // ADC_CS HIGH
 }
 
-static void ADC_read(){
+static void ADC_read(uint8_t ADCin){
 
     if(SPI_mode != SPI_ADC){
         SPI_close(spiHandle0);  //
@@ -186,7 +186,7 @@ static void ADC_read(){
     ZM_spi_transaction(SPI_ADC_SIZE, spi_ADC_txbuf, spi_ADC_rxbuf);
     PIN_setOutputValue(pin_handle, ADC_CS, 1);  // ADC_CS HIGH
 
-    DecodeADC(spi_ADC_rxbuf);
+    DecodeADC(ADCin);
 
     SimpleProfile_SetParameter(BLE_CIS_BUFF_CHAR, SPI_ADC_SIZE, spi_ADC_rxbuf);
     SimpleProfile_SetParameter(BLE_DAT_BUFF_CHAR, SPI_ADC_SIZE, spi_ADC_rxbuf);
@@ -198,15 +198,63 @@ static void ADC_read(){
 // Vin = 10*GNDS - 10Vo
 //     = 10*GNDS - 10*(0xDDDD*187.5 uV)
 //     = 15 v - 10*(0xDDDD*187.5 uV)
-static void DecodeADC(uint8_t *ADCrx){
-    uint16_t MSB = ADCrx[0]*256;
-    uint16_t LSB = ADCrx[1];
+static void DecodeADC(uint8_t ADCin){
+    uint16_t MSB = spi_ADC_rxbuf[0]*256;
+    uint16_t LSB = spi_ADC_rxbuf[1];
 
-    double Vo = 10*(MSB+LSB)*187.5;
-    Vo = 15000000 - Vo;
+    double LSBuV = 0;
+    ADCin = ADCin & 0b00001110;
 
-    ADCrx[2] = (uint8_t) (Vo/1000000);
-    ADCrx[3] = (uint8_t) (Vo-ADCrx[0]*1000000)/10000;
+    switch(ADCin){
+        case 0x00:{
+            LSBuV = 187.5;
+            break;
+        }
+
+        case 0x02:{
+            LSBuV = 125;
+            break;
+        }
+
+        case 0x04:{
+            LSBuV = 62.5;
+            break;
+        }
+
+        case 0x06:{
+            LSBuV = 31.25;
+            break;
+        }
+
+        case 0x08:{
+            LSBuV = 15.625;
+            break;
+        }
+
+        case 0x0A:{
+            LSBuV = 7.8125;
+            break;
+        }
+        case 0x0C:{
+            LSBuV = 7.8125;
+            break;
+        }
+        case 0x0E:{
+            LSBuV = 7.8125;
+            break;
+        }
+
+        default:{
+            LSBuV = 187.5;
+            break;
+        }
+    }
+
+    double Vo = 1500000 - (MSB+LSB)*LSBuV;
+    Vo = Vo / 100000;
+
+    spi_ADC_rxbuf[2] = (uint8_t) (Vo/1);
+    spi_ADC_rxbuf[3] = (uint8_t) (100*Vo - 100*spi_ADC_rxbuf[2]);
 
 }