Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal] Revamp notifiers (#8424)

Browse Source 
This changes the HAL notifier interface to:
- Use wpiutil signal objects. This means waiting is done through the
`WPI_WaitObject` API instead of a dedicated function and allows for
higher level code to simultaneously wait on notifiers and other events.
- Interval timers are supported at the HAL layer
- Handlers are now required to acknowledge notifications. This is
invisible to users unless they're directly using the HAL API.
- For interval timers, an overrun count is maintained to detect if the
handler didn't acknowledge

The underlying implementation still uses condition variables for the
actual waiting. In basic testing using this approach seemed to be lower
jitter than timerfd.

Currently, the simulation and systemcore implementations are nearly
identical except for a few additional sim hook bits. This could be
refactored, but keeping them separate may make sense to keep the
systemcore implementation easy to read and reason about, or if we ever
choose to use a different underlying timer implementation on systemcore.

The simulation side API is unchanged in form but does change in
function--waiting for notifiers now only waits for currently running (or
newly signaled) notifiers to acknowledge. To avoid a race condition in
sim stepTiming, users of the low level API must make any alarm updates
(especially for one-shot alarms) prior to acknowledging the previous
alarm.

The only current use of the interval timer feature is the `Notifier`
class. The `TimedRobot` implementation still uses a single notifier and
its own interval timing logic to ensure consistent callback order. Using
separate notifiers for each user-level interval would substantially
increase complexity. `Watchdog` also doesn't use the interval timer, as
it's looking for an amount of time since the last `set` call rather than
a recurring interval time.

To reduce flicker, the sim GUI uses a fade out when a timeout goes from
set to unset.

This fixes tsan for wpilib and commands, and also fixes some spurious
test failures.
This commit is contained in:
Peter Johnson
2025-11-29 11:00:18 -08:00
committed by GitHub
parent 704b6ccaee
commit 02c8d5c9db
22 changed files with 738 additions and 712 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.github/workflows/sanitizers.yml vendored
View File

@@ -23,7 +23,7 @@ jobs:
- name: tsan
cmake-flags: "-DCMAKE_BUILD_TYPE=Tsan"
ctest-env: "TSAN_OPTIONS=second_deadlock_stack=1"
ctest-flags: "-E 'cscore|cameraserver|wpilibc|commandsv2'"
ctest-flags: "-E 'cscore|cameraserver'"
- name: ubsan
cmake-flags: "-DCMAKE_BUILD_TYPE=Ubsan"
ctest-env: ""

80
hal/src/main/java/org/wpilib/hardware/hal/NotifierJNI.java
View File

@@ -14,23 +14,23 @@ package org.wpilib.hardware.hal;
*/
public class NotifierJNI extends JNIWrapper {
/**
* Initializes a notifier.
* Creates a notifier.
*
* <p>A notifier is an FPGA controller timer that triggers at requested intervals based on the
* FPGA time. This can be used to make precise control loops.
* <p>A notifier is an timer that alarms at an initial and (optionally) repeated intervals. This
* can be used to make precise control loops.
*
* @return the created notifier
* @see "HAL_InitializeNotifier"
* @see "HAL_CreateNotifier"
*/
public static native int initializeNotifier();
public static native int createNotifier();
/**
* Sets the HAL notifier thread priority.
*
* <p>The HAL notifier thread is responsible for managing the FPGA's notifier interrupt and waking
* up user's Notifiers when it's their time to run. Giving the HAL notifier thread real-time
* priority helps ensure the user's real-time Notifiers, if any, are notified to run in a timely
* manner.
* <p>The HAL notifier thread is responsible for managing the system's notifier interrupt and
* waking up user's Notifiers when it's their time to run. Giving the HAL notifier thread
* real-time priority helps ensure the user's real-time Notifiers, if any, are notified to run in
* a timely manner.
*
* @param realTime Set to true to set a real-time priority, false for standard priority.
* @param priority Priority to set the thread to. For real-time, this is 1-99 with 99 being
@@ -50,40 +50,37 @@ public class NotifierJNI extends JNIWrapper {
public static native void setNotifierName(int notifierHandle, String name);
/**
* Stops a notifier from running.
* Destroys a notifier.
*
* <p>This will cause any call into waitForNotifierAlarm to return with time = 0.
* <p>Destruction wakes up any waiters.
*
* @param notifierHandle the notifier handle
* @see "HAL_StopNotifier"
* @see "HAL_DestroyNotifier"
*/
public static native void stopNotifier(int notifierHandle);
public static native void destroyNotifier(int notifierHandle);
/**
* Cleans a notifier.
* Updates the initial and interval alarm times for a notifier.
*
* <p>Note this also stops a notifier if it is already running.
* <p>The alarmTime is an absolute time (using the WPI now() time base) if absolute is true, or
* relative to the current time if absolute is false.
*
* <p>If intervalTime is non-zero, the notifier will alarm periodically following alarmTime at the
* given interval.
*
* <p>If an absolute alarmTime is in the past, the notifier will alarm immediately.
*
* @param notifierHandle the notifier handle
* @see "HAL_CleanNotifier"
* @param alarmTime the first alarm time (in microseconds)
* @param intervalTime the periodic interval time (in microseconds)
* @param absolute true if the alarm time is absolute
* @see "HAL_SetNotifierAlarm"
*/
public static native void cleanNotifier(int notifierHandle);
public static native void setNotifierAlarm(
int notifierHandle, long alarmTime, long intervalTime, boolean absolute);
/**
* Updates the trigger time for a notifier.
*
* <p>Note that this time is an absolute time relative to getFPGATime()
*
* @param notifierHandle the notifier handle
* @param triggerTime the updated trigger time
* @see "HAL_UpdateNotifierAlarm"
*/
public static native void updateNotifierAlarm(int notifierHandle, long triggerTime);
/**
* Cancels the next notifier alarm.
*
* <p>This does not cause waitForNotifierAlarm to return.
* Cancels all future notifier alarms for a notifier.
*
* @param notifierHandle the notifier handle
* @see "HAL_CancelNotifierAlarm"
@@ -91,16 +88,23 @@ public class NotifierJNI extends JNIWrapper {
public static native void cancelNotifierAlarm(int notifierHandle);
/**
* Waits for the next alarm for the specific notifier.
*
* <p>This is a blocking call until either the time elapses or stopNotifier gets called. If the
* latter occurs, this function will return zero and any loops using this function should exit.
* Failing to do so can lead to use-after-frees.
* Indicates the notifier alarm has been serviced. This must be called before waiting for the next
* alarm.
*
* @param notifierHandle the notifier handle
* @return the FPGA time the notifier returned
*/
public static native long waitForNotifierAlarm(int notifierHandle);
public static native void acknowledgeNotifierAlarm(int notifierHandle);
/**
* Gets the overrun count for a notifier.
*
* <p>An overrun occurs when a notifier's alarm is not serviced before the next scheduled alarm
* time.
*
* @param notifierHandle the notifier handle
* @return overrun count
*/
public static native int getNotifierOverrun(int notifierHandle);
/** Utility class. */
private NotifierJNI() {}

75
hal/src/main/native/cpp/jni/NotifierJNI.cpp
View File

@@ -11,6 +11,7 @@
#include "org_wpilib_hardware_hal_NotifierJNI.h"
#include "wpi/hal/Notifier.h"
#include "wpi/util/jni_util.hpp"
#include "wpi/util/string.h"
using namespace wpi::hal;
@@ -18,15 +19,15 @@ extern "C" {
/*
* Class: org_wpilib_hardware_hal_NotifierJNI
* Method: initializeNotifier
* Method: createNotifier
* Signature: ()I
*/
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_NotifierJNI_initializeNotifier
Java_org_wpilib_hardware_hal_NotifierJNI_createNotifier
(JNIEnv* env, jclass)
{
int32_t status = 0;
HAL_NotifierHandle notifierHandle = HAL_InitializeNotifier(&status);
HAL_NotifierHandle notifierHandle = HAL_CreateNotifier(&status);
if (notifierHandle <= 0 || !CheckStatusForceThrow(env, status)) {
return 0; // something went wrong in HAL
@@ -58,51 +59,40 @@ Java_org_wpilib_hardware_hal_NotifierJNI_setNotifierName
(JNIEnv* env, jclass cls, jint notifierHandle, jstring name)
{
int32_t status = 0;
HAL_SetNotifierName((HAL_NotifierHandle)notifierHandle,
wpi::util::java::JStringRef{env, name}.c_str(), &status);
wpi::util::java::JStringRef jname{env, name};
WPI_String wpiName = wpi::util::make_string(jname);
HAL_SetNotifierName((HAL_NotifierHandle)notifierHandle, &wpiName, &status);
CheckStatus(env, status);
}
/*
* Class: org_wpilib_hardware_hal_NotifierJNI
* Method: stopNotifier
* Method: destroyNotifier
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_NotifierJNI_stopNotifier
(JNIEnv* env, jclass cls, jint notifierHandle)
{
int32_t status = 0;
HAL_StopNotifier((HAL_NotifierHandle)notifierHandle, &status);
CheckStatus(env, status);
}
/*
* Class: org_wpilib_hardware_hal_NotifierJNI
* Method: cleanNotifier
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_NotifierJNI_cleanNotifier
Java_org_wpilib_hardware_hal_NotifierJNI_destroyNotifier
(JNIEnv* env, jclass, jint notifierHandle)
{
if (notifierHandle != HAL_kInvalidHandle) {
HAL_CleanNotifier((HAL_NotifierHandle)notifierHandle);
HAL_DestroyNotifier((HAL_NotifierHandle)notifierHandle);
}
}
/*
* Class: org_wpilib_hardware_hal_NotifierJNI
* Method: updateNotifierAlarm
* Signature: (IJ)V
* Method: setNotifierAlarm
* Signature: (IJJZ)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_NotifierJNI_updateNotifierAlarm
(JNIEnv* env, jclass cls, jint notifierHandle, jlong triggerTime)
Java_org_wpilib_hardware_hal_NotifierJNI_setNotifierAlarm
(JNIEnv* env, jclass cls, jint notifierHandle, jlong alarmTime,
jlong intervalTime, jboolean absolute)
{
int32_t status = 0;
HAL_UpdateNotifierAlarm((HAL_NotifierHandle)notifierHandle,
static_cast<uint64_t>(triggerTime), &status);
HAL_SetNotifierAlarm((HAL_NotifierHandle)notifierHandle,
static_cast<uint64_t>(alarmTime),
static_cast<uint64_t>(intervalTime), absolute, &status);
CheckStatus(env, status);
}
@@ -122,20 +112,35 @@ Java_org_wpilib_hardware_hal_NotifierJNI_cancelNotifierAlarm
/*
* Class: org_wpilib_hardware_hal_NotifierJNI
* Method: waitForNotifierAlarm
* Signature: (I)J
* Method: acknowledgeNotifierAlarm
* Signature: (I)V
*/
JNIEXPORT jlong JNICALL
Java_org_wpilib_hardware_hal_NotifierJNI_waitForNotifierAlarm
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_NotifierJNI_acknowledgeNotifierAlarm
(JNIEnv* env, jclass cls, jint notifierHandle)
{
int32_t status = 0;
uint64_t time =
HAL_WaitForNotifierAlarm((HAL_NotifierHandle)notifierHandle, &status);
HAL_AcknowledgeNotifierAlarm((HAL_NotifierHandle)notifierHandle, &status);
CheckStatus(env, status);
}
/*
* Class: org_wpilib_hardware_hal_NotifierJNI
* Method: getNotifierOverrun
* Signature: (I)I
*/
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_NotifierJNI_getNotifierOverrun
(JNIEnv* env, jclass cls, jint notifierHandle)
{
int32_t status = 0;
int32_t count =
HAL_GetNotifierOverrun((HAL_NotifierHandle)notifierHandle, &status);
CheckStatus(env, status);
return (jlong)time;
return (jint)count;
}
} // extern "C"

102
hal/src/main/native/include/wpi/hal/Notifier.h
View File

@@ -6,8 +6,12 @@
#include <stdint.h>
#ifdef __cplusplus
#include <string_view>
#endif
#include "wpi/hal/Types.h"
#include "wpi/util/nodiscard.h"
#include "wpi/util/string.h"
/**
* @defgroup hal_notifier Notifier Functions
@@ -20,20 +24,20 @@ extern "C" {
#endif
/**
* Initializes a notifier.
* Creates a notifier.
*
* A notifier is an FPGA controller timer that triggers at requested intervals
* based on the FPGA time. This can be used to make precise control loops.
* A notifier is an timer that alarms at an initial and (optionally) repeated
* intervals. This can be used to make precise control loops.
*
* @param[out] status Error status variable. 0 on success.
* @return the created notifier
*/
HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status);
HAL_NotifierHandle HAL_CreateNotifier(int32_t* status);
/**
* Sets the HAL notifier thread priority.
*
* The HAL notifier thread is responsible for managing the FPGA's notifier
* The HAL notifier thread is responsible for managing the system's notifier
* interrupt and waking up user's Notifiers when it's their time to run.
* Giving the HAL notifier thread real-time priority helps ensure the user's
* real-time Notifiers, if any, are notified to run in a timely manner.
@@ -56,45 +60,41 @@ HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority,
* @param[in] name name
* @param[out] status Error status variable. 0 on success.
*/
void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
int32_t* status);
void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle,
const struct WPI_String* name, int32_t* status);
/**
* Stops a notifier from running.
* Destroys a notifier.
*
* This will cause any call into HAL_WaitForNotifierAlarm to return with time =
* 0.
*
* @param[in] notifierHandle the notifier handle
* @param[out] status Error status variable. 0 on success.
*/
void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status);
/**
* Cleans a notifier.
*
* Note this also stops a notifier if it is already running.
* Destruction wakes up any waiters.
*
* @param[in] notifierHandle the notifier handle
*/
void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle);
void HAL_DestroyNotifier(HAL_NotifierHandle notifierHandle);
/**
* Updates the trigger time for a notifier.
* Updates the initial and interval alarm times for a notifier.
*
* Note that this time is an absolute time relative to HAL_GetFPGATime()
* The alarmTime is an absolute time (using the WPI_Now() time base) if
* absolute is true, or relative to the current time if absolute is false.
*
* If intervalTime is non-zero, the notifier will alarm periodically following
* alarmTime at the given interval.
*
* If an absolute alarmTime is in the past, the notifier will alarm immediately.
*
* @param[in] notifierHandle the notifier handle
* @param[in] triggerTime the updated trigger time
* @param[in] alarmTime the first alarm time (in microseconds)
* @param[in] intervalTime the periodic interval time (in microseconds)
* @param[in] absolute true if the alarm time is absolute
* @param[out] status Error status variable. 0 on success.
*/
void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
uint64_t triggerTime, int32_t* status);
void HAL_SetNotifierAlarm(HAL_NotifierHandle notifierHandle, uint64_t alarmTime,
uint64_t intervalTime, HAL_Bool absolute,
int32_t* status);
/**
* Cancels the next notifier alarm.
*
* This does not cause HAL_WaitForNotifierAlarm to return.
* Cancels all future notifier alarms for a notifier.
*
* @param[in] notifierHandle the notifier handle
* @param[out] status Error status variable. 0 on success.
@@ -103,22 +103,44 @@ void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
int32_t* status);
/**
* Waits for the next alarm for the specific notifier.
*
* This is a blocking call until either the time elapses or HAL_StopNotifier
* gets called. If the latter occurs, this function will return zero and any
* loops using this function should exit. Failing to do so can lead to
* use-after-frees.
* Indicates the notifier alarm has been serviced. This must be called before
* waiting for the next alarm.
*
* @param[in] notifierHandle the notifier handle
* @param[out] status Error status variable. 0 on success.
* @return the FPGA time the notifier returned
* @param[out] status Error status variable. 0 on success.
*/
WPI_NODISCARD
uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
void HAL_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle,
int32_t* status);
/**
* Gets the overrun count for a notifier.
*
* An overrun occurs when a notifier's alarm is not serviced before the next
* scheduled alarm time.
*
* @param[in] notifierHandle the notifier handle
* @param[out] status Error status variable. 0 on success.
* @return overrun count
*/
int32_t HAL_GetNotifierOverrun(HAL_NotifierHandle notifierHandle,
int32_t* status);
#ifdef __cplusplus
} // extern "C"
#endif
/** @} */
#ifdef __cplusplus
/**
* Sets the name of a notifier.
*
* @param[in] notifierHandle the notifier handle
* @param[in] name name
* @param[out] status Error status variable. 0 on success.
*/
inline void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle,
std::string_view name, int32_t* status) {
WPI_String nameStr = wpi::util::make_string(name);
HAL_SetNotifierName(notifierHandle, &nameStr, status);
}
#endif

