Copter: add terrain failsafe

ArduCopter/ArduCopter.cpp

@@ -450,6 +450,9 @@ void Copter::three_hz_loop()
     // check if we've lost contact with the ground station
     failsafe_gcs_check();
 
+    // check if we've lost terrain data
+    failsafe_terrain_check();
+
 #if AC_FENCE == ENABLED
     // check if we have breached a fence
     fence_check();

ArduCopter/Copter.h

@@ -282,10 +282,13 @@ private:
         uint8_t battery             : 1; // 2   // A status flag for the battery failsafe
         uint8_t gcs                 : 1; // 4   // A status flag for the ground station failsafe
         uint8_t ekf                 : 1; // 5   // true if ekf failsafe has occurred
+        uint8_t terrain             : 1; // 6   // true if the missing terrain data failsafe has occurred
 
         int8_t radio_counter;            // number of iterations with throttle below throttle_fs_value
 
         uint32_t last_heartbeat_ms;      // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
+        uint32_t terrain_first_failure_ms;  // the first time terrain data access failed - used to calculate the duration of the failure
+        uint32_t terrain_last_failure_ms;   // the most recent time terrain data access failed
     } failsafe;
 
     // sensor health for logging
@@ -853,6 +856,9 @@ private:
     void failsafe_battery_event(void);
     void failsafe_gcs_check();
     void failsafe_gcs_off_event(void);
+    void failsafe_terrain_check();
+    void failsafe_terrain_set_status(bool data_ok);
+    void failsafe_terrain_on_event();
     void set_mode_RTL_or_land_with_pause(mode_reason_t reason);
     void update_events();
     void failsafe_enable();

ArduCopter/GCS_Mavlink.cpp

@@ -33,7 +33,7 @@ NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan)
     uint32_t custom_mode = control_mode;
 
     // set system as critical if any failsafe have triggered
-    if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf)  {
+    if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf || failsafe.terrain)  {
         system_status = MAV_STATE_CRITICAL;
     }
 

ArduCopter/config.h

@@ -205,6 +205,11 @@
  #define FS_RADIO_TIMEOUT_MS            500     // RC Radio Failsafe triggers after 500 miliseconds with No RC Input
 #endif
 
+// missing terrain data failsafe
+#ifndef FS_TERRAIN_TIMEOUT_MS
+ #define FS_TERRAIN_TIMEOUT_MS          5000     // 5 seconds of missing terrain data will trigger failsafe (RTL)
+#endif
+
 #ifndef PREARM_DISPLAY_PERIOD
 # define PREARM_DISPLAY_PERIOD 30
 #endif

ArduCopter/defines.h

@@ -117,7 +117,8 @@ enum mode_reason_t {
     MODE_REASON_GPS_GLITCH,
     MODE_REASON_MISSION_END,
     MODE_REASON_THROTTLE_LAND_ESCAPE,
-    MODE_REASON_FENCE_BREACH
+    MODE_REASON_FENCE_BREACH,
+    MODE_REASON_TERRAIN_FAILSAFE
 };
 
 // Tuning enumeration
@@ -382,6 +383,7 @@ enum ThrowModeState {
 #define ERROR_SUBSYSTEM_FAILSAFE_ADSB       20
 #define ERROR_SUBSYSTEM_TERRAIN             21
 #define ERROR_SUBSYSTEM_NAVIGATION          22
+#define ERROR_SUBSYSTEM_FAILSAFE_TERRAIN    23
 // general error codes
 #define ERROR_CODE_ERROR_RESOLVED           0
 #define ERROR_CODE_FAILED_TO_INITIALISE     1

ArduCopter/events.cpp

@@ -130,6 +130,62 @@ void Copter::failsafe_gcs_off_event(void)
     Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_GCS, ERROR_CODE_FAILSAFE_RESOLVED);
 }
 
+// executes terrain failsafe if data is missing for longer than a few seconds
+//  missing_data should be set to true if the vehicle failed to navigate because of missing data, false if navigation is proceeding successfully
+void Copter::failsafe_terrain_check()
+{
+    // trigger with 5 seconds of failures while in AUTO mode
+    bool valid_mode = (control_mode == AUTO || control_mode == GUIDED || control_mode == RTL);
+    bool timeout = (failsafe.terrain_last_failure_ms - failsafe.terrain_first_failure_ms) > FS_TERRAIN_TIMEOUT_MS;
+    bool trigger_event = valid_mode && timeout;
+
+    // check for clearing of event
+    if (trigger_event != failsafe.terrain) {
+        if (trigger_event) {
+            failsafe_terrain_on_event();
+        } else {
+            Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_ERROR_RESOLVED);
+            failsafe.terrain = false;
+        }
+    }
+}
+
+// set terrain data status (found or not found)
+void Copter::failsafe_terrain_set_status(bool data_ok)
+{
+    uint32_t now = millis();
+
+    // record time of first and latest failures (i.e. duration of failures)
+    if (!data_ok) {
+        failsafe.terrain_last_failure_ms = now;
+        if (failsafe.terrain_first_failure_ms == 0) {
+            failsafe.terrain_first_failure_ms = now;
+        }
+    } else {
+        // failures cleared after 0.1 seconds of persistent successes
+        if (now - failsafe.terrain_last_failure_ms > 100) {
+            failsafe.terrain_last_failure_ms = 0;
+            failsafe.terrain_first_failure_ms = 0;
+        }
+    }
+}
+
+// terrain failsafe action
+void Copter::failsafe_terrain_on_event()
+{
+    failsafe.terrain = true;
+    gcs_send_text(MAV_SEVERITY_CRITICAL,"Failsafe: Terrain data missing");
+    Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_TERRAIN, ERROR_CODE_FAILSAFE_OCCURRED);
+
+    if (should_disarm_on_failsafe()) {
+        init_disarm_motors();
+    } else if (control_mode == RTL) {
+        rtl_restart_without_terrain();
+    } else {
+        set_mode_RTL_or_land_with_pause(MODE_REASON_TERRAIN_FAILSAFE);
+    }
+}
+
 // set_mode_RTL_or_land_with_pause - sets mode to RTL if possible or LAND with 4 second delay before descent starts
 //  this is always called from a failsafe so we trigger notification to pilot
 void Copter::set_mode_RTL_or_land_with_pause(mode_reason_t reason)