diff --git a/hal/src/main/java/org/wpilib/hardware/hal/simulation/SimulatorJNI.java b/hal/src/main/java/org/wpilib/hardware/hal/simulation/SimulatorJNI.java index 3ca71ce596..929829169c 100644 --- a/hal/src/main/java/org/wpilib/hardware/hal/simulation/SimulatorJNI.java +++ b/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); diff --git a/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp index 2aab0b293d..79b34922fb 100644 --- a/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp +++ b/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); } /* diff --git a/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h b/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h index e5ac98c5d2..8f389c5d69 100644 --- a/hal/src/main/native/include/wpi/hal/simulation/MockHooks.h +++ b/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); diff --git a/hal/src/main/native/sim/MockHooks.cpp b/hal/src/main/native/sim/MockHooks.cpp index 97d574a61f..34600effb4 100644 --- a/hal/src/main/native/sim/MockHooks.cpp +++ b/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) { diff --git a/hal/src/main/native/systemcore/mockdata/MockHooks.cpp b/hal/src/main/native/systemcore/mockdata/MockHooks.cpp index a619cfb439..f17d5b87b0 100644 --- a/hal/src/main/native/systemcore/mockdata/MockHooks.cpp +++ b/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) {} diff --git a/hal/src/main/python/hal/simulation/mockhooks.py b/hal/src/main/python/hal/simulation/mockhooks.py index fc4ef11b22..eae82ee8b9 100644 --- a/hal/src/main/python/hal/simulation/mockhooks.py +++ b/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) diff --git a/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp b/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp index 803836558c..5e498564f5 100644 --- a/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp +++ b/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) { diff --git a/wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp b/wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp index 45a51271f8..3888b74255 100644 --- a/wpilibc/src/main/native/include/wpi/simulation/SimHooks.hpp +++ b/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. diff --git a/wpilibj/src/main/java/org/wpilib/simulation/SimHooks.java b/wpilibj/src/main/java/org/wpilib/simulation/SimHooks.java index ba3c51d556..d6b09e0e0d 100644 --- a/wpilibj/src/main/java/org/wpilib/simulation/SimHooks.java +++ b/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); } /**