diff --git a/ohmpi.py b/ohmpi.py
index f5a670c9304456a15486e1f934235e4d34701ff9..482172c6399fa30f1b7583df134621b0d1633c03 100644
--- a/ohmpi.py
+++ b/ohmpi.py
@@ -142,13 +142,7 @@ class OhmPi(object):
 
             # current injection module
             if self.idps:
-                self.pin2 = self.MCP_board.get_pin(2) # dsp +
-                self.pin2.direction = Direction.OUTPUT
-                self.pin2.value = True
-                self.pin3 = self.MCP_board.get_pin(3) # dsp -
-                self.pin3.direction = Direction.OUTPUT
-                self.pin3.value = True
-                time.sleep(4)
+                self.switch_dps('on')
                 self.DPS = minimalmodbus.Instrument(port='/dev/ttyUSB0', slaveaddress=1)  # port name, address (decimal)
                 self.DPS.serial.baudrate = 9600  # Baud rate 9600 as listed in doc
                 self.DPS.serial.bytesize = 8  #
@@ -157,12 +151,13 @@ class OhmPi(object):
                 self.DPS.serial.parity = 'N'  # No parity
                 self.DPS.mode = minimalmodbus.MODE_RTU  # RTU mode
                 self.DPS.write_register(0x0001, 1000, 0)  # max current allowed (100 mA for relays)
-                print(self.DPS.read_register(0x05,2 ))  # max current allowed (100 mA for relays) #voltage
-                
-                self.pin2.value = False
-                self.pin3.value = False
                 # (last number) 0 is for mA, 3 is for A
 
+                #self.soh_logger.debug(f'Battery voltage: {self.DPS.read_register(0x05,2 ):.3f}') TODO: SOH logger
+                print(self.DPS.read_register(0x05,2 ))
+                self.switch_dps('off')
+
+
             # injection courant and measure (TODO check if it works, otherwise back in run_measurement())
             self.pin0 = self.MCP_board.get_pin(0)
             self.pin0.direction = Direction.OUTPUT
@@ -328,6 +323,30 @@ class OhmPi(object):
         
         if strategy == 'constant':
             vab = volt
+
+            self.DPS.write_register(0x0000, volt, 2)
+            self.DPS.write_register(0x09, 1)  # DPS5005 on
+            time.sleep(best_tx_injtime)  # inject for given tx time
+            # autogain
+            self.ads_current = ads.ADS1115(self.i2c, gain=2 / 3, data_rate=860, address=self.ads_current_address)
+            self.ads_voltage = ads.ADS1115(self.i2c, gain=2 / 3, data_rate=860, address=self.ads_voltage_address)
+            gain_current = self._gain_auto(AnalogIn(self.ads_current, ads.P0))
+            gain_voltage0 = self._gain_auto(AnalogIn(self.ads_voltage, ads.P0))
+            gain_voltage2 = self._gain_auto(AnalogIn(self.ads_voltage, ads.P2))
+            gain_voltage = np.min([gain_voltage0, gain_voltage2])  # TODO: separate gain for P0 and P2
+            self.ads_current = ads.ADS1115(self.i2c, gain=gain_current, data_rate=860, address=self.ads_current_address)
+            self.ads_voltage = ads.ADS1115(self.i2c, gain=gain_voltage, data_rate=860, address=self.ads_voltage_address)
+            # we measure the voltage on both A0 and A2 to guess the polarity
+            I = AnalogIn(self.ads_current, ads.P0).voltage * 1000. / 50 / self.r_shunt  # noqa measure current
+            U0 = AnalogIn(self.ads_voltage, ads.P0).voltage * 1000.  # noqa measure voltage
+            U2 = AnalogIn(self.ads_voltage, ads.P2).voltage * 1000.  # noqa
+
+            # check polarity
+            polarity = 1  # by default, we guessed it right
+            vmn = U0
+            if U0 < 0:  # we guessed it wrong, let's use a correction factor
+                polarity = -1
+                vmn = U2
         
         elif strategy == 'vmax':
             # implement different strategy
@@ -357,9 +376,12 @@ class OhmPi(object):
                 self.ads_current = ads.ADS1115(self.i2c, gain=gain_current, data_rate=860, address=self.ads_current_address)
                 self.ads_voltage = ads.ADS1115(self.i2c, gain=gain_voltage, data_rate=860, address=self.ads_voltage_address)
                 # we measure the voltage on both A0 and A2 to guess the polarity
-                I = AnalogIn(self.ads_current, ads.P0).voltage * 1000. / 50 / self.r_shunt  # noqa measure current
-                U0 = AnalogIn(self.ads_voltage, ads.P0).voltage * 1000.  # noqa measure voltage
-                U2 = AnalogIn(self.ads_voltage, ads.P2).voltage * 1000.  # noqa
+                for i in range(10):
+                    I = AnalogIn(self.ads_current, ads.P0).voltage * 1000. / 50 / self.r_shunt  # noqa measure current
+                    U0 = AnalogIn(self.ads_voltage, ads.P0).voltage * 1000.  # noqa measure voltage
+                    U2 = AnalogIn(self.ads_voltage, ads.P2).voltage * 1000.  # noqa
+                    time.sleep(best_tx_injtime)
+                    print(U0,U2,I)
 
                 # check polarity
                 polarity = 1  # by default, we guessed it right
@@ -374,7 +396,6 @@ class OhmPi(object):
                 volt = volt - 2
                 self.DPS.write_register(0x0000, volt, 2)
                 #self.DPS.write_register(0x09, 1)  # DPS5005 on
