diff --git a/frappy_psi/ultrasound.py b/frappy_psi/ultrasound.py
index a142fb5..e099ea7 100644
--- a/frappy_psi/ultrasound.py
+++ b/frappy_psi/ultrasound.py
@@ -102,17 +102,19 @@ class ControlRoi(Roi, Readable):
             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:
                 newfreq = freq + inphase * self.slope
-                fdif = freq - self._old[0]
-                if abs(fdif) >= self.minstep:
-                    idif = inphase - self._old[1]
-                    self.slope = - fdif / idif
+                self.log.info('fdif %r minstep %r', fdif, self.minstep)
             else:
                 # do a 'test' step
                 newfreq = freq + self.minstep
-        self.old = (freq, inphase)
+        if abs(fdif) > self.minstep * 0.99:
+            idif = inphase - self._old[1]
+            if idif:
+                self.slope = - fdif / idif
+            self._old = (freq, inphase)
         if self._skipctrl > 0:  # do no control for some time after changing frequency
             self._skipctrl -= 1
         elif self.control_active: