From 15001afb312e13266749f2ded35944e475804b02 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Tue, 16 Jul 2024 22:56:36 -0700 Subject: [PATCH] [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 | 33 +++++++---- .../src/main/native/include/frc/TimedRobot.h | 23 ++++---- .../edu/wpi/first/wpilibj/TimedRobot.java | 56 +++++++++++-------- 3 files changed, 66 insertions(+), 46 deletions(-) diff --git a/wpilibc/src/main/native/cpp/TimedRobot.cpp b/wpilibc/src/main/native/cpp/TimedRobot.cpp index 0b7b9cb3a2b..538d1b2cf47 100644 --- a/wpilibc/src/main/native/cpp/TimedRobot.cpp +++ b/wpilibc/src/main/native/cpp/TimedRobot.cpp @@ -14,7 +14,6 @@ #include #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(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(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 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(period.value() * 1e6)}, + std::chrono::microseconds{static_cast(offset.value() * 1e6)}); } diff --git a/wpilibc/src/main/native/include/frc/TimedRobot.h b/wpilibc/src/main/native/include/frc/TimedRobot.h index dcc510af2b8..647285474ad 100644 --- a/wpilibc/src/main/native/include/frc/TimedRobot.h +++ b/wpilibc/src/main/native/include/frc/TimedRobot.h @@ -4,6 +4,7 @@ #pragma once +#include #include #include #include @@ -14,7 +15,7 @@ #include #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 func; - units::second_t period; - units::second_t expirationTime; + std::chrono::microseconds period; + std::chrono::microseconds expirationTime; /** * Construct a callback container. @@ -84,15 +85,15 @@ 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 func, units::second_t startTime, - units::second_t period, units::second_t offset) + Callback(std::function 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; @@ -100,7 +101,7 @@ class TimedRobot : public IterativeRobotBase { }; hal::Handle m_notifier; - units::second_t m_startTime; + std::chrono::microseconds m_startTime; wpi::priority_queue, std::greater> m_callbacks; diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java index 180a17b457d..2251d712d9f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java +++ b/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 { 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 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))); } /**