From 561078ce294f2de487f43b2554773e8f4e103cdf Mon Sep 17 00:00:00 2001 From: Thad House Date: Tue, 19 Nov 2024 04:56:32 +0000 Subject: [PATCH] [hal] Cache sim TCP data to update during HAL_RefreshDSData (#7360) --- hal/src/main/native/sim/DriverStation.cpp | 123 +++++++++++++----- .../cpp/mockdata/DriverStationDataTest.cpp | 2 + .../first/wpilibj/hal/MatchInfoDataTest.java | 3 + 3 files changed, 92 insertions(+), 36 deletions(-) diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp index f50deaceb51..abf33f5a34d 100644 --- a/hal/src/main/native/sim/DriverStation.cpp +++ b/hal/src/main/native/sim/DriverStation.cpp @@ -55,6 +55,7 @@ struct FRCDriverStation { ~FRCDriverStation() { gShutdown = true; } wpi::EventVector newDataEvents; wpi::mutex cacheMutex; + wpi::mutex tcpCacheMutex; }; } // namespace @@ -90,6 +91,29 @@ static std::atomic currentCache{nullptr}; static JoystickDataCache* lastGiven = &caches[1]; static JoystickDataCache* cacheToUpdate = &caches[2]; +namespace { +struct TcpCache { + TcpCache() { std::memset(this, 0, sizeof(*this)); } + void Update(); + void CloneTo(TcpCache* other) { std::memcpy(other, this, sizeof(*this)); } + + HAL_MatchInfo matchInfo; + HAL_JoystickDescriptor descriptors[HAL_kMaxJoysticks]; +}; +static_assert(std::is_standard_layout_v); +} // namespace + +static TcpCache tcpCache; +static TcpCache tcpCurrent; + +void TcpCache::Update() { + SimDriverStationData->GetMatchInfo(&matchInfo); + + for (int i = 0; i < HAL_kMaxJoysticks; i++) { + SimDriverStationData->GetJoystickDescriptor(i, &descriptors[i]); + } +} + static ::FRCDriverStation* driverStation; namespace hal::init { @@ -256,32 +280,47 @@ void HAL_GetAllJoystickData(HAL_JoystickAxes* axes, HAL_JoystickPOVs* povs, int32_t HAL_GetJoystickDescriptor(int32_t joystickNum, HAL_JoystickDescriptor* desc) { CHECK_JOYSTICK_NUMBER(joystickNum); - SimDriverStationData->GetJoystickDescriptor(joystickNum, desc); + std::scoped_lock lock{driverStation->tcpCacheMutex}; + *desc = tcpCurrent.descriptors[joystickNum]; return 0; } HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum) { - HAL_JoystickDescriptor desc; - SimDriverStationData->GetJoystickDescriptor(joystickNum, &desc); - return desc.isXbox; + HAL_JoystickDescriptor joystickDesc; + if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) { + return 0; + } else { + return joystickDesc.isXbox; + } } int32_t HAL_GetJoystickType(int32_t joystickNum) { - HAL_JoystickDescriptor desc; - SimDriverStationData->GetJoystickDescriptor(joystickNum, &desc); - return desc.type; + HAL_JoystickDescriptor joystickDesc; + if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) { + return -1; + } else { + return joystickDesc.type; + } } void HAL_GetJoystickName(struct WPI_String* name, int32_t joystickNum) { - HAL_JoystickDescriptor desc; - SimDriverStationData->GetJoystickDescriptor(joystickNum, &desc); - size_t len = std::strlen(desc.name); + HAL_JoystickDescriptor joystickDesc; + const char* cName = joystickDesc.name; + if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) { + cName = ""; + } + auto len = std::strlen(cName); auto write = WPI_AllocateString(name, len); - std::memcpy(write, desc.name, len); + std::memcpy(write, cName, len); } int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis) { - return 0; + HAL_JoystickDescriptor joystickDesc; + if (HAL_GetJoystickDescriptor(joystickNum, &joystickDesc) < 0) { + return -1; + } else { + return joystickDesc.axisTypes[axis]; + } } int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs, @@ -300,7 +339,8 @@ double HAL_GetMatchTime(int32_t* status) { } int32_t HAL_GetMatchInfo(HAL_MatchInfo* info) { - SimDriverStationData->GetMatchInfo(info); + std::scoped_lock lock{driverStation->tcpCacheMutex}; + *info = tcpCurrent.matchInfo; return 0; } @@ -329,31 +369,42 @@ HAL_Bool HAL_RefreshDSData(void) { return false; } bool dsAttached = SimDriverStationData->dsAttached; - std::scoped_lock lock{driverStation->cacheMutex}; - JoystickDataCache* prev = currentCache.exchange(nullptr); - if (prev != nullptr) { - currentRead = prev; + JoystickDataCache* prev; + { + std::scoped_lock lock{driverStation->cacheMutex}; + prev = currentCache.exchange(nullptr); + if (prev != nullptr) { + currentRead = prev; + } + // If newest state shows we have a DS attached, just use the + // control word out of the cache, As it will be the one in sync + // with the data. If no data has been updated, at this point, + // and a DS wasn't attached previously, this will still return + // a zeroed out control word, with is the correct state for + // no new data. + if (!dsAttached) { + // If the DS is not attached, we need to zero out the control word. + // This is because HAL_RefreshDSData is called asynchronously from + // the DS data. The dsAttached variable comes directly from netcomm + // and could be updated before the caches are. If that happens, + // we would end up returning the previous cached control word, + // which is out of sync with the current control word and could + // break invariants such as which alliance station is in used. + // Also, when the DS has never been connected the rest of the fields + // in control word are garbage, so we also need to zero out in that + // case too + std::memset(¤tRead->controlWord, 0, + sizeof(currentRead->controlWord)); + } + newestControlWord = currentRead->controlWord; } - // If newest state shows we have a DS attached, just use the - // control word out of the cache, As it will be the one in sync - // with the data. If no data has been updated, at this point, - // and a DS wasn't attached previously, this will still return - // a zeroed out control word, with is the correct state for - // no new data. - if (!dsAttached) { - // If the DS is not attached, we need to zero out the control word. - // This is because HAL_RefreshDSData is called asynchronously from - // the DS data. The dsAttached variable comes directly from netcomm - // and could be updated before the caches are. If that happens, - // we would end up returning the previous cached control word, - // which is out of sync with the current control word and could - // break invariants such as which alliance station is in used. - // Also, when the DS has never been connected the rest of the fields - // in control word are garbage, so we also need to zero out in that - // case too - std::memset(¤tRead->controlWord, 0, sizeof(currentRead->controlWord)); + + { + tcpCache.Update(); + std::scoped_lock tcpLock(driverStation->tcpCacheMutex); + tcpCache.CloneTo(&tcpCurrent); } - newestControlWord = currentRead->controlWord; + return prev != nullptr; } diff --git a/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp b/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp index 5ff1a316e6a..ff99967bff6 100644 --- a/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp +++ b/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp @@ -131,6 +131,8 @@ TEST(DriverStationTest, EventInfo) { info.replayNumber = 42; HALSIM_SetMatchInfo(&info); + HAL_RefreshDSData(); + HAL_MatchInfo dataBack; HAL_GetMatchInfo(&dataBack); 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 b2b22a29463..d5e464f309e 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 @@ -11,6 +11,7 @@ import edu.wpi.first.hal.MatchInfoData; import edu.wpi.first.hal.simulation.DriverStationDataJNI; import edu.wpi.first.wpilibj.DriverStation.MatchType; +import edu.wpi.first.wpilibj.simulation.DriverStationSim; import org.junit.jupiter.api.Test; class MatchInfoDataTest { @@ -19,6 +20,8 @@ void testSetMatchInfo() { MatchType matchType = MatchType.Qualification; DriverStationDataJNI.setMatchInfo("Event Name", "Game Message", 174, 191, matchType.ordinal()); + DriverStationSim.notifyNewData(); + MatchInfoData outMatchInfo = new MatchInfoData(); DriverStationJNI.getMatchInfo(outMatchInfo);