diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/SimHooks.java b/hal/src/main/java/edu/wpi/first/hal/sim/SimHooks.java index 6c46111a61..98054976ee 100644 --- a/hal/src/main/java/edu/wpi/first/hal/sim/SimHooks.java +++ b/hal/src/main/java/edu/wpi/first/hal/sim/SimHooks.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -24,4 +24,20 @@ public final class SimHooks { public static void restartTiming() { SimulatorJNI.restartTiming(); } + + public static void pauseTiming() { + SimulatorJNI.pauseTiming(); + } + + public static void resumeTiming() { + SimulatorJNI.resumeTiming(); + } + + public static boolean isTimingPaused() { + return SimulatorJNI.isTimingPaused(); + } + + public static void stepTiming(long delta) { + SimulatorJNI.stepTiming(delta); + } } diff --git a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SimulatorJNI.java b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SimulatorJNI.java index bb86005582..bfd8505772 100644 --- a/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SimulatorJNI.java +++ b/hal/src/main/java/edu/wpi/first/hal/sim/mockdata/SimulatorJNI.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -13,5 +13,9 @@ public class SimulatorJNI extends JNIWrapper { public static native void waitForProgramStart(); public static native void setProgramStarted(); public static native void restartTiming(); + public static native void pauseTiming(); + public static native void resumeTiming(); + public static native boolean isTimingPaused(); + public static native void stepTiming(long delta); public static native void resetHandles(); } diff --git a/hal/src/main/native/include/mockdata/MockHooks.h b/hal/src/main/native/include/mockdata/MockHooks.h index 02401b063b..318ff21135 100644 --- a/hal/src/main/native/include/mockdata/MockHooks.h +++ b/hal/src/main/native/include/mockdata/MockHooks.h @@ -14,6 +14,10 @@ void HALSIM_WaitForProgramStart(void); void HALSIM_SetProgramStarted(void); HAL_Bool HALSIM_GetProgramStarted(void); void HALSIM_RestartTiming(void); +void HALSIM_PauseTiming(void); +void HALSIM_ResumeTiming(void); +HAL_Bool HALSIM_IsTimingPaused(void); +void HALSIM_StepTiming(uint64_t delta); typedef int32_t (*HALSIM_SendErrorHandler)( HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode, const char* details, diff --git a/hal/src/main/native/sim/MockHooks.cpp b/hal/src/main/native/sim/MockHooks.cpp index 25c1d9f33e..59086ae0a1 100644 --- a/hal/src/main/native/sim/MockHooks.cpp +++ b/hal/src/main/native/sim/MockHooks.cpp @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -13,10 +13,12 @@ #include #include "MockHooksInternal.h" +#include "NotifierInternal.h" static std::atomic programStarted{false}; static std::atomic programStartTime{0}; +static std::atomic programPauseTime{0}; namespace hal { namespace init { @@ -25,12 +27,32 @@ void InitializeMockHooks() {} } // namespace hal namespace hal { -void RestartTiming() { programStartTime = wpi::Now(); } +void RestartTiming() { + programStartTime = wpi::Now(); + if (programPauseTime != 0) programPauseTime = programStartTime.load(); +} + +void PauseTiming() { + if (programPauseTime == 0) programPauseTime = wpi::Now(); +} + +void ResumeTiming() { + if (programPauseTime != 0) { + programStartTime += wpi::Now() - programPauseTime; + programPauseTime = 0; + } +} + +bool IsTimingPaused() { return programPauseTime != 0; } + +void StepTiming(uint64_t delta) { + if (programPauseTime != 0) programPauseTime += delta; +} int64_t GetFPGATime() { - auto now = wpi::Now(); - auto currentTime = now - programStartTime; - return currentTime; + uint64_t curTime = programPauseTime; + if (curTime == 0) curTime = wpi::Now(); + return curTime - programStartTime; } double GetFPGATimestamp() { return GetFPGATime() * 1.0e-6; } @@ -56,4 +78,21 @@ void HALSIM_SetProgramStarted(void) { SetProgramStarted(); } HAL_Bool HALSIM_GetProgramStarted(void) { return GetProgramStarted(); } void HALSIM_RestartTiming(void) { RestartTiming(); } + +void HALSIM_PauseTiming(void) { + PauseTiming(); + PauseNotifiers(); +} + +void HALSIM_ResumeTiming(void) { + ResumeTiming(); + ResumeNotifiers(); +} + +HAL_Bool HALSIM_IsTimingPaused(void) { return IsTimingPaused(); } + +void HALSIM_StepTiming(uint64_t delta) { + StepTiming(delta); + WakeupNotifiers(); +} } // extern "C" diff --git a/hal/src/main/native/sim/MockHooksInternal.h b/hal/src/main/native/sim/MockHooksInternal.h index e8c09a9901..a69e9bf062 100644 --- a/hal/src/main/native/sim/MockHooksInternal.h +++ b/hal/src/main/native/sim/MockHooksInternal.h @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 2017-2019 FIRST. All Rights Reserved. */ /* Open Source Software - may be modified and shared by FRC teams. The code */ /* must be accompanied by the FIRST BSD license file in the root directory of */ /* the project. */ @@ -14,6 +14,14 @@ namespace hal { void RestartTiming(); +void PauseTiming(); + +void ResumeTiming(); + +bool IsTimingPaused(); + +void StepTiming(uint64_t delta); + int64_t GetFPGATime(); double GetFPGATimestamp(); diff --git a/hal/src/main/native/sim/Notifier.cpp b/hal/src/main/native/sim/Notifier.cpp index 1e94d1e344..211f7b22e2 100644 --- a/hal/src/main/native/sim/Notifier.cpp +++ b/hal/src/main/native/sim/Notifier.cpp @@ -7,6 +7,7 @@ #include "hal/Notifier.h" +#include #include #include #include @@ -17,15 +18,16 @@ #include #include "HALInitializer.h" +#include "NotifierInternal.h" #include "hal/HAL.h" #include "hal/cpp/fpga_clock.h" #include "hal/handles/UnlimitedHandleResource.h" +#include "mockdata/NotifierData.h" namespace { struct Notifier { std::string name; uint64_t waitTime; - bool updatedAlarm = false; bool active = true; bool running = false; wpi::mutex mutex; @@ -52,6 +54,7 @@ class NotifierHandleContainer }; static NotifierHandleContainer* notifierHandles; +static std::atomic notifiersPaused{false}; namespace hal { namespace init { @@ -60,6 +63,19 @@ void InitializeNotifier() { notifierHandles = &nH; } } // namespace init + +void PauseNotifiers() { notifiersPaused = true; } + +void ResumeNotifiers() { + notifiersPaused = false; + WakeupNotifiers(); +} + +void WakeupNotifiers() { + notifierHandles->ForEach([](HAL_NotifierHandle handle, Notifier* notifier) { + notifier->cond.notify_all(); + }); +} } // namespace hal extern "C" { @@ -117,7 +133,6 @@ void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle, std::scoped_lock lock(notifier->mutex); notifier->waitTime = triggerTime; notifier->running = true; - notifier->updatedAlarm = true; } // We wake up any waiters to change how long they're sleeping for @@ -143,27 +158,22 @@ uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle, std::unique_lock lock(notifier->mutex); while (notifier->active) { double waitTime; - if (!notifier->running) { + if (!notifier->running || notifiersPaused) { waitTime = (HAL_GetFPGATime(status) * 1e-6) + 1000.0; // If not running, wait 1000 seconds } else { waitTime = notifier->waitTime * 1e-6; } - // Don't wait twice - notifier->updatedAlarm = false; - auto timeoutTime = hal::fpga_clock::epoch() + std::chrono::duration(waitTime); notifier->cond.wait_until(lock, timeoutTime); - if (notifier->updatedAlarm) { - notifier->updatedAlarm = false; - continue; - } if (!notifier->running) continue; if (!notifier->active) break; + uint64_t curTime = HAL_GetFPGATime(status); + if (curTime < notifier->waitTime) continue; notifier->running = false; - return HAL_GetFPGATime(status); + return curTime; } return 0; } diff --git a/hal/src/main/native/sim/NotifierInternal.h b/hal/src/main/native/sim/NotifierInternal.h new file mode 100644 index 0000000000..84232d2a83 --- /dev/null +++ b/hal/src/main/native/sim/NotifierInternal.h @@ -0,0 +1,14 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +namespace hal { +void PauseNotifiers(); +void ResumeNotifiers(); +void WakeupNotifiers(); +} // namespace hal diff --git a/hal/src/main/native/sim/jni/SimulatorJNI.cpp b/hal/src/main/native/sim/jni/SimulatorJNI.cpp index bba8f01521..9226f91850 100644 --- a/hal/src/main/native/sim/jni/SimulatorJNI.cpp +++ b/hal/src/main/native/sim/jni/SimulatorJNI.cpp @@ -142,6 +142,54 @@ Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_restartTiming HALSIM_RestartTiming(); } +/* + * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI + * Method: pauseTiming + * Signature: ()V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_pauseTiming + (JNIEnv*, jclass) +{ + HALSIM_PauseTiming(); +} + +/* + * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI + * Method: resumeTiming + * Signature: ()V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_resumeTiming + (JNIEnv*, jclass) +{ + HALSIM_ResumeTiming(); +} + +/* + * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI + * Method: isTimingPaused + * Signature: ()Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_isTimingPaused + (JNIEnv*, jclass) +{ + return HALSIM_IsTimingPaused(); +} + +/* + * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI + * Method: stepTiming + * Signature: (J)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_hal_sim_mockdata_SimulatorJNI_stepTiming + (JNIEnv*, jclass, jlong delta) +{ + HALSIM_StepTiming(delta); +} + /* * Class: edu_wpi_first_hal_sim_mockdata_SimulatorJNI * Method: resetHandles