diff --git a/software/src/utils/timer.py b/software/src/utils/timer.py
index 99d7552..ae43f60 100644
--- a/software/src/utils/timer.py
+++ b/software/src/utils/timer.py
@@ -50,29 +50,36 @@ class TimerManager(object):
         heapq.heapify(self.timers)
         return i
 
+    def _next_timeout(self):
+        if len(self.timers) == 0:
+            if self.timer_debug:
+                print("timer: worker: queue empty")
+            return None
+        cur_nearest = self.timers[0][0]
+        next_timeout = cur_nearest - time.ticks_ms()
+        if self.timer_debug:
+            if next_timeout > 0:
+                print(f"timer: worker: next is {self.timers[0]}, sleep {next_timeout} ms")
+            else:
+                print(f"timer: worker: {self.timers[0]} elapsed @{cur_nearest}, delay {-next_timeout} ms")
+        return next_timeout
+
+    async def _wait(self, timeout):
+        try:
+            await asyncio.wait_for_ms(self.worker_event.wait(), timeout)
+            if self.timer_debug:
+                print("timer: worker: event")
+            # got woken up due to event
+            self.worker_event.clear()
+            return True
+        except asyncio.TimeoutError:
+            return False
+
     async def _timer_worker(self):
         while True:
-            if len(self.timers) == 0:
-                # Nothing to do
-                await self.worker_event.wait()
-                if self.timer_debug:
-                    print("_timer_worker: event 0")
-                self.worker_event.clear()
-                continue
-            cur_nearest = self.timers[0][0]
-            wait_time = cur_nearest - time.ticks_ms()
-            if wait_time > 0:
-                if self.timer_debug:
-                    print(f"_timer_worker: next is {self.timers[0]}, sleep {wait_time} ms")
-                try:
-                    await asyncio.wait_for_ms(self.worker_event.wait(), wait_time)
-                    if self.timer_debug:
-                        print("_timer_worker: event 1")
-                    # got woken up due to event
-                    self.worker_event.clear()
-                    continue
-                except asyncio.TimeoutError:
-                    pass
+            next_timeout = self._next_timeout()
+            if next_timeout is None or next_timeout > 0:
+                await self._wait(next_timeout)
             else:
                 _, callback = heapq.heappop(self.timers)
                 safe_callback(callback, "timer callback")