diff --git a/frappy_psi/ultrasound.py b/frappy_psi/ultrasound.py
index e099ea7..e98894b 100644
--- a/frappy_psi/ultrasound.py
+++ b/frappy_psi/ultrasound.py
@@ -98,26 +98,28 @@ class ControlRoi(Roi, Readable):
     def doPoll(self):
         inphase = self.value[0]
         freq = self.freq.target
+        newfreq = None
         if freq != self._freq_target:
             self._freq_target = freq
             # do no control 2 times after changing frequency
             self._skipctrl = 2
-        fdif = freq - self._old[0]
-        if self.control_active:
-            if self._old:
+        if self._old:
+            fdif = freq - self._old[0]
+            if self.control_active:
                 newfreq = freq + inphase * self.slope
                 self.log.info('fdif %r minstep %r', fdif, self.minstep)
-            else:
-                # do a 'test' step
-                newfreq = freq + self.minstep
-        if abs(fdif) > self.minstep * 0.99:
-            idif = inphase - self._old[1]
-            if idif:
-                self.slope = - fdif / idif
+            if abs(fdif) > self.minstep * 0.99:
+                idif = inphase - self._old[1]
+                if idif:
+                    self.slope = - fdif / idif
+                self._old = (freq, inphase)
+        else:
             self._old = (freq, inphase)
+            if self.control_active:
+                newfreq = freq + self.minstep
         if self._skipctrl > 0:  # do no control for some time after changing frequency
             self._skipctrl -= 1
-        elif self.control_active:
+        elif newfreq is not None:
             self._freq_target = self.freq.write_target(clamp(freq - self.maxstep, newfreq, freq + self.maxstep))