From 75783b211ad6847006340fbd67eb17b86ac84672 Mon Sep 17 00:00:00 2001 From: Markus Zolliker Date: Wed, 2 Apr 2025 14:55:33 +0200 Subject: [PATCH] ultrasound.ControlRoi: fix control mechanism Change-Id: I9d01de260d3bdc63eb1004ba4f714d38d1c0508c --- frappy_psi/ultrasound.py | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) 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))