[hal] Add more shutdown checks and motor safety shutdown (#4510)

hal/src/main/native/sim/DriverStation.cpp

@@ -168,17 +168,26 @@ int32_t HAL_SendConsoleLine(const char* line) {
 }
 
 int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
+  if (gShutdown) {
+    return INCOMPATIBLE_STATE;
+  }
   std::scoped_lock lock{driverStation->cacheMutex};
   *controlWord = newestControlWord;
   return 0;
 }
 
 HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status) {
+  if (gShutdown) {
+    return HAL_AllianceStationID_kRed1;
+  }
   std::scoped_lock lock{driverStation->cacheMutex};
   return currentRead->allianceStation;
 }
 
 int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
+  if (gShutdown) {
+    return INCOMPATIBLE_STATE;
+  }
   CHECK_JOYSTICK_NUMBER(joystickNum);
   std::scoped_lock lock{driverStation->cacheMutex};
   *axes = currentRead->axes[joystickNum];
@@ -186,6 +195,9 @@ int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes) {
 }
 
 int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) {
+  if (gShutdown) {
+    return INCOMPATIBLE_STATE;
+  }
   CHECK_JOYSTICK_NUMBER(joystickNum);
   std::scoped_lock lock{driverStation->cacheMutex};
   *povs = currentRead->povs[joystickNum];
@@ -194,6 +206,9 @@ int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs) {
 
 int32_t HAL_GetJoystickButtons(int32_t joystickNum,
                                HAL_JoystickButtons* buttons) {
+  if (gShutdown) {
+    return INCOMPATIBLE_STATE;
+  }
   CHECK_JOYSTICK_NUMBER(joystickNum);
   std::scoped_lock lock{driverStation->cacheMutex};
   *buttons = currentRead->buttons[joystickNum];
@@ -202,6 +217,9 @@ int32_t HAL_GetJoystickButtons(int32_t joystickNum,
 
 void HAL_GetAllJoystickData(HAL_JoystickAxes* axes, HAL_JoystickPOVs* povs,
                             HAL_JoystickButtons* buttons) {
+  if (gShutdown) {
+    return;
+  }
   std::scoped_lock lock{driverStation->cacheMutex};
   std::memcpy(axes, currentRead->axes, sizeof(currentRead->axes));
   std::memcpy(povs, currentRead->povs, sizeof(currentRead->povs));
@@ -252,6 +270,9 @@ int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
 }
 
 double HAL_GetMatchTime(int32_t* status) {
+  if (gShutdown) {
+    return 0;
+  }
   std::scoped_lock lock{driverStation->cacheMutex};
   return currentRead->matchTime;
 }
@@ -282,6 +303,9 @@ void HAL_ObserveUserProgramTest(void) {
 }
 
 void HAL_RefreshDSData(void) {
+  if (gShutdown) {
+    return;
+  }
   HAL_ControlWord controlWord;
   std::memset(&controlWord, 0, sizeof(controlWord));
   controlWord.enabled = SimDriverStationData->enabled;
@@ -313,6 +337,9 @@ void HAL_RemoveNewDataEventHandle(WPI_EventHandle handle) {
 }
 
 HAL_Bool HAL_GetOutputsEnabled(void) {
+  if (gShutdown) {
+    return false;
+  }
   std::scoped_lock lock{driverStation->cacheMutex};
   return newestControlWord.enabled && newestControlWord.dsAttached;
 }
@@ -321,6 +348,9 @@ HAL_Bool HAL_GetOutputsEnabled(void) {
 
 namespace hal {
 void NewDriverStationData() {
+  if (gShutdown) {
+    return;
+  }
   cacheToUpdate->Update();
   {
     std::scoped_lock lock{driverStation->cacheMutex};

wpilibc/src/main/native/cpp/MotorSafety.cpp

@@ -49,8 +49,6 @@ void Thread::Main() {
 
   HAL_RemoveNewDataEventHandle(event.GetHandle());
 }
-
-static wpi::SafeThreadOwner<Thread> m_owner;
 }  // namespace
 
 static std::atomic_bool gShutdown{false};
@@ -59,6 +57,7 @@ namespace {
 struct MotorSafetyManager {
   ~MotorSafetyManager() { gShutdown = true; }
 
+  wpi::SafeThreadOwner<Thread> thread;
   wpi::SmallPtrSet<MotorSafety*, 32> instanceList;
   wpi::mutex listMutex;
   bool threadStarted = false;
@@ -76,6 +75,10 @@ void ResetMotorSafety() {
   auto& manager = GetManager();
   std::scoped_lock lock(manager.listMutex);
   manager.instanceList.clear();
+  manager.thread.Stop();
+  manager.thread.Join();
+  manager.thread = wpi::SafeThreadOwner<Thread>{};
+  manager.threadStarted = false;
 }
 }  // namespace frc::impl
 #endif
@@ -86,7 +89,7 @@ MotorSafety::MotorSafety() {
   manager.instanceList.insert(this);
   if (!manager.threadStarted) {
     manager.threadStarted = true;
-    m_owner.Start();
+    manager.thread.Start();
   }
 }
 

wpilibc/src/main/native/include/frc/RobotBase.h

@@ -21,6 +21,9 @@ namespace frc {
 int RunHALInitialization();
 
 namespace impl {
+#ifndef __FRC_ROBORIO__
+void ResetMotorSafety();
+#endif
 
 template <class Robot>
 void RunRobot(wpi::mutex& m, Robot** robot) {
@@ -103,6 +106,9 @@ int StartRobot() {
     impl::RunRobot<Robot>(m, &robot);
   }
 
+#ifndef __FRC_ROBORIO__
+  frc::impl::ResetMotorSafety();
+#endif
   HAL_Shutdown();
 
   return 0;

wpilibc/src/test/native/cpp/main.cpp

@@ -6,9 +6,18 @@
 
 #include "gtest/gtest.h"
 
+#ifndef __FRC_ROBORIO__
+namespace frc::impl {
+void ResetMotorSafety();
+}
+#endif
+
 int main(int argc, char** argv) {
   HAL_Initialize(500, 0);
   ::testing::InitGoogleTest(&argc, argv);
   int ret = RUN_ALL_TESTS();
+#ifndef __FRC_ROBORIO__
+  frc::impl::ResetMotorSafety();
+#endif
   return ret;
 }