5
hal/src/main/native/include/wpi/hal/simulation/NotifierData.h
View File

@@ -13,8 +13,9 @@ extern "C" {
struct HALSIM_NotifierInfo {
HAL_NotifierHandle handle;
char name[64];
uint64_t timeout;
HAL_Bool waitTimeValid;
uint64_t alarmTime;
uint64_t intervalTime;
int32_t overrunCount;
};
uint64_t HALSIM_GetNextNotifierTimeout(void);

432
hal/src/main/native/sim/Notifier.cpp
View File

@@ -4,317 +4,330 @@
#include "wpi/hal/Notifier.h"
#include <sys/types.h>
#include <atomic>
#include <chrono>
#include <cstdio>
#include <cstring>
#include <functional>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "HALInitializer.h"
#include "NotifierInternal.h"
#include "wpi/hal/Errors.h"
#include "wpi/hal/HALBase.h"
#include "wpi/hal/cpp/fpga_clock.h"
#include "wpi/hal/Threads.h"
#include "wpi/hal/Types.h"
#include "wpi/hal/handles/UnlimitedHandleResource.h"
#include "wpi/hal/simulation/NotifierData.h"
#include "wpi/util/SafeThread.hpp"
#include "wpi/util/SmallVector.hpp"
#include "wpi/util/StringExtras.hpp"
#include "wpi/util/condition_variable.hpp"
#include "wpi/util/mutex.hpp"
#include "wpi/util/Synchronization.h"
#include "wpi/util/priority_queue.hpp"
namespace {
struct Notifier {
std::string name;
uint64_t waitTime = UINT64_MAX;
bool active = true;
bool waitTimeValid = false; // True if waitTime is set and in the future
bool waitingForAlarm = false; // True if in HAL_WaitForNotifierAlarm()
uint64_t waitCount = 0; // Counts calls to HAL_WaitForNotifierAlarm()
wpi::util::mutex mutex;
wpi::util::condition_variable cond;
std::atomic<uint64_t> alarmTime = UINT64_MAX;
uint64_t intervalTime = 0;
std::atomic<int32_t> userOverrunCount = 0;
int32_t overrunCount = 0;
std::atomic_flag handlerSignaled{};
};
} // namespace
using namespace wpi::hal;
static wpi::util::mutex notifiersWaiterMutex;
static wpi::util::condition_variable notifiersWaiterCond;
class NotifierHandleContainer
: public UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
HAL_HandleEnum::Notifier> {
class NotifierThread : public wpi::util::SafeThread {
public:
~NotifierHandleContainer() {
ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
{
std::scoped_lock lock(notifier->mutex);
notifier->active = false;
notifier->waitTimeValid = false;
}
notifier->cond.notify_all(); // wake up any waiting threads
});
notifiersWaiterCond.notify_all();
}
void Main() override;
void ProcessAlarms(wpi::util::SmallVectorImpl<HAL_NotifierHandle>* signaled);
bool m_paused = false;
UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
HAL_HandleEnum::Notifier>
m_handles;
struct Alarm {
HAL_NotifierHandle handle;
std::shared_ptr<Notifier> notifier;
bool operator==(const Alarm& rhs) const { return handle == rhs.handle; }
bool operator>(const Alarm& rhs) const {
return notifier->alarmTime > rhs.notifier->alarmTime;
}
};
wpi::util::priority_queue<Alarm, std::vector<Alarm>, std::greater<Alarm>>
m_alarmQueue;
};
static NotifierHandleContainer* notifierHandles;
static std::atomic<bool> notifiersPaused{false};
class NotifierInstance {
public:
NotifierInstance() { owner.Start(); }
wpi::util::SafeThreadOwner<NotifierThread> owner;
};
namespace wpi::hal {
namespace init {
static NotifierInstance* notifierInstance;
namespace wpi::hal::init {
void InitializeNotifier() {
static NotifierHandleContainer nH;
notifierHandles = &nH;
static NotifierInstance n;
notifierInstance = &n;
}
} // namespace init
} // namespace wpi::hal::init
void PauseNotifiers() {
notifiersPaused = true;
}
void ResumeNotifiers() {
notifiersPaused = false;
WakeupNotifiers();
}
void WakeupNotifiers() {
notifierHandles->ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
notifier->cond.notify_all();
});
}
void WaitNotifiers() {
std::unique_lock ulock(notifiersWaiterMutex);
wpi::util::SmallVector<HAL_NotifierHandle, 8> waiters;
// Wait for all Notifiers to hit HAL_WaitForNotifierAlarm()
notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
std::scoped_lock lock(notifier->mutex);
if (notifier->active && !notifier->waitingForAlarm) {
waiters.emplace_back(handle);
void NotifierThread::Main() {
std::unique_lock lock(m_mutex);
while (m_active) {
if (m_paused || m_alarmQueue.empty()) {
// No alarms, wait indefinitely
m_cond.wait(lock);
continue;
}
});
for (;;) {
int count = 0;
int end = waiters.size();
while (count < end) {
auto& it = waiters[count];
if (auto notifier = notifierHandles->Get(it)) {
std::scoped_lock lock(notifier->mutex);
if (notifier->active && !notifier->waitingForAlarm) {
++count;
continue;
}
}
// No longer need to wait for it, put at end so it can be erased
std::swap(it, waiters[--end]);
// Wait until next alarm
const Alarm& alarm = m_alarmQueue.top();
int32_t status = 0;
uint64_t curTime = HAL_GetFPGATime(&status);
if (alarm.notifier->alarmTime > curTime) {
m_cond.wait_for(
lock, std::chrono::microseconds{alarm.notifier->alarmTime - curTime});
}
if (count == 0) {
if (!m_active) {
break;
}
waiters.resize(count);
notifiersWaiterCond.wait_for(ulock, std::chrono::duration<double>(1));
// Check paused again as we may have been paused while waiting
if (m_paused) {
continue;
}
ProcessAlarms(nullptr);
}
}
void WakeupWaitNotifiers() {
std::unique_lock ulock(notifiersWaiterMutex);
void NotifierThread::ProcessAlarms(
wpi::util::SmallVectorImpl<HAL_NotifierHandle>* signaled) {
int32_t status = 0;
uint64_t curTime = HAL_GetFPGATime(&status);
wpi::util::SmallVector<std::pair<HAL_NotifierHandle, uint64_t>, 8> waiters;
// Wake up Notifiers that have expired timeouts
notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
std::scoped_lock lock(notifier->mutex);
// Process alarms
while (!m_alarmQueue.empty() &&
m_alarmQueue.top().notifier->alarmTime <= curTime) {
Alarm alarm = m_alarmQueue.pop();
HAL_NotifierHandle handle = alarm.handle;
Notifier& notifier = *alarm.notifier;
// Only wait for the Notifier if it has a valid timeout that's expired
if (notifier->active && notifier->waitTimeValid &&
curTime >= notifier->waitTime) {
waiters.emplace_back(handle, notifier->waitCount);
notifier->cond.notify_all();
}
});
for (;;) {
int count = 0;
int end = waiters.size();
while (count < end) {
auto& it = waiters[count];
if (auto notifier = notifierHandles->Get(it.first)) {
std::scoped_lock lock(notifier->mutex);
// waitCount is used here instead of waitingForAlarm because we want to
// wait until HAL_WaitForNotifierAlarm() is exited, then reentered
if (notifier->active && notifier->waitCount == it.second) {
++count;
continue;
}
if (notifier.intervalTime > 0) {
// Schedule next alarm
notifier.alarmTime += notifier.intervalTime;
if (curTime >= notifier.alarmTime) {
// We missed at least one interval
int32_t missed = static_cast<int32_t>((curTime - notifier.alarmTime) /
notifier.intervalTime) +
1;
notifier.overrunCount += missed;
notifier.alarmTime +=
missed * notifier.intervalTime; // Skip missed intervals
}
// No longer need to wait for it, put at end so it can be erased
it.swap(waiters[--end]);
// Reinsert into queue
m_alarmQueue.push(std::move(alarm));
} else {
// Disable one-shot alarm
notifier.alarmTime = UINT64_MAX;
}
if (count == 0) {
break;
// If the last call was acknowledged, signal the handler
if (!notifier.handlerSignaled.test_and_set()) {
if (signaled) {
signaled->emplace_back(handle);
}
// copy the overrun count for the handler to read, reset the local count
notifier.userOverrunCount = notifier.overrunCount;
notifier.overrunCount = 0;
wpi::util::SetSignalObject(handle);
}
waiters.resize(count);
notifiersWaiterCond.wait_for(ulock, std::chrono::duration<double>(1));
}
}
} // namespace wpi::hal
void wpi::hal::PauseNotifiers() {
auto thr = notifierInstance->owner.GetThread();
thr->m_paused = true;
}
void wpi::hal::ResumeNotifiers() {
auto thr = notifierInstance->owner.GetThread();
thr->m_paused = false;
thr->m_cond.notify_all();
}
void wpi::hal::WakeupNotifiers() {
auto thr = notifierInstance->owner.GetThread();
thr->ProcessAlarms(nullptr);
}
static void DoWaitNotifiers(
wpi::util::detail::SafeThreadProxy<NotifierThread>& thr,
wpi::util::SmallVectorImpl<HAL_NotifierHandle>& signaled) {
// Wait for signaled notifiers to acknowledge their last alarm
for (;;) {
signaled.erase(std::remove_if(signaled.begin(), signaled.end(),
[&](HAL_NotifierHandle handle) {
auto notifier = thr->m_handles.Get(handle);
return !notifier ||
!notifier->handlerSignaled.test();
}),
signaled.end());
if (signaled.empty()) {
break;
}
thr->m_cond.wait_for(thr.GetLock(), std::chrono::milliseconds{1});
}
}
void wpi::hal::WaitNotifiers() {
auto thr = notifierInstance->owner.GetThread();
wpi::util::SmallVector<HAL_NotifierHandle, 8> signaled;
thr->m_handles.ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
if (notifier->handlerSignaled.test()) {
signaled.emplace_back(handle);
}
});
DoWaitNotifiers(thr, signaled);
}
void wpi::hal::WakeupWaitNotifiers() {
auto thr = notifierInstance->owner.GetThread();
wpi::util::SmallVector<HAL_NotifierHandle, 8> signaled;
thr->ProcessAlarms(&signaled);
DoWaitNotifiers(thr, signaled);
}
extern "C" {
HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) {
HAL_NotifierHandle HAL_CreateNotifier(int32_t* status) {
wpi::hal::init::CheckInit();
std::shared_ptr<Notifier> notifier = std::make_shared<Notifier>();
HAL_NotifierHandle handle = notifierHandles->Allocate(notifier);
HAL_NotifierHandle handle =
notifierInstance->owner.GetThread()->m_handles.Allocate(notifier);
if (handle == HAL_kInvalidHandle) {
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
wpi::util::CreateSignalObject(handle);
return handle;
}
HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority,
int32_t* status) {
return true;
auto native = notifierInstance->owner.GetNativeThreadHandle();
return HAL_SetThreadPriority(&native, realTime, priority, status);
}
void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle,
const WPI_String* name, int32_t* status) {
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Get(notifierHandle);
if (!notifier) {
return;
}
std::scoped_lock lock(notifier->mutex);
notifier->name = name;
notifier->name = wpi::util::to_string_view(name);
}
void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
void HAL_DestroyNotifier(HAL_NotifierHandle notifierHandle) {
wpi::util::DestroySignalObject(notifierHandle);
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Free(notifierHandle);
thr->m_alarmQueue.remove({notifierHandle, notifier});
}
void HAL_SetNotifierAlarm(HAL_NotifierHandle notifierHandle, uint64_t alarmTime,
uint64_t intervalTime, HAL_Bool absolute,
int32_t* status) {
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Get(notifierHandle);
if (!notifier) {
return;
}
{
std::scoped_lock lock(notifier->mutex);
notifier->active = false;
notifier->waitTimeValid = false;
}
notifier->cond.notify_all();
}
void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle) {
auto notifier = notifierHandles->Free(notifierHandle);
if (!notifier) {
return;
if (!absolute) {
alarmTime += HAL_GetFPGATime(status);
}
// Just in case HAL_StopNotifier() wasn't called...
{
std::scoped_lock lock(notifier->mutex);
notifier->active = false;
notifier->waitTimeValid = false;
uint64_t prevWakeup = UINT64_MAX;
if (!thr->m_alarmQueue.empty()) {
prevWakeup = thr->m_alarmQueue.top().notifier->alarmTime;
thr->m_alarmQueue.remove({notifierHandle, notifier});
}
notifier->cond.notify_all();
}
notifier->alarmTime = alarmTime;
notifier->intervalTime = intervalTime;
notifier->overrunCount = 0;
thr->m_alarmQueue.push({notifierHandle, notifier});
void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
uint64_t triggerTime, int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
if (!notifier) {
return;
// wake up notifier thread if needed
if (alarmTime < prevWakeup) {
thr->m_cond.notify_all();
}
{
std::scoped_lock lock(notifier->mutex);
notifier->waitTime = triggerTime;
notifier->waitTimeValid = (triggerTime != UINT64_MAX);
}
// We wake up any waiters to change how long they're sleeping for
notifier->cond.notify_all();
}
void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Get(notifierHandle);
if (!notifier) {
return;
}
{
std::scoped_lock lock(notifier->mutex);
notifier->waitTimeValid = false;
}
thr->m_alarmQueue.remove({notifierHandle, notifier});
notifier->alarmTime = UINT64_MAX;
}
uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
void HAL_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle,
int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
auto notifier =
notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle);
if (!notifier) {
return 0;
return;
}
notifier->handlerSignaled.clear();
}
std::unique_lock ulock(notifiersWaiterMutex);
std::unique_lock lock(notifier->mutex);
notifier->waitingForAlarm = true;
++notifier->waitCount;
ulock.unlock();
notifiersWaiterCond.notify_all();
while (notifier->active) {
uint64_t curTime = HAL_GetFPGATime(status);
if (notifier->waitTimeValid && curTime >= notifier->waitTime) {
notifier->waitTimeValid = false;
notifier->waitingForAlarm = false;
return curTime;
}
double waitDuration;
if (!notifier->waitTimeValid || notifiersPaused) {
// If not running, wait 1000 seconds
waitDuration = 1000.0;
} else {
waitDuration = (notifier->waitTime - curTime) * 1e-6;
}
notifier->cond.wait_for(lock, std::chrono::duration<double>(waitDuration));
int32_t HAL_GetNotifierOverrun(HAL_NotifierHandle notifierHandle,
int32_t* status) {
auto notifier =
notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle);
if (!notifier) {
return -1;
}
notifier->waitingForAlarm = false;
return 0;
return notifier->userOverrunCount;
}
uint64_t HALSIM_GetNextNotifierTimeout(void) {
uint64_t timeout = UINT64_MAX;
notifierHandles->ForEach([&](HAL_NotifierHandle, Notifier* notifier) {
std::scoped_lock lock(notifier->mutex);
if (notifier->active && notifier->waitTimeValid &&
timeout > notifier->waitTime) {
timeout = notifier->waitTime;
}
});
return timeout;
auto thr = notifierInstance->owner.GetThread();
if (thr->m_alarmQueue.empty()) {
return UINT64_MAX;
}
return thr->m_alarmQueue.top().notifier->alarmTime;
}
int32_t HALSIM_GetNumNotifiers(void) {
auto thr = notifierInstance->owner.GetThread();
int32_t count = 0;
notifierHandles->ForEach([&](HAL_NotifierHandle, Notifier* notifier) {
std::scoped_lock lock(notifier->mutex);
if (notifier->active) {
++count;
}
});
thr->m_handles.ForEach([&](auto, auto) { ++count; });
return count;
}
int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size) {
auto thr = notifierInstance->owner.GetThread();
int32_t num = 0;
notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
std::scoped_lock lock(notifier->mutex);
if (!notifier->active) {
return;
}
thr->m_handles.ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
if (num < size) {
arr[num].handle = handle;
if (notifier->name.empty()) {
@@ -326,8 +339,9 @@ int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size) {
sizeof(arr[num].name) - 1);
arr[num].name[sizeof(arr[num].name) - 1] = '\0';
}
arr[num].timeout = notifier->waitTime;
arr[num].waitTimeValid = notifier->waitTimeValid;
arr[num].alarmTime = notifier->alarmTime;
arr[num].intervalTime = notifier->intervalTime;
arr[num].overrunCount = notifier->overrunCount;
}
++num;
});

