Rover: move failsafe_trigger from system to failsafe

APMrover2/failsafe.cpp

@@ -49,6 +49,48 @@ void Rover::failsafe_check()
     }
 }
 
+/*
+  called to set/unset a failsafe event.
+ */
+void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
+{
+    uint8_t old_bits = failsafe.bits;
+    if (on) {
+        failsafe.bits |= failsafe_type;
+    } else {
+        failsafe.bits &= ~failsafe_type;
+    }
+    if (old_bits == 0 && failsafe.bits != 0) {
+        // a failsafe event has started
+        failsafe.start_time = millis();
+    }
+    if (failsafe.triggered != 0 && failsafe.bits == 0) {
+        // a failsafe event has ended
+        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Failsafe ended");
+    }
+
+    failsafe.triggered &= failsafe.bits;
+
+    if (failsafe.triggered == 0 &&
+        failsafe.bits != 0 &&
+        millis() - failsafe.start_time > g.fs_timeout * 1000 &&
+        control_mode != RTL &&
+        control_mode != HOLD) {
+        failsafe.triggered = failsafe.bits;
+        gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", static_cast<uint32_t>(failsafe.triggered));
+        switch (g.fs_action) {
+            case 0:
+                break;
+            case 1:
+                set_mode(RTL);
+                break;
+            case 2:
+                set_mode(HOLD);
+                break;
+        }
+    }
+}
+
 #if ADVANCED_FAILSAFE == ENABLED
 /*
    check for AFS failsafe check

APMrover2/system.cpp

@@ -368,48 +368,6 @@ bool Rover::mavlink_set_mode(uint8_t mode)
     return false;
 }
 
-/*
-  called to set/unset a failsafe event.
- */
-void Rover::failsafe_trigger(uint8_t failsafe_type, bool on)
-{
-    uint8_t old_bits = failsafe.bits;
-    if (on) {
-        failsafe.bits |= failsafe_type;
-    } else {
-        failsafe.bits &= ~failsafe_type;
-    }
-    if (old_bits == 0 && failsafe.bits != 0) {
-        // a failsafe event has started
-        failsafe.start_time = millis();
-    }
-    if (failsafe.triggered != 0 && failsafe.bits == 0) {
-        // a failsafe event has ended
-        gcs_send_text_fmt(MAV_SEVERITY_INFO, "Failsafe ended");
-    }
-
-    failsafe.triggered &= failsafe.bits;
-
-    if (failsafe.triggered == 0 &&
-        failsafe.bits != 0 &&
-        millis() - failsafe.start_time > g.fs_timeout*1000 &&
-        control_mode != RTL &&
-        control_mode != HOLD) {
-        failsafe.triggered = failsafe.bits;
-        gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Failsafe trigger 0x%x", static_cast<uint32_t>(failsafe.triggered));
-        switch (g.fs_action) {
-        case 0:
-            break;
-        case 1:
-            set_mode(RTL);
-            break;
-        case 2:
-            set_mode(HOLD);
-            break;
-        }
-    }
-}
-
 void Rover::startup_INS_ground(void)
 {
     gcs_send_text(MAV_SEVERITY_INFO, "Warming up ADC");