From ba850bac3b695f2cb8d3b2a255a83f630c22390f Mon Sep 17 00:00:00 2001 From: Thad House Date: Sun, 23 Oct 2022 21:59:04 -0700 Subject: [PATCH] [hal] Add more shutdown checks and motor safety shutdown (#4510) --- hal/src/main/native/sim/DriverStation.cpp | 30 +++++++++++++++++++ wpilibc/src/main/native/cpp/MotorSafety.cpp | 9 ++++-- .../src/main/native/include/frc/RobotBase.h | 6 ++++ wpilibc/src/test/native/cpp/main.cpp | 9 ++++++ 4 files changed, 51 insertions(+), 3 deletions(-) diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp index 399080d5a5..b99a5dd910 100644 --- a/hal/src/main/native/sim/DriverStation.cpp +++ b/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}; diff --git a/wpilibc/src/main/native/cpp/MotorSafety.cpp b/wpilibc/src/main/native/cpp/MotorSafety.cpp index 0101e066f1..259722c487 100644 --- a/wpilibc/src/main/native/cpp/MotorSafety.cpp +++ b/wpilibc/src/main/native/cpp/MotorSafety.cpp @@ -49,8 +49,6 @@ void Thread::Main() { HAL_RemoveNewDataEventHandle(event.GetHandle()); } - -static wpi::SafeThreadOwner m_owner; } // namespace static std::atomic_bool gShutdown{false}; @@ -59,6 +57,7 @@ namespace { struct MotorSafetyManager { ~MotorSafetyManager() { gShutdown = true; } + wpi::SafeThreadOwner thread; wpi::SmallPtrSet 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{}; + 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(); } } diff --git a/wpilibc/src/main/native/include/frc/RobotBase.h b/wpilibc/src/main/native/include/frc/RobotBase.h index 5070525cb0..3ff4617f78 100644 --- a/wpilibc/src/main/native/include/frc/RobotBase.h +++ b/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 void RunRobot(wpi::mutex& m, Robot** robot) { @@ -103,6 +106,9 @@ int StartRobot() { impl::RunRobot(m, &robot); } +#ifndef __FRC_ROBORIO__ + frc::impl::ResetMotorSafety(); +#endif HAL_Shutdown(); return 0; diff --git a/wpilibc/src/test/native/cpp/main.cpp b/wpilibc/src/test/native/cpp/main.cpp index 6aea19a01e..ba2997526c 100644 --- a/wpilibc/src/test/native/cpp/main.cpp +++ b/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; }