diff --git a/hal/src/main/java/edu/wpi/first/hal/HAL.java b/hal/src/main/java/edu/wpi/first/hal/HAL.java index 5e07488c69..2fc90b3097 100644 --- a/hal/src/main/java/edu/wpi/first/hal/HAL.java +++ b/hal/src/main/java/edu/wpi/first/hal/HAL.java @@ -8,6 +8,8 @@ package edu.wpi.first.hal; import java.nio.ByteBuffer; +import java.util.ArrayList; +import java.util.List; /** * JNI Wrapper for HAL
. @@ -26,6 +28,40 @@ public final class HAL extends JNIWrapper { public static native void exitMain(); + private static native void simPeriodicBeforeNative(); + + public static final List s_simPeriodicBefore = new ArrayList<>(); + + /** + * Runs SimPeriodicBefore callbacks. IterativeRobotBase calls this prior + * to the user's simulationPeriodic code. + */ + public static void simPeriodicBefore() { + simPeriodicBeforeNative(); + synchronized (s_simPeriodicBefore) { + for (Runnable r : s_simPeriodicBefore) { + r.run(); + } + } + } + + private static native void simPeriodicAfterNative(); + + public static final List s_simPeriodicAfter = new ArrayList<>(); + + /** + * Runs SimPeriodicAfter callbacks. IterativeRobotBase calls this after + * the user's simulationPeriodic code. + */ + public static void simPeriodicAfter() { + simPeriodicAfterNative(); + synchronized (s_simPeriodicAfter) { + for (Runnable r : s_simPeriodicAfter) { + r.run(); + } + } + } + public static native void observeUserProgramStarting(); public static native void observeUserProgramDisabled(); diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java index 81475079b1..c8d8807258 100644 --- a/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java +++ b/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java @@ -7,6 +7,7 @@ package edu.wpi.first.hal.simulation; +import edu.wpi.first.hal.HAL; import edu.wpi.first.hal.JNIWrapper; public class SimulatorJNI extends JNIWrapper { @@ -21,4 +22,48 @@ public class SimulatorJNI extends JNIWrapper { public static native void stepTiming(long delta); public static native void stepTimingAsync(long delta); public static native void resetHandles(); + + public static class SimPeriodicBeforeCallback implements AutoCloseable { + private SimPeriodicBeforeCallback(Runnable r) { + m_run = r; + } + + @Override + public void close() { + synchronized (HAL.s_simPeriodicBefore) { + HAL.s_simPeriodicBefore.remove(m_run); + } + } + + private Runnable m_run; + } + + public static SimPeriodicBeforeCallback registerSimPeriodicBeforeCallback(Runnable r) { + synchronized (HAL.s_simPeriodicBefore) { + HAL.s_simPeriodicBefore.add(r); + } + return new SimPeriodicBeforeCallback(r); + } + + public static class SimPeriodicAfterCallback implements AutoCloseable { + private SimPeriodicAfterCallback(Runnable r) { + m_run = r; + } + + @Override + public void close() { + synchronized (HAL.s_simPeriodicAfter) { + HAL.s_simPeriodicAfter.remove(m_run); + } + } + + private Runnable m_run; + } + + public static SimPeriodicAfterCallback registerSimPeriodicAfterCallback(Runnable r) { + synchronized (HAL.s_simPeriodicAfter) { + HAL.s_simPeriodicAfter.add(r); + } + return new SimPeriodicAfterCallback(r); + } } diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index 17ef0b9cb2..63cb5a2da3 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/hal/src/main/native/athena/HAL.cpp @@ -422,6 +422,10 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) { void HAL_Shutdown(void) {} +void HAL_SimPeriodicBefore(void) {} + +void HAL_SimPeriodicAfter(void) {} + int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context, const char* feature) { if (feature == nullptr) { diff --git a/hal/src/main/native/athena/mockdata/MockHooks.cpp b/hal/src/main/native/athena/mockdata/MockHooks.cpp index 2e7bcfe212..8d1f268e7e 100644 --- a/hal/src/main/native/athena/mockdata/MockHooks.cpp +++ b/hal/src/main/native/athena/mockdata/MockHooks.cpp @@ -33,4 +33,18 @@ void HALSIM_SetSendError(HALSIM_SendErrorHandler handler) {} void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler) {} +int32_t HALSIM_RegisterSimPeriodicBeforeCallback( + HALSIM_SimPeriodicCallback callback, void* param) { + return 0; +} + +void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid) {} + +int32_t HALSIM_RegisterSimPeriodicAfterCallback( + HALSIM_SimPeriodicCallback callback, void* param) { + return 0; +} + +void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid) {} + } // extern "C" diff --git a/hal/src/main/native/cpp/jni/HAL.cpp b/hal/src/main/native/cpp/jni/HAL.cpp index bfd99b6ec3..57e3b11044 100644 --- a/hal/src/main/native/cpp/jni/HAL.cpp +++ b/hal/src/main/native/cpp/jni/HAL.cpp @@ -84,6 +84,30 @@ Java_edu_wpi_first_hal_HAL_exitMain HAL_ExitMain(); } +/* + * Class: edu_wpi_first_hal_HAL + * Method: simPeriodicBeforeNative + * Signature: ()V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_HAL_simPeriodicBeforeNative + (JNIEnv*, jclass) +{ + HAL_SimPeriodicBefore(); +} + +/* + * Class: edu_wpi_first_hal_HAL + * Method: simPeriodicAfterNative + * Signature: ()V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_HAL_simPeriodicAfterNative + (JNIEnv*, jclass) +{ + HAL_SimPeriodicAfter(); +} + /* * Class: edu_wpi_first_hal_HAL * Method: observeUserProgramStarting diff --git a/hal/src/main/native/include/hal/HALBase.h b/hal/src/main/native/include/hal/HALBase.h index ee5b054788..c7da58cb2d 100644 --- a/hal/src/main/native/include/hal/HALBase.h +++ b/hal/src/main/native/include/hal/HALBase.h @@ -157,6 +157,18 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode); */ void HAL_Shutdown(void); +/** + * Calls registered SimPeriodic "before" callbacks (only in simulation mode). + * This should be called prior to user code periodic simulation functions. + */ +void HAL_SimPeriodicBefore(void); + +/** + * Calls registered SimPeriodic "after" callbacks (only in simulation mode). + * This should be called after user code periodic simulation functions. + */ +void HAL_SimPeriodicAfter(void); + #ifdef __cplusplus } // extern "C" #endif diff --git a/hal/src/main/native/include/hal/simulation/MockHooks.h b/hal/src/main/native/include/hal/simulation/MockHooks.h index cef205e62e..0e91e73e7f 100644 --- a/hal/src/main/native/include/hal/simulation/MockHooks.h +++ b/hal/src/main/native/include/hal/simulation/MockHooks.h @@ -30,4 +30,13 @@ void HALSIM_SetSendError(HALSIM_SendErrorHandler handler); typedef int32_t (*HALSIM_SendConsoleLineHandler)(const char* line); void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler); +typedef void (*HALSIM_SimPeriodicCallback)(void* param); +int32_t HALSIM_RegisterSimPeriodicBeforeCallback( + HALSIM_SimPeriodicCallback callback, void* param); +void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid); + +int32_t HALSIM_RegisterSimPeriodicAfterCallback( + HALSIM_SimPeriodicCallback callback, void* param); +void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid); + } // extern "C" diff --git a/hal/src/main/native/sim/HAL.cpp b/hal/src/main/native/sim/HAL.cpp index 64c4a1090d..8153a25648 100644 --- a/hal/src/main/native/sim/HAL.cpp +++ b/hal/src/main/native/sim/HAL.cpp @@ -26,13 +26,38 @@ #include "hal/Extensions.h" #include "hal/handles/HandlesInternal.h" #include "hal/simulation/DriverStationData.h" +#include "hal/simulation/SimCallbackRegistry.h" #include "mockdata/RoboRioDataInternal.h" using namespace hal; +namespace { +class SimPeriodicCallbackRegistry : public impl::SimCallbackRegistryBase { + public: + int32_t Register(HALSIM_SimPeriodicCallback callback, void* param) { + std::scoped_lock lock(m_mutex); + return DoRegister(reinterpret_cast(callback), param); + } + + void operator()() const { +#ifdef _MSC_VER // work around VS2019 16.4.0 bug + std::scoped_lock lock(m_mutex); +#else + std::scoped_lock lock(m_mutex); +#endif + if (m_callbacks) { + for (auto&& cb : *m_callbacks) + reinterpret_cast(cb.callback)(cb.param); + } + } +}; +} // namespace + static HAL_RuntimeType runtimeType{HAL_Mock}; static wpi::spinlock gOnShutdownMutex; static std::vector> gOnShutdown; +static SimPeriodicCallbackRegistry gSimPeriodicBefore; +static SimPeriodicCallbackRegistry gSimPeriodicAfter; namespace hal { namespace init { @@ -333,6 +358,28 @@ void HAL_OnShutdown(void* param, void (*func)(void*)) { gOnShutdown.emplace_back(param, func); } +void HAL_SimPeriodicBefore(void) { gSimPeriodicBefore(); } + +void HAL_SimPeriodicAfter(void) { gSimPeriodicAfter(); } + +int32_t HALSIM_RegisterSimPeriodicBeforeCallback( + HALSIM_SimPeriodicCallback callback, void* param) { + return gSimPeriodicBefore.Register(callback, param); +} + +void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid) { + gSimPeriodicBefore.Cancel(uid); +} + +int32_t HALSIM_RegisterSimPeriodicAfterCallback( + HALSIM_SimPeriodicCallback callback, void* param) { + return gSimPeriodicAfter.Register(callback, param); +} + +void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid) { + gSimPeriodicAfter.Cancel(uid); +} + int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context, const char* feature) { return 0; // Do nothing for now diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp index 7b29130b91..c99af93aca 100644 --- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp +++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp @@ -171,7 +171,9 @@ void IterativeRobotBase::LoopFunc() { m_watchdog.AddEpoch("Shuffleboard::Update()"); if constexpr (IsSimulation()) { + HAL_SimPeriodicBefore(); SimulationPeriodic(); + HAL_SimPeriodicAfter(); m_watchdog.AddEpoch("SimulationPeriodic()"); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java index daa5bdba5e..87d23936b0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java @@ -292,7 +292,9 @@ public abstract class IterativeRobotBase extends RobotBase { m_watchdog.addEpoch("Shuffleboard.update()"); if (isSimulation()) { + HAL.simPeriodicBefore(); simulationPeriodic(); + HAL.simPeriodicAfter(); m_watchdog.addEpoch("simulationPeriodic()"); }