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, 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

2
crossConnIntegrationTests/src/main/native/cpp/TestEnvironment.cpp
View File

@@ -49,6 +49,7 @@ class TestEnvironment : public testing::Environment {
HAL_GetControlWord(&controlWord);
return controlWord.enabled && controlWord.dsAttached;
};
HAL_RefreshDSData();
while (!checkEnabled()) {
if (enableCounter > 50) {
// Robot did not enable properly after 5 seconds.
@@ -60,6 +61,7 @@ class TestEnvironment : public testing::Environment {
std::this_thread::sleep_for(100ms);
fmt::print("Waiting for enable: {}\n", enableCounter++);
HAL_RefreshDSData();
}
std::this_thread::sleep_for(500ms);
}

137
hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java Normal file
View File

@@ -0,0 +1,137 @@
// 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.
package edu.wpi.first.hal;
import java.nio.ByteBuffer;
public class DriverStationJNI extends JNIWrapper {
public static native void observeUserProgramStarting();
public static native void observeUserProgramDisabled();
public static native void observeUserProgramAutonomous();
public static native void observeUserProgramTeleop();
public static native void observeUserProgramTest();
public static void report(int resource, int instanceNumber) {
report(resource, instanceNumber, 0, "");
}
public static void report(int resource, int instanceNumber, int context) {
report(resource, instanceNumber, context, "");
}
/**
* Report the usage of a resource of interest.
*
* <p>Original signature: <code>uint32_t report(tResourceType, uint8_t, uint8_t, const
* char*)</code>
*
* @param resource one of the values in the tResourceType above (max value 51).
* @param instanceNumber an index that identifies the resource instance.
* @param context an optional additional context number for some cases (such as module number).
* Set to 0 to omit.
* @param feature a string to be included describing features in use on a specific resource.
* Setting the same resource more than once allows you to change the feature string.
* @return TODO
*/
public static native int report(int resource, int instanceNumber, int context, String feature);
public static native int nativeGetControlWord();
@SuppressWarnings("MissingJavadocMethod")
public static void getControlWord(ControlWord controlWord) {
int word = nativeGetControlWord();
controlWord.update(
(word & 1) != 0,
((word >> 1) & 1) != 0,
((word >> 2) & 1) != 0,
((word >> 3) & 1) != 0,
((word >> 4) & 1) != 0,
((word >> 5) & 1) != 0);
}
private static native int nativeGetAllianceStation();
public static final int kRed1AllianceStation = 0;
public static final int kRed2AllianceStation = 1;
public static final int kRed3AllianceStation = 2;
public static final int kBlue1AllianceStation = 3;
public static final int kBlue2AllianceStation = 4;
public static final int kBlue3AllianceStation = 5;
@SuppressWarnings("MissingJavadocMethod")
public static AllianceStationID getAllianceStation() {
switch (nativeGetAllianceStation()) {
case kRed1AllianceStation:
return AllianceStationID.Red1;
case kRed2AllianceStation:
return AllianceStationID.Red2;
case kRed3AllianceStation:
return AllianceStationID.Red3;
case kBlue1AllianceStation:
return AllianceStationID.Blue1;
case kBlue2AllianceStation:
return AllianceStationID.Blue2;
case kBlue3AllianceStation:
return AllianceStationID.Blue3;
default:
return null;
}
}
public static final int kMaxJoystickAxes = 12;
public static final int kMaxJoystickPOVs = 12;
public static final int kMaxJoysticks = 6;
public static native int getJoystickAxes(byte joystickNum, float[] axesArray);
public static native int getJoystickAxesRaw(byte joystickNum, int[] rawAxesArray);
public static native int getJoystickPOVs(byte joystickNum, short[] povsArray);
public static native int getJoystickButtons(byte joystickNum, ByteBuffer count);
public static native void getAllJoystickData(
float[] axesArray, byte[] rawAxesArray, short[] povsArray, long[] buttonsAndMetadata);
public static native int setJoystickOutputs(
byte joystickNum, int outputs, short leftRumble, short rightRumble);
public static native int getJoystickIsXbox(byte joystickNum);
public static native int getJoystickType(byte joystickNum);
public static native String getJoystickName(byte joystickNum);
public static native int getJoystickAxisType(byte joystickNum, byte axis);
public static native double getMatchTime();
public static native int getMatchInfo(MatchInfoData info);
public static native int sendError(
boolean isError,
int errorCode,
boolean isLVCode,
String details,
String location,
String callStack,
boolean printMsg);
public static native int sendConsoleLine(String line);
public static native void refreshDSData();
public static native void provideNewDataEventHandle(int handle);
public static native void removeNewDataEventHandle(int handle);
public static native boolean getOutputsActive();
private DriverStationJNI() {}
}

122
hal/src/main/java/edu/wpi/first/hal/HAL.java
View File

@@ -4,7 +4,6 @@
package edu.wpi.first.hal;
import java.nio.ByteBuffer;
import java.util.ArrayList;
import java.util.List;
@@ -13,8 +12,6 @@ import java.util.List;
* .
*/
public final class HAL extends JNIWrapper {
public static native void waitForDSData();
public static native boolean initialize(int timeout, int mode);
public static native void shutdown();
@@ -116,15 +113,13 @@ public final class HAL extends JNIWrapper {
}
}
public static native void observeUserProgramStarting();
public static native boolean getBrownedOut();
public static native void observeUserProgramDisabled();
public static native boolean getSystemActive();
public static native void observeUserProgramAutonomous();
public static native int getPortWithModule(byte module, byte channel);
public static native void observeUserProgramTeleop();
public static native void observeUserProgramTest();
public static native int getPort(byte channel);
public static void report(int resource, int instanceNumber) {
report(resource, instanceNumber, 0, "");
@@ -134,114 +129,9 @@ public final class HAL extends JNIWrapper {
report(resource, instanceNumber, context, "");
}
/**
* Report the usage of a resource of interest.
*
* <p>Original signature: <code>uint32_t report(tResourceType, uint8_t, uint8_t, const
* char*)</code>
*
* @param resource one of the values in the tResourceType above (max value 51).
* @param instanceNumber an index that identifies the resource instance.
* @param context an optional additional context number for some cases (such as module number).
* Set to 0 to omit.
* @param feature a string to be included describing features in use on a specific resource.
* Setting the same resource more than once allows you to change the feature string.
* @return TODO
*/
public static native int report(int resource, int instanceNumber, int context, String feature);
public static native int nativeGetControlWord();
/**
* Get the current DriverStation control word.
*
* @param controlWord Storage for control word.
*/
public static void getControlWord(ControlWord controlWord) {
int word = nativeGetControlWord();
controlWord.update(
(word & 1) != 0,
((word >> 1) & 1) != 0,
((word >> 2) & 1) != 0,
((word >> 3) & 1) != 0,
((word >> 4) & 1) != 0,
((word >> 5) & 1) != 0);
public static int report(int resource, int instanceNumber, int context, String feature) {
return DriverStationJNI.report(resource, instanceNumber, context, feature);
}
private static native int nativeGetAllianceStation();
/**
* Get the alliance station.
*
* @return The alliance station.
*/
public static AllianceStationID getAllianceStation() {
switch (nativeGetAllianceStation()) {
case 0:
return AllianceStationID.Red1;
case 1:
return AllianceStationID.Red2;
case 2:
return AllianceStationID.Red3;
case 3:
return AllianceStationID.Blue1;
case 4:
return AllianceStationID.Blue2;
case 5:
return AllianceStationID.Blue3;
default:
return null;
}
}
public static native boolean isNewControlData();
public static native void releaseDSMutex();
public static native boolean waitForDSDataTimeout(double timeout);
public static final int kMaxJoystickAxes = 12;
public static final int kMaxJoystickPOVs = 12;
public static native short getJoystickAxes(byte joystickNum, float[] axesArray);
public static native short getJoystickPOVs(byte joystickNum, short[] povsArray);
public static native int getJoystickButtons(byte joystickNum, ByteBuffer count);
public static native int setJoystickOutputs(
byte joystickNum, int outputs, short leftRumble, short rightRumble);
public static native int getJoystickIsXbox(byte joystickNum);
public static native int getJoystickType(byte joystickNum);
public static native String getJoystickName(byte joystickNum);
public static native int getJoystickAxisType(byte joystickNum, byte axis);
public static native double getMatchTime();
public static native boolean getSystemActive();
public static native boolean getBrownedOut();
public static native int getMatchInfo(MatchInfoData info);
public static native int sendError(
boolean isError,
int errorCode,
boolean isLVCode,
String details,
String location,
String callStack,
boolean printMsg);
public static native int sendConsoleLine(String line);
public static native int getPortWithModule(byte module, byte channel);
public static native int getPort(byte channel);
private HAL() {}
}

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,

6
hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp
View File

@@ -61,6 +61,9 @@ TEST(DriverStationTest, Joystick) {
HALSIM_SetJoystickPOVs(joystickUnderTest, &set_povs);
HALSIM_SetJoystickButtons(joystickUnderTest, &set_buttons);
HALSIM_NotifyDriverStationNewData();
HAL_RefreshDSData();
// Check the set values
HAL_GetJoystickAxes(joystickUnderTest, &axes);
HAL_GetJoystickPOVs(joystickUnderTest, &povs);
@@ -89,6 +92,9 @@ TEST(DriverStationTest, Joystick) {
// Reset
HALSIM_ResetDriverStationData();
HALSIM_NotifyDriverStationNewData();
HAL_RefreshDSData();
for (int joystickNum = 0; joystickNum < 6; ++joystickNum) {
HAL_GetJoystickAxes(joystickNum, &axes);
HAL_GetJoystickPOVs(joystickNum, &povs);

2
simulation/halsim_ws_server/src/test/native/cpp/main.cpp
View File

@@ -157,6 +157,8 @@ TEST_F(WebServerIntegrationTest, DriverStation) {
using namespace std::chrono_literals;
std::this_thread::sleep_for(1s);
HAL_RefreshDSData();
// Compare results
HAL_ControlWord cw;
HAL_GetControlWord(&cw);

1
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java
View File

@@ -34,5 +34,6 @@ public final class MockHardwareExtension implements BeforeAllCallback {
DriverStationSim.setAutonomous(false);
DriverStationSim.setEnabled(true);
DriverStationSim.setTest(true);
DriverStationSim.notifyNewData();
}
}

1
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java
View File

@@ -31,7 +31,6 @@ public class CommandTestBase {
DriverStationSim.setEnabled(enabled);
DriverStationSim.notifyNewData();
DriverStation.isNewControlData();
while (DriverStation.isEnabled() != enabled) {
try {
Thread.sleep(1);

2
wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp
View File

@@ -19,6 +19,7 @@ CommandScheduler CommandTestBase::GetScheduler() {
void CommandTestBase::SetUp() {
frc::sim::DriverStationSim::SetEnabled(true);
frc::sim::DriverStationSim::NotifyNewData();
}
void CommandTestBase::TearDown() {
@@ -27,4 +28,5 @@ void CommandTestBase::TearDown() {
void CommandTestBase::SetDSEnabled(bool enabled) {
frc::sim::DriverStationSim::SetEnabled(enabled);
frc::sim::DriverStationSim::NotifyNewData();
}

8
wpilibc/src/main/native/cpp/DataLogManager.cpp
View File

@@ -147,8 +147,13 @@ void Thread::Main() {
m_log, "systemTime",
"{\"source\":\"DataLogManager\",\"format\":\"time_t_us\"}"};
wpi::Event newDataEvent;
DriverStation::ProvideRefreshedDataEventHandle(newDataEvent.GetHandle());
for (;;) {
bool newData = DriverStation::WaitForData(0.25_s);
bool timedOut = false;
bool newData =
wpi::WaitForObject(newDataEvent.GetHandle(), 0.25, &timedOut);
if (!m_active) {
break;
}
@@ -236,6 +241,7 @@ void Thread::Main() {
sysTimeEntry.Append(wpi::GetSystemTime(), wpi::Now());
}
}
DriverStation::RemoveRefreshedDataEventHandle(newDataEvent.GetHandle());
}
void Thread::StartNTLog() {

140
wpilibc/src/main/native/cpp/DriverStation.cpp
View File

@@ -26,6 +26,7 @@
#include <networktables/NetworkTableInstance.h>
#include <networktables/StringTopic.h>
#include <wpi/DataLog.h>
#include <wpi/EventVector.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include <wpi/timestamp.h>
@@ -117,6 +118,7 @@ struct Instance {
Instance();
~Instance();
wpi::EventVector refreshEvents;
MatchDataSender matchDataSender;
std::atomic<DataLogSender*> dataLogSender{nullptr};
@@ -127,14 +129,6 @@ struct Instance {
std::array<uint32_t, DriverStation::kJoystickPorts> joystickButtonsPressed;
std::array<uint32_t, DriverStation::kJoystickPorts> joystickButtonsReleased;
// Internal Driver Station thread
std::thread dsThread;
std::atomic<bool> isRunning{false};
mutable wpi::mutex waitForDataMutex;
wpi::condition_variable waitForDataCond;
int waitForDataCounter = 0;
bool silenceJoystickWarning = false;
// Robot state status variables
@@ -154,7 +148,6 @@ static Instance& GetInstance() {
return instance;
}
static void Run();
static void SendMatchData();
/**
@@ -185,15 +178,6 @@ static inline void ReportJoystickUnpluggedWarning(const S& format,
ReportJoystickUnpluggedWarningV(format, fmt::make_format_args(args...));
}
static int& GetDSLastCount() {
// 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;
}
Instance::Instance() {
HAL_Initialize(500, 0);
@@ -205,16 +189,9 @@ Instance::Instance() {
previousButtonStates[i].count = 0;
previousButtonStates[i].buttons = 0;
}
dsThread = std::thread(&Run);
}
Instance::~Instance() {
isRunning = false;
// Trigger a DS mutex release in case there is no driver station running.
HAL_ReleaseDSMutex();
dsThread.join();
if (dataLogSender) {
delete dataLogSender.load();
}
@@ -513,18 +490,6 @@ bool DriverStation::IsDSAttached() {
return controlWord.dsAttached;
}
bool DriverStation::IsNewControlData() {
auto& inst = ::GetInstance();
std::unique_lock lock(inst.waitForDataMutex);
int& lastCount = GetDSLastCount();
int currentCount = inst.waitForDataCounter;
if (lastCount == currentCount) {
return false;
}
lastCount = currentCount;
return true;
}
bool DriverStation::IsFMSAttached() {
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
@@ -597,36 +562,6 @@ int DriverStation::GetLocation() {
}
}
void DriverStation::WaitForData() {
WaitForData(0_s);
}
bool DriverStation::WaitForData(units::second_t timeout) {
auto& inst = ::GetInstance();
auto timeoutTime = std::chrono::steady_clock::now() +
std::chrono::steady_clock::duration{timeout};
std::unique_lock lock(inst.waitForDataMutex);
int& lastCount = GetDSLastCount();
int currentCount = inst.waitForDataCounter;
if (lastCount != currentCount) {
lastCount = currentCount;
return true;
}
while (inst.waitForDataCounter == currentCount) {
if (timeout > 0_s) {
auto timedOut = inst.waitForDataCond.wait_until(lock, timeoutTime);
if (timedOut == std::cv_status::timeout) {
return false;
}
} else {
inst.waitForDataCond.wait(lock);
}
}
lastCount = inst.waitForDataCounter;
return true;
}
double DriverStation::GetMatchTime() {
int32_t status = 0;
return HAL_GetMatchTime(&status);
@@ -640,37 +575,14 @@ double DriverStation::GetBatteryVoltage() {
return voltage;
}
void DriverStation::InDisabled(bool entering) {
::GetInstance().userInDisabled = entering;
}
void DriverStation::InAutonomous(bool entering) {
::GetInstance().userInAutonomous = entering;
}
void DriverStation::InTeleop(bool entering) {
::GetInstance().userInTeleop = entering;
}
void DriverStation::InTest(bool entering) {
::GetInstance().userInTest = entering;
}
void DriverStation::WakeupWaitForData() {
auto& inst = ::GetInstance();
std::scoped_lock waitLock(inst.waitForDataMutex);
// Nofify all threads
inst.waitForDataCounter++;
inst.waitForDataCond.notify_all();
}
/**
* Copy data from the DS task for the user.
*
* If no new data exists, it will just be returned, otherwise
* the data will be copied from the DS polling loop.
*/
void GetData() {
void DriverStation::RefreshData() {
HAL_RefreshDSData();
auto& inst = ::GetInstance();
{
// Compute the pressed and released buttons
@@ -692,13 +604,24 @@ void GetData() {
}
}
DriverStation::WakeupWaitForData();
inst.refreshEvents.Wakeup();
SendMatchData();
if (auto sender = inst.dataLogSender.load()) {
sender->Send(wpi::Now());
}
}
void DriverStation::ProvideRefreshedDataEventHandle(WPI_EventHandle handle) {
auto& inst = ::GetInstance();
inst.refreshEvents.Add(handle);
}
void DriverStation::RemoveRefreshedDataEventHandle(WPI_EventHandle handle) {
auto& inst = ::GetInstance();
inst.refreshEvents.Remove(handle);
}
void DriverStation::SilenceJoystickConnectionWarning(bool silence) {
::GetInstance().silenceJoystickWarning = silence;
}
@@ -747,37 +670,6 @@ void ReportJoystickUnpluggedWarningV(fmt::string_view format,
}
}
void Run() {
auto& inst = GetInstance();
inst.isRunning = true;
int safetyCounter = 0;
while (inst.isRunning) {
HAL_WaitForDSData();
GetData();
if (DriverStation::IsDisabled()) {
safetyCounter = 0;
}
if (++safetyCounter >= 4) {
MotorSafety::CheckMotors();
safetyCounter = 0;
}
if (inst.userInDisabled) {
HAL_ObserveUserProgramDisabled();
}
if (inst.userInAutonomous) {
HAL_ObserveUserProgramAutonomous();
}
if (inst.userInTeleop) {
HAL_ObserveUserProgramTeleop();
}
if (inst.userInTest) {
HAL_ObserveUserProgramTest();
}
}
}
void SendMatchData() {
int32_t status = 0;
HAL_AllianceStationID alliance = HAL_GetAllianceStation(&status);

3
wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
View File

@@ -4,6 +4,8 @@
#include "frc/IterativeRobotBase.h"
#include <frc/DriverStation.h>
#include <fmt/format.h>
#include <hal/DriverStation.h>
#include <networktables/NetworkTableInstance.h>
@@ -97,6 +99,7 @@ units::second_t IterativeRobotBase::GetPeriod() const {
}
void IterativeRobotBase::LoopFunc() {
DriverStation::RefreshData();
m_watchdog.Reset();
// Get current mode

81
wpilibc/src/main/native/cpp/MotorSafety.cpp
View File

@@ -7,6 +7,8 @@
#include <algorithm>
#include <utility>
#include <hal/DriverStation.h>
#include <wpi/SafeThread.h>
#include <wpi/SmallPtrSet.h>
#include "frc/DriverStation.h"
@@ -14,26 +16,84 @@
using namespace frc;
static wpi::SmallPtrSet<MotorSafety*, 32> instanceList;
static wpi::mutex listMutex;
namespace {
class Thread : public wpi::SafeThread {
public:
Thread() {}
void Main() override;
};
void Thread::Main() {
wpi::Event event{true, false};
HAL_ProvideNewDataEventHandle(event.GetHandle());
int safetyCounter = 0;
while (m_active) {
bool timedOut = false;
bool signaled = wpi::WaitForObject(event.GetHandle(), 0.1, &timedOut);
if (signaled) {
HAL_ControlWord controlWord;
std::memset(&controlWord, 0, sizeof(controlWord));
HAL_GetControlWord(&controlWord);
if (!(controlWord.enabled && controlWord.dsAttached)) {
safetyCounter = 0;
}
if (++safetyCounter >= 4) {
MotorSafety::CheckMotors();
safetyCounter = 0;
}
} else {
safetyCounter = 0;
}
}
HAL_RemoveNewDataEventHandle(event.GetHandle());
}
static wpi::SafeThreadOwner<Thread> m_owner;
} // namespace
static std::atomic_bool gShutdown{false};
namespace {
struct MotorSafetyManager {
~MotorSafetyManager() { gShutdown = true; }
wpi::SmallPtrSet<MotorSafety*, 32> instanceList;
wpi::mutex listMutex;
bool threadStarted = false;
};
} // namespace
static MotorSafetyManager& GetManager() {
static MotorSafetyManager manager;
return manager;
}
#ifndef __FRC_ROBORIO__
namespace frc::impl {
void ResetMotorSafety() {
std::scoped_lock lock(listMutex);
instanceList.clear();
auto& manager = GetManager();
std::scoped_lock lock(manager.listMutex);
manager.instanceList.clear();
}
} // namespace frc::impl
#endif
MotorSafety::MotorSafety() {
std::scoped_lock lock(listMutex);
instanceList.insert(this);
auto& manager = GetManager();
std::scoped_lock lock(manager.listMutex);
manager.instanceList.insert(this);
if (!manager.threadStarted) {
manager.threadStarted = true;
m_owner.Start();
}
}
MotorSafety::~MotorSafety() {
std::scoped_lock lock(listMutex);
instanceList.erase(this);
auto& manager = GetManager();
std::scoped_lock lock(manager.listMutex);
manager.instanceList.erase(this);
}
MotorSafety::MotorSafety(MotorSafety&& rhs)
@@ -111,8 +171,9 @@ void MotorSafety::Check() {
}
void MotorSafety::CheckMotors() {
std::scoped_lock lock(listMutex);
for (auto elem : instanceList) {
auto& manager = GetManager();
std::scoped_lock lock(manager.listMutex);
for (auto elem : manager.instanceList) {
elem->Check();
}
}

64
wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp Normal file
View File

@@ -0,0 +1,64 @@
// 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 "frc/internal/DriverStationModeThread.h"
#include <hal/DriverStation.h>
#include <wpi/Synchronization.h>
#include "frc/DriverStation.h"
using namespace frc::internal;
DriverStationModeThread::DriverStationModeThread() {
m_keepAlive = true;
m_thread = std::thread{[&] { Run(); }};
}
DriverStationModeThread::~DriverStationModeThread() {
m_keepAlive = false;
if (m_thread.joinable()) {
m_thread.join();
}
}
void DriverStationModeThread::InAutonomous(bool entering) {
m_userInAutonomous = entering;
}
void DriverStationModeThread::InDisabled(bool entering) {
m_userInDisabled = entering;
}
void DriverStationModeThread::InTeleop(bool entering) {
m_userInTeleop = entering;
}
void DriverStationModeThread::InTest(bool entering) {
m_userInTest = entering;
}
void DriverStationModeThread::Run() {
wpi::Event event{true, false};
HAL_ProvideNewDataEventHandle(event.GetHandle());
while (m_keepAlive.load()) {
bool timedOut = false;
wpi::WaitForObject(event.GetHandle(), 0.1, &timedOut);
frc::DriverStation::RefreshData();
if (m_userInDisabled) {
HAL_ObserveUserProgramDisabled();
}
if (m_userInAutonomous) {
HAL_ObserveUserProgramAutonomous();
}
if (m_userInTeleop) {
HAL_ObserveUserProgramTeleop();
}
if (m_userInTest) {
HAL_ObserveUserProgramTest();
}
}
HAL_RemoveNewDataEventHandle(event.GetHandle());
}

7
wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp
View File

@@ -7,6 +7,7 @@
#include <memory>
#include <utility>
#include <hal/DriverStation.h>
#include <hal/simulation/DriverStationData.h>
#include <hal/simulation/MockHooks.h>
@@ -154,8 +155,12 @@ void DriverStationSim::SetMatchTime(double matchTime) {
}
void DriverStationSim::NotifyNewData() {
wpi::Event waitEvent{true};
HAL_ProvideNewDataEventHandle(waitEvent.GetHandle());
HALSIM_NotifyDriverStationNewData();
DriverStation::WaitForData();
wpi::WaitForObject(waitEvent.GetHandle());
HAL_RemoveNewDataEventHandle(waitEvent.GetHandle());
frc::DriverStation::RefreshData();
}
void DriverStationSim::SetSendError(bool shouldSend) {

8
wpilibc/src/main/native/cppcs/RobotBase.cpp
View File

@@ -204,10 +204,6 @@ bool RobotBase::IsTest() const {
return DriverStation::IsTest();
}
bool RobotBase::IsNewDataAvailable() const {
return DriverStation::IsNewControlData();
}
std::thread::id RobotBase::GetThreadId() {
return m_threadId;
}
@@ -256,8 +252,8 @@ RobotBase::RobotBase() {
}
}
// Call DriverStation::InDisabled() to kick off DS thread
DriverStation::InDisabled(true);
// Call DriverStation::RefreshData() to kick things off
DriverStation::RefreshData();
// First and one-time initialization
LiveWindow::SetEnabled(false);

88
wpilibc/src/main/native/include/frc/DriverStation.h
View File

@@ -7,6 +7,7 @@
#include <string>
#include <units/time.h>
#include <wpi/Synchronization.h>
namespace wpi::log {
class DataLog;
@@ -215,17 +216,6 @@ class DriverStation final {
*/
static bool IsDSAttached();
/**
* Has a new control packet from the driver station arrived since the last
* time this function was called?
*
* Warning: If you call this function from more than one place at the same
* time, you will not get the intended behavior.
*
* @return True if the control data has been updated since the last call.
*/
static bool IsNewControlData();
/**
* Is the driver station attached to a Field Management System?
*
@@ -289,41 +279,6 @@ class DriverStation final {
*/
static int GetLocation();
/**
* Wait until a new packet comes from the driver station.
*
* This blocks on a semaphore, so the waiting is efficient.
*
* This is a good way to delay processing until there is new driver station
* data to act on.
*
* Checks if new control data has arrived since the last waitForData call
* on the current thread. If new data has not arrived, returns immediately.
*/
static void WaitForData();
/**
* Wait until a new packet comes from the driver station, or wait for a
* timeout.
*
* Checks if new control data has arrived since the last waitForData call
* on the current thread. If new data has not arrived, returns immediately.
*
* If the timeout is less then or equal to 0, wait indefinitely.
*
* Timeout is in milliseconds
*
* This blocks on a semaphore, so the waiting is efficient.
*
* This is a good way to delay processing until there is new driver station
* data to act on.
*
* @param timeout Timeout
*
* @return true if new data, otherwise false
*/
static bool WaitForData(units::second_t timeout);
/**
* Return the approximate match time.
*
@@ -348,45 +303,10 @@ class DriverStation final {
*/
static double GetBatteryVoltage();
/**
* Only to be used to tell the Driver Station what code you claim to be
* executing for diagnostic purposes only.
*
* @param entering If true, starting disabled code; if false, leaving disabled
* code.
*/
static void InDisabled(bool entering);
static void RefreshData();
/**
* Only to be used to tell the Driver Station what code you claim to be
* executing for diagnostic purposes only.
*
* @param entering If true, starting autonomous code; if false, leaving
* autonomous code.
*/
static void InAutonomous(bool entering);
/**
* Only to be used to tell the Driver Station what code you claim to be
* executing for diagnostic purposes only.
*
* @param entering If true, starting teleop code; if false, leaving teleop
* code.
*/
static void InTeleop(bool entering);
/**
* Only to be used to tell the Driver Station what code you claim to be
* executing for diagnostic purposes only.
*
* @param entering If true, starting test code; if false, leaving test code.
*/
static void InTest(bool entering);
/**
* Forces WaitForData() to return immediately.
*/
static void WakeupWaitForData();
static void ProvideRefreshedDataEventHandle(WPI_EventHandle handle);
static void RemoveRefreshedDataEventHandle(WPI_EventHandle handle);
/**
* Allows the user to specify whether they want joystick connection warnings

8
wpilibc/src/main/native/include/frc/RobotBase.h
View File

@@ -174,14 +174,6 @@ class RobotBase {
*/
bool IsTest() const;
/**
* Indicates if new data is available from the driver station.
*
* @return Has new data arrived over the network since the last time this
* function was called?
*/
bool IsNewDataAvailable() const;
/**
* Gets the ID of the main robot thread.
*/

36
wpilibc/src/main/native/include/frc/internal/DriverStationModeThread.h Normal file
View File

@@ -0,0 +1,36 @@
// 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.
#pragma once
#include <atomic>
#include <thread>
namespace frc::internal {
class DriverStationModeThread {
public:
DriverStationModeThread();
~DriverStationModeThread();
DriverStationModeThread(const DriverStationModeThread& other) = delete;
DriverStationModeThread(DriverStationModeThread&& other) = delete;
DriverStationModeThread& operator=(const DriverStationModeThread& other) =
delete;
DriverStationModeThread& operator=(DriverStationModeThread&& other) = delete;
void InAutonomous(bool entering);
void InDisabled(bool entering);
void InTeleop(bool entering);
void InTest(bool entering);
private:
std::atomic_bool m_keepAlive{false};
std::thread m_thread;
void Run();
bool m_userInDisabled{false};
bool m_userInAutonomous{false};
bool m_userInTeleop{false};
bool m_userInTest{false};
};
} // namespace frc::internal

4
wpilibc/src/test/native/cpp/TimedRobotTest.cpp
View File

@@ -418,6 +418,7 @@ TEST_F(TimedRobotTest, ModeChange) {
frc::sim::DriverStationSim::SetEnabled(true);
frc::sim::DriverStationSim::SetAutonomous(true);
frc::sim::DriverStationSim::SetTest(false);
frc::sim::DriverStationSim::NotifyNewData();
frc::sim::StepTiming(20_ms);
@@ -435,6 +436,7 @@ TEST_F(TimedRobotTest, ModeChange) {
frc::sim::DriverStationSim::SetEnabled(true);
frc::sim::DriverStationSim::SetAutonomous(false);
frc::sim::DriverStationSim::SetTest(false);
frc::sim::DriverStationSim::NotifyNewData();
frc::sim::StepTiming(20_ms);
@@ -452,6 +454,7 @@ TEST_F(TimedRobotTest, ModeChange) {
frc::sim::DriverStationSim::SetEnabled(true);
frc::sim::DriverStationSim::SetAutonomous(false);
frc::sim::DriverStationSim::SetTest(true);
frc::sim::DriverStationSim::NotifyNewData();
frc::sim::StepTiming(20_ms);
@@ -469,6 +472,7 @@ TEST_F(TimedRobotTest, ModeChange) {
frc::sim::DriverStationSim::SetEnabled(false);
frc::sim::DriverStationSim::SetAutonomous(false);
frc::sim::DriverStationSim::SetTest(false);
frc::sim::DriverStationSim::NotifyNewData();
frc::sim::StepTiming(20_ms);

7
wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp
View File

@@ -132,6 +132,7 @@ TEST(DriverStationTest, AllianceStationId) {
// B1
allianceStation = HAL_AllianceStationID_kBlue1;
DriverStationSim::SetAllianceStationId(allianceStation);
frc::sim::DriverStationSim::NotifyNewData();
EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId());
EXPECT_EQ(DriverStation::kBlue, DriverStation::GetAlliance());
EXPECT_EQ(1, DriverStation::GetLocation());
@@ -141,6 +142,7 @@ TEST(DriverStationTest, AllianceStationId) {
// B2
allianceStation = HAL_AllianceStationID_kBlue2;
DriverStationSim::SetAllianceStationId(allianceStation);
frc::sim::DriverStationSim::NotifyNewData();
EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId());
EXPECT_EQ(DriverStation::kBlue, DriverStation::GetAlliance());
EXPECT_EQ(2, DriverStation::GetLocation());
@@ -150,6 +152,7 @@ TEST(DriverStationTest, AllianceStationId) {
// B3
allianceStation = HAL_AllianceStationID_kBlue3;
DriverStationSim::SetAllianceStationId(allianceStation);
frc::sim::DriverStationSim::NotifyNewData();
EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId());
EXPECT_EQ(DriverStation::kBlue, DriverStation::GetAlliance());
EXPECT_EQ(3, DriverStation::GetLocation());
@@ -159,6 +162,7 @@ TEST(DriverStationTest, AllianceStationId) {
// R1
allianceStation = HAL_AllianceStationID_kRed1;
DriverStationSim::SetAllianceStationId(allianceStation);
frc::sim::DriverStationSim::NotifyNewData();
EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId());
EXPECT_EQ(DriverStation::kRed, DriverStation::GetAlliance());
EXPECT_EQ(1, DriverStation::GetLocation());
@@ -168,6 +172,7 @@ TEST(DriverStationTest, AllianceStationId) {
// R2
allianceStation = HAL_AllianceStationID_kRed2;
DriverStationSim::SetAllianceStationId(allianceStation);
frc::sim::DriverStationSim::NotifyNewData();
EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId());
EXPECT_EQ(DriverStation::kRed, DriverStation::GetAlliance());
EXPECT_EQ(2, DriverStation::GetLocation());
@@ -177,6 +182,7 @@ TEST(DriverStationTest, AllianceStationId) {
// R3
allianceStation = HAL_AllianceStationID_kRed3;
DriverStationSim::SetAllianceStationId(allianceStation);
frc::sim::DriverStationSim::NotifyNewData();
EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId());
EXPECT_EQ(DriverStation::kRed, DriverStation::GetAlliance());
EXPECT_EQ(3, DriverStation::GetLocation());
@@ -211,6 +217,7 @@ TEST(DriverStationTest, MatchTime) {
false);
constexpr double kTestTime = 19.174;
DriverStationSim::SetMatchTime(kTestTime);
frc::sim::DriverStationSim::NotifyNewData();
EXPECT_EQ(kTestTime, DriverStationSim::GetMatchTime());
EXPECT_EQ(kTestTime, DriverStation::GetMatchTime());
EXPECT_TRUE(callback.WasTriggered());

12
wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
View File

@@ -87,13 +87,18 @@ int main(void) {
return 1;
}
WPI_EventHandle eventHandle = WPI_CreateEvent(1, 0);
HAL_ProvideNewDataEventHandle(eventHandle);
while (1) {
// Wait for DS data, with a timeout
HAL_Bool validData = HAL_WaitForDSDataTimeout(1.0);
if (!validData) {
int timed_out = 0;
int signaled = WPI_WaitForObjectTimeout(eventHandle, 1.0, &timed_out);
if (!signaled) {
// Do something here on no packet
continue;
}
enum DriverStationMode dsMode = getDSMode();
switch (dsMode) {
case DisabledMode:
@@ -116,6 +121,9 @@ int main(void) {
}
// Clean up resources
HAL_RemoveNewDataEventHandle(eventHandle);
WPI_DestroyEvent(eventHandle);
status = 0;
HAL_FreeDIOPort(dio);
HAL_FreePWMPort(pwmPort, &status);

30
wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
View File

@@ -5,6 +5,7 @@
#include "Robot.h"
#include <frc/DriverStation.h>
#include <frc/internal/DriverStationModeThread.h>
#include <frc/livewindow/LiveWindow.h>
#include <frc/shuffleboard/Shuffleboard.h>
#include <hal/DriverStation.h>
@@ -23,41 +24,46 @@ void Robot::Test() {}
void Robot::StartCompetition() {
RobotInit();
frc::internal::DriverStationModeThread modeThread;
wpi::Event event{true, false};
frc::DriverStation::ProvideRefreshedDataEventHandle(event.GetHandle());
// Tell the DS that the robot is ready to be enabled
HAL_ObserveUserProgramStarting();
while (!m_exit) {
if (IsDisabled()) {
frc::DriverStation::InDisabled(true);
modeThread.InDisabled(true);
Disabled();
frc::DriverStation::InDisabled(false);
modeThread.InDisabled(false);
while (IsDisabled()) {
frc::DriverStation::WaitForData();
wpi::WaitForObject(event.GetHandle());
}
} else if (IsAutonomous()) {
frc::DriverStation::InAutonomous(true);
modeThread.InAutonomous(true);
Autonomous();
frc::DriverStation::InAutonomous(false);
modeThread.InAutonomous(false);
while (IsAutonomousEnabled()) {
frc::DriverStation::WaitForData();
wpi::WaitForObject(event.GetHandle());
}
} else if (IsTest()) {
frc::LiveWindow::SetEnabled(true);
frc::Shuffleboard::EnableActuatorWidgets();
frc::DriverStation::InTest(true);
modeThread.InTest(true);
Test();
frc::DriverStation::InTest(false);
modeThread.InTest(false);
while (IsTest() && IsEnabled()) {
frc::DriverStation::WaitForData();
wpi::WaitForObject(event.GetHandle());
}
frc::LiveWindow::SetEnabled(false);
frc::Shuffleboard::DisableActuatorWidgets();
} else {
frc::DriverStation::InTeleop(true);
modeThread.InTeleop(true);
Teleop();
frc::DriverStation::InTeleop(false);
modeThread.InTeleop(false);
while (IsTeleopEnabled()) {
frc::DriverStation::WaitForData();
wpi::WaitForObject(event.GetHandle());
}
}
}

8
wpilibcIntegrationTests/src/main/native/cpp/DriverStationTest.cpp
View File

@@ -4,6 +4,7 @@
#include "frc/DriverStation.h" // NOLINT(build/include_order)
#include <hal/DriverStation.h>
#include <units/math.h>
#include <units/time.h>
@@ -20,11 +21,16 @@
TEST(DriverStationTest, WaitForData) {
units::microsecond_t initialTime(frc::RobotController::GetFPGATime());
wpi::Event waitEvent{true};
HAL_ProvideNewDataEventHandle(waitEvent.GetHandle());
// 20ms waiting intervals * 50 = 1s
for (int i = 0; i < 50; i++) {
frc::DriverStation::WaitForData();
wpi::WaitForObject(waitEvent.GetHandle());
}
HAL_RemoveNewDataEventHandle(waitEvent.GetHandle());
units::microsecond_t finalTime(frc::RobotController::GetFPGATime());
EXPECT_NEAR_UNITS(1_s, finalTime - initialTime, 200_ms);

2
wpilibcIntegrationTests/src/main/native/cpp/TestEnvironment.cpp
View File

@@ -45,6 +45,7 @@ class TestEnvironment : public testing::Environment {
fmt::print("Started coms\n");
int enableCounter = 0;
frc::DriverStation::RefreshData();
while (!frc::DriverStation::IsEnabled()) {
if (enableCounter > 50) {
// Robot did not enable properly after 5 seconds.
@@ -56,6 +57,7 @@ class TestEnvironment : public testing::Environment {
std::this_thread::sleep_for(100ms);
fmt::print("Waiting for enable: {}\n", enableCounter++);
frc::DriverStation::RefreshData();
}
}

6
wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java
View File

@@ -16,12 +16,12 @@ public class DSControlWord {
* <p>Upon construction, the current Driver Station control word is read and stored internally.
*/
public DSControlWord() {
update();
refresh();
}
/** Update internal Driver Station control word. */
public void update() {
DriverStation.updateControlWordFromCache(m_controlWord);
public void refresh() {
DriverStation.refreshControlWordFromCache(m_controlWord);
}
/**

13
wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java
View File

@@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.util.concurrent.Event;
import edu.wpi.first.util.datalog.DataLog;
import edu.wpi.first.util.datalog.IntegerLogEntry;
import edu.wpi.first.util.datalog.StringLogEntry;
@@ -279,12 +280,19 @@ public final class DataLogManager {
new IntegerLogEntry(
m_log, "systemTime", "{\"source\":\"DataLogManager\",\"format\":\"time_t_us\"}");
Event newDataEvent = new Event();
DriverStation.provideRefreshedDataEventHandle(newDataEvent.getHandle());
while (!Thread.interrupted()) {
boolean newData = DriverStation.waitForData(0.25);
boolean timedOut;
try {
timedOut = WPIUtilJNI.waitForObjectTimeout(newDataEvent.getHandle(), 0.25);
} catch (InterruptedException e) {
break;
}
if (Thread.interrupted()) {
break;
}
if (!newData) {
if (timedOut) {
timeoutCount++;
// pause logging after being disconnected for 10 seconds
if (timeoutCount > 40 && !paused) {
@@ -371,5 +379,6 @@ public final class DataLogManager {
sysTimeEntry.append(WPIUtilJNI.getSystemTime(), WPIUtilJNI.now());
}
}
newDataEvent.close();
}
}

428
wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
View File

@@ -6,6 +6,7 @@ package edu.wpi.first.wpilibj;
import edu.wpi.first.hal.AllianceStationID;
import edu.wpi.first.hal.ControlWord;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.MatchInfoData;
import edu.wpi.first.networktables.BooleanPublisher;
@@ -13,6 +14,7 @@ import edu.wpi.first.networktables.IntegerPublisher;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.networktables.StringPublisher;
import edu.wpi.first.util.EventVector;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.util.datalog.BooleanArrayLogEntry;
import edu.wpi.first.util.datalog.BooleanLogEntry;
@@ -20,9 +22,6 @@ import edu.wpi.first.util.datalog.DataLog;
import edu.wpi.first.util.datalog.FloatArrayLogEntry;
import edu.wpi.first.util.datalog.IntegerArrayLogEntry;
import java.nio.ByteBuffer;
import java.util.concurrent.TimeUnit;
import java.util.concurrent.locks.Condition;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;
/** Provide access to the network communication data to / from the Driver Station. */
@@ -37,16 +36,25 @@ public final class DriverStation {
private static class HALJoystickAxes {
public float[] m_axes;
public short m_count;
public int m_count;
HALJoystickAxes(int count) {
m_axes = new float[count];
}
}
private static class HALJoystickAxesRaw {
public int[] m_axes;
public int m_count;
HALJoystickAxesRaw(int count) {
m_axes = new int[count];
}
}
private static class HALJoystickPOVs {
public short[] m_povs;
public short m_count;
public int m_count;
HALJoystickPOVs(int count) {
m_povs = new short[count];
@@ -73,15 +81,6 @@ public final class DriverStation {
private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
private static double m_nextMessageTime;
private static class DriverStationTask implements Runnable {
DriverStationTask() {}
@Override
public void run() {
DriverStation.run();
}
} /* DriverStationTask */
@SuppressWarnings("MemberName")
private static class MatchDataSender {
NetworkTable table;
@@ -126,7 +125,7 @@ public final class DriverStation {
}
private void sendMatchData() {
AllianceStationID allianceID = HAL.getAllianceStation();
AllianceStationID allianceID = DriverStationJNI.getAllianceStation();
boolean isRedAlliance = false;
int stationNumber = 1;
switch (allianceID) {
@@ -172,7 +171,7 @@ public final class DriverStation {
} finally {
m_cacheDataMutex.unlock();
}
currentControlWord = HAL.nativeGetControlWord();
currentControlWord = DriverStationJNI.nativeGetControlWord();
if (oldIsRedAlliance != isRedAlliance) {
alliance.set(isRedAlliance);
@@ -368,16 +367,22 @@ public final class DriverStation {
// Joystick User Data
private static HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[kJoystickPorts];
private static HALJoystickAxesRaw[] m_joystickAxesRaw = new HALJoystickAxesRaw[kJoystickPorts];
private static HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[kJoystickPorts];
private static HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[kJoystickPorts];
private static MatchInfoData m_matchInfo = new MatchInfoData();
private static ControlWord m_controlWord = new ControlWord();
private static EventVector m_refreshEvents = new EventVector();
// Joystick Cached Data
private static HALJoystickAxes[] m_joystickAxesCache = new HALJoystickAxes[kJoystickPorts];
private static HALJoystickAxesRaw[] m_joystickAxesRawCache =
new HALJoystickAxesRaw[kJoystickPorts];
private static HALJoystickPOVs[] m_joystickPOVsCache = new HALJoystickPOVs[kJoystickPorts];
private static HALJoystickButtons[] m_joystickButtonsCache =
new HALJoystickButtons[kJoystickPorts];
private static MatchInfoData m_matchInfoCache = new MatchInfoData();
private static ControlWord m_controlWordCache = new ControlWord();
// Joystick button rising/falling edge flags
private static int[] m_joystickButtonsPressed = new int[kJoystickPorts];
@@ -389,31 +394,10 @@ public final class DriverStation {
private static final MatchDataSender m_matchDataSender;
private static DataLogSender m_dataLogSender;
// Internal Driver Station thread
private static Thread m_thread;
private static volatile boolean m_threadKeepAlive = true;
private static final ReentrantLock m_cacheDataMutex = new ReentrantLock();
private static final Lock m_waitForDataMutex;
private static final Condition m_waitForDataCond;
private static int m_waitForDataCount;
private static final ThreadLocal<Integer> m_lastCount = ThreadLocal.withInitial(() -> 0);
private static boolean m_silenceJoystickWarning;
// Robot state status variables
private static boolean m_userInDisabled;
private static boolean m_userInAutonomous;
private static boolean m_userInTeleop;
private static boolean m_userInTest;
// Control word variables
private static final ReentrantLock m_controlWordMutex = new ReentrantLock();
private static final ControlWord m_controlWordCache;
private static long m_lastControlWordUpdate;
/**
* DriverStation constructor.
*
@@ -424,42 +408,20 @@ public final class DriverStation {
static {
HAL.initialize(500, 0);
m_waitForDataCount = 0;
m_waitForDataMutex = new ReentrantLock();
m_waitForDataCond = m_waitForDataMutex.newCondition();
for (int i = 0; i < kJoystickPorts; i++) {
m_joystickButtons[i] = new HALJoystickButtons();
m_joystickAxes[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes);
m_joystickPOVs[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs);
m_joystickAxes[i] = new HALJoystickAxes(DriverStationJNI.kMaxJoystickAxes);
m_joystickAxesRaw[i] = new HALJoystickAxesRaw(DriverStationJNI.kMaxJoystickAxes);
m_joystickPOVs[i] = new HALJoystickPOVs(DriverStationJNI.kMaxJoystickPOVs);
m_joystickButtonsCache[i] = new HALJoystickButtons();
m_joystickAxesCache[i] = new HALJoystickAxes(HAL.kMaxJoystickAxes);
m_joystickPOVsCache[i] = new HALJoystickPOVs(HAL.kMaxJoystickPOVs);
m_joystickAxesCache[i] = new HALJoystickAxes(DriverStationJNI.kMaxJoystickAxes);
m_joystickAxesRawCache[i] = new HALJoystickAxesRaw(DriverStationJNI.kMaxJoystickAxes);
m_joystickPOVsCache[i] = new HALJoystickPOVs(DriverStationJNI.kMaxJoystickPOVs);
}
m_controlWordCache = new ControlWord();
m_lastControlWordUpdate = 0;
m_matchDataSender = new MatchDataSender();
m_thread = new Thread(new DriverStationTask(), "FRCDriverStation");
m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2);
m_thread.start();
}
/** Kill the thread. */
public static synchronized void release() {
m_threadKeepAlive = false;
if (m_thread != null) {
try {
m_thread.join();
} catch (InterruptedException ex) {
Thread.currentThread().interrupt();
}
m_thread = null;
}
}
/**
@@ -537,7 +499,8 @@ public final class DriverStation {
}
}
}
HAL.sendError(isError, code, false, error, locString, traceString.toString(), true);
DriverStationJNI.sendError(
isError, code, false, error, locString, traceString.toString(), true);
}
/**
@@ -666,7 +629,7 @@ public final class DriverStation {
if (stick < 0 || stick >= kJoystickPorts) {
throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
}
if (axis < 0 || axis >= HAL.kMaxJoystickAxes) {
if (axis < 0 || axis >= DriverStationJNI.kMaxJoystickAxes) {
throw new IllegalArgumentException("Joystick axis is out of range");
}
@@ -699,7 +662,7 @@ public final class DriverStation {
if (stick < 0 || stick >= kJoystickPorts) {
throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
}
if (pov < 0 || pov >= HAL.kMaxJoystickPOVs) {
if (pov < 0 || pov >= DriverStationJNI.kMaxJoystickPOVs) {
throw new IllegalArgumentException("Joystick POV is out of range");
}
@@ -760,10 +723,10 @@ public final class DriverStation {
}
/**
* Returns the number of POVs on a given joystick port.
* Returns the number of povs on a given joystick port.
*
* @param stick The joystick port number
* @return The number of POVs on the indicated joystick
* @return The number of povs on the indicated joystick
*/
public static int getStickPOVCount(int stick) {
if (stick < 0 || stick >= kJoystickPorts) {
@@ -808,7 +771,7 @@ public final class DriverStation {
throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
}
return HAL.getJoystickIsXbox((byte) stick) == 1;
return DriverStationJNI.getJoystickIsXbox((byte) stick) == 1;
}
/**
@@ -822,7 +785,7 @@ public final class DriverStation {
throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
}
return HAL.getJoystickType((byte) stick);
return DriverStationJNI.getJoystickType((byte) stick);
}
/**
@@ -836,7 +799,7 @@ public final class DriverStation {
throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
}
return HAL.getJoystickName((byte) stick);
return DriverStationJNI.getJoystickName((byte) stick);
}
/**
@@ -851,7 +814,7 @@ public final class DriverStation {
throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
}
return HAL.getJoystickAxisType((byte) stick, (byte) axis);
return DriverStationJNI.getJoystickAxisType((byte) stick, (byte) axis);
}
/**
@@ -875,12 +838,11 @@ public final class DriverStation {
* @return True if the robot is enabled, false otherwise.
*/
public static boolean isEnabled() {
m_controlWordMutex.lock();
m_cacheDataMutex.lock();
try {
updateControlWord(false);
return m_controlWordCache.getEnabled() && m_controlWordCache.getDSAttached();
return m_controlWord.getEnabled() && m_controlWord.getDSAttached();
} finally {
m_controlWordMutex.unlock();
m_cacheDataMutex.unlock();
}
}
@@ -899,12 +861,11 @@ public final class DriverStation {
* @return True if the robot is e-stopped, false otherwise.
*/
public static boolean isEStopped() {
m_controlWordMutex.lock();
m_cacheDataMutex.lock();
try {
updateControlWord(false);
return m_controlWordCache.getEStop();
return m_controlWord.getEStop();
} finally {
m_controlWordMutex.unlock();
m_cacheDataMutex.unlock();
}
}
@@ -915,12 +876,11 @@ public final class DriverStation {
* @return True if autonomous mode should be enabled, false otherwise.
*/
public static boolean isAutonomous() {
m_controlWordMutex.lock();
m_cacheDataMutex.lock();
try {
updateControlWord(false);
return m_controlWordCache.getAutonomous();
return m_controlWord.getAutonomous();
} finally {
m_controlWordMutex.unlock();
m_cacheDataMutex.unlock();
}
}
@@ -931,12 +891,11 @@ public final class DriverStation {
* @return True if autonomous should be set and the robot should be enabled.
*/
public static boolean isAutonomousEnabled() {
m_controlWordMutex.lock();
m_cacheDataMutex.lock();
try {
updateControlWord(false);
return m_controlWordCache.getAutonomous() && m_controlWordCache.getEnabled();
return m_controlWord.getAutonomous() && m_controlWord.getEnabled();
} finally {
m_controlWordMutex.unlock();
m_cacheDataMutex.unlock();
}
}
@@ -957,14 +916,13 @@ public final class DriverStation {
* @return True if operator-controlled mode should be set and the robot should be enabled.
*/
public static boolean isTeleopEnabled() {
m_controlWordMutex.lock();
m_cacheDataMutex.lock();
try {
updateControlWord(false);
return !m_controlWordCache.getAutonomous()
&& !m_controlWordCache.getTest()
&& m_controlWordCache.getEnabled();
return !m_controlWord.getAutonomous()
&& !m_controlWord.getTest()
&& m_controlWord.getEnabled();
} finally {
m_controlWordMutex.unlock();
m_cacheDataMutex.unlock();
}
}
@@ -975,12 +933,11 @@ public final class DriverStation {
* @return True if test mode should be enabled, false otherwise.
*/
public static boolean isTest() {
m_controlWordMutex.lock();
m_cacheDataMutex.lock();
try {
updateControlWord(false);
return m_controlWordCache.getTest();
return m_controlWord.getTest();
} finally {
m_controlWordMutex.unlock();
m_cacheDataMutex.unlock();
}
}
@@ -990,47 +947,25 @@ public final class DriverStation {
* @return True if Driver Station is attached, false otherwise.
*/
public static boolean isDSAttached() {
m_controlWordMutex.lock();
m_cacheDataMutex.lock();
try {
updateControlWord(false);
return m_controlWordCache.getDSAttached();
return m_controlWord.getDSAttached();
} finally {
m_controlWordMutex.unlock();
m_cacheDataMutex.unlock();
}
}
/**
* Gets if 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.
*/
public static boolean isNewControlData() {
m_waitForDataMutex.lock();
try {
int currentCount = m_waitForDataCount;
if (m_lastCount.get() != currentCount) {
m_lastCount.set(currentCount);
return true;
}
} finally {
m_waitForDataMutex.unlock();
}
return false;
}
/**
* Gets if the driver station attached to a Field Management System.
*
* @return true if the robot is competing on a field being controlled by a Field Management System
*/
public static boolean isFMSAttached() {
m_controlWordMutex.lock();
m_cacheDataMutex.lock();
try {
updateControlWord(false);
return m_controlWordCache.getFMSAttached();
return m_controlWord.getFMSAttached();
} finally {
m_controlWordMutex.unlock();
m_cacheDataMutex.unlock();
}
}
@@ -1121,7 +1056,7 @@ public final class DriverStation {
* @return the current alliance
*/
public static Alliance getAlliance() {
AllianceStationID allianceStationID = HAL.getAllianceStation();
AllianceStationID allianceStationID = DriverStationJNI.getAllianceStation();
if (allianceStationID == null) {
return Alliance.Invalid;
}
@@ -1148,7 +1083,7 @@ public final class DriverStation {
* @return the location of the team's driver station controls: 1, 2, or 3
*/
public static int getLocation() {
AllianceStationID allianceStationID = HAL.getAllianceStation();
AllianceStationID allianceStationID = DriverStationJNI.getAllianceStation();
if (allianceStationID == null) {
return 0;
}
@@ -1170,68 +1105,6 @@ public final class DriverStation {
}
}
/**
* Wait for new data from the driver station.
*
* <p>Checks if new control data has arrived since the last waitForData call on the current
* thread. If new data has not arrived, returns immediately.
*/
public static void waitForData() {
waitForData(0);
}
/**
* Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data
* only.
*
* <p>Checks if new control data has arrived since the last waitForData call on the current
* thread. If new data has not arrived, returns immediately.
*
* @param timeoutSeconds The maximum time in seconds to wait.
* @return true if there is new data, otherwise false
*/
public static boolean waitForData(double timeoutSeconds) {
long startTimeMicroS = RobotController.getFPGATime();
long timeoutMicroS = (long) (timeoutSeconds * 1e6);
m_waitForDataMutex.lock();
try {
int currentCount = m_waitForDataCount;
if (m_lastCount.get() != currentCount) {
m_lastCount.set(currentCount);
return true;
}
while (m_waitForDataCount == currentCount) {
if (timeoutMicroS > 0) {
long nowMicroS = RobotController.getFPGATime();
if (nowMicroS < startTimeMicroS + timeoutMicroS) {
// We still have time to wait
boolean signaled =
m_waitForDataCond.await(
startTimeMicroS + timeoutMicroS - nowMicroS, TimeUnit.MICROSECONDS);
if (!signaled) {
// Return false if a timeout happened
return false;
}
} else {
// Time has elapsed.
return false;
}
} else {
m_waitForDataCond.await();
}
}
m_lastCount.set(m_waitForDataCount);
// Return true if we have received a proper signal
return true;
} catch (InterruptedException ex) {
// return false on a thread interrupt
Thread.currentThread().interrupt();
return false;
} finally {
m_waitForDataMutex.unlock();
}
}
/**
* Return the approximate match time. The FMS does not send an official match time to the robots,
* but does send an approximate match time. The value will count down the time remaining in the
@@ -1242,58 +1115,7 @@ public final class DriverStation {
* @return Time remaining in current match period (auto or teleop) in seconds
*/
public static double getMatchTime() {
return HAL.getMatchTime();
}
/**
* Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
* purposes only.
*
* @param entering If true, starting disabled code; if false, leaving disabled code
*/
public static void inDisabled(boolean entering) {
m_userInDisabled = entering;
}
/**
* Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
* purposes only.
*
* @param entering If true, starting autonomous code; if false, leaving autonomous code
*/
public static void inAutonomous(boolean entering) {
m_userInAutonomous = entering;
}
/**
* Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
* purposes only.
*
* @param entering If true, starting teleop code; if false, leaving teleop code
*/
public static void inTeleop(boolean entering) {
m_userInTeleop = entering;
}
/**
* Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
* purposes only.
*
* @param entering If true, starting test code; if false, leaving test code
*/
public static void inTest(boolean entering) {
m_userInTest = entering;
}
/** Forces waitForData() to return immediately. */
public static void wakeupWaitForData() {
m_waitForDataMutex.lock();
try {
m_waitForDataCount++;
m_waitForDataCond.signalAll();
} finally {
m_waitForDataMutex.unlock();
}
return DriverStationJNI.getMatchTime();
}
/**
@@ -1317,30 +1139,43 @@ public final class DriverStation {
return !isFMSAttached() && m_silenceJoystickWarning;
}
/**
* Refresh the passed in control word to contain the current control word cache.
*
* @param word Word to refresh.
*/
public static void refreshControlWordFromCache(ControlWord word) {
m_cacheDataMutex.lock();
try {
word.update(m_controlWord);
} finally {
m_cacheDataMutex.unlock();
}
}
/**
* Copy data from the DS task for the user. If no new data exists, it will just be returned,
* otherwise the data will be copied from the DS polling loop.
*/
protected static void getData() {
public static void refreshData() {
DriverStationJNI.refreshDSData();
// Get the status of all of the joysticks
for (byte stick = 0; stick < kJoystickPorts; stick++) {
m_joystickAxesCache[stick].m_count =
HAL.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes);
DriverStationJNI.getJoystickAxes(stick, m_joystickAxesCache[stick].m_axes);
m_joystickAxesRawCache[stick].m_count =
DriverStationJNI.getJoystickAxesRaw(stick, m_joystickAxesRawCache[stick].m_axes);
m_joystickPOVsCache[stick].m_count =
HAL.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs);
m_joystickButtonsCache[stick].m_buttons = HAL.getJoystickButtons(stick, m_buttonCountBuffer);
DriverStationJNI.getJoystickPOVs(stick, m_joystickPOVsCache[stick].m_povs);
m_joystickButtonsCache[stick].m_buttons =
DriverStationJNI.getJoystickButtons(stick, m_buttonCountBuffer);
m_joystickButtonsCache[stick].m_count = m_buttonCountBuffer.get(0);
}
HAL.getMatchInfo(m_matchInfoCache);
DriverStationJNI.getMatchInfo(m_matchInfoCache);
m_controlWordMutex.lock();
try {
// Force a control word update, to make sure the data is the newest.
updateControlWord(true);
} finally {
m_controlWordMutex.unlock();
}
DriverStationJNI.getControlWord(m_controlWordCache);
DataLogSender dataLogSender;
// lock joystick mutex to swap cache data
@@ -1361,6 +1196,10 @@ public final class DriverStation {
m_joystickAxes = m_joystickAxesCache;
m_joystickAxesCache = currentAxes;
HALJoystickAxesRaw[] currentAxesRaw = m_joystickAxesRaw;
m_joystickAxesRaw = m_joystickAxesRawCache;
m_joystickAxesRawCache = currentAxesRaw;
HALJoystickButtons[] currentButtons = m_joystickButtons;
m_joystickButtons = m_joystickButtonsCache;
m_joystickButtonsCache = currentButtons;
@@ -1373,18 +1212,31 @@ public final class DriverStation {
m_matchInfo = m_matchInfoCache;
m_matchInfoCache = currentInfo;
ControlWord currentWord = m_controlWord;
m_controlWord = m_controlWordCache;
m_controlWordCache = currentWord;
dataLogSender = m_dataLogSender;
} finally {
m_cacheDataMutex.unlock();
}
wakeupWaitForData();
m_refreshEvents.wakeup();
m_matchDataSender.sendMatchData();
if (dataLogSender != null) {
dataLogSender.send(WPIUtilJNI.now());
}
}
public static void provideRefreshedDataEventHandle(int handle) {
m_refreshEvents.add(handle);
}
public static void removeRefreshedDataEventHandle(int handle) {
m_refreshEvents.remove(handle);
}
/**
* Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
* the DS.
@@ -1411,68 +1263,6 @@ public final class DriverStation {
}
}
/** Provides the service routine for the DS polling m_thread. */
private static void run() {
int safetyCounter = 0;
while (m_threadKeepAlive) {
HAL.waitForDSData();
getData();
if (isDisabled()) {
safetyCounter = 0;
}
safetyCounter++;
if (safetyCounter >= 4) {
MotorSafety.checkMotors();
safetyCounter = 0;
}
if (m_userInDisabled) {
HAL.observeUserProgramDisabled();
}
if (m_userInAutonomous) {
HAL.observeUserProgramAutonomous();
}
if (m_userInTeleop) {
HAL.observeUserProgramTeleop();
}
if (m_userInTest) {
HAL.observeUserProgramTest();
}
}
}
/**
* Forces a control word cache update, and update the passed in control word.
*
* @param word Word to update.
*/
public static void updateControlWordFromCache(ControlWord word) {
m_controlWordMutex.lock();
try {
updateControlWord(true);
word.update(m_controlWordCache);
} finally {
m_controlWordMutex.unlock();
}
}
/**
* Updates the data in the control word cache. Updates if the force parameter is set, or if 50ms
* have passed since the last update.
*
* <p>Must be called with m_controlWordMutex lock held.
*
* @param force True to force an update to the cache, otherwise update if 50ms have passed.
*/
private static void updateControlWord(boolean force) {
long now = System.currentTimeMillis();
if (now - m_lastControlWordUpdate > 50 || force) {
HAL.getControlWord(m_controlWordCache);
m_lastControlWordUpdate = now;
}
}
/**
* Starts logging DriverStation data to data log. Repeated calls are ignored.
*

8
wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
View File

@@ -4,7 +4,7 @@
package edu.wpi.first.wpilibj;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.wpilibj.event.BooleanEvent;
import edu.wpi.first.wpilibj.event.EventLoop;
import java.util.HashMap;
@@ -248,7 +248,7 @@ public class GenericHID {
*/
public void setOutput(int outputNumber, boolean value) {
m_outputs = (m_outputs & ~(1 << (outputNumber - 1))) | ((value ? 1 : 0) << (outputNumber - 1));
HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
DriverStationJNI.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
}
/**
@@ -258,7 +258,7 @@ public class GenericHID {
*/
public void setOutputs(int value) {
m_outputs = value;
HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
DriverStationJNI.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
}
/**
@@ -279,6 +279,6 @@ public class GenericHID {
} else {
m_rightRumble = (short) (value * 65535);
}
HAL.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
DriverStationJNI.setJoystickOutputs((byte) m_port, m_outputs, m_leftRumble, m_rightRumble);
}
}

13
wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
@@ -253,10 +254,12 @@ public abstract class IterativeRobotBase extends RobotBase {
}
protected void loopFunc() {
DriverStation.refreshData();
m_watchdog.reset();
m_word.refresh();
// Get current mode
m_word.update();
Mode mode = Mode.kNone;
if (m_word.isDisabled()) {
mode = Mode.kDisabled;
@@ -305,19 +308,19 @@ public abstract class IterativeRobotBase extends RobotBase {
// Call the appropriate function depending upon the current robot mode
if (mode == Mode.kDisabled) {
HAL.observeUserProgramDisabled();
DriverStationJNI.observeUserProgramDisabled();
disabledPeriodic();
m_watchdog.addEpoch("disabledPeriodic()");
} else if (mode == Mode.kAutonomous) {
HAL.observeUserProgramAutonomous();
DriverStationJNI.observeUserProgramAutonomous();
autonomousPeriodic();
m_watchdog.addEpoch("autonomousPeriodic()");
} else if (mode == Mode.kTeleop) {
HAL.observeUserProgramTeleop();
DriverStationJNI.observeUserProgramTeleop();
teleopPeriodic();
m_watchdog.addEpoch("teleopPeriodic()");
} else {
HAL.observeUserProgramTest();
DriverStationJNI.observeUserProgramTest();
testPeriodic();
m_watchdog.addEpoch("testPeriodic()");
}

1
wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java
View File

@@ -29,6 +29,7 @@ public abstract class MotorSafety {
public MotorSafety() {
synchronized (m_listMutex) {
m_instanceList.add(this);
// TODO Threads
}
}

9
wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
View File

@@ -274,15 +274,6 @@ public abstract class RobotBase implements AutoCloseable {
return DriverStation.isTeleopEnabled();
}
/**
* Indicates if new data is available from the driver station.
*
* @return Has new data arrived over the network since the last time this function was called?
*/
public boolean isNewDataAvailable() {
return DriverStation.isNewControlData();
}
/** Provide an alternate "main loop" via startCompetition(). */
public abstract void startCompetition();

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
@@ -110,7 +111,7 @@ public class TimedRobot extends IterativeRobotBase {
// Tell the DS that the robot is ready to be enabled
System.out.println("********** Robot program startup complete **********");
HAL.observeUserProgramStarting();
DriverStationJNI.observeUserProgramStarting();
// Loop forever, calling the appropriate mode-dependent function
while (true) {

112
wpilibj/src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java Normal file
View File

@@ -0,0 +1,112 @@
// 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.
package edu.wpi.first.wpilibj.internal;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.wpilibj.DriverStation;
import java.util.concurrent.atomic.AtomicBoolean;
/*
* For internal use only.
*/
public class DriverStationModeThread implements AutoCloseable {
private final AtomicBoolean m_keepAlive = new AtomicBoolean();
private final Thread m_thread;
private boolean m_userInDisabled;
private boolean m_userInAutonomous;
private boolean m_userInTeleop;
private boolean m_userInTest;
/** Internal use only. */
public DriverStationModeThread() {
m_keepAlive.set(true);
m_thread = new Thread(this::run, "DriverStationMode");
m_thread.start();
}
private void run() {
int handle = WPIUtilJNI.createEvent(true, false);
DriverStationJNI.provideNewDataEventHandle(handle);
while (m_keepAlive.get()) {
try {
WPIUtilJNI.waitForObjectTimeout(handle, 0.1);
} catch (InterruptedException e) {
DriverStationJNI.removeNewDataEventHandle(handle);
WPIUtilJNI.destroyEvent(handle);
Thread.currentThread().interrupt();
return;
}
DriverStation.refreshData();
if (m_userInDisabled) {
DriverStationJNI.observeUserProgramDisabled();
}
if (m_userInAutonomous) {
DriverStationJNI.observeUserProgramAutonomous();
}
if (m_userInTeleop) {
DriverStationJNI.observeUserProgramTeleop();
}
if (m_userInTest) {
DriverStationJNI.observeUserProgramTest();
}
}
DriverStationJNI.removeNewDataEventHandle(handle);
WPIUtilJNI.destroyEvent(handle);
}
/**
* Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
* purposes only.
*
* @param entering If true, starting disabled code; if false, leaving disabled code
*/
public void inDisabled(boolean entering) {
m_userInDisabled = entering;
}
/**
* Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
* purposes only.
*
* @param entering If true, starting autonomous code; if false, leaving autonomous code
*/
public void inAutonomous(boolean entering) {
m_userInAutonomous = entering;
}
/**
* Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
* purposes only.
*
* @param entering If true, starting teleop code; if false, leaving teleop code
*/
public void inTeleop(boolean entering) {
m_userInTeleop = entering;
}
/**
* Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
* purposes only.
*
* @param entering If true, starting test code; if false, leaving test code
*/
public void inTest(boolean entering) {
m_userInTest = entering;
}
@Override
public void close() {
m_keepAlive.set(false);
try {
m_thread.join();
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
}

13
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java
View File

@@ -5,8 +5,10 @@
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.AllianceStationID;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.hal.simulation.DriverStationDataJNI;
import edu.wpi.first.hal.simulation.NotifyCallback;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.wpilibj.DriverStation;
/** Class to control a simulated driver station. */
@@ -310,8 +312,17 @@ public final class DriverStationSim {
/** Updates DriverStation data so that new values are visible to the user program. */
public static void notifyNewData() {
int handle = WPIUtilJNI.createEvent(false, false);
DriverStationJNI.provideNewDataEventHandle(handle);
DriverStationDataJNI.notifyNewData();
DriverStation.waitForData();
try {
WPIUtilJNI.waitForObject(handle);
} catch (InterruptedException e) {
e.printStackTrace();
}
DriverStationJNI.removeNewDataEventHandle(handle);
WPIUtilJNI.destroyEvent(handle);
DriverStation.refreshData();
}
/**

4
wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java
View File

@@ -7,7 +7,7 @@ package edu.wpi.first.wpilibj.hal;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.hal.MatchInfoData;
import edu.wpi.first.hal.simulation.DriverStationDataJNI;
import edu.wpi.first.wpilibj.DriverStation.MatchType;
@@ -20,7 +20,7 @@ class MatchInfoDataTest {
DriverStationDataJNI.setMatchInfo("Event Name", "Game Message", 174, 191, matchType.ordinal());
MatchInfoData outMatchInfo = new MatchInfoData();
HAL.getMatchInfo(outMatchInfo);
DriverStationJNI.getMatchInfo(outMatchInfo);
assertAll(
() -> assertEquals("Event Name", outMatchInfo.eventName),

7
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java
View File

@@ -137,6 +137,7 @@ class DriverStationSimTest {
// B1
allianceStation = AllianceStationID.Blue1;
DriverStationSim.setAllianceStationId(allianceStation);
DriverStationSim.notifyNewData();
assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance());
assertEquals(1, DriverStation.getLocation());
@@ -146,6 +147,7 @@ class DriverStationSimTest {
// B2
allianceStation = AllianceStationID.Blue2;
DriverStationSim.setAllianceStationId(allianceStation);
DriverStationSim.notifyNewData();
assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance());
assertEquals(2, DriverStation.getLocation());
@@ -155,6 +157,7 @@ class DriverStationSimTest {
// B3
allianceStation = AllianceStationID.Blue3;
DriverStationSim.setAllianceStationId(allianceStation);
DriverStationSim.notifyNewData();
assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance());
assertEquals(3, DriverStation.getLocation());
@@ -164,6 +167,7 @@ class DriverStationSimTest {
// R1
allianceStation = AllianceStationID.Red1;
DriverStationSim.setAllianceStationId(allianceStation);
DriverStationSim.notifyNewData();
assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance());
assertEquals(1, DriverStation.getLocation());
@@ -173,6 +177,7 @@ class DriverStationSimTest {
// R2
allianceStation = AllianceStationID.Red2;
DriverStationSim.setAllianceStationId(allianceStation);
DriverStationSim.notifyNewData();
assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance());
assertEquals(2, DriverStation.getLocation());
@@ -182,6 +187,7 @@ class DriverStationSimTest {
// R3
allianceStation = AllianceStationID.Red3;
DriverStationSim.setAllianceStationId(allianceStation);
DriverStationSim.notifyNewData();
assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance());
assertEquals(3, DriverStation.getLocation());
@@ -230,6 +236,7 @@ class DriverStationSimTest {
try (CallbackStore cb = DriverStationSim.registerMatchTimeCallback(callback, false)) {
final double testTime = 19.174;
DriverStationSim.setMatchTime(testTime);
DriverStationSim.notifyNewData();
assertEquals(testTime, DriverStationSim.getMatchTime());
assertEquals(testTime, DriverStation.getMatchTime());
assertTrue(callback.wasTriggered());

55
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java
View File

@@ -4,9 +4,11 @@
package edu.wpi.first.wpilibj.templates.educational;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.internal.DriverStationModeThread;
/** Educational robot base class. */
public class EducationalRobot extends RobotBase {
@@ -34,40 +36,65 @@ public class EducationalRobot extends RobotBase {
public void startCompetition() {
robotInit();
DriverStationModeThread modeThread = new DriverStationModeThread();
int event = WPIUtilJNI.createEvent(false, false);
DriverStation.provideRefreshedDataEventHandle(event);
// Tell the DS that the robot is ready to be enabled
HAL.observeUserProgramStarting();
DriverStationJNI.observeUserProgramStarting();
while (!Thread.currentThread().isInterrupted() && !m_exit) {
if (isDisabled()) {
DriverStation.inDisabled(true);
modeThread.inDisabled(true);
disabled();
DriverStation.inDisabled(false);
modeThread.inDisabled(false);
while (isDisabled()) {
DriverStation.waitForData();
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
} else if (isAutonomous()) {
DriverStation.inAutonomous(true);
modeThread.inAutonomous(true);
autonomous();
DriverStation.inAutonomous(false);
modeThread.inAutonomous(false);
while (isAutonomousEnabled()) {
DriverStation.waitForData();
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
} else if (isTest()) {
DriverStation.inTest(true);
modeThread.inTest(true);
test();
DriverStation.inTest(false);
modeThread.inTest(false);
while (isTest() && isEnabled()) {
DriverStation.waitForData();
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
} else {
DriverStation.inTeleop(true);
modeThread.inTeleop(true);
teleop();
DriverStation.inTeleop(false);
modeThread.inTeleop(false);
while (isTeleopEnabled()) {
DriverStation.waitForData();
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
}
}
DriverStation.removeRefreshedDataEventHandle(event);
modeThread.close();
}
@Override

61
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
View File

@@ -4,11 +4,11 @@
package edu.wpi.first.wpilibj.templates.robotbaseskeleton;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.internal.DriverStationModeThread;
/**
* The VM is configured to automatically run this class. If you change the name of this class or the
@@ -31,44 +31,65 @@ public class Robot extends RobotBase {
public void startCompetition() {
robotInit();
DriverStationModeThread modeThread = new DriverStationModeThread();
int event = WPIUtilJNI.createEvent(false, false);
DriverStation.provideRefreshedDataEventHandle(event);
// Tell the DS that the robot is ready to be enabled
HAL.observeUserProgramStarting();
DriverStationJNI.observeUserProgramStarting();
while (!Thread.currentThread().isInterrupted() && !m_exit) {
if (isDisabled()) {
DriverStation.inDisabled(true);
modeThread.inDisabled(true);
disabled();
DriverStation.inDisabled(false);
modeThread.inDisabled(false);
while (isDisabled()) {
DriverStation.waitForData();
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
} else if (isAutonomous()) {
DriverStation.inAutonomous(true);
modeThread.inAutonomous(true);
autonomous();
DriverStation.inAutonomous(false);
modeThread.inAutonomous(false);
while (isAutonomousEnabled()) {
DriverStation.waitForData();
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
} else if (isTest()) {
LiveWindow.setEnabled(true);
Shuffleboard.enableActuatorWidgets();
DriverStation.inTest(true);
modeThread.inTest(true);
test();
DriverStation.inTest(false);
modeThread.inTest(false);
while (isTest() && isEnabled()) {
DriverStation.waitForData();
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
LiveWindow.setEnabled(false);
Shuffleboard.disableActuatorWidgets();
} else {
DriverStation.inTeleop(true);
modeThread.inTeleop(true);
teleop();
DriverStation.inTeleop(false);
modeThread.inTeleop(false);
while (isTeleopEnabled()) {
DriverStation.waitForData();
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
}
}
DriverStation.removeRefreshedDataEventHandle(event);
modeThread.close();
}
@Override

55
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/EducationalRobot.java
View File

@@ -4,9 +4,11 @@
package edu.wpi.first.wpilibj.templates.romieducational;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.RobotBase;
import edu.wpi.first.wpilibj.internal.DriverStationModeThread;
/** Educational robot base class. */
public class EducationalRobot extends RobotBase {
@@ -34,40 +36,65 @@ public class EducationalRobot extends RobotBase {
public void startCompetition() {
robotInit();
DriverStationModeThread modeThread = new DriverStationModeThread();
int event = WPIUtilJNI.createEvent(false, false);
DriverStation.provideRefreshedDataEventHandle(event);
// Tell the DS that the robot is ready to be enabled
HAL.observeUserProgramStarting();
DriverStationJNI.observeUserProgramStarting();
while (!Thread.currentThread().isInterrupted() && !m_exit) {
if (isDisabled()) {
DriverStation.inDisabled(true);
modeThread.inDisabled(true);
disabled();
DriverStation.inDisabled(false);
modeThread.inDisabled(false);
while (isDisabled()) {
DriverStation.waitForData();
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
} else if (isAutonomous()) {
DriverStation.inAutonomous(true);
modeThread.inAutonomous(true);
autonomous();
DriverStation.inAutonomous(false);
modeThread.inAutonomous(false);
while (isAutonomousEnabled()) {
DriverStation.waitForData();
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
} else if (isTest()) {
DriverStation.inTest(true);
modeThread.inTest(true);
test();
DriverStation.inTest(false);
modeThread.inTest(false);
while (isTest() && isEnabled()) {
DriverStation.waitForData();
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
} else {
DriverStation.inTeleop(true);
modeThread.inTeleop(true);
teleop();
DriverStation.inTeleop(false);
modeThread.inTeleop(false);
while (isTeleopEnabled()) {
DriverStation.waitForData();
try {
WPIUtilJNI.waitForObject(event);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
}
}
DriverStation.removeRefreshedDataEventHandle(event);
modeThread.close();
}
@Override

14
wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriverStationTest.java
View File

@@ -6,6 +6,8 @@ package edu.wpi.first.wpilibj;
import static org.junit.Assert.assertEquals;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.util.WPIUtilJNI;
import edu.wpi.first.wpilibj.test.AbstractComsSetup;
import java.util.logging.Logger;
import org.junit.Test;
@@ -24,13 +26,23 @@ public class DriverStationTest extends AbstractComsSetup {
public void waitForDataTest() {
long startTime = RobotController.getFPGATime();
int handle = WPIUtilJNI.createEvent(false, false);
DriverStationJNI.provideNewDataEventHandle(handle);
// Wait for data 50 times
for (int i = 0; i < 50; i++) {
DriverStation.waitForData();
try {
WPIUtilJNI.waitForObject(handle);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
long endTime = RobotController.getFPGATime();
long difference = endTime - startTime;
DriverStationJNI.removeNewDataEventHandle(handle);
WPIUtilJNI.destroyEvent(handle);
assertEquals(
"DriverStation waitForData did not wait long enough",
TIMER_RUNTIME,

5
wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java
View File

@@ -4,6 +4,7 @@
package edu.wpi.first.wpilibj.test;
import edu.wpi.first.hal.DriverStationJNI;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.MockDS;
@@ -40,7 +41,7 @@ public abstract class AbstractComsSetup {
try {
// Set some implementations so that the static methods work properly
HAL.initialize(500, 0);
HAL.observeUserProgramStarting();
DriverStationJNI.observeUserProgramStarting();
DriverStation.getAlliance();
ds = new MockDS();
@@ -56,6 +57,7 @@ public abstract class AbstractComsSetup {
// Wait until the robot is enabled before starting the tests
int enableCounter = 0;
DriverStation.refreshData();
while (!DriverStation.isEnabled()) {
if (enableCounter > 50) {
// Robot did not enable properly after 5 seconds.
@@ -69,6 +71,7 @@ public abstract class AbstractComsSetup {
ex.printStackTrace();
}
TestBench.out().println("Waiting for enable: " + enableCounter++);
DriverStation.refreshData();
}
TestBench.out().println();

54
wpiutil/src/main/java/edu/wpi/first/util/EventVector.java Normal file
View File

@@ -0,0 +1,54 @@
// 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.
package edu.wpi.first.util;
import java.util.ArrayList;
import java.util.List;
import java.util.concurrent.locks.ReentrantLock;
public class EventVector {
private ReentrantLock m_lock = new ReentrantLock();
private List<Integer> m_events = new ArrayList<>();
/**
* Adds an event to the event vector.
*
* @param handle The event to add
*/
public void add(int handle) {
m_lock.lock();
try {
m_events.add(handle);
} finally {
m_lock.unlock();
}
}
/**
* Removes an event from the event vector.
*
* @param handle The event to remove
*/
public void remove(int handle) {
m_lock.lock();
try {
m_events.removeIf(x -> x.intValue() == handle);
} finally {
m_lock.unlock();
}
}
/** Wakes up all events in the event vector. */
public void wakeup() {
m_lock.lock();
try {
for (Integer eventHandle : m_events) {
WPIUtilJNI.setEvent(eventHandle);
}
} finally {
m_lock.unlock();
}
}
}

51
wpiutil/src/main/native/include/wpi/EventVector.h Normal file
View File

@@ -0,0 +1,51 @@
// 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.
#pragma once
#include "wpi/SmallVector.h"
#include "wpi/Synchronization.h"
#include "wpi/mutex.h"
namespace wpi {
struct EventVector {
wpi::mutex mutex;
wpi::SmallVector<WPI_EventHandle, 4> events;
/**
* Adds an event to the event vector.
*
* @param handle the event to add
*/
void Add(WPI_EventHandle handle) {
std::scoped_lock lock{mutex};
events.emplace_back(handle);
}
/**
* Removes an event from the event vector.
*
* @param handle The event to remove
*/
void Remove(WPI_EventHandle handle) {
std::scoped_lock lock{mutex};
auto it = std::find_if(
events.begin(), events.end(),
[=](const WPI_EventHandle fromHandle) { return fromHandle == handle; });
if (it != events.end()) {
events.erase(it);
}
}
/**
* Wakes up all events in the event vector.
*/
void Wakeup() {
std::scoped_lock lock{mutex};
for (auto&& handle : events) {
wpi::SetEvent(handle);
}
}
};
} // namespace wpi

2
wpiutil/src/main/native/include/wpi/Synchronization.h
View File

@@ -4,9 +4,9 @@
#pragma once
#ifdef __cplusplus
#include <climits> // NOLINT
#ifdef __cplusplus
#include <initializer_list>
#include <span>
#endif