-                #time.sleep(best_tx_injtime)
                 I = AnalogIn(self.ads_current, ads.P0).voltage * 1000. / 50 / self.r_shunt
                 U0 = AnalogIn(self.ads_voltage, ads.P0).voltage * 1000.
                 U2 = AnalogIn(self.ads_voltage, ads.P2).voltage * 1000.
@@ -394,7 +415,6 @@ class OhmPi(object):
                 factor = factor_vmn
             #print('factor', factor_I, factor_vmn)
             vab = factor * volt #* 0.8
-            print(factor, volt, vab)
             if vab > tx_max:
                 vab = tx_max
 
@@ -411,10 +431,11 @@ class OhmPi(object):
                     break
 
                 # set voltage for test
+                self.DPS.write_register(0x0000, volt, 2)
                 if count==1:
                     self.DPS.write_register(0x09, 1)  # DPS5005 on
-                    time.sleep(best_tx_injtime)  # inject for given tx time
-                self.DPS.write_register(0x0000, volt, 2)
+                time.sleep(best_tx_injtime)  # inject for given tx time
+
                 # autogain
                 self.ads_current = ads.ADS1115(self.i2c, gain=2 / 3, data_rate=860, address=self.ads_current_address)
                 self.ads_voltage = ads.ADS1115(self.i2c, gain=2 / 3, data_rate=860, address=self.ads_voltage_address)
@@ -462,8 +483,8 @@ class OhmPi(object):
         # print('polarity', polarity)
         self.pin0.value = False
         self.pin1.value = False
-        # compute constant
-        c = vmn / I
+        # # compute constant
+        # c = vmn / I
         Rab = (volt * 1000.) / I  # noqa
 
         self.exec_logger.debug(f'Rab = {Rab:.2f} Ohms')
@@ -819,16 +840,10 @@ class OhmPi(object):
             self.pin7.direction = Direction.OUTPUT
             self.pin7.value = False
             
-            if self.sequence is None :
+            if self.sequence is None:
                 if self.idps:
-                    self.pin2 = self.MCP_board.get_pin(2) # dsp +
-                    self.pin2.direction = Direction.OUTPUT
-                    self.pin2.value = True
-                    self.pin3 = self.MCP_board.get_pin(3) # dsp -
-                    self.pin3.direction = Direction.OUTPUT
-                    self.pin3.value = True
-                    time.sleep(5)
-                    
+                    self.switch_dps('on')
+
             self.pin5 = self.MCP_board.get_pin(5) #IHM on mesaurement
             self.pin5.direction = Direction.OUTPUT
             self.pin5.value = True
@@ -1145,8 +1160,8 @@ class OhmPi(object):
         self.data_logger.info(dd)
         self.pin5.value = False #IHM led on measurement off 
         if self.sequence is None :
-            self.pin2.value = False # DSP + off
-            self.pin3.value = False # DSP - off
+            self.switch_dps('off')
+
         return d
 
     def run_multiple_sequences(self, cmd_id=None, sequence_delay=None, nb_meas=None, **kwargs):
@@ -1234,15 +1249,17 @@ class OhmPi(object):
 
             # call the switch_mux function to switch to the right electrodes
             self.switch_mux_on(quad)
-            self.mcp = MCP23008(self.i2c, address=0x24)
-            self.pin2 = self.MCP_board.get_pin(2) # dsp -
-            self.pin2.direction = Direction.OUTPUT
-            self.pin2.value = True
-            self.pin3 = self.MCP_board.get_pin(3) # dsp -
-            self.pin3.direction = Direction.OUTPUT
-            self.pin3.value = True
-            time.sleep(4)
 
+            # switch on DPS
+            self.switch_dps('on')
+            # self.MCP_board = MCP23008(self.i2c, address=0x24)
+            # self.pin2 = self.MCP_board.get_pin(2)  # dsp +
+            # self.pin2.direction = Direction.OUTPUT
+            # self.pin2.value = True
+            # self.pin3 = self.MCP_board.get_pin(3)  # dsp -
+            # self.pin3.direction = Direction.OUTPUT
+            # self.pin3.value = True
+            # time.sleep(4)
             # run a measurement
             if self.on_pi:
                 acquired_data = self.run_measurement(quad, **kwargs)
@@ -1278,8 +1295,8 @@ class OhmPi(object):
             # save data and print in a text file
             self.append_and_save(filename, acquired_data)
             self.exec_logger.debug(f'quadrupole {i + 1:d}/{n:d}')
-        self.pin2.value = False
-        self.pin3.value = False
+
+        self.switch_dps('off')
         self.status = 'idle'
 
     def run_sequence_async(self, cmd_id=None, **kwargs):
@@ -1448,6 +1465,29 @@ class OhmPi(object):
             else:
                 self.exec_logger.warning(f'Unable to address electrode nr {electrode_nr}')
 
+    def switch_dps(self,state='off'):
+        """Switches DPS on or off.
+
+            Parameters
+            ----------
+            state : str
+                'on', 'off'
+            """
+        self.pin2 = self.MCP_board.get_pin(2) # dsp -
+        self.pin2.direction = Direction.OUTPUT
+        self.pin3 = self.MCP_board.get_pin(3) # dsp -
+        self.pin3.direction = Direction.OUTPUT
+        if state == 'on':
+            self.pin2.value = True
+            self.pin3.value = True
+            self.exec_logger.debug(f'Switching DPS on')
+            time.sleep(4)
+        elif state == 'off':
+            self.pin2.value = False
+            self.pin3.value = False
+            self.exec_logger.debug(f'Switching DPS off')
+
+
     def switch_mux_on(self, quadrupole, cmd_id=None):
         """Switches on multiplexer relays for given quadrupole.