From 4a401b89d75781481048fdcf811e026344852a28 Mon Sep 17 00:00:00 2001 From: Thad House Date: Fri, 21 Oct 2022 22:01:55 -0700 Subject: [PATCH] [hal, wpilib] New DS thread model and implementation (#3787) 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. --- .../src/main/native/cpp/TestEnvironment.cpp | 2 + .../edu/wpi/first/hal/DriverStationJNI.java | 137 ++++++ hal/src/main/java/edu/wpi/first/hal/HAL.java | 122 +---- .../main/native/athena/FRCDriverStation.cpp | 233 ++++----- hal/src/main/native/athena/HAL.cpp | 3 +- .../main/native/cpp/jni/DriverStationJNI.cpp | 450 ++++++++++++++++++ hal/src/main/native/cpp/jni/HAL.cpp | 356 -------------- .../main/native/include/hal/DriverStation.h | 50 +- .../native/include/hal/DriverStationTypes.h | 1 + hal/src/main/native/sim/DriverStation.cpp | 235 ++++----- hal/src/main/native/sim/HAL.cpp | 6 +- .../native/sim/mockdata/DriverStationData.cpp | 6 +- .../cpp/mockdata/DriverStationDataTest.cpp | 6 + .../src/test/native/cpp/main.cpp | 2 + .../first/wpilibj2/MockHardwareExtension.java | 1 + .../wpilibj2/command/CommandTestBase.java | 1 - .../cpp/frc2/command/CommandTestBase.cpp | 2 + .../src/main/native/cpp/DataLogManager.cpp | 8 +- wpilibc/src/main/native/cpp/DriverStation.cpp | 140 +----- .../main/native/cpp/IterativeRobotBase.cpp | 3 + wpilibc/src/main/native/cpp/MotorSafety.cpp | 81 +++- .../cpp/internal/DriverStationModeThread.cpp | 64 +++ .../cpp/simulation/DriverStationSim.cpp | 7 +- wpilibc/src/main/native/cppcs/RobotBase.cpp | 8 +- .../main/native/include/frc/DriverStation.h | 88 +--- .../src/main/native/include/frc/RobotBase.h | 8 - .../frc/internal/DriverStationModeThread.h | 36 ++ .../src/test/native/cpp/TimedRobotTest.cpp | 4 + .../cpp/simulation/DriverStationSimTest.cpp | 7 + .../src/main/cpp/examples/HAL/c/Robot.c | 12 +- .../templates/robotbaseskeleton/cpp/Robot.cpp | 30 +- .../src/main/native/cpp/DriverStationTest.cpp | 8 +- .../src/main/native/cpp/TestEnvironment.cpp | 2 + .../edu/wpi/first/wpilibj/DSControlWord.java | 6 +- .../edu/wpi/first/wpilibj/DataLogManager.java | 13 +- .../edu/wpi/first/wpilibj/DriverStation.java | 428 +++++------------ .../edu/wpi/first/wpilibj/GenericHID.java | 8 +- .../wpi/first/wpilibj/IterativeRobotBase.java | 13 +- .../edu/wpi/first/wpilibj/MotorSafety.java | 1 + .../java/edu/wpi/first/wpilibj/RobotBase.java | 9 - .../edu/wpi/first/wpilibj/TimedRobot.java | 3 +- .../internal/DriverStationModeThread.java | 112 +++++ .../wpilibj/simulation/DriverStationSim.java | 13 +- .../first/wpilibj/hal/MatchInfoDataTest.java | 4 +- .../simulation/DriverStationSimTest.java | 7 + .../educational/EducationalRobot.java | 55 ++- .../templates/robotbaseskeleton/Robot.java | 61 ++- .../romieducational/EducationalRobot.java | 55 ++- .../wpi/first/wpilibj/DriverStationTest.java | 14 +- .../first/wpilibj/test/AbstractComsSetup.java | 5 +- .../java/edu/wpi/first/util/EventVector.java | 54 +++ .../src/main/native/include/wpi/EventVector.h | 51 ++ .../main/native/include/wpi/Synchronization.h | 2 +- 53 files changed, 1649 insertions(+), 1384 deletions(-) create mode 100644 hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java create mode 100644 hal/src/main/native/cpp/jni/DriverStationJNI.cpp create mode 100644 wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp create mode 100644 wpilibc/src/main/native/include/frc/internal/DriverStationModeThread.h create mode 100644 wpilibj/src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java create mode 100644 wpiutil/src/main/java/edu/wpi/first/util/EventVector.java create mode 100644 wpiutil/src/main/native/include/wpi/EventVector.h diff --git a/crossConnIntegrationTests/src/main/native/cpp/TestEnvironment.cpp b/crossConnIntegrationTests/src/main/native/cpp/TestEnvironment.cpp index ee8c2b1b24..458046c87e 100644 --- a/crossConnIntegrationTests/src/main/native/cpp/TestEnvironment.cpp +++ b/crossConnIntegrationTests/src/main/native/cpp/TestEnvironment.cpp @@ -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); } diff --git a/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java b/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java new file mode 100644 index 0000000000..f40a38cb72 --- /dev/null +++ b/hal/src/main/java/edu/wpi/first/hal/DriverStationJNI.java @@ -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. + * + *

Original signature: uint32_t report(tResourceType, uint8_t, uint8_t, const + * char*) + * + * @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() {} +} diff --git a/hal/src/main/java/edu/wpi/first/hal/HAL.java b/hal/src/main/java/edu/wpi/first/hal/HAL.java index 5a63d50abd..68e6ec8493 100644 --- a/hal/src/main/java/edu/wpi/first/hal/HAL.java +++ b/hal/src/main/java/edu/wpi/first/hal/HAL.java @@ -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. - * - *

