[wpilib] Fix repeat TimedRobot callbacks on loop overrun (#4101)

If one of the *Init() functions takes several multiples of the nominal
loop time, the callbacks after that will run, then increment their
expiration time by the nominal loop time. Since the new expiration time
is still in the past, this will cause the callback to get repeatedly run
in quick succession until its expiration time catches up with the
current time.

This change keeps incrementing the expiration time until it's in the
future, which will avoid repeated runs. This doesn't delay other
callbacks, so they'll get a chance to run once before their expiration
times are corrected.

The other option is correcting all the expiration times at once, which
would starve the other callbacks even longer so that the callback
scheduling returns to a regular cadence sooner. The problem with this
approach is if a previous callback overruns the start of the next
callback, the next callback could potentially never get a chance to run.

wpilibc/src/main/native/cpp/TimedRobot.cpp

@@ -14,7 +14,6 @@
 #include <hal/Notifier.h>
 
 #include "frc/Errors.h"
-#include "frc/Timer.h"
 
 using namespace frc;
 
@@ -37,29 +36,36 @@ void TimedRobot::StartCompetition() {
     auto callback = m_callbacks.pop();
 
     int32_t status = 0;
-    HAL_UpdateNotifierAlarm(
-        m_notifier, static_cast<uint64_t>(callback.expirationTime * 1e6),
-        &status);
+    HAL_UpdateNotifierAlarm(m_notifier, callback.expirationTime.count(),
+                            &status);
     FRC_CheckErrorStatus(status, "UpdateNotifierAlarm");
 
-    uint64_t curTime = HAL_WaitForNotifierAlarm(m_notifier, &status);
-    if (curTime == 0 || status != 0) {
+    std::chrono::microseconds currentTime{
+        HAL_WaitForNotifierAlarm(m_notifier, &status)};
+    if (currentTime.count() == 0 || status != 0) {
       break;
     }
 
     callback.func();
 
-    callback.expirationTime += callback.period;
+    // Increment the expiration time by the number of full periods it's behind
+    // plus one to avoid rapid repeat fires from a large loop overrun. We assume
+    // currentTime ≥ expirationTime rather than checking for it since the
+    // callback wouldn't be running otherwise.
+    callback.expirationTime +=
+        callback.period + (currentTime - callback.expirationTime) /
+                              callback.period * callback.period;
     m_callbacks.push(std::move(callback));
 
     // Process all other callbacks that are ready to run
-    while (static_cast<uint64_t>(m_callbacks.top().expirationTime * 1e6) <=
-           curTime) {
+    while (m_callbacks.top().expirationTime <= currentTime) {
       callback = m_callbacks.pop();
 
       callback.func();
 
-      callback.expirationTime += callback.period;
+      callback.expirationTime +=
+          callback.period + (currentTime - callback.expirationTime) /
+                                callback.period * callback.period;
       m_callbacks.push(std::move(callback));
     }
   }
@@ -71,7 +77,7 @@ void TimedRobot::EndCompetition() {
 }
 
 TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) {
-  m_startTime = Timer::GetFPGATimestamp();
+  m_startTime = std::chrono::microseconds{RobotController::GetFPGATime()};
   AddPeriodic([=, this] { LoopFunc(); }, period);
 
   int32_t status = 0;
@@ -94,5 +100,8 @@ TimedRobot::~TimedRobot() {
 
 void TimedRobot::AddPeriodic(std::function<void()> callback,
                              units::second_t period, units::second_t offset) {
-  m_callbacks.emplace(callback, m_startTime, period, offset);
+  m_callbacks.emplace(
+      callback, m_startTime,
+      std::chrono::microseconds{static_cast<int64_t>(period.value() * 1e6)},
+      std::chrono::microseconds{static_cast<int64_t>(offset.value() * 1e6)});
 }

wpilibc/src/main/native/include/frc/TimedRobot.h

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <chrono>
 #include <functional>
 #include <utility>
 #include <vector>
@@ -14,7 +15,7 @@
 #include <wpi/priority_queue.h>
 
 #include "frc/IterativeRobotBase.h"
-#include "frc/Timer.h"
+#include "frc/RobotController.h"
 
 namespace frc {
 
@@ -73,8 +74,8 @@ class TimedRobot : public IterativeRobotBase {
   class Callback {
    public:
     std::function<void()> func;
-    units::second_t period;
-    units::second_t expirationTime;
+    std::chrono::microseconds period;
+    std::chrono::microseconds expirationTime;
 
     /**
      * Construct a callback container.
@@ -84,23 +85,23 @@ class TimedRobot : public IterativeRobotBase {
      * @param period    The period at which to run the callback.
      * @param offset    The offset from the common starting time.
      */
-    Callback(std::function<void()> func, units::second_t startTime,
-             units::second_t period, units::second_t offset)
+    Callback(std::function<void()> func, std::chrono::microseconds startTime,
+             std::chrono::microseconds period, std::chrono::microseconds offset)
         : func{std::move(func)},
           period{period},
-          expirationTime{startTime + offset +
-                         units::math::floor(
-                             (Timer::GetFPGATimestamp() - startTime) / period) *
-                             period +
-                         period} {}
+          expirationTime(
+              startTime + offset + period +
+              (std::chrono::microseconds{frc::RobotController::GetFPGATime()} -
+               startTime) /
+                  period * period) {}
 
     bool operator>(const Callback& rhs) const {
       return expirationTime > rhs.expirationTime;
     }
   };
 
   hal::Handle<HAL_NotifierHandle> m_notifier;
-  units::second_t m_startTime;
+  std::chrono::microseconds m_startTime;
 
   wpi::priority_queue<Callback, std::vector<Callback>, std::greater<Callback>>
       m_callbacks;

wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java

@@ -26,26 +26,26 @@ public class TimedRobot extends IterativeRobotBase {
   @SuppressWarnings("MemberName")
   static class Callback implements Comparable<Callback> {
     public Runnable func;
-    public double period;
-    public double expirationTime;
+    public long period;
+    public long expirationTime;
 
     /**
      * Construct a callback container.
      *
      * @param func The callback to run.
-     * @param startTimeSeconds The common starting point for all callback scheduling in seconds.
-     * @param periodSeconds The period at which to run the callback in seconds.
-     * @param offsetSeconds The offset from the common starting time in seconds.
+     * @param startTimeSeconds The common starting point for all callback scheduling in
+     *     microseconds.
+     * @param periodSeconds The period at which to run the callback in microseconds.
+     * @param offsetSeconds The offset from the common starting time in microseconds.
      */
-    Callback(Runnable func, double startTimeSeconds, double periodSeconds, double offsetSeconds) {
+    Callback(Runnable func, long startTimeUs, long periodUs, long offsetUs) {
       this.func = func;
-      this.period = periodSeconds;
+      this.period = periodUs;
       this.expirationTime =
-          startTimeSeconds
-              + offsetSeconds
-              + Math.floor((Timer.getFPGATimestamp() - startTimeSeconds) / this.period)
-                  * this.period
-              + this.period;
+          startTimeUs
+              + offsetUs
+              + this.period
+              + (RobotController.getFPGATime() - startTimeUs) / this.period * this.period;
     }
 
     @Override
@@ -62,7 +62,7 @@ public int hashCode() {
     public int compareTo(Callback rhs) {
       // Elements with sooner expiration times are sorted as lesser. The head of
       // Java's PriorityQueue is the least element.
-      return Double.compare(expirationTime, rhs.expirationTime);
+      return Long.compare(expirationTime, rhs.expirationTime);
     }
   }
 
@@ -73,7 +73,7 @@ public int compareTo(Callback rhs) {
   // just passed to the JNI bindings.
   private final int m_notifier = NotifierJNI.initializeNotifier();
 
-  private double m_startTime;
+  private long m_startTimeUs;
 
   private final PriorityQueue<Callback> m_callbacks = new PriorityQueue<>();
 
@@ -89,7 +89,7 @@ protected TimedRobot() {
    */
   protected TimedRobot(double period) {
     super(period);
-    m_startTime = Timer.getFPGATimestamp();
+    m_startTimeUs = RobotController.getFPGATime();
     addPeriodic(this::loopFunc, period);
     NotifierJNI.setNotifierName(m_notifier, "TimedRobot");
 
@@ -122,25 +122,33 @@ public void startCompetition() {
       // at the end of the loop.
       var callback = m_callbacks.poll();
 
-      NotifierJNI.updateNotifierAlarm(m_notifier, (long) (callback.expirationTime * 1e6));
+      NotifierJNI.updateNotifierAlarm(m_notifier, callback.expirationTime);
 
-      long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
-      if (curTime == 0) {
+      long currentTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
+      if (currentTime == 0) {
         break;
       }
 
       callback.func.run();
 
-      callback.expirationTime += callback.period;
+      // Increment the expiration time by the number of full periods it's behind
+      // plus one to avoid rapid repeat fires from a large loop overrun. We
+      // assume currentTime ≥ expirationTime rather than checking for it since
+      // the callback wouldn't be running otherwise.
+      callback.expirationTime +=
+          callback.period
+              + (currentTime - callback.expirationTime) / callback.period * callback.period;
       m_callbacks.add(callback);
 
       // Process all other callbacks that are ready to run
-      while ((long) (m_callbacks.peek().expirationTime * 1e6) <= curTime) {
+      while (m_callbacks.peek().expirationTime <= currentTime) {
         callback = m_callbacks.poll();
 
         callback.func.run();
 
-        callback.expirationTime += callback.period;
+        callback.expirationTime +=
+            callback.period
+                + (currentTime - callback.expirationTime) / callback.period * callback.period;
         m_callbacks.add(callback);
       }
     }
@@ -162,7 +170,7 @@ public void endCompetition() {
    * @param periodSeconds The period at which to run the callback in seconds.
    */
   public final void addPeriodic(Runnable callback, double periodSeconds) {
-    m_callbacks.add(new Callback(callback, m_startTime, periodSeconds, 0.0));
+    m_callbacks.add(new Callback(callback, m_startTimeUs, (long) (periodSeconds * 1e6), 0));
   }
 
   /**
@@ -177,7 +185,9 @@ public final void addPeriodic(Runnable callback, double periodSeconds) {
    *     scheduling a callback in a different timeslot relative to TimedRobot.
    */
   public final void addPeriodic(Runnable callback, double periodSeconds, double offsetSeconds) {
-    m_callbacks.add(new Callback(callback, m_startTime, periodSeconds, offsetSeconds));
+    m_callbacks.add(
+        new Callback(
+            callback, m_startTimeUs, (long) (periodSeconds * 1e6), (long) (offsetSeconds * 1e6)));
   }
 
   /**