From 63d9e945b8a58b6f4e0ce5e5587cf3367b0cc6a5 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sun, 18 Feb 2024 14:30:40 -0800 Subject: [PATCH] [hal] HAL_RefreshDSData: Zero out control word on DS disconnect, use cache in sim (#6380) --- .../main/native/athena/FRCDriverStation.cpp | 26 +++++++---- hal/src/main/native/sim/DriverStation.cpp | 43 ++++++++++++++----- .../command/button/RobotModeTriggersTest.java | 9 ++-- .../command/button/RobotModeTriggersTest.cpp | 8 ++-- 4 files changed, 59 insertions(+), 27 deletions(-) diff --git a/hal/src/main/native/athena/FRCDriverStation.cpp b/hal/src/main/native/athena/FRCDriverStation.cpp index c30fbb94c59..b0aeb05acc2 100644 --- a/hal/src/main/native/athena/FRCDriverStation.cpp +++ b/hal/src/main/native/athena/FRCDriverStation.cpp @@ -539,15 +539,25 @@ HAL_Bool HAL_RefreshDSData(void) { } // 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. Otherwise use the state that shows disconnected. - if (controlWord.dsAttached) { - newestControlWord = currentRead->controlWord; - } else { - // Zero out the control word. When the DS has never been connected - // this returns garbage. And there is no way we can detect that. - std::memset(&controlWord, 0, sizeof(controlWord)); - newestControlWord = controlWord; + // 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 (!controlWord.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; } uint32_t mask = tcpMask.exchange(0); diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp index b245eed59d5..2ad13de861b 100644 --- a/hal/src/main/native/sim/DriverStation.cpp +++ b/hal/src/main/native/sim/DriverStation.cpp @@ -44,6 +44,7 @@ struct JoystickDataCache { HAL_JoystickButtons buttons[kJoystickPorts]; HAL_AllianceStationID allianceStation; double matchTime; + HAL_ControlWord controlWord; }; static_assert(std::is_standard_layout_v); // static_assert(std::is_trivial_v); @@ -65,6 +66,16 @@ void JoystickDataCache::Update() { } allianceStation = SimDriverStationData->allianceStationId; matchTime = SimDriverStationData->matchTime; + + HAL_ControlWord tmpControlWord; + std::memset(&tmpControlWord, 0, sizeof(tmpControlWord)); + tmpControlWord.enabled = SimDriverStationData->enabled; + tmpControlWord.autonomous = SimDriverStationData->autonomous; + tmpControlWord.test = SimDriverStationData->test; + tmpControlWord.eStop = SimDriverStationData->eStop; + tmpControlWord.fmsAttached = SimDriverStationData->fmsAttached; + tmpControlWord.dsAttached = SimDriverStationData->dsAttached; + this->controlWord = tmpControlWord; } #define CHECK_JOYSTICK_NUMBER(stickNum) \ @@ -322,20 +333,32 @@ HAL_Bool HAL_RefreshDSData(void) { if (gShutdown) { return false; } - 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; + bool dsAttached = SimDriverStationData->dsAttached; std::scoped_lock lock{driverStation->cacheMutex}; JoystickDataCache* prev = currentCache.exchange(nullptr); if (prev != nullptr) { currentRead = prev; } - newestControlWord = 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)); + } + newestControlWord = currentRead->controlWord; return prev != nullptr; } @@ -369,6 +392,7 @@ void NewDriverStationData() { if (gShutdown) { return; } + SimDriverStationData->dsAttached = true; cacheToUpdate->Update(); JoystickDataCache* given = cacheToUpdate; @@ -382,7 +406,6 @@ void NewDriverStationData() { } lastGiven = given; - SimDriverStationData->dsAttached = true; driverStation->newDataEvents.Wakeup(); SimDriverStationData->CallNewDataCallbacks(); } diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java index b2b41c4dfb6..f7df7f0bcb6 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/RobotModeTriggersTest.java @@ -6,7 +6,6 @@ import static org.junit.jupiter.api.Assertions.assertTrue; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.simulation.DriverStationSim; import edu.wpi.first.wpilibj2.command.CommandTestBase; import org.junit.jupiter.api.Test; @@ -18,7 +17,7 @@ void autonomousTest() { DriverStationSim.setAutonomous(true); DriverStationSim.setTest(false); DriverStationSim.setEnabled(true); - DriverStation.refreshData(); + DriverStationSim.notifyNewData(); Trigger auto = RobotModeTriggers.autonomous(); assertTrue(auto.getAsBoolean()); } @@ -29,7 +28,7 @@ void teleopTest() { DriverStationSim.setAutonomous(false); DriverStationSim.setTest(false); DriverStationSim.setEnabled(true); - DriverStation.refreshData(); + DriverStationSim.notifyNewData(); Trigger teleop = RobotModeTriggers.teleop(); assertTrue(teleop.getAsBoolean()); } @@ -40,7 +39,7 @@ void testModeTest() { DriverStationSim.setAutonomous(false); DriverStationSim.setTest(true); DriverStationSim.setEnabled(true); - DriverStation.refreshData(); + DriverStationSim.notifyNewData(); Trigger test = RobotModeTriggers.test(); assertTrue(test.getAsBoolean()); } @@ -51,7 +50,7 @@ void disabledTest() { DriverStationSim.setAutonomous(false); DriverStationSim.setTest(false); DriverStationSim.setEnabled(false); - DriverStation.refreshData(); + DriverStationSim.notifyNewData(); Trigger disabled = RobotModeTriggers.disabled(); assertTrue(disabled.getAsBoolean()); } diff --git a/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp b/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp index cb9e9664ba0..adc9ec7732c 100644 --- a/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp +++ b/wpilibNewCommands/src/test/native/cpp/frc2/command/button/RobotModeTriggersTest.cpp @@ -18,7 +18,7 @@ TEST(RobotModeTriggersTest, Autonomous) { DriverStationSim::SetAutonomous(true); DriverStationSim::SetTest(false); DriverStationSim::SetEnabled(true); - frc::DriverStation::RefreshData(); + DriverStationSim::NotifyNewData(); Trigger autonomous = RobotModeTriggers::Autonomous(); EXPECT_TRUE(autonomous.Get()); } @@ -28,7 +28,7 @@ TEST(RobotModeTriggersTest, Teleop) { DriverStationSim::SetAutonomous(false); DriverStationSim::SetTest(false); DriverStationSim::SetEnabled(true); - frc::DriverStation::RefreshData(); + DriverStationSim::NotifyNewData(); Trigger teleop = RobotModeTriggers::Teleop(); EXPECT_TRUE(teleop.Get()); } @@ -38,7 +38,7 @@ TEST(RobotModeTriggersTest, Disabled) { DriverStationSim::SetAutonomous(false); DriverStationSim::SetTest(false); DriverStationSim::SetEnabled(false); - frc::DriverStation::RefreshData(); + DriverStationSim::NotifyNewData(); Trigger disabled = RobotModeTriggers::Disabled(); EXPECT_TRUE(disabled.Get()); } @@ -48,7 +48,7 @@ TEST(RobotModeTriggersTest, TestMode) { DriverStationSim::SetAutonomous(false); DriverStationSim::SetTest(true); DriverStationSim::SetEnabled(true); - frc::DriverStation::RefreshData(); + DriverStationSim::NotifyNewData(); Trigger test = RobotModeTriggers::Test(); EXPECT_TRUE(test.Get()); }