Original signature: uint32_t report(tResourceType, uint8_t, uint8_t, const - * char*) - * - * @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() {} } diff --git a/hal/src/main/native/athena/FRCDriverStation.cpp b/hal/src/main/native/athena/FRCDriverStation.cpp index e7ef1948f9..b9c6c73521 100644 --- a/hal/src/main/native/athena/FRCDriverStation.cpp +++ b/hal/src/main/native/athena/FRCDriverStation.cpp @@ -13,11 +13,14 @@ #include #include #include +#include #include +#include #include #include #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); +// static_assert(std::is_trivial_v); + +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(&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(&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)); -} - 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(&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(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)); + 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 diff --git a/hal/src/main/native/athena/HAL.cpp b/hal/src/main/native/athena/HAL.cpp index 5eba6e4b4c..08abf14bb6 100644 --- a/hal/src/main/native/athena/HAL.cpp +++ b/hal/src/main/native/athena/HAL.cpp @@ -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) { diff --git a/hal/src/main/native/cpp/jni/DriverStationJNI.cpp b/hal/src/main/native/cpp/jni/DriverStationJNI.cpp new file mode 100644 index 0000000000..f050f02129 --- /dev/null +++ b/hal/src/main/native/cpp/jni/DriverStationJNI.cpp @@ -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 + +#include + +#include +#include + +#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(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(axes.count), static_cast(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(axes.count), static_cast(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(povs.count), static_cast(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(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" diff --git a/hal/src/main/native/cpp/jni/HAL.cpp b/hal/src/main/native/cpp/jni/HAL.cpp index 4e260324c9..b603a76cb3 100644 --- a/hal/src/main/native/cpp/jni/HAL.cpp +++ b/hal/src/main/native/cpp/jni/HAL.cpp @@ -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(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(axes.count), static_cast(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(povs.count), static_cast(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(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(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(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 diff --git a/hal/src/main/native/include/hal/DriverStation.h b/hal/src/main/native/include/hal/DriverStation.h index 1839cfc99c..fabc45f905 100644 --- a/hal/src/main/native/include/hal/DriverStation.h +++ b/hal/src/main/native/include/hal/DriverStation.h @@ -6,6 +6,8 @@ #include +#include + #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. diff --git a/hal/src/main/native/include/hal/DriverStationTypes.h b/hal/src/main/native/include/hal/DriverStationTypes.h index 21f9088bd8..277c2860f2 100644 --- a/hal/src/main/native/include/hal/DriverStationTypes.h +++ b/hal/src/main/native/include/hal/DriverStationTypes.h @@ -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; diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp index 723cdacfa8..399080d5a5 100644 --- a/hal/src/main/native/sim/DriverStation.cpp +++ b/hal/src/main/native/sim/DriverStation.cpp @@ -14,32 +14,78 @@ #include #include +#include #include #include #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 sendErrorHandler{nullptr}; static std::atomic 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); +// static_assert(std::is_trivial_v); + +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(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 diff --git a/hal/src/main/native/sim/HAL.cpp b/hal/src/main/native/sim/HAL.cpp index 9856f0110d..eca7c2578b 100644 --- a/hal/src/main/native/sim/HAL.cpp +++ b/hal/src/main/native/sim/HAL.cpp @@ -57,6 +57,10 @@ static std::vector> 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; diff --git a/hal/src/main/native/sim/mockdata/DriverStationData.cpp b/hal/src/main/native/sim/mockdata/DriverStationData.cpp index 1c76a7a7ca..3756d6e492 100644 --- a/hal/src/main/native/sim/mockdata/DriverStationData.cpp +++ b/hal/src/main/native/sim/mockdata/DriverStationData.cpp @@ -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, diff --git a/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp b/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp index 4925e162b7..59658cbaa1 100644 --- a/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp +++ b/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp @@ -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); diff --git a/simulation/halsim_ws_server/src/test/native/cpp/main.cpp b/simulation/halsim_ws_server/src/test/native/cpp/main.cpp index 382656fd8a..a9f739a5a8 100644 --- a/simulation/halsim_ws_server/src/test/native/cpp/main.cpp +++ b/simulation/halsim_ws_server/src/test/native/cpp/main.cpp @@ -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); diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java index b0ffc88651..2de1a07583 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java @@ -34,5 +34,6 @@ public final class MockHardwareExtension implements BeforeAllCallback { DriverStationSim.setAutonomous(false); DriverStationSim.setEnabled(true); DriverStationSim.setTest(true); + DriverStationSim.notifyNewData(); } } diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java index 1f308c2ed1..0876de1504 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java @@ -31,7 +31,6 @@ public class CommandTestBase { DriverStationSim.setEnabled(enabled); DriverStationSim.notifyNewData(); - DriverStation.isNewControlData(); while (DriverStation.isEnabled() != enabled) { try { Thread.sleep(1); diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp index 4dccc0ca6d..6e27d248c0 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp @@ -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(); } diff --git a/wpilibc/src/main/native/cpp/DataLogManager.cpp b/wpilibc/src/main/native/cpp/DataLogManager.cpp index c5546c42ba..b92faa387b 100644 --- a/wpilibc/src/main/native/cpp/DataLogManager.cpp +++ b/wpilibc/src/main/native/cpp/DataLogManager.cpp @@ -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() { diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp index b351b79b0e..aead3439af 100644 --- a/wpilibc/src/main/native/cpp/DriverStation.cpp +++ b/wpilibc/src/main/native/cpp/DriverStation.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -117,6 +118,7 @@ struct Instance { Instance(); ~Instance(); + wpi::EventVector refreshEvents; MatchDataSender matchDataSender; std::atomic dataLogSender{nullptr}; @@ -127,14 +129,6 @@ struct Instance { std::array joystickButtonsPressed; std::array joystickButtonsReleased; - // Internal Driver Station thread - std::thread dsThread; - std::atomic 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); diff --git a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp index 3732dd0f67..d783139aa1 100644 --- a/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp +++ b/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp @@ -4,6 +4,8 @@ #include "frc/IterativeRobotBase.h" +#include + #include #include #include @@ -97,6 +99,7 @@ units::second_t IterativeRobotBase::GetPeriod() const { } void IterativeRobotBase::LoopFunc() { + DriverStation::RefreshData(); m_watchdog.Reset(); // Get current mode diff --git a/wpilibc/src/main/native/cpp/MotorSafety.cpp b/wpilibc/src/main/native/cpp/MotorSafety.cpp index 388a887a97..0101e066f1 100644 --- a/wpilibc/src/main/native/cpp/MotorSafety.cpp +++ b/wpilibc/src/main/native/cpp/MotorSafety.cpp @@ -7,6 +7,8 @@ #include #include +#include +#include #include #include "frc/DriverStation.h" @@ -14,26 +16,84 @@ using namespace frc; -static wpi::SmallPtrSet 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 m_owner; +} // namespace + +static std::atomic_bool gShutdown{false}; + +namespace { +struct MotorSafetyManager { + ~MotorSafetyManager() { gShutdown = true; } + + wpi::SmallPtrSet 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(); } } diff --git a/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp b/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp new file mode 100644 index 0000000000..7c28bad08a --- /dev/null +++ b/wpilibc/src/main/native/cpp/internal/DriverStationModeThread.cpp @@ -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 +#include + +#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()); +} diff --git a/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp index 5f2c6458d4..61c29492ae 100644 --- a/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -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) { diff --git a/wpilibc/src/main/native/cppcs/RobotBase.cpp b/wpilibc/src/main/native/cppcs/RobotBase.cpp index 2206bff8ed..edf6291364 100644 --- a/wpilibc/src/main/native/cppcs/RobotBase.cpp +++ b/wpilibc/src/main/native/cppcs/RobotBase.cpp @@ -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); diff --git a/wpilibc/src/main/native/include/frc/DriverStation.h b/wpilibc/src/main/native/include/frc/DriverStation.h index 4d54579132..4cbf6daabb 100644 --- a/wpilibc/src/main/native/include/frc/DriverStation.h +++ b/wpilibc/src/main/native/include/frc/DriverStation.h @@ -7,6 +7,7 @@ #include #include +#include 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 diff --git a/wpilibc/src/main/native/include/frc/RobotBase.h b/wpilibc/src/main/native/include/frc/RobotBase.h index a536cda2c7..5070525cb0 100644 --- a/wpilibc/src/main/native/include/frc/RobotBase.h +++ b/wpilibc/src/main/native/include/frc/RobotBase.h @@ -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. */ diff --git a/wpilibc/src/main/native/include/frc/internal/DriverStationModeThread.h b/wpilibc/src/main/native/include/frc/internal/DriverStationModeThread.h new file mode 100644 index 0000000000..f4fd11ca82 --- /dev/null +++ b/wpilibc/src/main/native/include/frc/internal/DriverStationModeThread.h @@ -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 +#include + +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 diff --git a/wpilibc/src/test/native/cpp/TimedRobotTest.cpp b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp index ea9656bb01..4ccce1b92d 100644 --- a/wpilibc/src/test/native/cpp/TimedRobotTest.cpp +++ b/wpilibc/src/test/native/cpp/TimedRobotTest.cpp @@ -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); diff --git a/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp index d01ca27937..6aa31c6805 100644 --- a/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp @@ -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()); diff --git a/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c b/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c index 0db402bf76..b0905f9d6e 100644 --- a/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c +++ b/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c @@ -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); diff --git a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp index 29bae0bf1d..a521b47257 100644 --- a/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp +++ b/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp @@ -5,6 +5,7 @@ #include "Robot.h" #include +#include #include #include #include @@ -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()); } } } diff --git a/wpilibcIntegrationTests/src/main/native/cpp/DriverStationTest.cpp b/wpilibcIntegrationTests/src/main/native/cpp/DriverStationTest.cpp index e4a2721a48..0369717533 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/DriverStationTest.cpp +++ b/wpilibcIntegrationTests/src/main/native/cpp/DriverStationTest.cpp @@ -4,6 +4,7 @@ #include "frc/DriverStation.h" // NOLINT(build/include_order) +#include #include #include @@ -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); diff --git a/wpilibcIntegrationTests/src/main/native/cpp/TestEnvironment.cpp b/wpilibcIntegrationTests/src/main/native/cpp/TestEnvironment.cpp index 1f5e3602fc..59a3a58443 100644 --- a/wpilibcIntegrationTests/src/main/native/cpp/TestEnvironment.cpp +++ b/wpilibcIntegrationTests/src/main/native/cpp/TestEnvironment.cpp @@ -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(); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java index 37e046df0f..8837d4167a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java @@ -16,12 +16,12 @@ public class DSControlWord { *

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); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java index 42d6be0d41..927c14d16a 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DataLogManager.java @@ -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(); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java index ce8a76c88a..139e2edc27 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -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 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. - * - *

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. - * - *

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. - * - *

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. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java index 5650954a4a..f05bfb6de7 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java @@ -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); } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java index 19f81e15b5..1115130a61 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java @@ -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()"); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java index 5fb10b4ff8..a6b50b1ec7 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java @@ -29,6 +29,7 @@ public abstract class MotorSafety { public MotorSafety() { synchronized (m_listMutex) { m_instanceList.add(this); + // TODO Threads } } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java index 07b0806248..1bf859366f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java @@ -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(); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java index 972df950ed..5a53b145f0 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java @@ -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) { diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java new file mode 100644 index 0000000000..ca71c9f6c2 --- /dev/null +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/internal/DriverStationModeThread.java @@ -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(); + } + } +} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java index aa0bf84d66..a429e71c6f 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java @@ -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(); } /** diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java index 855ace54e2..b2b22a2946 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java @@ -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), diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java index fb0a058f37..5bbf9aae57 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java @@ -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()); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java index 76d13a4a89..bb40caae8d 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java @@ -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 diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java index 787395c941..f08986476a 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java @@ -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 diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/EducationalRobot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/EducationalRobot.java index 8fed60df7e..5a84915d30 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/EducationalRobot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romieducational/EducationalRobot.java @@ -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 diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriverStationTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriverStationTest.java index b2e727dac3..cd67cbdb18 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriverStationTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriverStationTest.java @@ -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, diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java index 734115cdc3..056ff5ff1c 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java @@ -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(); diff --git a/wpiutil/src/main/java/edu/wpi/first/util/EventVector.java b/wpiutil/src/main/java/edu/wpi/first/util/EventVector.java new file mode 100644 index 0000000000..c1b32a5bb5 --- /dev/null +++ b/wpiutil/src/main/java/edu/wpi/first/util/EventVector.java @@ -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 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(); + } + } +} diff --git a/wpiutil/src/main/native/include/wpi/EventVector.h b/wpiutil/src/main/native/include/wpi/EventVector.h new file mode 100644 index 0000000000..fc54df709d --- /dev/null +++ b/wpiutil/src/main/native/include/wpi/EventVector.h @@ -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 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 diff --git a/wpiutil/src/main/native/include/wpi/Synchronization.h b/wpiutil/src/main/native/include/wpi/Synchronization.h index 774f17ea9b..4ac3c8c811 100644 --- a/wpiutil/src/main/native/include/wpi/Synchronization.h +++ b/wpiutil/src/main/native/include/wpi/Synchronization.h @@ -4,9 +4,9 @@ #pragma once +#ifdef __cplusplus #include // NOLINT -#ifdef __cplusplus #include #include #endif