[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)});
 }