From 4bd1f526abd499399384740bf20ed98ebcf03600 Mon Sep 17 00:00:00 2001
From: Dustin Spicuzza <dustin@virtualroadside.com>
Date: Sat, 19 Feb 2022 23:42:10 -0500
Subject: [PATCH] [wpilibc] Prevent StopMotor from terminating robot during
 MotorSafety check (#4038)

- Nothing else in that function can throw, so protecting StopMotor should be sufficient
- Fixes #4036
---
 wpilibc/src/main/native/cpp/MotorSafety.cpp       | 10 +++++++++-
 wpilibc/src/main/native/include/frc/MotorSafety.h |  6 ++++++
 2 files changed, 15 insertions(+), 1 deletion(-)

diff --git a/wpilibc/src/main/native/cpp/MotorSafety.cpp b/wpilibc/src/main/native/cpp/MotorSafety.cpp
index 91b4dfb328..388a887a97 100644
--- a/wpilibc/src/main/native/cpp/MotorSafety.cpp
+++ b/wpilibc/src/main/native/cpp/MotorSafety.cpp
@@ -98,7 +98,15 @@ void MotorSafety::Check() {
   if (stopTime < Timer::GetFPGATimestamp()) {
     FRC_ReportError(err::Timeout, "{}... Output not updated often enough",
                     GetDescription());
-    StopMotor();
+
+    try {
+      StopMotor();
+    } catch (frc::RuntimeError& e) {
+      e.Report();
+    } catch (std::exception& e) {
+      FRC_ReportError(err::Error, "{} StopMotor threw unexpected exception: {}",
+                      GetDescription(), e.what());
+    }
   }
 }
 
diff --git a/wpilibc/src/main/native/include/frc/MotorSafety.h b/wpilibc/src/main/native/include/frc/MotorSafety.h
index f84af1796f..1bfd34a81a 100644
--- a/wpilibc/src/main/native/include/frc/MotorSafety.h
+++ b/wpilibc/src/main/native/include/frc/MotorSafety.h
@@ -91,6 +91,12 @@ class MotorSafety {
   static void CheckMotors();
 
   virtual void StopMotor() = 0;
+
+  /**
+   * The return value from this method is printed out when an error occurs
+   *
+   * This method must not throw!
+   */
   virtual std::string GetDescription() const = 0;
 
  private: