Sub: disarm motors if failsafe action fails

7 years ago · 4fae5cdfcf
1 changed files with 6 additions and 2 deletions
--- a/ArduSub/failsafe.cpp
+++ b/ArduSub/failsafe.cpp
@ -346,9 +346,13 @@ void Sub::failsafe_gcs_check()
				@@ -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();
+        }
    }
 }