diff --git a/hal/src/main/native/athena/FRCDriverStation.cpp b/hal/src/main/native/athena/FRCDriverStation.cpp index 03c9935d32..a0c6d1e13b 100644 --- a/hal/src/main/native/athena/FRCDriverStation.cpp +++ b/hal/src/main/native/athena/FRCDriverStation.cpp @@ -13,12 +13,12 @@ #include #include +#include #include #include #include #include "HAL/DriverStation.h" -#include "HALInitializer.h" static_assert(sizeof(int32_t) >= sizeof(int), "FRC_NetworkComm status variable is larger than 32 bits"); @@ -28,11 +28,232 @@ struct HAL_JoystickAxesInt { int16_t axes[HAL_kMaxJoystickAxes]; }; +static constexpr int kJoystickPorts = 6; + +// Joystick User Data +static std::unique_ptr m_joystickAxes; +static std::unique_ptr m_joystickPOVs; +static std::unique_ptr m_joystickButtons; +static std::unique_ptr m_joystickDescriptor; +static std::unique_ptr m_matchInfo; + +// Joystick Cached Data +static std::unique_ptr m_joystickAxesCache; +static std::unique_ptr m_joystickPOVsCache; +static std::unique_ptr m_joystickButtonsCache; +static std::unique_ptr m_joystickDescriptorCache; +static std::unique_ptr m_matchInfoCache; + +static wpi::mutex m_cacheDataMutex; + +// Control word variables +static HAL_ControlWord m_controlWordCache; +static std::chrono::steady_clock::time_point m_lastControlWordUpdate; +static wpi::mutex m_controlWordMutex; + +// Message and Data variables static wpi::mutex msgMutex; static wpi::condition_variable* newDSDataAvailableCond; static wpi::mutex newDSDataAvailableMutex; static int newDSDataAvailableCounter{0}; +static void InitializeDriverStationCaches() { + m_joystickAxes = std::make_unique(kJoystickPorts); + m_joystickPOVs = std::make_unique(kJoystickPorts); + m_joystickButtons = std::make_unique(kJoystickPorts); + m_joystickDescriptor = + std::make_unique(kJoystickPorts); + m_matchInfo = std::make_unique(); + m_joystickAxesCache = std::make_unique(kJoystickPorts); + m_joystickPOVsCache = std::make_unique(kJoystickPorts); + m_joystickButtonsCache = + std::make_unique(kJoystickPorts); + m_joystickDescriptorCache = + std::make_unique(kJoystickPorts); + m_matchInfoCache = std::make_unique(); + + // All joysticks should default to having zero axes, povs and buttons, so + // uninitialized memory doesn't get sent to speed controllers. + for (unsigned int i = 0; i < kJoystickPorts; i++) { + m_joystickAxes[i].count = 0; + m_joystickPOVs[i].count = 0; + m_joystickButtons[i].count = 0; + m_joystickDescriptor[i].isXbox = 0; + m_joystickDescriptor[i].type = -1; + m_joystickDescriptor[i].name[0] = '\0'; + + m_joystickAxesCache[i].count = 0; + m_joystickPOVsCache[i].count = 0; + m_joystickButtonsCache[i].count = 0; + m_joystickDescriptorCache[i].isXbox = 0; + m_joystickDescriptorCache[i].type = -1; + m_joystickDescriptorCache[i].name[0] = '\0'; + } +} + +static int32_t HAL_GetJoystickAxesInternal(int32_t joystickNum, + HAL_JoystickAxes* axes) { + HAL_JoystickAxesInt axesInt; + + int retVal = FRC_NetworkCommunication_getJoystickAxes( + joystickNum, reinterpret_cast(&axesInt), + HAL_kMaxJoystickAxes); + + // copy integer values to double values + axes->count = axesInt.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]; + if (value < 0) { + axes->axes[i] = value / 128.0; + } else { + axes->axes[i] = value / 127.0; + } + } + + return retVal; +} + +static int32_t HAL_GetJoystickPOVsInternal(int32_t joystickNum, + HAL_JoystickPOVs* povs) { + return FRC_NetworkCommunication_getJoystickPOVs( + joystickNum, reinterpret_cast(povs), + HAL_kMaxJoystickPOVs); +} + +static int32_t HAL_GetJoystickButtonsInternal(int32_t joystickNum, + HAL_JoystickButtons* buttons) { + return FRC_NetworkCommunication_getJoystickButtons( + joystickNum, &buttons->buttons, &buttons->count); +} +/** + * Retrieve the Joystick Descriptor for particular slot + * @param desc [out] descriptor (data transfer object) to fill in. desc is + * filled in regardless of success. In other words, if descriptor is not + * available, desc is filled in with default values matching the init-values in + * Java and C++ Driverstation for when caller requests a too-large joystick + * index. + * + * @return error code reported from Network Comm back-end. Zero is good, + * nonzero is bad. + */ +static int32_t HAL_GetJoystickDescriptorInternal(int32_t joystickNum, + HAL_JoystickDescriptor* desc) { + desc->isXbox = 0; + desc->type = std::numeric_limits::max(); + desc->name[0] = '\0'; + desc->axisCount = + HAL_kMaxJoystickAxes; /* set to the desc->axisTypes's capacity */ + desc->buttonCount = 0; + desc->povCount = 0; + int retval = FRC_NetworkCommunication_getJoystickDesc( + joystickNum, &desc->isXbox, &desc->type, + reinterpret_cast(&desc->name), &desc->axisCount, + reinterpret_cast(&desc->axisTypes), &desc->buttonCount, + &desc->povCount); + /* check the return, if there is an error and the RIOimage predates FRC2017, + * then axisCount needs to be cleared */ + if (retval != 0) { + /* set count to zero so downstream code doesn't decode invalid axisTypes. */ + desc->axisCount = 0; + } + 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; + int status = FRC_NetworkCommunication_getMatchInfo( + info->eventName, &matchType, &info->matchNumber, &info->replayNumber, + info->gameSpecificMessage, &info->gameSpecificMessageSize); + + info->matchType = static_cast(matchType); + + *(std::end(info->eventName) - 1) = '\0'; + + return status; +} + +static void UpdateDriverStationControlWord(bool force, + HAL_ControlWord& controlWord) { + auto now = std::chrono::steady_clock::now(); + std::lock_guard lock(m_controlWordMutex); + // Update every 50 ms or on force. + if ((now - m_lastControlWordUpdate > std::chrono::milliseconds(50)) || + force) { + HAL_GetControlWordInternal(&m_controlWordCache); + m_lastControlWordUpdate = now; + } + controlWord = m_controlWordCache; +} + +static void UpdateDriverStationDataCaches() { + // Get the status of all of the joysticks, and save to the cache + for (uint8_t stick = 0; stick < kJoystickPorts; stick++) { + HAL_GetJoystickAxesInternal(stick, &m_joystickAxesCache[stick]); + HAL_GetJoystickPOVsInternal(stick, &m_joystickPOVsCache[stick]); + HAL_GetJoystickButtonsInternal(stick, &m_joystickButtonsCache[stick]); + HAL_GetJoystickDescriptorInternal(stick, &m_joystickDescriptorCache[stick]); + } + // Grab match specific data + HAL_GetMatchInfoInternal(m_matchInfoCache.get()); + + // Force a control word update, to make sure the data is the newest. + HAL_ControlWord controlWord; + UpdateDriverStationControlWord(true, controlWord); + + { + // Obtain a lock on the data, swap the cached data into the main data arrays + std::lock_guard lock(m_cacheDataMutex); + + m_joystickAxes.swap(m_joystickAxesCache); + m_joystickPOVs.swap(m_joystickPOVsCache); + m_joystickButtons.swap(m_joystickButtonsCache); + m_joystickDescriptor.swap(m_joystickDescriptorCache); + m_matchInfo.swap(m_matchInfoCache); + } +} + +class DriverStationThread : public wpi::SafeThread { + public: + void Main() { + std::unique_lock lock(m_mutex); + while (m_active) { + m_cond.wait(lock, [&] { return !m_active || m_notify; }); + if (!m_active) break; + m_notify = false; + + UpdateDriverStationDataCaches(); + + std::lock_guard lock(newDSDataAvailableMutex); + // Nofify all threads + newDSDataAvailableCounter++; + newDSDataAvailableCond->notify_all(); + } + } + + bool m_notify = false; +}; + +class DriverStationThreadOwner + : public wpi::SafeThreadOwner { + public: + void Notify() { + auto thr = GetThread(); + if (!thr) return; + thr->m_notify = true; + thr->m_cond.notify_one(); + } +}; + +static std::unique_ptr dsThread = nullptr; + namespace hal { namespace init { void InitializeFRCDriverStation() { @@ -101,8 +322,40 @@ int32_t HAL_SendError(HAL_Bool isError, int32_t errorCode, HAL_Bool isLVCode, int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) { std::memset(controlWord, 0, sizeof(HAL_ControlWord)); - return FRC_NetworkCommunication_getControlWord( - reinterpret_cast(controlWord)); + UpdateDriverStationControlWord(false, *controlWord); + return 0; +} + +int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) { + std::unique_lock lock(m_cacheDataMutex); + *axes = m_joystickAxes[joystickNum]; + return 0; +} + +int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) { + std::unique_lock lock(m_cacheDataMutex); + *povs = m_joystickPOVs[joystickNum]; + return 0; +} + +int32_t HAL_GetJoystickButtons(int32_t joystickNum, + HAL_JoystickButtons* buttons) { + std::unique_lock lock(m_cacheDataMutex); + *buttons = m_joystickButtons[joystickNum]; + return 0; +} + +int32_t HAL_GetJoystickDescriptor(int32_t joystickNum, + HAL_JoystickDescriptor* desc) { + std::unique_lock lock(m_cacheDataMutex); + *desc = m_joystickDescriptor[joystickNum]; + return 0; +} + +int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) { + std::unique_lock lock(m_cacheDataMutex); + *info = *m_matchInfo; + return 0; } HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) { @@ -112,64 +365,6 @@ HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) { return allianceStation; } -int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) { - HAL_JoystickAxesInt axesInt; - - int retVal = FRC_NetworkCommunication_getJoystickAxes( - joystickNum, reinterpret_cast(&axesInt), - HAL_kMaxJoystickAxes); - - // copy integer values to double values - axes->count = axesInt.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]; - if (value < 0) { - axes->axes[i] = value / 128.0; - } else { - axes->axes[i] = value / 127.0; - } - } - - return retVal; -} - -int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) { - return FRC_NetworkCommunication_getJoystickPOVs( - joystickNum, reinterpret_cast(povs), - HAL_kMaxJoystickPOVs); -} - -int32_t HAL_GetJoystickButtons(int32_t joystickNum, - HAL_JoystickButtons* buttons) { - return FRC_NetworkCommunication_getJoystickButtons( - joystickNum, &buttons->buttons, &buttons->count); -} - -int32_t HAL_GetJoystickDescriptor(int32_t joystickNum, - HAL_JoystickDescriptor* desc) { - desc->isXbox = 0; - desc->type = std::numeric_limits::max(); - desc->name[0] = '\0'; - desc->axisCount = - HAL_kMaxJoystickAxes; /* set to the desc->axisTypes's capacity */ - desc->buttonCount = 0; - desc->povCount = 0; - int retval = FRC_NetworkCommunication_getJoystickDesc( - joystickNum, &desc->isXbox, &desc->type, - reinterpret_cast(&desc->name), &desc->axisCount, - reinterpret_cast(&desc->axisTypes), &desc->buttonCount, - &desc->povCount); - /* check the return, if there is an error and the RIOimage predates FRC2017, - * then axisCount needs to be cleared */ - if (retval != 0) { - /* set count to zero so downstream code doesn't decode invalid axisTypes. */ - desc->axisCount = 0; - } - return retval; -} - HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum) { HAL_JoystickDescriptor joystickDesc; if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) { @@ -226,67 +421,6 @@ double HAL_GetMatchTime(int32_t* status) { return matchTime; } -int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) { - uint16_t gameSpecificMessageSize = 0; - int status = FRC_NetworkCommunication_getMatchInfo( - nullptr, nullptr, nullptr, nullptr, nullptr, &gameSpecificMessageSize); - if (status < 0) { - info->eventName = nullptr; - info->gameSpecificMessage = nullptr; - return status; - } - info->eventName = static_cast(std::malloc(256)); - gameSpecificMessageSize = ((gameSpecificMessageSize + 1023) / 1024) * 1024; - uint16_t originalGameSpecificSize = gameSpecificMessageSize; - uint8_t* gameSpecificMessage = - static_cast(std::malloc(gameSpecificMessageSize)); - MatchType_t matchType = MatchType_t::kMatchType_none; - uint16_t matchNumber = 0; - uint8_t replayNumber = 0; - status = FRC_NetworkCommunication_getMatchInfo( - info->eventName, &matchType, &matchNumber, &replayNumber, - gameSpecificMessage, &gameSpecificMessageSize); - if (status < 0) { - std::free(info->eventName); - std::free(gameSpecificMessage); - info->eventName = nullptr; - info->gameSpecificMessage = nullptr; - return status; - } - if (gameSpecificMessageSize >= originalGameSpecificSize) { - // Data has updated between size and read calls. Retry. - // Unless large lag, this call will be right. - std::free(gameSpecificMessage); - gameSpecificMessageSize = ((gameSpecificMessageSize + 1023) / 1024) * 1024; - gameSpecificMessage = - static_cast(std::malloc(gameSpecificMessageSize)); - int status = FRC_NetworkCommunication_getMatchInfo( - nullptr, nullptr, nullptr, nullptr, gameSpecificMessage, - &gameSpecificMessageSize); - if (status < 0) { - std::free(info->eventName); - std::free(gameSpecificMessage); - info->eventName = nullptr; - info->gameSpecificMessage = nullptr; - return status; - } - } - info->eventName[255] = '\0'; - info->matchType = static_cast(matchType); - info->matchNumber = matchNumber; - info->replayNumber = replayNumber; - info->gameSpecificMessage = reinterpret_cast(gameSpecificMessage); - info->gameSpecificMessage[gameSpecificMessageSize] = '\0'; - return status; -} - -void HAL_FreeMatchInfo(HAL_MatchInfo* info) { - std::free(info->eventName); - std::free(info->gameSpecificMessage); - info->eventName = nullptr; - info->gameSpecificMessage = nullptr; -} - void HAL_ObserveUserProgramStarting(void) { FRC_NetworkCommunication_observeUserProgramStarting(); } @@ -323,8 +457,16 @@ HAL_Bool HAL_IsNewControlData(void) { return true; } +/** + * Waits for the newest DS packet to arrive. Note that this is a blocking call. + */ void HAL_WaitForDSData(void) { HAL_WaitForDSDataTimeout(0); } +/** + * 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. Returns true on new data, false on timeout. + */ HAL_Bool HAL_WaitForDSDataTimeout(double timeout) { auto timeoutTime = std::chrono::steady_clock::now() + std::chrono::duration(timeout); @@ -351,14 +493,15 @@ static void newDataOccur(uint32_t refNum) { // Since we could get other values, require our specific handle // to signal our threads if (refNum != refNumber) return; - std::lock_guard lock(newDSDataAvailableMutex); - // Nofify all threads - newDSDataAvailableCounter++; - newDSDataAvailableCond->notify_all(); + dsThread->Notify(); } +/* + * 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) { - hal::init::CheckInit(); static std::atomic_bool initialized{false}; static wpi::mutex initializeMutex; // Initial check, as if it's true initialization has finished @@ -368,6 +511,11 @@ void HAL_InitializeDriverStation(void) { // Second check in case another thread was waiting if (initialized) return; + InitializeDriverStationCaches(); + + dsThread = std::make_unique(); + dsThread->Start(); + // Set up the occur function internally with NetComm NetCommRPCProxy_SetOccurFuncPointer(newDataOccur); // Set up our occur reference number @@ -376,6 +524,10 @@ void HAL_InitializeDriverStation(void) { 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" diff --git a/hal/src/main/native/cpp/jni/HAL.cpp b/hal/src/main/native/cpp/jni/HAL.cpp index 099bf35e6d..ba2d4ec9ef 100644 --- a/hal/src/main/native/cpp/jni/HAL.cpp +++ b/hal/src/main/native/cpp/jni/HAL.cpp @@ -407,7 +407,6 @@ Java_edu_wpi_first_wpilibj_hal_HAL_getMatchInfo if (status == 0) { SetMatchInfoObject(env, info, matchInfo); } - HAL_FreeMatchInfo(&matchInfo); return status; } diff --git a/hal/src/main/native/cpp/jni/HALUtil.cpp b/hal/src/main/native/cpp/jni/HALUtil.cpp index dd7d70e50f..bf87a65c4a 100644 --- a/hal/src/main/native/cpp/jni/HALUtil.cpp +++ b/hal/src/main/native/cpp/jni/HALUtil.cpp @@ -256,10 +256,13 @@ void SetMatchInfoObject(JNIEnv* env, jobject matchStatus, env->GetMethodID(matchInfoDataCls, "setData", "(Ljava/lang/String;Ljava/lang/String;III)V"); - env->CallVoidMethod(matchStatus, func, MakeJString(env, matchInfo.eventName), - MakeJString(env, matchInfo.gameSpecificMessage), - (jint)matchInfo.matchNumber, (jint)matchInfo.replayNumber, - (jint)matchInfo.matchType); + env->CallVoidMethod( + matchStatus, func, MakeJString(env, matchInfo.eventName), + MakeJString(env, wpi::StringRef{reinterpret_cast( + matchInfo.gameSpecificMessage), + matchInfo.gameSpecificMessageSize}), + (jint)matchInfo.matchNumber, (jint)matchInfo.replayNumber, + (jint)matchInfo.matchType); } void SetAccumulatorResultObject(JNIEnv* env, jobject accumulatorResult, diff --git a/hal/src/main/native/include/HAL/DriverStation.h b/hal/src/main/native/include/HAL/DriverStation.h index 5ce038527a..31c38d87c3 100644 --- a/hal/src/main/native/include/HAL/DriverStation.h +++ b/hal/src/main/native/include/HAL/DriverStation.h @@ -92,11 +92,12 @@ struct HAL_JoystickDescriptor { typedef struct HAL_JoystickDescriptor HAL_JoystickDescriptor; struct HAL_MatchInfo { - char* eventName; + char eventName[64]; HAL_MatchType matchType; uint16_t matchNumber; uint8_t replayNumber; - char* gameSpecificMessage; + uint8_t gameSpecificMessage[64]; + uint16_t gameSpecificMessageSize; }; typedef struct HAL_MatchInfo HAL_MatchInfo; @@ -264,20 +265,11 @@ double HAL_GetMatchTime(int32_t* status); /** * Gets info about a specific match. * - * Note that the match info must be freed with HAL_FreeMatchInfo - * * @param info the match info (output) * @return the error code, or 0 for success */ int32_t HAL_GetMatchInfo(HAL_MatchInfo* info); -/** - * Frees a match info structure allocated with HAL_GetMatchInfo. - * - * @param info the match info - */ -void HAL_FreeMatchInfo(HAL_MatchInfo* info); - #ifndef HAL_USE_LABVIEW /** diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp index f945b3068c..bd6a203d88 100644 --- a/hal/src/main/native/sim/DriverStation.cpp +++ b/hal/src/main/native/sim/DriverStation.cpp @@ -174,10 +174,6 @@ int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) { return 0; } -void HAL_FreeMatchInfo(HAL_MatchInfo* info) { - SimDriverStationData->FreeMatchInfo(info); -} - void HAL_ObserveUserProgramStarting(void) { HALSIM_SetProgramStarted(); } void HAL_ObserveUserProgramDisabled(void) { diff --git a/hal/src/main/native/sim/MockData/DriverStationData.cpp b/hal/src/main/native/sim/MockData/DriverStationData.cpp index 811fa1303c..1486f85f32 100644 --- a/hal/src/main/native/sim/MockData/DriverStationData.cpp +++ b/hal/src/main/native/sim/MockData/DriverStationData.cpp @@ -18,13 +18,6 @@ struct JoystickOutputStore { int32_t leftRumble = 0; int32_t rightRumble = 0; }; -struct MatchInfoDataStore { - std::string eventName; - std::string gameSpecificMessage; - int32_t replayNumber = 0; - int32_t matchNumber = 0; - HAL_MatchType matchType = HAL_MatchType::HAL_kMatchType_none; -}; } // namespace hal using namespace hal; @@ -76,7 +69,7 @@ void DriverStationData::ResetData() { { std::lock_guard lock(m_matchInfoMutex); - m_matchInfo = std::make_unique(); + m_matchInfo = std::make_unique(); } } @@ -402,22 +395,7 @@ void DriverStationData::GetJoystickOutputs(int32_t joystickNum, } void DriverStationData::GetMatchInfo(HAL_MatchInfo* info) { std::lock_guard lock(m_matchInfoMutex); - auto eventLen = m_matchInfo->eventName.size(); - info->eventName = static_cast(std::malloc(eventLen + 1)); - std::memcpy(info->eventName, m_matchInfo->eventName.c_str(), eventLen); - auto gameLen = m_matchInfo->gameSpecificMessage.size(); - info->gameSpecificMessage = static_cast(std::malloc(gameLen + 1)); - std::memcpy(info->gameSpecificMessage, - m_matchInfo->gameSpecificMessage.c_str(), gameLen); - info->gameSpecificMessage[gameLen] = '\0'; - info->eventName[eventLen] = '\0'; - info->matchNumber = m_matchInfo->matchNumber; - info->replayNumber = m_matchInfo->replayNumber; - info->matchType = m_matchInfo->matchType; -} -void DriverStationData::FreeMatchInfo(const HAL_MatchInfo* info) { - std::free(info->eventName); - std::free(info->gameSpecificMessage); + *info = *m_matchInfo; } void DriverStationData::SetJoystickAxes(int32_t joystickNum, @@ -453,11 +431,8 @@ void DriverStationData::SetJoystickOutputs(int32_t joystickNum, int64_t outputs, void DriverStationData::SetMatchInfo(const HAL_MatchInfo* info) { std::lock_guard lock(m_matchInfoMutex); - m_matchInfo->eventName = info->eventName; - m_matchInfo->gameSpecificMessage = info->gameSpecificMessage; - m_matchInfo->matchNumber = info->matchNumber; - m_matchInfo->matchType = info->matchType; - m_matchInfo->replayNumber = info->replayNumber; + *m_matchInfo = *info; + *(std::end(m_matchInfo->eventName) - 1) = '\0'; } void DriverStationData::NotifyNewData() { HAL_ReleaseDSMutex(); } diff --git a/hal/src/main/native/sim/MockData/DriverStationDataInternal.h b/hal/src/main/native/sim/MockData/DriverStationDataInternal.h index 2bbdd257c3..66fcc03d38 100644 --- a/hal/src/main/native/sim/MockData/DriverStationDataInternal.h +++ b/hal/src/main/native/sim/MockData/DriverStationDataInternal.h @@ -18,7 +18,6 @@ namespace hal { struct JoystickOutputStore; -struct MatchInfoDataStore; class DriverStationData { public: @@ -133,7 +132,7 @@ class DriverStationData { std::unique_ptr m_joystickOutputs; std::unique_ptr m_joystickDescriptor; - std::unique_ptr m_matchInfo; + std::unique_ptr m_matchInfo; }; extern DriverStationData* SimDriverStationData; } // namespace hal diff --git a/hal/src/test/native/cpp/MockData/DriverStationDataTests.cpp b/hal/src/test/native/cpp/MockData/DriverStationDataTests.cpp index ef1b7e6d32..51462b1c59 100644 --- a/hal/src/test/native/cpp/MockData/DriverStationDataTests.cpp +++ b/hal/src/test/native/cpp/MockData/DriverStationDataTests.cpp @@ -117,8 +117,11 @@ TEST(DriverStationTests, EventInfoTest) { std::string eventName = "UnitTest"; std::string gameData = "Insert game specific info here :D"; HAL_MatchInfo info; - info.eventName = const_cast(eventName.c_str()); - info.gameSpecificMessage = const_cast(gameData.c_str()); + std::snprintf(info.eventName, sizeof(info.eventName), "%s", + eventName.c_str()); + std::snprintf(reinterpret_cast(info.gameSpecificMessage), + sizeof(info.gameSpecificMessage), "%s", gameData.c_str()); + info.gameSpecificMessageSize = gameData.size(); info.matchNumber = 5; info.matchType = HAL_MatchType::HAL_kMatchType_qualification; info.replayNumber = 42; @@ -127,13 +130,14 @@ TEST(DriverStationTests, EventInfoTest) { HAL_MatchInfo dataBack; HAL_GetMatchInfo(&dataBack); + std::string gsm{reinterpret_cast(dataBack.gameSpecificMessage), + dataBack.gameSpecificMessageSize}; + EXPECT_STREQ(eventName.c_str(), dataBack.eventName); - EXPECT_STREQ(gameData.c_str(), dataBack.gameSpecificMessage); + EXPECT_EQ(gameData, gsm); EXPECT_EQ(5, dataBack.matchNumber); EXPECT_EQ(HAL_MatchType::HAL_kMatchType_qualification, dataBack.matchType); EXPECT_EQ(42, dataBack.replayNumber); - - HAL_FreeMatchInfo(&dataBack); } } // namespace hal diff --git a/wpilibc/src/main/native/cpp/DriverStation.cpp b/wpilibc/src/main/native/cpp/DriverStation.cpp index 1a38faabd4..29eb05ce7f 100644 --- a/wpilibc/src/main/native/cpp/DriverStation.cpp +++ b/wpilibc/src/main/native/cpp/DriverStation.cpp @@ -25,13 +25,6 @@ #include "WPIErrors.h" namespace frc { -struct MatchInfoData { - std::string eventName; - std::string gameSpecificMessage; - int matchNumber = 0; - int replayNumber = 0; - DriverStation::MatchType matchType = DriverStation::MatchType::kNone; -}; class MatchDataSender { public: @@ -122,16 +115,16 @@ bool DriverStation::GetStickButton(int stick, int button) { return false; } - std::unique_lock lock(m_cacheDataMutex); - if (button > m_joystickButtons[stick].count) { - // Unlock early so error printing isn't locked. - lock.unlock(); + HAL_JoystickButtons buttons; + HAL_GetJoystickButtons(stick, &buttons); + + if (button > buttons.count) { ReportJoystickUnpluggedWarning( "Joystick Button missing, check if all controllers are plugged in"); return false; } - return m_joystickButtons[stick].buttons & 1 << (button - 1); + return buttons.buttons & 1 << (button - 1); } bool DriverStation::GetStickButtonPressed(int stick, int button) { @@ -145,15 +138,15 @@ bool DriverStation::GetStickButtonPressed(int stick, int button) { return false; } - std::unique_lock lock(m_cacheDataMutex); - if (button > m_joystickButtons[stick].count) { - // Unlock early so error printing isn't locked. - lock.unlock(); + HAL_JoystickButtons buttons; + HAL_GetJoystickButtons(stick, &buttons); + + if (button > buttons.count) { ReportJoystickUnpluggedWarning( "Joystick Button missing, check if all controllers are plugged in"); return false; } - + std::unique_lock lock(m_buttonEdgeMutex); // If button was pressed, clear flag and return true if (m_joystickButtonsPressed[stick] & 1 << (button - 1)) { m_joystickButtonsPressed[stick] &= ~(1 << (button - 1)); @@ -174,15 +167,15 @@ bool DriverStation::GetStickButtonReleased(int stick, int button) { return false; } - std::unique_lock lock(m_cacheDataMutex); - if (button > m_joystickButtons[stick].count) { - // Unlock early so error printing isn't locked. - lock.unlock(); + HAL_JoystickButtons buttons; + HAL_GetJoystickButtons(stick, &buttons); + + if (button > buttons.count) { ReportJoystickUnpluggedWarning( "Joystick Button missing, check if all controllers are plugged in"); return false; } - + std::unique_lock lock(m_buttonEdgeMutex); // If button was released, clear flag and return true if (m_joystickButtonsReleased[stick] & 1 << (button - 1)) { m_joystickButtonsReleased[stick] &= ~(1 << (button - 1)); @@ -202,16 +195,16 @@ double DriverStation::GetStickAxis(int stick, int axis) { return 0.0; } - std::unique_lock lock(m_cacheDataMutex); - if (axis >= m_joystickAxes[stick].count) { - // Unlock early so error printing isn't locked. - lock.unlock(); + HAL_JoystickAxes axes; + HAL_GetJoystickAxes(stick, &axes); + + if (axis >= axes.count) { ReportJoystickUnpluggedWarning( "Joystick Axis missing, check if all controllers are plugged in"); return 0.0; } - return m_joystickAxes[stick].axes[axis]; + return axes.axes[axis]; } int DriverStation::GetStickPOV(int stick, int pov) { @@ -224,16 +217,16 @@ int DriverStation::GetStickPOV(int stick, int pov) { return -1; } - std::unique_lock lock(m_cacheDataMutex); - if (pov >= m_joystickPOVs[stick].count) { - // Unlock early so error printing isn't locked. - lock.unlock(); + HAL_JoystickPOVs povs; + HAL_GetJoystickPOVs(stick, &povs); + + if (pov >= povs.count) { ReportJoystickUnpluggedWarning( "Joystick POV missing, check if all controllers are plugged in"); return -1; } - return m_joystickPOVs[stick].povs[pov]; + return povs.povs[pov]; } int DriverStation::GetStickButtons(int stick) const { @@ -242,8 +235,10 @@ int DriverStation::GetStickButtons(int stick) const { return 0; } - std::lock_guard lock(m_cacheDataMutex); - return m_joystickButtons[stick].buttons; + HAL_JoystickButtons buttons; + HAL_GetJoystickButtons(stick, &buttons); + + return buttons.buttons; } int DriverStation::GetStickAxisCount(int stick) const { @@ -252,8 +247,10 @@ int DriverStation::GetStickAxisCount(int stick) const { return 0; } - std::lock_guard lock(m_cacheDataMutex); - return m_joystickAxes[stick].count; + HAL_JoystickAxes axes; + HAL_GetJoystickAxes(stick, &axes); + + return axes.count; } int DriverStation::GetStickPOVCount(int stick) const { @@ -262,8 +259,10 @@ int DriverStation::GetStickPOVCount(int stick) const { return 0; } - std::lock_guard lock(m_cacheDataMutex); - return m_joystickPOVs[stick].count; + HAL_JoystickPOVs povs; + HAL_GetJoystickPOVs(stick, &povs); + + return povs.count; } int DriverStation::GetStickButtonCount(int stick) const { @@ -272,8 +271,10 @@ int DriverStation::GetStickButtonCount(int stick) const { return 0; } - std::lock_guard lock(m_cacheDataMutex); - return m_joystickButtons[stick].count; + HAL_JoystickButtons buttons; + HAL_GetJoystickButtons(stick, &buttons); + + return buttons.count; } bool DriverStation::GetJoystickIsXbox(int stick) const { @@ -282,8 +283,10 @@ bool DriverStation::GetJoystickIsXbox(int stick) const { return false; } - std::lock_guard lock(m_cacheDataMutex); - return static_cast(m_joystickDescriptor[stick].isXbox); + HAL_JoystickDescriptor descriptor; + HAL_GetJoystickDescriptor(stick, &descriptor); + + return static_cast(descriptor.isXbox); } int DriverStation::GetJoystickType(int stick) const { @@ -292,8 +295,10 @@ int DriverStation::GetJoystickType(int stick) const { return -1; } - std::lock_guard lock(m_cacheDataMutex); - return static_cast(m_joystickDescriptor[stick].type); + HAL_JoystickDescriptor descriptor; + HAL_GetJoystickDescriptor(stick, &descriptor); + + return static_cast(descriptor.type); } std::string DriverStation::GetJoystickName(int stick) const { @@ -301,8 +306,10 @@ std::string DriverStation::GetJoystickName(int stick) const { wpi_setWPIError(BadJoystickIndex); } - std::lock_guard lock(m_cacheDataMutex); - return m_joystickDescriptor[stick].name; + HAL_JoystickDescriptor descriptor; + HAL_GetJoystickDescriptor(stick, &descriptor); + + return descriptor.name; } int DriverStation::GetJoystickAxisType(int stick, int axis) const { @@ -311,43 +318,45 @@ int DriverStation::GetJoystickAxisType(int stick, int axis) const { return -1; } - std::lock_guard lock(m_cacheDataMutex); - return m_joystickDescriptor[stick].axisTypes[axis]; + HAL_JoystickDescriptor descriptor; + HAL_GetJoystickDescriptor(stick, &descriptor); + + return static_cast(descriptor.axisTypes); } bool DriverStation::IsEnabled() const { HAL_ControlWord controlWord; - UpdateControlWord(false, controlWord); + HAL_GetControlWord(&controlWord); return controlWord.enabled && controlWord.dsAttached; } bool DriverStation::IsDisabled() const { HAL_ControlWord controlWord; - UpdateControlWord(false, controlWord); + HAL_GetControlWord(&controlWord); return !(controlWord.enabled && controlWord.dsAttached); } bool DriverStation::IsAutonomous() const { HAL_ControlWord controlWord; - UpdateControlWord(false, controlWord); + HAL_GetControlWord(&controlWord); return controlWord.autonomous; } bool DriverStation::IsOperatorControl() const { HAL_ControlWord controlWord; - UpdateControlWord(false, controlWord); + HAL_GetControlWord(&controlWord); return !(controlWord.autonomous || controlWord.test); } bool DriverStation::IsTest() const { HAL_ControlWord controlWord; - UpdateControlWord(false, controlWord); + HAL_GetControlWord(&controlWord); return controlWord.test; } bool DriverStation::IsDSAttached() const { HAL_ControlWord controlWord; - UpdateControlWord(false, controlWord); + HAL_GetControlWord(&controlWord); return controlWord.dsAttached; } @@ -355,7 +364,7 @@ bool DriverStation::IsNewControlData() const { return HAL_IsNewControlData(); } bool DriverStation::IsFMSAttached() const { HAL_ControlWord controlWord; - UpdateControlWord(false, controlWord); + HAL_GetControlWord(&controlWord); return controlWord.fmsAttached; } @@ -374,28 +383,34 @@ bool DriverStation::IsBrownedOut() const { } std::string DriverStation::GetGameSpecificMessage() const { - std::lock_guard lock(m_cacheDataMutex); - return m_matchInfo->gameSpecificMessage; + HAL_MatchInfo info; + HAL_GetMatchInfo(&info); + return std::string(reinterpret_cast(info.gameSpecificMessage), + info.gameSpecificMessageSize); } std::string DriverStation::GetEventName() const { - std::lock_guard lock(m_cacheDataMutex); - return m_matchInfo->eventName; + HAL_MatchInfo info; + HAL_GetMatchInfo(&info); + return info.eventName; } DriverStation::MatchType DriverStation::GetMatchType() const { - std::lock_guard lock(m_cacheDataMutex); - return m_matchInfo->matchType; + HAL_MatchInfo info; + HAL_GetMatchInfo(&info); + return static_cast(info.matchType); } int DriverStation::GetMatchNumber() const { - std::lock_guard lock(m_cacheDataMutex); - return m_matchInfo->matchNumber; + HAL_MatchInfo info; + HAL_GetMatchInfo(&info); + return info.matchNumber; } int DriverStation::GetReplayNumber() const { - std::lock_guard lock(m_cacheDataMutex); - return m_matchInfo->replayNumber; + HAL_MatchInfo info; + HAL_GetMatchInfo(&info); + return info.replayNumber; } DriverStation::Alliance DriverStation::GetAlliance() const { @@ -468,50 +483,24 @@ double DriverStation::GetBatteryVoltage() const { } void DriverStation::GetData() { - // Get the status of all of the joysticks, and save to the cache - for (uint8_t stick = 0; stick < kJoystickPorts; stick++) { - HAL_GetJoystickAxes(stick, &m_joystickAxesCache[stick]); - HAL_GetJoystickPOVs(stick, &m_joystickPOVsCache[stick]); - HAL_GetJoystickButtons(stick, &m_joystickButtonsCache[stick]); - HAL_GetJoystickDescriptor(stick, &m_joystickDescriptorCache[stick]); - } - // Grab match specific data - HAL_MatchInfo matchInfo; - auto status = HAL_GetMatchInfo(&matchInfo); - if (status == 0) { - m_matchInfoCache->eventName = matchInfo.eventName; - m_matchInfoCache->matchNumber = matchInfo.matchNumber; - m_matchInfoCache->replayNumber = matchInfo.replayNumber; - m_matchInfoCache->matchType = - static_cast(matchInfo.matchType); - m_matchInfoCache->gameSpecificMessage = matchInfo.gameSpecificMessage; - } - HAL_FreeMatchInfo(&matchInfo); - - // Force a control word update, to make sure the data is the newest. - HAL_ControlWord controlWord; - UpdateControlWord(true, controlWord); - { - // Obtain a write lock on the data, swap the cached data into the - // main data arrays - std::lock_guard lock(m_cacheDataMutex); + // Compute the pressed and released buttons + HAL_JoystickButtons currentButtons; + std::unique_lock lock(m_buttonEdgeMutex); for (int32_t i = 0; i < kJoystickPorts; i++) { + HAL_GetJoystickButtons(i, ¤tButtons); + // If buttons weren't pressed and are now, set flags in m_buttonsPressed m_joystickButtonsPressed[i] |= - ~m_joystickButtons[i].buttons & m_joystickButtonsCache[i].buttons; + ~m_previousButtonStates[i].buttons & currentButtons.buttons; // If buttons were pressed and aren't now, set flags in m_buttonsReleased m_joystickButtonsReleased[i] |= - m_joystickButtons[i].buttons & ~m_joystickButtonsCache[i].buttons; - } + m_previousButtonStates[i].buttons & ~currentButtons.buttons; - m_joystickAxes.swap(m_joystickAxesCache); - m_joystickPOVs.swap(m_joystickPOVsCache); - m_joystickButtons.swap(m_joystickButtonsCache); - m_joystickDescriptor.swap(m_joystickDescriptorCache); - m_matchInfo.swap(m_matchInfoCache); + m_previousButtonStates[i] = currentButtons; + } } { @@ -527,41 +516,16 @@ void DriverStation::GetData() { DriverStation::DriverStation() { HAL_Initialize(500, 0); m_waitForDataCounter = 0; - m_joystickAxes = std::make_unique(kJoystickPorts); - m_joystickPOVs = std::make_unique(kJoystickPorts); - m_joystickButtons = std::make_unique(kJoystickPorts); - m_joystickDescriptor = - std::make_unique(kJoystickPorts); - m_matchInfo = std::make_unique(); - m_joystickAxesCache = std::make_unique(kJoystickPorts); - m_joystickPOVsCache = std::make_unique(kJoystickPorts); - m_joystickButtonsCache = - std::make_unique(kJoystickPorts); - m_joystickDescriptorCache = - std::make_unique(kJoystickPorts); - m_matchInfoCache = std::make_unique(); m_matchDataSender = std::make_unique(); // All joysticks should default to having zero axes, povs and buttons, so // uninitialized memory doesn't get sent to speed controllers. for (unsigned int i = 0; i < kJoystickPorts; i++) { - m_joystickAxes[i].count = 0; - m_joystickPOVs[i].count = 0; - m_joystickButtons[i].count = 0; - m_joystickDescriptor[i].isXbox = 0; - m_joystickDescriptor[i].type = -1; - m_joystickDescriptor[i].name[0] = '\0'; - - m_joystickAxesCache[i].count = 0; - m_joystickPOVsCache[i].count = 0; - m_joystickButtonsCache[i].count = 0; - m_joystickDescriptorCache[i].isXbox = 0; - m_joystickDescriptorCache[i].type = -1; - m_joystickDescriptorCache[i].name[0] = '\0'; - m_joystickButtonsPressed[i] = 0; m_joystickButtonsReleased[i] = 0; + m_previousButtonStates[i].count = 0; + m_previousButtonStates[i].buttons = 0; } m_dsThread = std::thread(&DriverStation::Run, this); @@ -603,19 +567,6 @@ void DriverStation::Run() { } } -void DriverStation::UpdateControlWord(bool force, - HAL_ControlWord& controlWord) const { - auto now = std::chrono::steady_clock::now(); - std::lock_guard lock(m_controlWordMutex); - // Update every 50 ms or on force. - if ((now - m_lastControlWordUpdate > std::chrono::milliseconds(50)) || - force) { - HAL_GetControlWord(&m_controlWordCache); - m_lastControlWordUpdate = now; - } - controlWord = m_controlWordCache; -} - void DriverStation::SendMatchData() { int32_t status = 0; HAL_AllianceStationID alliance = HAL_GetAllianceStation(&status); @@ -648,28 +599,22 @@ void DriverStation::SendMatchData() { break; } - MatchInfoData tmpDataStore; - { - std::lock_guard lock(m_cacheDataMutex); - tmpDataStore = *m_matchInfo; - } + HAL_MatchInfo tmpDataStore; + HAL_GetMatchInfo(&tmpDataStore); m_matchDataSender->alliance.SetBoolean(isRedAlliance); m_matchDataSender->station.SetDouble(stationNumber); m_matchDataSender->eventName.SetString(tmpDataStore.eventName); m_matchDataSender->gameSpecificMessage.SetString( - tmpDataStore.gameSpecificMessage); + std::string(reinterpret_cast(tmpDataStore.gameSpecificMessage), + tmpDataStore.gameSpecificMessageSize)); m_matchDataSender->matchNumber.SetDouble(tmpDataStore.matchNumber); m_matchDataSender->replayNumber.SetDouble(tmpDataStore.replayNumber); m_matchDataSender->matchType.SetDouble( static_cast(tmpDataStore.matchType)); HAL_ControlWord ctlWord; - { - // Valid, as in other places we guarentee ctlWord >= int32 - std::lock_guard lock(m_controlWordMutex); - ctlWord = m_controlWordCache; - } + HAL_GetControlWord(&ctlWord); int32_t wordInt = 0; std::memcpy(&wordInt, &ctlWord, sizeof(wordInt)); m_matchDataSender->controlWord.SetDouble(wordInt); diff --git a/wpilibc/src/main/native/include/DriverStation.h b/wpilibc/src/main/native/include/DriverStation.h index 8cb8c25719..3927a81716 100644 --- a/wpilibc/src/main/native/include/DriverStation.h +++ b/wpilibc/src/main/native/include/DriverStation.h @@ -24,7 +24,6 @@ namespace frc { -struct MatchInfoData; class MatchDataSender; /** @@ -408,36 +407,13 @@ class DriverStation : public ErrorBase { void Run(); - /** - * Gets ControlWord data from the cache. If 50ms has passed, or the force - * parameter is set, the cached data is updated. Otherwise the data is just - * copied from the cache. - * - * @param force True to force an update to the cache, otherwise update if 50ms - * have passed. - * @param controlWord Structure to put the return control word data into. - */ - void UpdateControlWord(bool force, HAL_ControlWord& controlWord) const; - void SendMatchData(); - // Joystick User Data - std::unique_ptr m_joystickAxes; - std::unique_ptr m_joystickPOVs; - std::unique_ptr m_joystickButtons; - std::unique_ptr m_joystickDescriptor; - std::unique_ptr m_matchInfo; - - // Joystick Cached Data - std::unique_ptr m_joystickAxesCache; - std::unique_ptr m_joystickPOVsCache; - std::unique_ptr m_joystickButtonsCache; - std::unique_ptr m_joystickDescriptorCache; - std::unique_ptr m_matchInfoCache; - std::unique_ptr m_matchDataSender; // Joystick button rising/falling edge flags + wpi::mutex m_buttonEdgeMutex; + std::array m_previousButtonStates; std::array m_joystickButtonsPressed; std::array m_joystickButtonsReleased; @@ -449,19 +425,12 @@ class DriverStation : public ErrorBase { wpi::condition_variable m_waitForDataCond; int m_waitForDataCounter; - mutable wpi::mutex m_cacheDataMutex; - // Robot state status variables bool m_userInDisabled = false; bool m_userInAutonomous = false; bool m_userInTeleop = false; bool m_userInTest = false; - // Control word variables - mutable HAL_ControlWord m_controlWordCache; - mutable std::chrono::steady_clock::time_point m_lastControlWordUpdate; - mutable wpi::mutex m_controlWordMutex; - double m_nextMessageTime = 0; }; 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 b504cd3984..5d57190060 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java @@ -559,12 +559,7 @@ public class DriverStation { throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); } - m_cacheDataMutex.lock(); - try { - return HAL.getJoystickIsXbox((byte) stick) == 1; - } finally { - m_cacheDataMutex.unlock(); - } + return HAL.getJoystickIsXbox((byte) stick) == 1; } /** @@ -578,12 +573,7 @@ public class DriverStation { throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); } - m_cacheDataMutex.lock(); - try { - return HAL.getJoystickType((byte) stick); - } finally { - m_cacheDataMutex.unlock(); - } + return HAL.getJoystickType((byte) stick); } /** @@ -597,12 +587,7 @@ public class DriverStation { throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); } - m_cacheDataMutex.lock(); - try { - return HAL.getJoystickName((byte) stick); - } finally { - m_cacheDataMutex.unlock(); - } + return HAL.getJoystickName((byte) stick); } /** @@ -617,12 +602,7 @@ public class DriverStation { throw new IllegalArgumentException("Joystick index is out of range, should be 0-5"); } - m_cacheDataMutex.lock(); - try { - return HAL.getJoystickAxisType((byte) stick, (byte) axis); - } finally { - m_cacheDataMutex.unlock(); - } + return HAL.getJoystickAxisType((byte) stick, (byte) axis); } /**