waitForNotifierAlarm provides loopStartTime, _getFPGATime for setNextStartTime

Signed-off-by: Mike Stitt <mike@stitt.cc>

Author: MikeStitt

subprojects/robotpy-wpilib/wpilib/timedrobotpy.py

@@ -91,6 +91,9 @@ def __lt__(self, other) -> bool:
     def __bool__(self) -> bool:
         return True
 
+    def __repr__(self) -> str:
+        return f"{{func={self.func.__name__}, _periodUs={self._periodUs}, expirationUs={self.expirationUs}}}"
+
 
 class _OrderedList:
     def __init__(self) -> None:
@@ -119,7 +122,7 @@ def __iter__(self) -> Iterable[Any]:
     def __contains__(self, item) -> bool:
         return item in self._data
 
-    def __str__(self) -> str:
+    def __repr__(self) -> str:
         return str(sorted(self._data))
 
 
@@ -155,7 +158,6 @@ def __init__(self, period: wpimath.units.seconds = kDefaultPeriod) -> None:
         self.addPeriodic(self._loopFunc, period=self._periodS)
 
         self._notifier, status = initializeNotifier()
-        print(f"{self._notifier}, {status} = initializeNotifier()")
         if status != 0:
             raise RuntimeError(
                 f"initializeNotifier() returned {self._notifier}, {status}"
@@ -193,33 +195,35 @@ def startCompetition(self) -> None:
                 if status != 0:
                     raise RuntimeError(f"updateNotifierAlarm() returned {status}")
 
-                currentTimeUs, status = waitForNotifierAlarm(self._notifier)
+                self._loopStartTimeUs, status = waitForNotifierAlarm(self._notifier)
                 if status != 0:
                     raise RuntimeError(
-                        f"waitForNotifierAlarm() returned currentTimeUs={currentTimeUs} status={status}"
+                        f"waitForNotifierAlarm() returned _loopStartTimeUs={self._loopStartTimeUs} status={status}"
                     )
 
-                if currentTimeUs == 0:
+                if self._loopStartTimeUs == 0:
                     # when HAL_StopNotifier(self.notifier) is called the above waitForNotifierAlarm
-                    # will return a currentTimeUs==0 and the API requires robots to stop any loops.
+                    # will return a _loopStartTimeUs==0 and the API requires robots to stop any loops.
                     # See the API for waitForNotifierAlarm
                     break
 
-                self._loopStartTimeUs = _getFPGATime()
-                self._runCallbackAndReschedule(callback, currentTimeUs)
+                self._runCallbackAndReschedule(callback)
 
                 #  Process all other callbacks that are ready to run
-                while self._callbacks.peek().expirationUs <= currentTimeUs:
+                while self._callbacks.peek().expirationUs <= self._loopStartTimeUs:
                     callback = self._callbacks.pop()
-                    self._runCallbackAndReschedule(callback, currentTimeUs)
+                    self._runCallbackAndReschedule(callback)
         finally:
+            # pytests hang on PC when we don't force a call to self._stopNotifier()
             self._stopNotifier()
 
-    def _runCallbackAndReschedule(
-        self, callback: _Callback, currentTimeUs: microsecondsAsInt
-    ) -> None:
+    def _runCallbackAndReschedule(self, callback: _Callback) -> None:
         callback.func()
-        callback.setNextStartTimeUs(currentTimeUs)
+        # The c++ implementation used the current time before the callback ran,
+        # to reschedule. By using _getFPGATime(), on a callback.func()
+        # that ran long we immediately push the next invocation to the
+        # following period.
+        callback.setNextStartTimeUs(_getFPGATime())
         self._callbacks.add(callback)
 
     def _stopNotifier(self):