Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-02 02:51:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal, wpilib] New DS thread model and implementation (#3787)

Browse Source 
The current DS thread model has some pretty major issues. It makes it difficult to know if all data is from the same remote packet, and if the data changes while the robot loop is running. Additionally, the DS thread is used for a few other things (MotorSafety and State Tracking for EducationalRobot). This also makes sim difficult, as user code has to wait for the thread to know it has new data.

This change completely rethinks how threading works in the driver station model.

First, the DS HAL system receives a new data callback, either from Netcomm or DriverStationSim. Inside the context of this callback, all the low latency data is read and put into a cache. Doing some investigation on the robot side, this is perfectly safe to do, and also ensures a ds packet will not be parsed before we finish reading the current packet data.

After all data is read, the cache is swapped with a 2nd buffer. This buffer just stores the data, none of the HAL DS calls read from this buffer. An event is then fired, stating there is new data ready to go.

Robot code calls HAL_UpdateDSData(). This swaps the 2nd buffer with a 3rd buffer, which always contains the current data. This data will not be updated until HAL_UpdateDSData is called again. Which solves the state problem.

The high level driver station classes have. an updateData() call, which calls HAL_UpdateDSData, and then update button state variables, then data log and update the NT FMS data table (Java also caches across the JNI boundary here, but that could trivially be removed). An extra event provider is provided, allowing other threads to know when this call has been completed.

IterativeRobotBase calls DS.updateData() at the beginning of each loop, and only once per loop. This means all commands will always have the same state.

All of this means there is no longer a DS thread. Everything happens synchronously. This means Sim and testing is easier, as you can just call DriverStationSim.NotifyNewData(), and then DriverStation.UpdateData(), and you can guarantee that all the DriverStation.*** data is up to date.

As for Motor Safety and Educational Robot State Handling, those can all be handled by their own threads. The Educational Thread only needs to run under EducationalRobot, and MotorSafety will only be started if there is a motor safety object enabled.
This commit is contained in:
Thad House
2022-10-21 22:01:55 -07:00
committed by GitHub
parent c195b4fc46
commit 4a401b89d7
53 changed files with 1649 additions and 1384 deletions
Download Patch File Download Diff File Expand all files Collapse all files

233
hal/src/main/native/athena/FRCDriverStation.cpp
View File

@@ -13,11 +13,14 @@
#include <FRC_NetworkCommunication/FRCComm.h>
#include <FRC_NetworkCommunication/NetCommRPCProxy_Occur.h>
#include <fmt/format.h>
#include <wpi/EventVector.h>
#include <wpi/SafeThread.h>
#include <wpi/SmallVector.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include "hal/DriverStation.h"
#include "hal/Errors.h"
static_assert(sizeof(int32_t) >= sizeof(int),
"FRC_NetworkComm status variable is larger than 32 bits");
@@ -27,25 +30,45 @@ struct HAL_JoystickAxesInt {
int16_t axes[HAL_kMaxJoystickAxes];
};
static constexpr int kJoystickPorts = 6;
namespace {
struct JoystickDataCache {
JoystickDataCache() { std::memset(this, 0, sizeof(*this)); }
void Update();
HAL_JoystickAxes axes[HAL_kMaxJoysticks];
HAL_JoystickPOVs povs[HAL_kMaxJoysticks];
HAL_JoystickButtons buttons[HAL_kMaxJoysticks];
HAL_AllianceStationID allianceStation;
float matchTime;
bool updated;
};
static_assert(std::is_standard_layout_v<JoystickDataCache>);
// static_assert(std::is_trivial_v<JoystickDataCache>);
struct FRCDriverStation {
wpi::EventVector newDataEvents;
};
} // namespace
static ::FRCDriverStation* driverStation;
// Message and Data variables
static wpi::mutex msgMutex;
static int32_t HAL_GetJoystickAxesInternal(int32_t joystickNum,
HAL_JoystickAxes* axes) {
HAL_JoystickAxesInt axesInt;
JoystickAxes_t netcommAxes;
int retVal = FRC_NetworkCommunication_getJoystickAxes(
joystickNum, reinterpret_cast<JoystickAxes_t*>(&axesInt),
HAL_kMaxJoystickAxes);
joystickNum, &netcommAxes, HAL_kMaxJoystickAxes);
// copy integer values to double values
axes->count = axesInt.count;
axes->count = netcommAxes.count;
// current scaling is -128 to 127, can easily be patched in the future by
// changing this function.
for (int32_t i = 0; i < axesInt.count; i++) {
int8_t value = axesInt.axes[i];
for (int32_t i = 0; i < netcommAxes.count; i++) {
int8_t value = netcommAxes.axes[i];
axes->raw[i] = value;
if (value < 0) {
axes->axes[i] = value / 128.0;
} else {
@@ -68,6 +91,30 @@ static int32_t HAL_GetJoystickButtonsInternal(int32_t joystickNum,
return FRC_NetworkCommunication_getJoystickButtons(
joystickNum, &buttons->buttons, &buttons->count);
}
void JoystickDataCache::Update() {
for (int i = 0; i < HAL_kMaxJoysticks; i++) {
HAL_GetJoystickAxesInternal(i, &axes[i]);
HAL_GetJoystickPOVsInternal(i, &povs[i]);
HAL_GetJoystickButtonsInternal(i, &buttons[i]);
}
FRC_NetworkCommunication_getAllianceStation(
reinterpret_cast<AllianceStationID_t*>(&allianceStation));
FRC_NetworkCommunication_getMatchTime(&matchTime);
}
#define CHECK_JOYSTICK_NUMBER(stickNum) \
if ((stickNum) < 0 || (stickNum) >= HAL_kMaxJoysticks) \
return PARAMETER_OUT_OF_RANGE
static HAL_ControlWord newestControlWord;
static JoystickDataCache caches[3];
static JoystickDataCache* currentRead = &caches[0];
static JoystickDataCache* currentCache = &caches[1];
static JoystickDataCache* cacheToUpdate = &caches[2];
static wpi::mutex cacheMutex;
/**
* Retrieve the Joystick Descriptor for particular slot.
*
@@ -102,12 +149,6 @@ static int32_t HAL_GetJoystickDescriptorInternal(int32_t joystickNum,
return retval;
}
static int32_t HAL_GetControlWordInternal(HAL_ControlWord* controlWord) {
std::memset(controlWord, 0, sizeof(HAL_ControlWord));
return FRC_NetworkCommunication_getControlWord(
reinterpret_cast<ControlWord_t*>(controlWord));
}
static int32_t HAL_GetMatchInfoInternal(HAL_MatchInfo* info) {
MatchType_t matchType = MatchType_t::kMatchType_none;
info->gameSpecificMessageSize = sizeof(info->gameSpecificMessage);
@@ -126,16 +167,11 @@ static int32_t HAL_GetMatchInfoInternal(HAL_MatchInfo* info) {
return status;
}
static wpi::mutex* newDSDataAvailableMutex;
static wpi::condition_variable* newDSDataAvailableCond;
static int newDSDataAvailableCounter{0};
namespace hal::init {
void InitializeFRCDriverStation() {
static wpi::mutex newMutex;
newDSDataAvailableMutex = &newMutex;
static wpi::condition_variable newCond;
newDSDataAvailableCond = &newCond;
std::memset(&newestControlWord, 0, sizeof(newestControlWord));
static FRCDriverStation ds;
driverStation = &ds;
}
} // namespace hal::init
@@ -248,20 +284,39 @@ int32_t HAL_SendConsoleLine(const char* line) {
}
int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
return HAL_GetControlWordInternal(controlWord);
std::scoped_lock lock{cacheMutex};
*controlWord = newestControlWord;
return 0;
}
int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
return HAL_GetJoystickAxesInternal(joystickNum, axes);
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*axes = currentRead->axes[joystickNum];
return 0;
}
int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) {
return HAL_GetJoystickPOVsInternal(joystickNum, povs);
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*povs = currentRead->povs[joystickNum];
return 0;
}
int32_t HAL_GetJoystickButtons(int32_t joystickNum,
HAL_JoystickButtons* buttons) {
return HAL_GetJoystickButtonsInternal(joystickNum, buttons);
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{cacheMutex};
*buttons = currentRead->buttons[joystickNum];
return 0;
}
void HAL_GetAllJoystickData(HAL_JoystickAxes* axes, HAL_JoystickPOVs* povs,
HAL_JoystickButtons* buttons) {
std::scoped_lock lock{cacheMutex};
std::memcpy(axes, currentRead->axes, sizeof(currentRead->axes));
std::memcpy(povs, currentRead->povs, sizeof(currentRead->povs));
std::memcpy(buttons, currentRead->buttons, sizeof(currentRead->buttons));
}
int32_t HAL_GetJoystickDescriptor(int32_t joystickNum,
@@ -274,10 +329,8 @@ int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) {
}
HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
HAL_AllianceStationID allianceStation;
*status = FRC_NetworkCommunication_getAllianceStation(
reinterpret_cast<AllianceStationID_t*>(&allianceStation));
return allianceStation;
std::scoped_lock lock{cacheMutex};
return currentRead->allianceStation;
}
HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum) {
@@ -317,6 +370,7 @@ void HAL_FreeJoystickName(char* name) {
}
int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis) {
CHECK_JOYSTICK_NUMBER(joystickNum);
HAL_JoystickDescriptor joystickDesc;
if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) {
return -1;
@@ -327,14 +381,14 @@ int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis) {
int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
int32_t leftRumble, int32_t rightRumble) {
CHECK_JOYSTICK_NUMBER(joystickNum);
return FRC_NetworkCommunication_setJoystickOutputs(joystickNum, outputs,
leftRumble, rightRumble);
}
double HAL_GetMatchTime(int32_t* status) {
float matchTime;
*status = FRC_NetworkCommunication_getMatchTime(&matchTime);
return matchTime;
std::scoped_lock lock{cacheMutex};
return currentRead->matchTime;
}
void HAL_ObserveUserProgramStarting(void) {
@@ -357,55 +411,6 @@ void HAL_ObserveUserProgramTest(void) {
FRC_NetworkCommunication_observeUserProgramTest();
}
static int& GetThreadLocalLastCount() {
// There is a rollover error condition here. At Packet# = n * (uintmax), this
// will return false when instead it should return true. However, this at a
// 20ms rate occurs once every 2.7 years of DS connected runtime, so not
// worth the cycles to check.
thread_local int lastCount{0};
return lastCount;
}
HAL_Bool HAL_IsNewControlData(void) {
std::scoped_lock lock{*newDSDataAvailableMutex};
int& lastCount = GetThreadLocalLastCount();
int currentCount = newDSDataAvailableCounter;
if (lastCount == currentCount) {
return false;
}
lastCount = currentCount;
return true;
}
void HAL_WaitForDSData(void) {
HAL_WaitForDSDataTimeout(0);
}
HAL_Bool HAL_WaitForDSDataTimeout(double timeout) {
std::unique_lock lock{*newDSDataAvailableMutex};
int& lastCount = GetThreadLocalLastCount();
int currentCount = newDSDataAvailableCounter;
if (lastCount != currentCount) {
lastCount = currentCount;
return true;
}
auto timeoutTime =
std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
while (newDSDataAvailableCounter == currentCount) {
if (timeout > 0) {
auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime);
if (timedOut == std::cv_status::timeout) {
return false;
}
} else {
newDSDataAvailableCond->wait(lock);
}
}
lastCount = newDSDataAvailableCounter;
return true;
}
// Constant number to be used for our occur handle
constexpr int32_t refNumber = 42;
@@ -415,45 +420,47 @@ static void newDataOccur(uint32_t refNum) {
if (refNum != refNumber) {
return;
}
std::scoped_lock lock{*newDSDataAvailableMutex};
// Notify all threads
++newDSDataAvailableCounter;
newDSDataAvailableCond->notify_all();
cacheToUpdate->Update();
{
std::scoped_lock lock{cacheMutex};
std::swap(currentCache, cacheToUpdate);
currentCache->updated = true;
}
driverStation->newDataEvents.Wakeup();
}
/*
* Call this to initialize the driver station communication. This will properly
* handle multiple calls. However note that this CANNOT be called from a library
* that interfaces with LabVIEW.
*/
void HAL_InitializeDriverStation(void) {
static std::atomic_bool initialized{false};
static wpi::mutex initializeMutex;
// Initial check, as if it's true initialization has finished
if (initialized) {
return;
void HAL_RefreshDSData(void) {
HAL_ControlWord controlWord;
std::memset(&controlWord, 0, sizeof(controlWord));
FRC_NetworkCommunication_getControlWord(
reinterpret_cast<ControlWord_t*>(&controlWord));
std::scoped_lock lock{cacheMutex};
if (currentCache->updated) {
std::swap(currentCache, currentRead);
currentCache->updated = false;
}
newestControlWord = controlWord;
}
std::scoped_lock lock(initializeMutex);
// Second check in case another thread was waiting
if (initialized) {
return;
}
void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) {
driverStation->newDataEvents.Add(handle);
}
void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle) {
driverStation->newDataEvents.Remove(handle);
}
HAL_Bool HAL_GetOutputsEnabled(void) {
return FRC_NetworkCommunication_getWatchdogActive();
}
} // extern "C"
namespace hal {
void InitializeDriverStation() {
// Set up the occur function internally with NetComm
NetCommRPCProxy_SetOccurFuncPointer(newDataOccur);
// Set up our occur reference number
setNewDataOccurRef(refNumber);
initialized = true;
}
/*
* Releases the DS Mutex to allow proper shutdown of any threads that are
* waiting on it.
*/
void HAL_ReleaseDSMutex(void) {
newDataOccur(refNumber);
}
} // extern "C"
} // namespace hal

3
hal/src/main/native/athena/HAL.cpp
View File

@@ -40,6 +40,7 @@ static uint64_t dsStartTime;
using namespace hal;
namespace hal {
void InitializeDriverStation();
namespace init {
void InitializeHAL() {
InitializeCTREPCM();
@@ -431,7 +432,7 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
return false;
}
HAL_InitializeDriverStation();
hal::InitializeDriverStation();
dsStartTime = HAL_GetFPGATime(&status);
if (status != 0) {

450
hal/src/main/native/cpp/jni/DriverStationJNI.cpp Normal file
View File

@@ -0,0 +1,450 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <jni.h>
#include <cassert>
#include <fmt/format.h>
#include <wpi/jni_util.h>
#include "HALUtil.h"
#include "edu_wpi_first_hal_DriverStationJNI.h"
#include "hal/DriverStation.h"
#include "hal/FRCUsageReporting.h"
#include "hal/HALBase.h"
// TODO Static asserts
using namespace hal;
using namespace wpi::java;
extern "C" {
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: observeUserProgramStarting
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_observeUserProgramStarting
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramStarting();
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: observeUserProgramDisabled
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_observeUserProgramDisabled
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramDisabled();
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: observeUserProgramAutonomous
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_observeUserProgramAutonomous
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramAutonomous();
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: observeUserProgramTeleop
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_observeUserProgramTeleop
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramTeleop();
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: observeUserProgramTest
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_observeUserProgramTest
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramTest();
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: report
* Signature: (IIILjava/lang/String;)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_report
(JNIEnv* paramEnv, jclass, jint paramResource, jint paramInstanceNumber,
jint paramContext, jstring paramFeature)
{
JStringRef featureStr{paramEnv, paramFeature};
jint returnValue = HAL_Report(paramResource, paramInstanceNumber,
paramContext, featureStr.c_str());
return returnValue;
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: nativeGetControlWord
* Signature: ()I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_nativeGetControlWord
(JNIEnv*, jclass)
{
static_assert(sizeof(HAL_ControlWord) == sizeof(jint),
"Java int must match the size of control word");
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
jint retVal = 0;
std::memcpy(&retVal, &controlWord, sizeof(HAL_ControlWord));
return retVal;
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: nativeGetAllianceStation
* Signature: ()I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_nativeGetAllianceStation
(JNIEnv*, jclass)
{
int32_t status = 0;
auto allianceStation = HAL_GetAllianceStation(&status);
return static_cast<jint>(allianceStation);
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: getJoystickAxesRaw
* Signature: (B[I)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_getJoystickAxesRaw
(JNIEnv* env, jclass, jbyte joystickNum, jintArray axesRawArray)
{
HAL_JoystickAxes axes;
HAL_GetJoystickAxes(joystickNum, &axes);
jsize javaSize = env->GetArrayLength(axesRawArray);
if (axes.count > javaSize) {
ThrowIllegalArgumentException(
env,
fmt::format("Native array size larger then passed in java array "
"size\nNative Size: {} Java Size: {}",
static_cast<int>(axes.count), static_cast<int>(javaSize)));
return 0;
}
jint raw[HAL_kMaxJoystickAxes];
for (int16_t i = 0; i < axes.count; i++) {
raw[i] = axes.raw[i];
}
env->SetIntArrayRegion(axesRawArray, 0, axes.count, raw);
return axes.count;
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: getJoystickAxes
* Signature: (B[F)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_getJoystickAxes
(JNIEnv* env, jclass, jbyte joystickNum, jfloatArray axesArray)
{
HAL_JoystickAxes axes;
HAL_GetJoystickAxes(joystickNum, &axes);
jsize javaSize = env->GetArrayLength(axesArray);
if (axes.count > javaSize) {
ThrowIllegalArgumentException(
env,
fmt::format("Native array size larger then passed in java array "
"size\nNative Size: {} Java Size: {}",
static_cast<int>(axes.count), static_cast<int>(javaSize)));
return 0;
}
env->SetFloatArrayRegion(axesArray, 0, axes.count, axes.axes);
return axes.count;
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: getJoystickPOVs
* Signature: (B[S)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_getJoystickPOVs
(JNIEnv* env, jclass, jbyte joystickNum, jshortArray povsArray)
{
HAL_JoystickPOVs povs;
HAL_GetJoystickPOVs(joystickNum, &povs);
jsize javaSize = env->GetArrayLength(povsArray);
if (povs.count > javaSize) {
ThrowIllegalArgumentException(
env,
fmt::format("Native array size larger then passed in java array "
"size\nNative Size: {} Java Size: {}",
static_cast<int>(povs.count), static_cast<int>(javaSize)));
return 0;
}
env->SetShortArrayRegion(povsArray, 0, povs.count, povs.povs);
return povs.count;
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: getAllJoystickData
* Signature: ([F[B[S[J)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_getAllJoystickData
(JNIEnv* env, jclass cls, jfloatArray axesArray, jbyteArray rawAxesArray,
jshortArray povsArray, jlongArray buttonsAndMetadataArray)
{
HAL_JoystickAxes axes[HAL_kMaxJoysticks];
HAL_JoystickPOVs povs[HAL_kMaxJoysticks];
HAL_JoystickButtons buttons[HAL_kMaxJoysticks];
HAL_GetAllJoystickData(axes, povs, buttons);
CriticalJFloatArrayRef jAxes(env, axesArray);
CriticalJByteArrayRef jRawAxes(env, rawAxesArray);
CriticalJShortArrayRef jPovs(env, povsArray);
CriticalJLongArrayRef jButtons(env, buttonsAndMetadataArray);
static_assert(sizeof(jAxes[0]) == sizeof(axes[0].axes[0]));
static_assert(sizeof(jRawAxes[0]) == sizeof(axes[0].raw[0]));
static_assert(sizeof(jPovs[0]) == sizeof(povs[0].povs[0]));
for (size_t i = 0; i < HAL_kMaxJoysticks; i++) {
std::memcpy(&jAxes[i * HAL_kMaxJoystickAxes], axes[i].axes,
sizeof(axes[i].axes));
std::memcpy(&jRawAxes[i * HAL_kMaxJoystickAxes], axes[i].raw,
sizeof(axes[i].raw));
std::memcpy(&jPovs[i * HAL_kMaxJoystickPOVs], povs[i].povs,
sizeof(povs[i].povs));
jButtons[i * 4] = axes[i].count;
jButtons[(i * 4) + 1] = povs[i].count;
jButtons[(i * 4) + 2] = buttons[i].count;
jButtons[(i * 4) + 3] = buttons[i].buttons;
}
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: getJoystickButtons
* Signature: (BLjava/lang/Object;)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_getJoystickButtons
(JNIEnv* env, jclass, jbyte joystickNum, jobject count)
{
HAL_JoystickButtons joystickButtons;
HAL_GetJoystickButtons(joystickNum, &joystickButtons);
jbyte* countPtr =
reinterpret_cast<jbyte*>(env->GetDirectBufferAddress(count));
*countPtr = joystickButtons.count;
return joystickButtons.buttons;
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: setJoystickOutputs
* Signature: (BISS)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_setJoystickOutputs
(JNIEnv*, jclass, jbyte port, jint outputs, jshort leftRumble,
jshort rightRumble)
{
return HAL_SetJoystickOutputs(port, outputs, leftRumble, rightRumble);
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: getJoystickIsXbox
* Signature: (B)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_getJoystickIsXbox
(JNIEnv*, jclass, jbyte port)
{
return HAL_GetJoystickIsXbox(port);
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: getJoystickType
* Signature: (B)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_getJoystickType
(JNIEnv*, jclass, jbyte port)
{
return HAL_GetJoystickType(port);
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: getJoystickName
* Signature: (B)Ljava/lang/String;
*/
JNIEXPORT jstring JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_getJoystickName
(JNIEnv* env, jclass, jbyte port)
{
char* joystickName = HAL_GetJoystickName(port);
jstring str = MakeJString(env, joystickName);
HAL_FreeJoystickName(joystickName);
return str;
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: getJoystickAxisType
* Signature: (BB)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_getJoystickAxisType
(JNIEnv*, jclass, jbyte joystickNum, jbyte axis)
{
return HAL_GetJoystickAxisType(joystickNum, axis);
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: getMatchTime
* Signature: ()D
*/
JNIEXPORT jdouble JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_getMatchTime
(JNIEnv* env, jclass)
{
int32_t status = 0;
return HAL_GetMatchTime(&status);
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: getMatchInfo
* Signature: (Ljava/lang/Object;)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_getMatchInfo
(JNIEnv* env, jclass, jobject info)
{
HAL_MatchInfo matchInfo;
auto status = HAL_GetMatchInfo(&matchInfo);
if (status == 0) {
SetMatchInfoObject(env, info, matchInfo);
}
return status;
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: sendError
* Signature: (ZIZLjava/lang/String;Ljava/lang/String;Ljava/lang/String;Z)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_sendError
(JNIEnv* env, jclass, jboolean isError, jint errorCode, jboolean isLVCode,
jstring details, jstring location, jstring callStack, jboolean printMsg)
{
JStringRef detailsStr{env, details};
JStringRef locationStr{env, location};
JStringRef callStackStr{env, callStack};
jint returnValue =
HAL_SendError(isError, errorCode, isLVCode, detailsStr.c_str(),
locationStr.c_str(), callStackStr.c_str(), printMsg);
return returnValue;
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: sendConsoleLine
* Signature: (Ljava/lang/String;)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_sendConsoleLine
(JNIEnv* env, jclass, jstring line)
{
JStringRef lineStr{env, line};
jint returnValue = HAL_SendConsoleLine(lineStr.c_str());
return returnValue;
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: refreshDSData
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_refreshDSData
(JNIEnv*, jclass)
{
HAL_RefreshDSData();
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: provideNewDataEventHandle
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_provideNewDataEventHandle
(JNIEnv*, jclass, jint handle)
{
HAL_ProvideNewDataEventHandle(handle);
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: removeNewDataEventHandle
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_removeNewDataEventHandle
(JNIEnv*, jclass, jint handle)
{
HAL_RemoveNewDataEventHandle(handle);
}
/*
* Class: edu_wpi_first_hal_DriverStationJNI
* Method: getOutputsActive
* Signature: ()Z
*/
JNIEXPORT jboolean JNICALL
Java_edu_wpi_first_hal_DriverStationJNI_getOutputsActive
(JNIEnv*, jclass)
{
return HAL_GetOutputsEnabled();
}
} // extern "C"

356
hal/src/main/native/cpp/jni/HAL.cpp
View File

@@ -106,310 +106,6 @@ Java_edu_wpi_first_hal_HAL_simPeriodicAfterNative
HAL_SimPeriodicAfter();
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: observeUserProgramStarting
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_HAL_observeUserProgramStarting
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramStarting();
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: observeUserProgramDisabled
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_HAL_observeUserProgramDisabled
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramDisabled();
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: observeUserProgramAutonomous
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_HAL_observeUserProgramAutonomous
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramAutonomous();
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: observeUserProgramTeleop
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_HAL_observeUserProgramTeleop
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramTeleop();
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: observeUserProgramTest
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_HAL_observeUserProgramTest
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramTest();
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: report
* Signature: (IIILjava/lang/String;)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_HAL_report
(JNIEnv* paramEnv, jclass, jint paramResource, jint paramInstanceNumber,
jint paramContext, jstring paramFeature)
{
JStringRef featureStr{paramEnv, paramFeature};
jint returnValue = HAL_Report(paramResource, paramInstanceNumber,
paramContext, featureStr.c_str());
return returnValue;
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: nativeGetControlWord
* Signature: ()I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_HAL_nativeGetControlWord
(JNIEnv*, jclass)
{
static_assert(sizeof(HAL_ControlWord) == sizeof(jint),
"Java int must match the size of control word");
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
jint retVal = 0;
std::memcpy(&retVal, &controlWord, sizeof(HAL_ControlWord));
return retVal;
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: nativeGetAllianceStation
* Signature: ()I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_HAL_nativeGetAllianceStation
(JNIEnv*, jclass)
{
int32_t status = 0;
auto allianceStation = HAL_GetAllianceStation(&status);
return static_cast<jint>(allianceStation);
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: getJoystickAxes
* Signature: (B[F)S
*/
JNIEXPORT jshort JNICALL
Java_edu_wpi_first_hal_HAL_getJoystickAxes
(JNIEnv* env, jclass, jbyte joystickNum, jfloatArray axesArray)
{
HAL_JoystickAxes axes;
HAL_GetJoystickAxes(joystickNum, &axes);
jsize javaSize = env->GetArrayLength(axesArray);
if (axes.count > javaSize) {
ThrowIllegalArgumentException(
env,
fmt::format("Native array size larger then passed in java array "
"size\nNative Size: {} Java Size: {}",
static_cast<int>(axes.count), static_cast<int>(javaSize)));
return 0;
}
env->SetFloatArrayRegion(axesArray, 0, axes.count, axes.axes);
return axes.count;
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: getJoystickPOVs
* Signature: (B[S)S
*/
JNIEXPORT jshort JNICALL
Java_edu_wpi_first_hal_HAL_getJoystickPOVs
(JNIEnv* env, jclass, jbyte joystickNum, jshortArray povsArray)
{
HAL_JoystickPOVs povs;
HAL_GetJoystickPOVs(joystickNum, &povs);
jsize javaSize = env->GetArrayLength(povsArray);
if (povs.count > javaSize) {
ThrowIllegalArgumentException(
env,
fmt::format("Native array size larger then passed in java array "
"size\nNative Size: {} Java Size: {}",
static_cast<int>(povs.count), static_cast<int>(javaSize)));
return 0;
}
env->SetShortArrayRegion(povsArray, 0, povs.count, povs.povs);
return povs.count;
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: getJoystickButtons
* Signature: (BLjava/lang/Object;)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_HAL_getJoystickButtons
(JNIEnv* env, jclass, jbyte joystickNum, jobject count)
{
HAL_JoystickButtons joystickButtons;
HAL_GetJoystickButtons(joystickNum, &joystickButtons);
jbyte* countPtr =
reinterpret_cast<jbyte*>(env->GetDirectBufferAddress(count));
*countPtr = joystickButtons.count;
return joystickButtons.buttons;
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: setJoystickOutputs
* Signature: (BISS)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_HAL_setJoystickOutputs
(JNIEnv*, jclass, jbyte port, jint outputs, jshort leftRumble,
jshort rightRumble)
{
return HAL_SetJoystickOutputs(port, outputs, leftRumble, rightRumble);
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: getJoystickIsXbox
* Signature: (B)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_HAL_getJoystickIsXbox
(JNIEnv*, jclass, jbyte port)
{
return HAL_GetJoystickIsXbox(port);
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: getJoystickType
* Signature: (B)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_HAL_getJoystickType
(JNIEnv*, jclass, jbyte port)
{
return HAL_GetJoystickType(port);
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: getJoystickName
* Signature: (B)Ljava/lang/String;
*/
JNIEXPORT jstring JNICALL
Java_edu_wpi_first_hal_HAL_getJoystickName
(JNIEnv* env, jclass, jbyte port)
{
char* joystickName = HAL_GetJoystickName(port);
jstring str = MakeJString(env, joystickName);
HAL_FreeJoystickName(joystickName);
return str;
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: getJoystickAxisType
* Signature: (BB)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_HAL_getJoystickAxisType
(JNIEnv*, jclass, jbyte joystickNum, jbyte axis)
{
return HAL_GetJoystickAxisType(joystickNum, axis);
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: isNewControlData
* Signature: ()Z
*/
JNIEXPORT jboolean JNICALL
Java_edu_wpi_first_hal_HAL_isNewControlData
(JNIEnv*, jclass)
{
return static_cast<jboolean>(HAL_IsNewControlData());
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: waitForDSData
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_HAL_waitForDSData
(JNIEnv* env, jclass)
{
HAL_WaitForDSData();
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: releaseDSMutex
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_HAL_releaseDSMutex
(JNIEnv* env, jclass)
{
HAL_ReleaseDSMutex();
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: waitForDSDataTimeout
* Signature: (D)Z
*/
JNIEXPORT jboolean JNICALL
Java_edu_wpi_first_hal_HAL_waitForDSDataTimeout
(JNIEnv*, jclass, jdouble timeout)
{
return static_cast<jboolean>(HAL_WaitForDSDataTimeout(timeout));
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: getMatchTime
* Signature: ()D
*/
JNIEXPORT jdouble JNICALL
Java_edu_wpi_first_hal_HAL_getMatchTime
(JNIEnv* env, jclass)
{
int32_t status = 0;
return HAL_GetMatchTime(&status);
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: getSystemActive
@@ -440,58 +136,6 @@ Java_edu_wpi_first_hal_HAL_getBrownedOut
return val;
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: getMatchInfo
* Signature: (Ljava/lang/Object;)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_HAL_getMatchInfo
(JNIEnv* env, jclass, jobject info)
{
HAL_MatchInfo matchInfo;
auto status = HAL_GetMatchInfo(&matchInfo);
if (status == 0) {
SetMatchInfoObject(env, info, matchInfo);
}
return status;
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: sendError
* Signature: (ZIZLjava/lang/String;Ljava/lang/String;Ljava/lang/String;Z)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_HAL_sendError
(JNIEnv* env, jclass, jboolean isError, jint errorCode, jboolean isLVCode,
jstring details, jstring location, jstring callStack, jboolean printMsg)
{
JStringRef detailsStr{env, details};
JStringRef locationStr{env, location};
JStringRef callStackStr{env, callStack};
jint returnValue =
HAL_SendError(isError, errorCode, isLVCode, detailsStr.c_str(),
locationStr.c_str(), callStackStr.c_str(), printMsg);
return returnValue;
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: sendConsoleLine
* Signature: (Ljava/lang/String;)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_HAL_sendConsoleLine
(JNIEnv* env, jclass, jstring line)
{
JStringRef lineStr{env, line};
jint returnValue = HAL_SendConsoleLine(lineStr.c_str());
return returnValue;
}
/*
* Class: edu_wpi_first_hal_HAL
* Method: getPortWithModule

50
hal/src/main/native/include/hal/DriverStation.h
View File

@@ -6,6 +6,8 @@
#include <stdint.h>
#include <wpi/Synchronization.h>
#include "hal/DriverStationTypes.h"
#include "hal/Types.h"
@@ -87,6 +89,9 @@ int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs);
int32_t HAL_GetJoystickButtons(int32_t joystickNum,
HAL_JoystickButtons* buttons);
void HAL_GetAllJoystickData(HAL_JoystickAxes* axes, HAL_JoystickPOVs* povs,
HAL_JoystickButtons* buttons);
/**
* Retrieves the Joystick Descriptor for particular slot.
*
@@ -183,6 +188,11 @@ int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
*/
double HAL_GetMatchTime(int32_t* status);
/**
* Gets if outputs are enabled by the control system.
*/
HAL_Bool HAL_GetOutputsEnabled(void);
/**
* Gets info about a specific match.
*
@@ -191,44 +201,10 @@ double HAL_GetMatchTime(int32_t* status);
*/
int32_t HAL_GetMatchInfo(HAL_MatchInfo* info);
/**
* Releases the DS Mutex to allow proper shutdown of any threads that are
* waiting on it.
*/
void HAL_ReleaseDSMutex(void);
void HAL_RefreshDSData(void);
/**
* Has a new control packet from the driver station arrived since the last
* time this function was called?
*
* @return true if the control data has been updated since the last call
*/
HAL_Bool HAL_IsNewControlData(void);
/**
* Waits for the newest DS packet to arrive. Note that this is a blocking call.
* Checks if new control data has arrived since the last HAL_WaitForDSData or
* HAL_IsNewControlData call. If new data has not arrived, waits for new data
* to arrive. Otherwise, returns immediately.
*/
void HAL_WaitForDSData(void);
/**
* Waits for the newest DS packet to arrive. If timeout is <= 0, this will wait
* forever. Otherwise, it will wait until either a new packet, or the timeout
* time has passed.
*
* @param[in] timeout timeout in seconds
* @return true for new data, false for timeout
*/
HAL_Bool HAL_WaitForDSDataTimeout(double timeout);
/**
* Initializes the driver station communication. This will properly
* handle multiple calls. However note that this CANNOT be called from a library
* that interfaces with LabVIEW.
*/
void HAL_InitializeDriverStation(void);
void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle);
void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle);
/**
* Sets the program starting flag in the DS.

1
hal/src/main/native/include/hal/DriverStationTypes.h
View File

@@ -69,6 +69,7 @@ HAL_ENUM(HAL_MatchType) {
struct HAL_JoystickAxes {
int16_t count;
float axes[HAL_kMaxJoystickAxes];
uint8_t raw[HAL_kMaxJoystickAxes];
};
typedef struct HAL_JoystickAxes HAL_JoystickAxes;

235
hal/src/main/native/sim/DriverStation.cpp
View File

@@ -14,32 +14,78 @@
#include <string>
#include <fmt/format.h>
#include <wpi/EventVector.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include "HALInitializer.h"
#include "hal/Errors.h"
#include "hal/cpp/fpga_clock.h"
#include "hal/simulation/MockHooks.h"
#include "mockdata/DriverStationDataInternal.h"
static wpi::mutex msgMutex;
static wpi::condition_variable* newDSDataAvailableCond;
static wpi::mutex newDSDataAvailableMutex;
static int newDSDataAvailableCounter{0};
static std::atomic_bool isFinalized{false};
static std::atomic<HALSIM_SendErrorHandler> sendErrorHandler{nullptr};
static std::atomic<HALSIM_SendConsoleLineHandler> sendConsoleLineHandler{
nullptr};
using namespace hal;
static constexpr int kJoystickPorts = 6;
namespace {
struct JoystickDataCache {
JoystickDataCache() { std::memset(this, 0, sizeof(*this)); }
void Update();
HAL_JoystickAxes axes[kJoystickPorts];
HAL_JoystickPOVs povs[kJoystickPorts];
HAL_JoystickButtons buttons[kJoystickPorts];
HAL_AllianceStationID allianceStation;
double matchTime;
bool updated;
};
static_assert(std::is_standard_layout_v<JoystickDataCache>);
// static_assert(std::is_trivial_v<JoystickDataCache>);
static std::atomic_bool gShutdown{false};
struct FRCDriverStation {
~FRCDriverStation() { gShutdown = true; }
wpi::EventVector newDataEvents;
wpi::mutex cacheMutex;
};
} // namespace
void JoystickDataCache::Update() {
for (int i = 0; i < kJoystickPorts; i++) {
SimDriverStationData->GetJoystickAxes(i, &axes[i]);
SimDriverStationData->GetJoystickPOVs(i, &povs[i]);
SimDriverStationData->GetJoystickButtons(i, &buttons[i]);
}
allianceStation = SimDriverStationData->allianceStationId;
matchTime = SimDriverStationData->matchTime;
}
#define CHECK_JOYSTICK_NUMBER(stickNum) \
if ((stickNum) < 0 || (stickNum) >= HAL_kMaxJoysticks) \
return PARAMETER_OUT_OF_RANGE
static HAL_ControlWord newestControlWord;
static JoystickDataCache caches[3];
static JoystickDataCache* currentRead = &caches[0];
static JoystickDataCache* currentCache = &caches[1];
static JoystickDataCache* cacheToUpdate = &caches[2];
static ::FRCDriverStation* driverStation;
namespace hal::init {
void InitializeDriverStation() {
static wpi::condition_variable nddaC;
newDSDataAvailableCond = &nddaC;
static FRCDriverStation ds;
driverStation = &ds;
}
} // namespace hal::init
using namespace hal;
extern "C" {
void HALSIM_SetSendError(HALSIM_SendErrorHandler handler) {
@@ -122,39 +168,49 @@ int32_t HAL_SendConsoleLine(const char* line) {
}
int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
std::memset(controlWord, 0, sizeof(HAL_ControlWord));
controlWord->enabled = SimDriverStationData->enabled;
controlWord->autonomous = SimDriverStationData->autonomous;
controlWord->test = SimDriverStationData->test;
controlWord->eStop = SimDriverStationData->eStop;
controlWord->fmsAttached = SimDriverStationData->fmsAttached;
controlWord->dsAttached = SimDriverStationData->dsAttached;
std::scoped_lock lock{driverStation->cacheMutex};
*controlWord = newestControlWord;
return 0;
}
HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
*status = 0;
return SimDriverStationData->allianceStationId;
std::scoped_lock lock{driverStation->cacheMutex};
return currentRead->allianceStation;
}
int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
SimDriverStationData->GetJoystickAxes(joystickNum, axes);
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{driverStation->cacheMutex};
*axes = currentRead->axes[joystickNum];
return 0;
}
int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) {
SimDriverStationData->GetJoystickPOVs(joystickNum, povs);
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{driverStation->cacheMutex};
*povs = currentRead->povs[joystickNum];
return 0;
}
int32_t HAL_GetJoystickButtons(int32_t joystickNum,
HAL_JoystickButtons* buttons) {
SimDriverStationData->GetJoystickButtons(joystickNum, buttons);
CHECK_JOYSTICK_NUMBER(joystickNum);
std::scoped_lock lock{driverStation->cacheMutex};
*buttons = currentRead->buttons[joystickNum];
return 0;
}
void HAL_GetAllJoystickData(HAL_JoystickAxes* axes, HAL_JoystickPOVs* povs,
HAL_JoystickButtons* buttons) {
std::scoped_lock lock{driverStation->cacheMutex};
std::memcpy(axes, currentRead->axes, sizeof(currentRead->axes));
std::memcpy(povs, currentRead->povs, sizeof(currentRead->povs));
std::memcpy(buttons, currentRead->buttons, sizeof(currentRead->buttons));
}
int32_t HAL_GetJoystickDescriptor(int32_t joystickNum,
HAL_JoystickDescriptor* desc) {
CHECK_JOYSTICK_NUMBER(joystickNum);
SimDriverStationData->GetJoystickDescriptor(joystickNum, desc);
return 0;
}
@@ -196,7 +252,8 @@ int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
}
double HAL_GetMatchTime(int32_t* status) {
return SimDriverStationData->matchTime;
std::scoped_lock lock{driverStation->cacheMutex};
return currentRead->matchTime;
}
int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) {
@@ -224,103 +281,57 @@ void HAL_ObserveUserProgramTest(void) {
// TODO
}
static int& GetThreadLocalLastCount() {
// There is a rollover error condition here. At Packet# = n * (uintmax), this
// will return false when instead it should return true. However, this at a
// 20ms rate occurs once every 2.7 years of DS connected runtime, so not
// worth the cycles to check.
thread_local int lastCount{0};
return lastCount;
void HAL_RefreshDSData(void) {
HAL_ControlWord controlWord;
std::memset(&controlWord, 0, sizeof(controlWord));
controlWord.enabled = SimDriverStationData->enabled;
controlWord.autonomous = SimDriverStationData->autonomous;
controlWord.test = SimDriverStationData->test;
controlWord.eStop = SimDriverStationData->eStop;
controlWord.fmsAttached = SimDriverStationData->fmsAttached;
controlWord.dsAttached = SimDriverStationData->dsAttached;
std::scoped_lock lock{driverStation->cacheMutex};
if (currentCache->updated) {
std::swap(currentCache, currentRead);
currentCache->updated = false;
}
newestControlWord = controlWord;
}
HAL_Bool HAL_IsNewControlData(void) {
std::scoped_lock lock(newDSDataAvailableMutex);
int& lastCount = GetThreadLocalLastCount();
int currentCount = newDSDataAvailableCounter;
if (lastCount == currentCount) {
return false;
}
lastCount = currentCount;
return true;
}
void HAL_WaitForDSData(void) {
HAL_WaitForDSDataTimeout(0);
}
HAL_Bool HAL_WaitForDSDataTimeout(double timeout) {
std::unique_lock lock(newDSDataAvailableMutex);
int& lastCount = GetThreadLocalLastCount();
int currentCount = newDSDataAvailableCounter;
if (lastCount != currentCount) {
lastCount = currentCount;
return true;
}
if (isFinalized.load()) {
return false;
}
auto timeoutTime =
std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
while (newDSDataAvailableCounter == currentCount) {
if (timeout > 0) {
auto timedOut = newDSDataAvailableCond->wait_until(lock, timeoutTime);
if (timedOut == std::cv_status::timeout) {
return false;
}
} else {
newDSDataAvailableCond->wait(lock);
}
}
lastCount = newDSDataAvailableCounter;
return true;
}
// Constant number to be used for our occur handle
constexpr int32_t refNumber = 42;
static int32_t newDataOccur(uint32_t refNum) {
// Since we could get other values, require our specific handle
// to signal our threads
if (refNum != refNumber) {
return 0;
}
SimDriverStationData->CallNewDataCallbacks();
std::scoped_lock lock(newDSDataAvailableMutex);
// Nofify all threads
newDSDataAvailableCounter++;
newDSDataAvailableCond->notify_all();
return 0;
}
void HAL_InitializeDriverStation(void) {
hal::init::CheckInit();
static std::atomic_bool initialized{false};
static wpi::mutex initializeMutex;
// Initial check, as if it's true initialization has finished
if (initialized) {
void HAL_ProvideNewDataEventHandle(WPI_EventHandle handle) {
if (gShutdown) {
return;
}
std::scoped_lock lock(initializeMutex);
// Second check in case another thread was waiting
if (initialized) {
return;
}
SimDriverStationData->ResetData();
std::atexit([]() {
isFinalized.store(true);
HAL_ReleaseDSMutex();
});
initialized = true;
driverStation->newDataEvents.Add(handle);
}
void HAL_ReleaseDSMutex(void) {
newDataOccur(refNumber);
void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle) {
if (gShutdown) {
return;
}
driverStation->newDataEvents.Remove(handle);
}
HAL_Bool HAL_GetOutputsEnabled(void) {
std::scoped_lock lock{driverStation->cacheMutex};
return newestControlWord.enabled && newestControlWord.dsAttached;
}
} // extern "C"
namespace hal {
void NewDriverStationData() {
cacheToUpdate->Update();
{
std::scoped_lock lock{driverStation->cacheMutex};
std::swap(currentCache, cacheToUpdate);
currentCache->updated = true;
}
driverStation->newDataEvents.Wakeup();
SimDriverStationData->CallNewDataCallbacks();
}
void InitializeDriverStation() {
SimDriverStationData->ResetData();
}
} // namespace hal

6
hal/src/main/native/sim/HAL.cpp
View File

@@ -57,6 +57,10 @@ static std::vector<std::pair<void*, void (*)(void*)>> gOnShutdown;
static SimPeriodicCallbackRegistry gSimPeriodicBefore;
static SimPeriodicCallbackRegistry gSimPeriodicAfter;
namespace hal {
void InitializeDriverStation();
} // namespace hal
namespace hal::init {
void InitializeHAL() {
InitializeAccelerometerData();
@@ -335,7 +339,7 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) {
hal::init::HAL_IsInitialized.store(true);
hal::RestartTiming();
HAL_InitializeDriverStation();
hal::InitializeDriverStation();
initialized = true;

6
hal/src/main/native/sim/mockdata/DriverStationData.cpp
View File

@@ -216,8 +216,12 @@ void DriverStationData::CallNewDataCallbacks() {
m_newDataCallbacks(&empty);
}
namespace hal {
void NewDriverStationData();
} // namespace hal
void DriverStationData::NotifyNewData() {
HAL_ReleaseDSMutex();
hal::NewDriverStationData();
}
void DriverStationData::SetJoystickButton(int32_t stick, int32_t button,