[hal] Update waitForProgramStart to optionally wait for first notifier (#8932)

This addresses a race condition caused by TimedRobot and other
frameworks not creating their first notifier alarm until after signaling
program start.

Default this to true because it's the most common desired use case when
combined with TimedRobot.

hal/src/main/java/org/wpilib/hardware/hal/simulation/SimulatorJNI.java

@@ -11,7 +11,7 @@ import org.wpilib.hardware.hal.JNIWrapper;
 public class SimulatorJNI extends JNIWrapper {
   public static native void setRuntimeType(int type);
 
-  public static native void waitForProgramStart();
+  public static native void waitForProgramStart(boolean waitForFirstNotifier);
 
   public static native void setProgramStarted(boolean started);
 

hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp

@@ -141,13 +141,13 @@ Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_setRuntimeType
 /*
  * Class:     org_wpilib_hardware_hal_simulation_SimulatorJNI
  * Method:    waitForProgramStart
- * Signature: ()V
+ * Signature: (Z)V
  */
 JNIEXPORT void JNICALL
 Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_waitForProgramStart
-  (JNIEnv*, jclass)
+  (JNIEnv*, jclass, jboolean waitForFirstNotifier)
 {
-  HALSIM_WaitForProgramStart();
+  HALSIM_WaitForProgramStart(waitForFirstNotifier);
 }
 
 /*

hal/src/main/native/include/wpi/hal/simulation/MockHooks.h

@@ -13,7 +13,7 @@ extern "C" {
 #endif
 
 void HALSIM_SetRuntimeType(HAL_RuntimeType type);
-void HALSIM_WaitForProgramStart(void);
+void HALSIM_WaitForProgramStart(HAL_Bool waitForFirstNotifier);
 void HALSIM_SetProgramStarted(HAL_Bool started);
 HAL_Bool HALSIM_GetProgramStarted(void);
 void HALSIM_SetProgramState(HAL_ControlWord controlWord);

hal/src/main/native/sim/MockHooks.cpp

@@ -77,7 +77,7 @@ bool GetProgramStarted() {
 using namespace wpi::hal;
 
 extern "C" {
-void HALSIM_WaitForProgramStart(void) {
+void HALSIM_WaitForProgramStart(HAL_Bool waitForFirstNotifier) {
   int count = 0;
   while (!programStarted) {
     count++;
@@ -86,6 +86,17 @@ void HALSIM_WaitForProgramStart(void) {
     }
     std::this_thread::sleep_for(std::chrono::milliseconds(1));
   }
+
+  // Frameworks observe program start before arming their first notifier alarm.
+  // Wait for that alarm so a following StepTiming() can see and service it.
+  while (waitForFirstNotifier &&
+         HALSIM_GetNextNotifierTimeout() == UINT64_MAX) {
+    count++;
+    if (count % 10 == 0) {
+      wpi::util::print("Waiting for first notifier alarm: {}\n", count);
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(1));
+  }
 }
 
 void HALSIM_SetProgramStarted(HAL_Bool started) {

hal/src/main/native/systemcore/mockdata/MockHooks.cpp

@@ -8,7 +8,7 @@ extern "C" {
 
 void HALSIM_SetRuntimeType(HAL_RuntimeType type) {}
 
-void HALSIM_WaitForProgramStart(void) {}
+void HALSIM_WaitForProgramStart(HAL_Bool waitForFirstNotifier) {}
 
 void HALSIM_SetProgramStarted(HAL_Bool started) {}
 

hal/src/main/python/hal/simulation/mockhooks.py

@@ -1,15 +1,20 @@
 import time
 import typing as T
 
-from ._simulation import getProgramStarted
+from ._simulation import getNextNotifierTimeout, getProgramStarted
 
 
-def waitForProgramStart(timeout: T.Optional[float] = None, delta: float = 0.001):
+def waitForProgramStart(
+    timeout: T.Optional[float] = None,
+    delta: float = 0.001,
+    waitForFirstNotifier: bool = True,
+):
     """
     Polls robot program and returns when it has reported that it started
 
     :param timeout: Amount of time to wait
     :param delta:   Amount of time to sleep between checks
+    :param waitForFirstNotifier: Wait for the first notifier alarm to be armed
     """
 
     # This is basically the same thing that the C version of this function
@@ -18,7 +23,9 @@ def waitForProgramStart(timeout: T.Optional[float] = None, delta: float = 0.001)
     until = None
     if timeout and timeout > 0:
         until = time.monotonic() + timeout
-    while not getProgramStarted():
+    while not getProgramStarted() or (
+        waitForFirstNotifier and getNextNotifierTimeout() == 0xFFFFFFFFFFFFFFFF
+    ):
         if until is not None and time.monotonic() > until:
             raise TimeoutError("Program did not start")
         time.sleep(delta)

wpilibc/src/main/native/cpp/simulation/SimHooks.cpp

@@ -12,8 +12,8 @@ void SetRuntimeType(HAL_RuntimeType type) {
   HALSIM_SetRuntimeType(type);
 }
 
-void WaitForProgramStart() {
-  HALSIM_WaitForProgramStart();
+void WaitForProgramStart(bool waitForFirstNotifier) {
+  HALSIM_WaitForProgramStart(waitForFirstNotifier);
 }
 
 void SetProgramStarted(bool started) {

wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp

@@ -21,8 +21,10 @@ void SetRuntimeType(HAL_RuntimeType type);
 
 /**
  * Waits until the user program has started.
+ *
+ * @param waitForFirstNotifier wait for the first notifier alarm to be armed
  */
-void WaitForProgramStart();
+void WaitForProgramStart(bool waitForFirstNotifier = true);
 
 /**
  * Sets flag that indicates if the user program has started.

wpilibj/src/main/java/org/wpilib/simulation/SimHooks.java

@@ -20,9 +20,18 @@ public final class SimHooks {
     SimulatorJNI.setRuntimeType(type);
   }
 
-  /** Waits until the user program has started. */
+  /** Waits until the user program has started and the first notifier alarm has been armed. */
   public static void waitForProgramStart() {
-    SimulatorJNI.waitForProgramStart();
+    waitForProgramStart(true);
+  }
+
+  /**
+   * Waits until the user program has started.
+   *
+   * @param waitForFirstNotifier wait for the first notifier alarm to be armed
+   */
+  public static void waitForProgramStart(boolean waitForFirstNotifier) {
+    SimulatorJNI.waitForProgramStart(waitForFirstNotifier);
   }
 
   /**