263
hal/src/main/native/systemcore/Notifier.cpp
View File

@@ -6,182 +6,233 @@
#include <atomic>
#include <chrono>
#include <cstdio>
#include <cstring>
#include <functional>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include "HALInitializer.h"
#include "wpi/hal/Errors.h"
#include "wpi/hal/HALBase.h"
#include "wpi/hal/cpp/fpga_clock.h"
#include "wpi/hal/Threads.h"
#include "wpi/hal/Types.h"
#include "wpi/hal/handles/UnlimitedHandleResource.h"
#include "wpi/hal/simulation/NotifierData.h"
#include "wpi/util/SmallVector.hpp"
#include "wpi/util/StringExtras.hpp"
#include "wpi/util/condition_variable.hpp"
#include "wpi/util/mutex.hpp"
#include "wpi/util/SafeThread.hpp"
#include "wpi/util/Synchronization.h"
#include "wpi/util/priority_queue.hpp"
namespace {
struct Notifier {
std::string name;
uint64_t waitTime = UINT64_MAX;
bool active = true;
bool waitTimeValid = false; // True if waitTime is set and in the future
wpi::util::mutex mutex;
wpi::util::condition_variable cond;
std::atomic<uint64_t> alarmTime = UINT64_MAX;
uint64_t intervalTime = 0;
std::atomic<int32_t> userOverrunCount = 0;
int32_t overrunCount = 0;
std::atomic_flag handlerSignaled{};
};
} // namespace
using namespace wpi::hal;
static wpi::util::mutex notifiersWaiterMutex;
static wpi::util::condition_variable notifiersWaiterCond;
class NotifierHandleContainer
: public UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
HAL_HandleEnum::Notifier> {
class NotifierThread : public wpi::util::SafeThread {
public:
~NotifierHandleContainer() {
ForEach([](HAL_NotifierHandle handle, Notifier* notifier) {
{
std::scoped_lock lock(notifier->mutex);
notifier->active = false;
notifier->waitTimeValid = false;
}
notifier->cond.notify_all(); // wake up any waiting threads
});
notifiersWaiterCond.notify_all();
}
void Main() override;
void ProcessAlarms();
UnlimitedHandleResource<HAL_NotifierHandle, Notifier,
HAL_HandleEnum::Notifier>
m_handles;
struct Alarm {
HAL_NotifierHandle handle;
std::shared_ptr<Notifier> notifier;
bool operator==(const Alarm& rhs) const { return handle == rhs.handle; }
bool operator>(const Alarm& rhs) const {
return notifier->alarmTime > rhs.notifier->alarmTime;
}
};
wpi::util::priority_queue<Alarm, std::vector<Alarm>, std::greater<Alarm>>
m_alarmQueue;
};
static NotifierHandleContainer* notifierHandles;
class NotifierInstance {
public:
NotifierInstance() { owner.Start(); }
wpi::util::SafeThreadOwner<NotifierThread> owner;
};
static NotifierInstance* notifierInstance;
namespace wpi::hal::init {
void InitializeNotifier() {
static NotifierHandleContainer nH;
notifierHandles = &nH;
static NotifierInstance n;
notifierInstance = &n;
}
} // namespace wpi::hal::init
void NotifierThread::Main() {
std::unique_lock lock(m_mutex);
while (m_active) {
if (m_alarmQueue.empty()) {
// No alarms, wait indefinitely
m_cond.wait(lock);
continue;
}
// Wait until next alarm
const Alarm& alarm = m_alarmQueue.top();
int32_t status = 0;
uint64_t curTime = HAL_GetFPGATime(&status);
if (alarm.notifier->alarmTime > curTime) {
m_cond.wait_for(
lock, std::chrono::microseconds{alarm.notifier->alarmTime - curTime});
}
if (!m_active) {
break;
}
ProcessAlarms();
}
}
void NotifierThread::ProcessAlarms() {
int32_t status = 0;
uint64_t curTime = HAL_GetFPGATime(&status);
while (!m_alarmQueue.empty() &&
m_alarmQueue.top().notifier->alarmTime <= curTime) {
Alarm alarm = m_alarmQueue.pop();
HAL_NotifierHandle handle = alarm.handle;
Notifier& notifier = *alarm.notifier;
if (notifier.intervalTime > 0) {
// Schedule next alarm
notifier.alarmTime += notifier.intervalTime;
if (curTime >= notifier.alarmTime) {
// We missed at least one interval
int32_t missed = static_cast<int32_t>((curTime - notifier.alarmTime) /
notifier.intervalTime) +
1;
notifier.overrunCount += missed;
notifier.alarmTime +=
missed * notifier.intervalTime; // Skip missed intervals
}
// Reinsert into queue
m_alarmQueue.push(std::move(alarm));
} else {
// Disable one-shot alarm
notifier.alarmTime = UINT64_MAX;
}
// If the last call was acknowledged, signal the handler
if (!notifier.handlerSignaled.test_and_set()) {
// copy the overrun count for the handler to read, reset the local count
notifier.userOverrunCount = notifier.overrunCount;
notifier.overrunCount = 0;
wpi::util::SetSignalObject(handle);
}
}
}
extern "C" {
HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) {
HAL_NotifierHandle HAL_CreateNotifier(int32_t* status) {
wpi::hal::init::CheckInit();
std::shared_ptr<Notifier> notifier = std::make_shared<Notifier>();
HAL_NotifierHandle handle = notifierHandles->Allocate(notifier);
HAL_NotifierHandle handle =
notifierInstance->owner.GetThread()->m_handles.Allocate(notifier);
if (handle == HAL_kInvalidHandle) {
*status = HAL_HANDLE_ERROR;
return HAL_kInvalidHandle;
}
wpi::util::CreateSignalObject(handle);
return handle;
}
HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority,
int32_t* status) {
// There is no thread, so this can be removed.
return true;
auto native = notifierInstance->owner.GetNativeThreadHandle();
return HAL_SetThreadPriority(&native, realTime, priority, status);
}
void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle,
const WPI_String* name, int32_t* status) {
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Get(notifierHandle);
if (!notifier) {
return;
}
std::scoped_lock lock(notifier->mutex);
notifier->name = name;
notifier->name = wpi::util::to_string_view(name);
}
void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
void HAL_DestroyNotifier(HAL_NotifierHandle notifierHandle) {
wpi::util::DestroySignalObject(notifierHandle);
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Free(notifierHandle);
thr->m_alarmQueue.remove({notifierHandle, notifier});
}
void HAL_SetNotifierAlarm(HAL_NotifierHandle notifierHandle, uint64_t alarmTime,
uint64_t intervalTime, HAL_Bool absolute,
int32_t* status) {
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Get(notifierHandle);
if (!notifier) {
return;
}
{
std::scoped_lock lock(notifier->mutex);
notifier->active = false;
notifier->waitTimeValid = false;
}
notifier->cond.notify_all();
}
void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle) {
auto notifier = notifierHandles->Free(notifierHandle);
if (!notifier) {
return;
if (!absolute) {
alarmTime += HAL_GetFPGATime(status);
}
// Just in case HAL_StopNotifier() wasn't called...
{
std::scoped_lock lock(notifier->mutex);
notifier->active = false;
notifier->waitTimeValid = false;
uint64_t prevWakeup = UINT64_MAX;
if (!thr->m_alarmQueue.empty()) {
prevWakeup = thr->m_alarmQueue.top().notifier->alarmTime;
thr->m_alarmQueue.remove({notifierHandle, notifier});
}
notifier->cond.notify_all();
}
notifier->alarmTime = alarmTime;
notifier->intervalTime = intervalTime;
notifier->overrunCount = 0;
thr->m_alarmQueue.push({notifierHandle, notifier});
void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
uint64_t triggerTime, int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
if (!notifier) {
return;
// wake up notifier thread if needed
if (alarmTime < prevWakeup) {
thr->m_cond.notify_all();
}
{
std::scoped_lock lock(notifier->mutex);
notifier->waitTime = triggerTime;
notifier->waitTimeValid = (triggerTime != UINT64_MAX);
}
// We wake up any waiters to change how long they're sleeping for
notifier->cond.notify_all();
}
void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
auto thr = notifierInstance->owner.GetThread();
auto notifier = thr->m_handles.Get(notifierHandle);
if (!notifier) {
return;
}
{
std::scoped_lock lock(notifier->mutex);
notifier->waitTimeValid = false;
}
thr->m_alarmQueue.remove({notifierHandle, notifier});
notifier->alarmTime = UINT64_MAX;
}
uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
void HAL_AcknowledgeNotifierAlarm(HAL_NotifierHandle notifierHandle,
int32_t* status) {
auto notifier = notifierHandles->Get(notifierHandle);
auto notifier =
notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle);
if (!notifier) {
return 0;
return;
}
notifier->handlerSignaled.clear();
}
std::unique_lock ulock(notifiersWaiterMutex);
std::unique_lock lock(notifier->mutex);
ulock.unlock();
notifiersWaiterCond.notify_all();
while (notifier->active) {
uint64_t curTime = HAL_GetFPGATime(status);
if (notifier->waitTimeValid && curTime >= notifier->waitTime) {
notifier->waitTimeValid = false;
return curTime;
}
double waitDuration;
if (!notifier->waitTimeValid) {
// If not running, wait 1000 seconds
waitDuration = 1000.0;
} else {
waitDuration = (notifier->waitTime - curTime) * 1e-6;
}
notifier->cond.wait_for(lock, std::chrono::duration<double>(waitDuration));
int32_t HAL_GetNotifierOverrun(HAL_NotifierHandle notifierHandle,
int32_t* status) {
auto notifier =
notifierInstance->owner.GetThread()->m_handles.Get(notifierHandle);
if (!notifier) {
return -1;
}
return 0;
return notifier->userOverrunCount;
}
} // extern "C"

