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()");
}