[wpilib] Timer: Add getLoopTimestamp()

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

@@ -14,6 +14,7 @@
 #include <hal/Notifier.h>
 
 #include "frc/Errors.h"
+#include "frc/Timer.h"
 
 using namespace frc;
 
@@ -45,6 +46,7 @@ void TimedRobot::StartCompetition() {
     if (currentTime.count() == 0 || status != 0) {
       break;
     }
+    Timer::SetLoopTimestamp(currentTime);
 
     callback.func();
 

wpilibc/src/main/native/cpp/Timer.cpp

@@ -30,6 +30,8 @@ units::second_t GetTime() {
 
 using namespace frc;
 
+units::second_t Timer::s_loopTimestamp = 0_s;
+
 Timer::Timer() {
   Reset();
 }

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

@@ -128,6 +128,29 @@ class Timer {
    */
   static units::second_t GetFPGATimestamp();
 
+  /**
+   * Return the system clock time in seconds for the start of the current
+   * periodic loop. This is in the same time base as GetFPGATimestamp(), but is
+   * stable through a loop. This value is only valid for robot programs that use
+   * TimedRobot. It is updated at the beginning of every periodic callback
+   * (including the normal periodic loop). Calling this from threads other than
+   * than the main periodic loop has undefined behavior.
+   *
+   * @return Robot running time in seconds, as of the start of the current
+   * periodic function.
+   */
+  static units::second_t GetLoopTimestamp() { return s_loopTimestamp; }
+
+  /**
+   * Sets the timestamp returned by GetLoopTimestamp(). Intended for library
+   * use; calling this from team code may result in unexpected behavior.
+   *
+   * @param timestamp timestamp in seconds
+   */
+  static void SetLoopTimestamp(units::second_t timestamp) {
+    s_loopTimestamp = timestamp;
+  }
+
   /**
    * Return the approximate match time.
    *
@@ -146,6 +169,7 @@ class Timer {
   static units::second_t GetMatchTime();
 
  private:
+  static units::second_t s_loopTimestamp;
   units::second_t m_startTime = 0_s;
   units::second_t m_accumulatedTime = 0_s;
   bool m_running = false;

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

@@ -127,6 +127,7 @@ public void startCompetition() {
       if (currentTime == 0) {
         break;
       }
+      Timer.setLoopTimestamp(currentTime / 1000000.0);
 
       callback.func.run();
 

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

@@ -11,6 +11,8 @@
  * get() won't return a negative duration.
  */
 public class Timer {
+  private static double s_loopTimestamp;
+
   /**
    * Return the system clock time in seconds. Return the time from the FPGA hardware clock in
    * seconds since the FPGA started.
@@ -21,6 +23,29 @@ public static double getFPGATimestamp() {
     return RobotController.getFPGATime() / 1000000.0;
   }
 
+  /**
+   * Return the system clock time in seconds for the start of the current periodic loop. This is in
+   * the same time base as getFPGATimestamp(), but is stable through a loop. This value is only
+   * valid for robot programs that use TimedRobot. It is updated at the beginning of every periodic
+   * callback (including the normal periodic loop). Calling this from threads other than than the
+   * main periodic loop has undefined behavior.
+   *
+   * @return Robot running time in seconds, as of the start of the current periodic function.
+   */
+  public static double getLoopTimestamp() {
+    return s_loopTimestamp;
+  }
+
+  /**
+   * Sets the timestamp returned by getLoopTimestamp(). Intended for library use; calling this from
+   * team code may result in unexpected behavior.
+   *
+   * @param timestamp timestamp in seconds
+   */
+  public static void setLoopTimestamp(double timestamp) {
+    s_loopTimestamp = timestamp;
+  }
+
   /**
    * Return the approximate match time. The FMS does not send an official match time to the robots,
    * but does send an approximate match time. The value will count down the time remaining in the