2
hal/src/main/python/hal/simulation/resethandles.cpp
View File

@@ -15,7 +15,7 @@ void HALSIM_ResetGlobalHandles() {
HALSIM_GetNotifierInfo(info, sz);
for (int i = 0; i < sz; i++) {
HAL_CleanNotifier(info->handle);
HAL_DestroyNotifier(info->handle);
}
}

14
hal/src/main/python/semiwrap/Notifier.yml
View File

@@ -2,11 +2,15 @@ strip_prefixes:
- HAL_
functions:
HAL_InitializeNotifier:
HAL_CreateNotifier:
HAL_SetNotifierName:
HAL_StopNotifier:
HAL_CleanNotifier:
HAL_UpdateNotifierAlarm:
overloads:
HAL_NotifierHandle, const struct WPI_String*, int32_t*:
ignore: true
HAL_NotifierHandle, std::string_view, int32_t*:
HAL_DestroyNotifier:
HAL_SetNotifierAlarm:
HAL_CancelNotifierAlarm:
HAL_WaitForNotifierAlarm:
HAL_GetNotifierOverrun:
HAL_AcknowledgeNotifierAlarm:
HAL_SetNotifierThreadPriority:

5
hal/src/main/python/semiwrap/simulation/NotifierData.yml
View File

@@ -10,5 +10,6 @@ classes:
attributes:
handle:
name:
timeout:
waitTimeValid:
alarmTime:
intervalTime:
overrunCount:

53
simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp
View File

@@ -54,20 +54,61 @@ static void DisplayTiming() {
ImGui::PopItemWidth();
static std::vector<HALSIM_NotifierInfo> notifiers;
struct NotifierLastValue {
uint64_t alarmTime = UINT64_MAX;
double displayTime = 0;
};
constexpr double fadeDuration = 0.25;
double curGuiTime = ImGui::GetTime();
static std::vector<NotifierLastValue> notifierFades;
int32_t num = HALSIM_GetNotifierInfo(notifiers.data(), notifiers.size());
if (static_cast<uint32_t>(num) > notifiers.size()) {
notifiers.resize(num);
HALSIM_GetNotifierInfo(notifiers.data(), notifiers.size());
notifierFades.resize(num);
num = HALSIM_GetNotifierInfo(notifiers.data(), notifiers.size());
}
for (int32_t i = 0; i < num; ++i) {
if (notifiers[i].alarmTime != UINT64_MAX) {
notifierFades[i].alarmTime = notifiers[i].alarmTime;
notifierFades[i].displayTime = curGuiTime;
} else if (curGuiTime > (notifierFades[i].displayTime + fadeDuration)) {
notifierFades[i].alarmTime = UINT64_MAX;
notifierFades[i].displayTime = 0;
}
}
if (num > 0) {
ImGui::Separator();
}
ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
for (int32_t i = 0; i < num; ++i) {
ImGui::LabelText(notifiers[i].name, "%.3f",
notifiers[i].timeout / 1000000.0);
if (ImGui::BeginTable("Notifiers", 4, ImGuiTableFlags_Borders)) {
ImGui::TableSetupColumn("Name", ImGuiTableColumnFlags_WidthStretch);
ImGui::TableSetupColumn("Alarm", ImGuiTableColumnFlags_WidthStretch |
ImGuiTableColumnFlags_DefaultSort);
ImGui::TableSetupColumn("Interval", ImGuiTableColumnFlags_WidthStretch);
ImGui::TableSetupColumn("Overruns", ImGuiTableColumnFlags_WidthStretch);
ImGui::TableHeadersRow();
for (int32_t i = 0; i < num; ++i) {
ImGui::TableNextRow();
ImGui::TableNextColumn();
ImGui::TextUnformatted(notifiers[i].name);
ImGui::TableNextColumn();
if (notifierFades[i].alarmTime != UINT64_MAX) {
ImGui::PushStyleVar(
ImGuiStyleVar_Alpha,
1.0 - (curGuiTime - notifierFades[i].displayTime) / fadeDuration);
ImGui::Text("%.3f", notifierFades[i].alarmTime / 1000000.0);
ImGui::PopStyleVar();
}
ImGui::TableNextColumn();
if (notifiers[i].intervalTime != 0) {
ImGui::Text("%.3f", notifiers[i].intervalTime / 1000000.0);
}
ImGui::TableNextColumn();
ImGui::Text("%d", notifiers[i].overrunCount);
}
ImGui::EndTable();
}
ImGui::PopItemWidth();
}
void TimingGui::Initialize() {

30
wpilibc/src/main/native/cpp/framework/TimedRobot.cpp
View File

@@ -25,6 +25,8 @@ void TimedRobot::StartCompetition() {
std::puts("\n********** Robot program startup complete **********");
HAL_ObserveUserProgramStarting();
bool first = true;
// Loop forever, calling the appropriate mode-dependent function
while (true) {
// We don't have to check there's an element in the queue first because
@@ -33,17 +35,25 @@ void TimedRobot::StartCompetition() {
auto callback = m_callbacks.pop();
int32_t status = 0;
HAL_UpdateNotifierAlarm(m_notifier, callback.expirationTime.count(),
&status);
HAL_SetNotifierAlarm(m_notifier, callback.expirationTime.count(), 0, true,
&status);
WPILIB_CheckErrorStatus(status, "UpdateNotifierAlarm");
std::chrono::microseconds currentTime{
HAL_WaitForNotifierAlarm(m_notifier, &status)};
if (currentTime.count() == 0 || status != 0) {
// Acknowledge previous alarm after setting the next one to avoid a race
// against getting the next notifier timeout in HALSIM StepTiming.
if (first) {
first = false;
} else {
HAL_AcknowledgeNotifierAlarm(m_notifier, &status);
WPILIB_CheckErrorStatus(status, "AcknowledgeNotifierAlarm");
}
if (WPI_WaitForObject(m_notifier) == 0) {
break;
}
m_loopStartTimeUs = RobotController::GetFPGATime();
std::chrono::microseconds currentTime{m_loopStartTimeUs};
callback.func();
@@ -71,8 +81,8 @@ void TimedRobot::StartCompetition() {
}
void TimedRobot::EndCompetition() {
int32_t status = 0;
HAL_StopNotifier(m_notifier, &status);
HAL_DestroyNotifier(m_notifier);
m_notifier = HAL_kInvalidHandle;
}
TimedRobot::TimedRobot(wpi::units::second_t period)
@@ -81,7 +91,7 @@ TimedRobot::TimedRobot(wpi::units::second_t period)
AddPeriodic([=, this] { LoopFunc(); }, period);
int32_t status = 0;
m_notifier = HAL_InitializeNotifier(&status);
m_notifier = HAL_CreateNotifier(&status);
WPILIB_CheckErrorStatus(status, "InitializeNotifier");
HAL_SetNotifierName(m_notifier, "TimedRobot", &status);
@@ -93,9 +103,7 @@ TimedRobot::TimedRobot(wpi::units::hertz_t frequency)
TimedRobot::~TimedRobot() {
if (m_notifier != HAL_kInvalidHandle) {
int32_t status = 0;
HAL_StopNotifier(m_notifier, &status);
WPILIB_ReportError(status, "StopNotifier");
HAL_DestroyNotifier(m_notifier);
}
}

92
wpilibc/src/main/native/cpp/system/Notifier.cpp
View File

@@ -6,13 +6,11 @@
#include <utility>
#include <fmt/format.h>
#include "wpi/hal/DriverStation.h"
#include "wpi/hal/Notifier.h"
#include "wpi/hal/Threads.h"
#include "wpi/system/Errors.hpp"
#include "wpi/system/Timer.hpp"
#include "wpi/util/Synchronization.h"
using namespace wpi;
@@ -22,8 +20,8 @@ Notifier::Notifier(std::function<void()> callback) {
}
m_callback = callback;
int32_t status = 0;
m_notifier = HAL_InitializeNotifier(&status);
WPILIB_CheckErrorStatus(status, "InitializeNotifier");
m_notifier = HAL_CreateNotifier(&status);
WPILIB_CheckErrorStatus(status, "CreateNotifier");
m_thread = std::thread([=, this] {
for (;;) {
@@ -32,8 +30,7 @@ Notifier::Notifier(std::function<void()> callback) {
if (notifier == 0) {
break;
}
uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
if (curTime == 0 || status != 0) {
if (WPI_WaitForObject(notifier) == 0) {
break;
}
@@ -41,19 +38,16 @@ Notifier::Notifier(std::function<void()> callback) {
{
std::scoped_lock lock(m_processMutex);
callback = m_callback;
if (m_periodic) {
m_expirationTime += m_period;
UpdateAlarm();
} else {
// Need to update the alarm to cause it to wait again
UpdateAlarm(UINT64_MAX);
}
}
// Call callback
if (callback) {
callback();
}
// Ack notifier
HAL_AcknowledgeNotifierAlarm(notifier, &status);
WPILIB_CheckErrorStatus(status, "AcknowledgeNotifier");
}
});
}
@@ -64,7 +58,7 @@ Notifier::Notifier(int priority, std::function<void()> callback) {
}
m_callback = callback;
int32_t status = 0;
m_notifier = HAL_InitializeNotifier(&status);
m_notifier = HAL_CreateNotifier(&status);
WPILIB_CheckErrorStatus(status, "InitializeNotifier");
m_thread = std::thread([=, this] {
@@ -75,8 +69,7 @@ Notifier::Notifier(int priority, std::function<void()> callback) {
if (notifier == 0) {
break;
}
uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
if (curTime == 0 || status != 0) {
if (WPI_WaitForObject(notifier) == 0) {
break;
}
@@ -84,13 +77,6 @@ Notifier::Notifier(int priority, std::function<void()> callback) {
{
std::scoped_lock lock(m_processMutex);
callback = m_callback;
if (m_periodic) {
m_expirationTime += m_period;
UpdateAlarm();
} else {
// need to update the alarm to cause it to wait again
UpdateAlarm(UINT64_MAX);
}
}
// call callback
@@ -111,32 +97,29 @@ Notifier::Notifier(int priority, std::function<void()> callback) {
throw;
}
}
// Ack notifier
HAL_AcknowledgeNotifierAlarm(notifier, &status);
WPILIB_CheckErrorStatus(status, "AcknowledgeNotifier");
}
});
}
Notifier::~Notifier() {
int32_t status = 0;
// atomically set handle to 0, then clean
HAL_NotifierHandle handle = m_notifier.exchange(0);
HAL_StopNotifier(handle, &status);
WPILIB_ReportError(status, "StopNotifier");
HAL_DestroyNotifier(handle);
// Join the thread to ensure the callback has exited.
if (m_thread.joinable()) {
m_thread.join();
}
HAL_CleanNotifier(handle);
}
Notifier::Notifier(Notifier&& rhs)
: m_thread(std::move(rhs.m_thread)),
m_notifier(rhs.m_notifier.load()),
m_callback(std::move(rhs.m_callback)),
m_expirationTime(std::move(rhs.m_expirationTime)),
m_period(std::move(rhs.m_period)),
m_periodic(std::move(rhs.m_periodic)) {
m_callback(std::move(rhs.m_callback)) {
rhs.m_notifier = HAL_kInvalidHandle;
}
@@ -145,19 +128,12 @@ Notifier& Notifier::operator=(Notifier&& rhs) {
m_notifier = rhs.m_notifier.load();
rhs.m_notifier = HAL_kInvalidHandle;
m_callback = std::move(rhs.m_callback);
m_expirationTime = std::move(rhs.m_expirationTime);
m_period = std::move(rhs.m_period);
m_periodic = std::move(rhs.m_periodic);
return *this;
}
void Notifier::SetName(std::string_view name) {
fmt::memory_buffer buf;
fmt::format_to(fmt::appender{buf}, "{}", name);
buf.push_back('\0'); // null terminate
int32_t status = 0;
HAL_SetNotifierName(m_notifier, buf.data(), &status);
HAL_SetNotifierName(m_notifier, name, &status);
}
void Notifier::SetCallback(std::function<void()> callback) {
@@ -166,19 +142,15 @@ void Notifier::SetCallback(std::function<void()> callback) {
}
void Notifier::StartSingle(wpi::units::second_t delay) {
std::scoped_lock lock(m_processMutex);
m_periodic = false;
m_period = delay;
m_expirationTime = Timer::GetFPGATimestamp() + m_period;
UpdateAlarm();
int32_t status = 0;
HAL_SetNotifierAlarm(m_notifier, static_cast<uint64_t>(delay * 1e6), 0, false,
&status);
}
void Notifier::StartPeriodic(wpi::units::second_t period) {
std::scoped_lock lock(m_processMutex);
m_periodic = true;
m_period = period;
m_expirationTime = Timer::GetFPGATimestamp() + m_period;
UpdateAlarm();
int32_t status = 0;
HAL_SetNotifierAlarm(m_notifier, static_cast<uint64_t>(period * 1e6),
static_cast<uint64_t>(period * 1e6), false, &status);
}
void Notifier::StartPeriodic(wpi::units::hertz_t frequency) {
@@ -186,26 +158,16 @@ void Notifier::StartPeriodic(wpi::units::hertz_t frequency) {
}
void Notifier::Stop() {
std::scoped_lock lock(m_processMutex);
m_periodic = false;
int32_t status = 0;
HAL_CancelNotifierAlarm(m_notifier, &status);
WPILIB_CheckErrorStatus(status, "CancelNotifierAlarm");
}
void Notifier::UpdateAlarm(uint64_t triggerTime) {
int32_t Notifier::GetOverrun() const {
int32_t status = 0;
// Return if we are being destructed, or were not created successfully
auto notifier = m_notifier.load();
if (notifier == 0) {
return;
}
HAL_UpdateNotifierAlarm(notifier, triggerTime, &status);
WPILIB_CheckErrorStatus(status, "UpdateNotifierAlarm");
}
void Notifier::UpdateAlarm() {
UpdateAlarm(static_cast<uint64_t>(m_expirationTime * 1e6));
int32_t overrun = HAL_GetNotifierOverrun(m_notifier, &status);
WPILIB_CheckErrorStatus(status, "GetNotifierOverrun");
return overrun;
}
bool Notifier::SetHALThreadPriority(bool realTime, int32_t priority) {

25
wpilibc/src/main/native/cpp/system/Watchdog.cpp
View File

@@ -11,9 +11,11 @@
#include <fmt/format.h>
#include "wpi/hal/HALBase.h"
#include "wpi/hal/Notifier.h"
#include "wpi/system/Errors.hpp"
#include "wpi/system/Timer.hpp"
#include "wpi/util/Synchronization.h"
#include "wpi/util/mutex.hpp"
#include "wpi/util/priority_queue.hpp"
@@ -47,7 +49,7 @@ class Watchdog::Impl {
Watchdog::Impl::Impl() {
int32_t status = 0;
m_notifier = HAL_InitializeNotifier(&status);
m_notifier = HAL_CreateNotifier(&status);
WPILIB_CheckErrorStatus(status, "starting watchdog notifier");
HAL_SetNotifierName(m_notifier, "Watchdog", &status);
@@ -55,18 +57,14 @@ Watchdog::Impl::Impl() {
}
Watchdog::Impl::~Impl() {
int32_t status = 0;
// atomically set handle to 0, then clean
HAL_NotifierHandle handle = m_notifier.exchange(0);
HAL_StopNotifier(handle, &status);
WPILIB_ReportError(status, "stopping watchdog notifier");
HAL_DestroyNotifier(handle);
// Join the thread to ensure the handler has exited.
if (m_thread.joinable()) {
m_thread.join();
}
HAL_CleanNotifier(handle);
}
void Watchdog::Impl::UpdateAlarm() {
@@ -79,11 +77,10 @@ void Watchdog::Impl::UpdateAlarm() {
if (m_watchdogs.empty()) {
HAL_CancelNotifierAlarm(notifier, &status);
} else {
HAL_UpdateNotifierAlarm(
notifier,
static_cast<uint64_t>(m_watchdogs.top()->m_expirationTime.value() *
1e6),
&status);
HAL_SetNotifierAlarm(notifier,
static_cast<uint64_t>(
m_watchdogs.top()->m_expirationTime.value() * 1e6),
0, true, &status);
}
WPILIB_CheckErrorStatus(status, "updating watchdog notifier alarm");
}
@@ -95,10 +92,10 @@ void Watchdog::Impl::Main() {
if (notifier == 0) {
break;
}
uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
if (curTime == 0 || status != 0) {
if (WPI_WaitForObject(notifier) == 0) {
break;
}
uint64_t curTime = HAL_GetFPGATime(&status);
std::unique_lock lock(m_mutex);
@@ -129,6 +126,8 @@ void Watchdog::Impl::Main() {
lock.lock();
UpdateAlarm();
HAL_AcknowledgeNotifierAlarm(notifier, &status);
}
}

2
wpilibc/src/main/native/include/wpi/framework/TimedRobot.hpp
View File

@@ -119,7 +119,7 @@ class TimedRobot : public IterativeRobotBase {
}
};
wpi::hal::Handle<HAL_NotifierHandle, HAL_CleanNotifier> m_notifier;
wpi::hal::Handle<HAL_NotifierHandle, HAL_DestroyNotifier> m_notifier;
std::chrono::microseconds m_startTime;
uint64_t m_loopStartTimeUs = 0;

33
wpilibc/src/main/native/include/wpi/system/Notifier.hpp
View File

@@ -130,6 +130,16 @@ class Notifier {
*/
void Stop();
/**
* Gets the overrun count.
*
* An overrun occurs when a notifier's alarm is not serviced before the next
* scheduled alarm time.
*
* @return overrun count
*/
int32_t GetOverrun() const;
/**
* Sets the HAL notifier thread priority.
*
@@ -148,18 +158,6 @@ class Notifier {
static bool SetHALThreadPriority(bool realTime, int32_t priority);
private:
/**
* Update the HAL alarm time.
*
* @param triggerTime the time at which the next alarm will be triggered
*/
void UpdateAlarm(uint64_t triggerTime);
/**
* Update the HAL alarm time based on m_expirationTime.
*/
void UpdateAlarm();
// The thread waiting on the HAL alarm
std::thread m_thread;
@@ -171,17 +169,6 @@ class Notifier {
// The user-provided callback
std::function<void()> m_callback;
// The time at which the callback should be called. Has the same zero as
// Timer::GetFPGATimestamp().
wpi::units::second_t m_expirationTime = 0_s;
// If periodic, stores the callback period; if single, stores the time until
// the callback call.
wpi::units::second_t m_period = 0_s;
// True if the callback is periodic
bool m_periodic = false;
};
} // namespace wpi

1
wpilibc/src/main/python/semiwrap/Notifier.yml
View File

@@ -18,4 +18,5 @@ classes:
ignore: true
wpi::units::second_t:
Stop:
GetOverrun:
SetHALThreadPriority:

77
wpilibc/src/main/python/wpilib/src/rpy/Notifier.cpp
View File

@@ -7,11 +7,12 @@
#include <utility>
#include <fmt/format.h>
#include "wpi/hal/Notifier.h"
#include "wpi/hal/Threads.h"
#include "wpi/system/Errors.hpp"
#include "wpi/system/Timer.hpp"
#include "wpi/util/Synchronization.h"
#include <pybind11/functional.h>
#include <gilsafe_object.h>
@@ -37,8 +38,8 @@ PyNotifier::PyNotifier(std::function<void()> handler) {
}
m_handler = handler;
int32_t status = 0;
m_notifier = HAL_InitializeNotifier(&status);
WPILIB_CheckErrorStatus(status, "InitializeNotifier");
m_notifier = HAL_CreateNotifier(&status);
WPILIB_CheckErrorStatus(status, "CreateNotifier");
std::function<void()> target([this] {
py::gil_scoped_release release;
@@ -50,8 +51,7 @@ PyNotifier::PyNotifier(std::function<void()> handler) {
if (notifier == 0) {
break;
}
uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
if (curTime == 0 || status != 0) {
if (WPI_WaitForObject(notifier) == 0) {
break;
}
@@ -59,13 +59,6 @@ PyNotifier::PyNotifier(std::function<void()> handler) {
{
std::scoped_lock lock(m_processMutex);
handler = m_handler;
if (m_periodic) {
m_expirationTime += m_period;
UpdateAlarm();
} else {
// need to update the alarm to cause it to wait again
UpdateAlarm(UINT64_MAX);
}
}
// call callback
@@ -76,6 +69,10 @@ PyNotifier::PyNotifier(std::function<void()> handler) {
handler();
}
// Ack notifier
HAL_AcknowledgeNotifierAlarm(notifier, &status);
WPILIB_CheckErrorStatus(status, "AcknowledgeNotifier");
}
} catch (...) {
_hang_thread_if_finalizing();
@@ -95,27 +92,20 @@ PyNotifier::PyNotifier(std::function<void()> handler) {
}
PyNotifier::~PyNotifier() {
int32_t status = 0;
// atomically set handle to 0, then clean
HAL_NotifierHandle handle = m_notifier.exchange(0);
HAL_StopNotifier(handle, &status);
WPILIB_ReportError(status, "StopNotifier");
HAL_DestroyNotifier(handle);
// Join the thread to ensure the handler has exited.
if (m_thread) {
m_thread.attr("join")();
}
HAL_CleanNotifier(handle);
}
PyNotifier::PyNotifier(PyNotifier &&rhs)
: m_thread(std::move(rhs.m_thread)),
m_notifier(rhs.m_notifier.load()),
m_handler(std::move(rhs.m_handler)),
m_expirationTime(std::move(rhs.m_expirationTime)),
m_period(std::move(rhs.m_period)),
m_periodic(std::move(rhs.m_periodic)) {
m_handler(std::move(rhs.m_handler)) {
rhs.m_notifier = HAL_kInvalidHandle;
}
@@ -124,19 +114,12 @@ PyNotifier &PyNotifier::operator=(PyNotifier &&rhs) {
m_notifier = rhs.m_notifier.load();
rhs.m_notifier = HAL_kInvalidHandle;
m_handler = std::move(rhs.m_handler);
m_expirationTime = std::move(rhs.m_expirationTime);
m_period = std::move(rhs.m_period);
m_periodic = std::move(rhs.m_periodic);
return *this;
}
void PyNotifier::SetName(std::string_view name) {
fmt::memory_buffer buf;
fmt::format_to(fmt::appender{buf}, "{}", name);
buf.push_back('\0'); // null terminate
int32_t status = 0;
HAL_SetNotifierName(m_notifier, buf.data(), &status);
HAL_SetNotifierName(m_notifier, name, &status);
}
void PyNotifier::SetCallback(std::function<void()> handler) {
@@ -145,45 +128,31 @@ void PyNotifier::SetCallback(std::function<void()> handler) {
}
void PyNotifier::StartSingle(wpi::units::second_t delay) {
std::scoped_lock lock(m_processMutex);
m_periodic = false;
m_period = delay;
m_expirationTime = Timer::GetFPGATimestamp() + m_period;
UpdateAlarm();
int32_t status = 0;
HAL_SetNotifierAlarm(m_notifier, static_cast<uint64_t>(delay * 1e6), 0, false,
&status);
}
void PyNotifier::StartPeriodic(wpi::units::second_t period) {
std::scoped_lock lock(m_processMutex);
m_periodic = true;
m_period = period;
m_expirationTime = Timer::GetFPGATimestamp() + m_period;
UpdateAlarm();
int32_t status = 0;
HAL_SetNotifierAlarm(m_notifier, static_cast<uint64_t>(period * 1e6),
static_cast<uint64_t>(period * 1e6), false, &status);
}
void PyNotifier::Stop() {
std::scoped_lock lock(m_processMutex);
m_periodic = false;
int32_t status = 0;
HAL_CancelNotifierAlarm(m_notifier, &status);
WPILIB_CheckErrorStatus(status, "CancelNotifierAlarm");
}
void PyNotifier::UpdateAlarm(uint64_t triggerTime) {
int32_t PyNotifier::GetOverrun() const {
int32_t status = 0;
// Return if we are being destructed, or were not created successfully
auto notifier = m_notifier.load();
if (notifier == 0) {
return;
}
HAL_UpdateNotifierAlarm(notifier, triggerTime, &status);
WPILIB_CheckErrorStatus(status, "UpdateNotifierAlarm");
}
void PyNotifier::UpdateAlarm() {
UpdateAlarm(static_cast<uint64_t>(m_expirationTime * 1e6));
int32_t overrun = HAL_GetNotifierOverrun(m_notifier, &status);
WPILIB_CheckErrorStatus(status, "GetNotifierOverrun");
return overrun;
}
bool PyNotifier::SetHALThreadPriority(bool realTime, int32_t priority) {
int32_t status = 0;
return HAL_SetNotifierThreadPriority(realTime, priority, &status);
}
}

31
wpilibc/src/main/python/wpilib/src/rpy/Notifier.h
View File

@@ -91,6 +91,16 @@ class PyNotifier {
*/
void Stop();
/**
* Gets the overrun count.
*
* An overrun occurs when a notifier's alarm is not serviced before the next
* scheduled alarm time.
*
* @return overrun count
*/
int32_t GetOverrun() const;
/**
* Sets the HAL notifier thread priority.
*
@@ -109,18 +119,6 @@ class PyNotifier {
static bool SetHALThreadPriority(bool realTime, int32_t priority);
private:
/**
* Update the HAL alarm time.
*
* @param triggerTime the time at which the next alarm will be triggered
*/
void UpdateAlarm(uint64_t triggerTime);
/**
* Update the HAL alarm time based on m_expirationTime.
*/
void UpdateAlarm();
// The thread waiting on the HAL alarm
py::object m_thread;
@@ -132,15 +130,6 @@ private:
// Address of the handler
std::function<void()> m_handler;
// The absolute expiration time
wpi::units::second_t m_expirationTime = 0_s;
// The relative time (either periodic or single)
wpi::units::second_t m_period = 0_s;
// True if this is a periodic event
bool m_periodic = false;
};
} // namespace wpi

29
wpilibj/src/main/java/org/wpilib/framework/TimedRobot.java
View File

@@ -13,6 +13,7 @@ import org.wpilib.hardware.hal.NotifierJNI;
import org.wpilib.system.RobotController;
import org.wpilib.units.measure.Frequency;
import org.wpilib.units.measure.Time;
import org.wpilib.util.WPIUtilJNI;
/**
* TimedRobot implements the IterativeRobotBase robot program framework.
@@ -69,7 +70,7 @@ public class TimedRobot extends IterativeRobotBase {
// The C pointer to the notifier object. We don't use it directly, it is
// just passed to the JNI bindings.
private final int m_notifier = NotifierJNI.initializeNotifier();
private final int m_notifier = NotifierJNI.createNotifier();
private long m_startTimeUs;
private long m_loopStartTimeUs;
@@ -115,8 +116,7 @@ public class TimedRobot extends IterativeRobotBase {
@Override
public void close() {
NotifierJNI.stopNotifier(m_notifier);
NotifierJNI.cleanNotifier(m_notifier);
NotifierJNI.destroyNotifier(m_notifier);
}
/** Provide an alternate "main loop" via startCompetition(). */
@@ -130,6 +130,8 @@ public class TimedRobot extends IterativeRobotBase {
System.out.println("********** Robot program startup complete **********");
DriverStationJNI.observeUserProgramStarting();
boolean first = true;
// Loop forever, calling the appropriate mode-dependent function
while (true) {
// We don't have to check there's an element in the queue first because
@@ -137,14 +139,25 @@ public class TimedRobot extends IterativeRobotBase {
// at the end of the loop.
var callback = m_callbacks.poll();
NotifierJNI.updateNotifierAlarm(m_notifier, callback.expirationTime);
NotifierJNI.setNotifierAlarm(m_notifier, callback.expirationTime, 0, true);
long currentTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
if (currentTime == 0) {
// Acknowledge previous alarm after setting the next one to avoid a race
// against getting the next notifier timeout in HALSIM StepTiming.
if (first) {
first = false;
} else {
NotifierJNI.acknowledgeNotifierAlarm(m_notifier);
}
try {
WPIUtilJNI.waitForObject(m_notifier);
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
break;
}
m_loopStartTimeUs = RobotController.getFPGATime();
long currentTime = RobotController.getFPGATime();
m_loopStartTimeUs = currentTime;
callback.func.run();
@@ -174,7 +187,7 @@ public class TimedRobot extends IterativeRobotBase {
/** Ends the main loop in startCompetition(). */
@Override
public void endCompetition() {
NotifierJNI.stopNotifier(m_notifier);
NotifierJNI.destroyNotifier(m_notifier);
}
/**

80
wpilibj/src/main/java/org/wpilib/system/Notifier.java
View File

@@ -13,6 +13,7 @@ import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.NotifierJNI;
import org.wpilib.units.measure.Frequency;
import org.wpilib.units.measure.Time;
import org.wpilib.util.WPIUtilJNI;
/**
* Notifiers run a user-provided callback function on a separate thread.
@@ -33,24 +34,13 @@ public class Notifier implements AutoCloseable {
// The user-provided callback.
private Runnable m_callback;
// The time, in seconds, at which the callback should be called. Has the same
// zero as RobotController.getFPGATime().
private double m_expirationTime;
// If periodic, stores the callback period in seconds; if single, stores the time until
// the callback call in seconds.
private double m_period;
// True if the callback is periodic
private boolean m_periodic;
@Override
public void close() {
int handle = m_notifier.getAndSet(0);
if (handle == 0) {
return;
}
NotifierJNI.stopNotifier(handle);
NotifierJNI.destroyNotifier(handle);
// Join the thread to ensure the callback has exited.
if (m_thread.isAlive()) {
try {
@@ -60,28 +50,9 @@ public class Notifier implements AutoCloseable {
Thread.currentThread().interrupt();
}
}
NotifierJNI.cleanNotifier(handle);
m_thread = null;
}
/**
* Update the alarm hardware to reflect the next alarm.
*
* @param triggerTimeMicroS the time in microseconds at which the next alarm will be triggered
*/
private void updateAlarm(long triggerTimeMicroS) {
int notifier = m_notifier.get();
if (notifier == 0) {
return;
}
NotifierJNI.updateNotifierAlarm(notifier, triggerTimeMicroS);
}
/** Update the alarm hardware to reflect the next alarm. */
private void updateAlarm() {
updateAlarm((long) (m_expirationTime * 1e6));
}
/**
* Create a Notifier with the given callback.
*
@@ -93,7 +64,7 @@ public class Notifier implements AutoCloseable {
requireNonNullParam(callback, "callback", "Notifier");
m_callback = callback;
m_notifier.set(NotifierJNI.initializeNotifier());
m_notifier.set(NotifierJNI.createNotifier());
m_thread =
new Thread(
@@ -103,8 +74,10 @@ public class Notifier implements AutoCloseable {
if (notifier == 0) {
break;
}
long curTime = NotifierJNI.waitForNotifierAlarm(notifier);
if (curTime == 0) {
try {
WPIUtilJNI.waitForObject(notifier);
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
break;
}
@@ -112,13 +85,6 @@ public class Notifier implements AutoCloseable {
m_processLock.lock();
try {
threadHandler = m_callback;
if (m_periodic) {
m_expirationTime += m_period;
updateAlarm();
} else {
// Need to update the alarm to cause it to wait again
updateAlarm(-1);
}
} finally {
m_processLock.unlock();
}
@@ -127,6 +93,9 @@ public class Notifier implements AutoCloseable {
if (threadHandler != null) {
threadHandler.run();
}
// Acknowledge the alarm
NotifierJNI.acknowledgeNotifierAlarm(notifier);
}
});
m_thread.setName("Notifier");
@@ -179,15 +148,7 @@ public class Notifier implements AutoCloseable {
* @param delay Time in seconds to wait before the callback is called.
*/
public void startSingle(double delay) {
m_processLock.lock();
try {
m_periodic = false;
m_period = delay;
m_expirationTime = RobotController.getFPGATime() * 1e-6 + delay;
updateAlarm();
} finally {
m_processLock.unlock();
}
NotifierJNI.setNotifierAlarm(m_notifier.get(), (long) (delay * 1e6), 0, false);
}
/**
@@ -209,15 +170,8 @@ public class Notifier implements AutoCloseable {
* call to this method.
*/
public void startPeriodic(double period) {
m_processLock.lock();
try {
m_periodic = true;
m_period = period;
m_expirationTime = RobotController.getFPGATime() * 1e-6 + period;
updateAlarm();
} finally {
m_processLock.unlock();
}
long periodMicroS = (long) (period * 1e6);
NotifierJNI.setNotifierAlarm(m_notifier.get(), periodMicroS, periodMicroS, false);
}
/**
@@ -256,13 +210,7 @@ public class Notifier implements AutoCloseable {
* complete.
*/
public void stop() {
m_processLock.lock();
try {
m_periodic = false;
NotifierJNI.cancelNotifierAlarm(m_notifier.get());
} finally {
m_processLock.unlock();
}
NotifierJNI.cancelNotifierAlarm(m_notifier.get());
}
/**

17
wpilibj/src/main/java/org/wpilib/system/Watchdog.java
View File

@@ -10,8 +10,10 @@ import java.io.Closeable;
import java.util.PriorityQueue;
import java.util.concurrent.locks.ReentrantLock;
import org.wpilib.driverstation.DriverStation;
import org.wpilib.hardware.hal.HALUtil;
import org.wpilib.hardware.hal.NotifierJNI;
import org.wpilib.units.measure.Time;
import org.wpilib.util.WPIUtilJNI;
/**
* A class that's a wrapper around a watchdog timer.
@@ -42,7 +44,7 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
private static int m_notifier;
static {
m_notifier = NotifierJNI.initializeNotifier();
m_notifier = NotifierJNI.createNotifier();
NotifierJNI.setNotifierName(m_notifier, "Watchdog");
startDaemonThread(Watchdog::schedulerFunc);
}
@@ -225,8 +227,8 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
if (m_watchdogs.isEmpty()) {
NotifierJNI.cancelNotifierAlarm(m_notifier);
} else {
NotifierJNI.updateNotifierAlarm(
m_notifier, (long) (m_watchdogs.peek().m_expirationTime * 1e6));
NotifierJNI.setNotifierAlarm(
m_notifier, (long) (m_watchdogs.peek().m_expirationTime * 1e6), 0, true);
}
}
@@ -239,10 +241,13 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
private static void schedulerFunc() {
while (!Thread.currentThread().isInterrupted()) {
long curTime = NotifierJNI.waitForNotifierAlarm(m_notifier);
if (curTime == 0) {
try {
WPIUtilJNI.waitForObject(m_notifier);
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
break;
}
long curTime = HALUtil.getFPGATime();
m_queueMutex.lock();
try {
@@ -273,6 +278,8 @@ public class Watchdog implements Closeable, Comparable<Watchdog> {
m_queueMutex.lock();
updateAlarm();
NotifierJNI.acknowledgeNotifierAlarm(m_notifier);
} finally {
m_queueMutex.unlock();
}