From 4fae5cdfcf3d82b50434c69a41cf0622a1fcb43f Mon Sep 17 00:00:00 2001
From: Jacob Walser <jwalser90@gmail.com>
Date: Sat, 21 Apr 2018 00:35:03 -0400
Subject: [PATCH] Sub: disarm motors if failsafe action fails

---
 ArduSub/failsafe.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

diff --git a/ArduSub/failsafe.cpp b/ArduSub/failsafe.cpp
index 4cc5896703..20b63a4739 100644
--- a/ArduSub/failsafe.cpp
+++ b/ArduSub/failsafe.cpp
@@ -346,9 +346,13 @@ void Sub::failsafe_gcs_check()
     if (g.failsafe_gcs == FS_GCS_DISARM) {
         init_disarm_motors();
     } else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) {
-        set_mode(ALT_HOLD, MODE_REASON_GCS_FAILSAFE);
+        if (!set_mode(ALT_HOLD, MODE_REASON_GCS_FAILSAFE)) {
+            init_disarm_motors();
+        }
     } else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) {
-        set_mode(SURFACE, MODE_REASON_GCS_FAILSAFE);
+        if (!set_mode(SURFACE, MODE_REASON_GCS_FAILSAFE)) {
+            init_disarm_motors();
+        }
     }
 }