[hal] HAL_RefreshDSData: Zero out control word on DS disconnect, use cache in sim (#6380)

Author: ThadHouse

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(&currentRead->controlWord, 0,
+                  sizeof(currentRead->controlWord));
     }
+    newestControlWord = currentRead->controlWord;
   }
 
   uint32_t mask = tcpMask.exchange(0);

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<JoystickDataCache>);
 // static_assert(std::is_trivial_v<JoystickDataCache>);
@@ -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(&currentRead->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();
 }

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());
   }

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());
 }