Copter: add dead reckon failsafe

ArduCopter/Copter.cpp

@@ -561,6 +561,9 @@ void Copter::three_hz_loop()
     // check if we've lost terrain data
     failsafe_terrain_check();
 
+    // check for deadreckoning failsafe
+    failsafe_deadreckon_check();
+
 #if AC_FENCE == ENABLED
     // check if we have breached a fence
     fence_check();

ArduCopter/Copter.h

@@ -391,12 +391,20 @@ private:
         uint8_t ekf                 : 1; // true if ekf failsafe has occurred
         uint8_t terrain             : 1; // true if the missing terrain data failsafe has occurred
         uint8_t adsb                : 1; // true if an adsb related failsafe has occurred
+        uint8_t deadreckon          : 1; // true if a dead reckoning failsafe has triggered
     } failsafe;
 
     bool any_failsafe_triggered() const {
-        return failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb;
+        return failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb || failsafe.deadreckon;
     }
 
+    // dead reckoning state
+    struct {
+        bool active;        // true if dead reckoning (position estimate using estimated airspeed, no position or velocity source)
+        bool timeout;       // true if dead reckoning has timedout and EKF's position and velocity estimate should no longer be trusted
+        uint32_t start_ms;  // system time that EKF began deadreckoning
+    } dead_reckoning;
+
     // Motor Output
     MOTOR_CLASS *motors;
     const struct AP_Param::GroupInfo *motors_var_info;
@@ -734,6 +742,7 @@ private:
     void failsafe_terrain_set_status(bool data_ok);
     void failsafe_terrain_on_event();
     void gpsglitch_check();
+    void failsafe_deadreckon_check();
     void set_mode_RTL_or_land_with_pause(ModeReason reason);
     void set_mode_SmartRTL_or_RTL(ModeReason reason);
     void set_mode_SmartRTL_or_land_with_pause(ModeReason reason);

ArduCopter/Parameters.cpp

@@ -1111,6 +1111,20 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("SURFTRAK_MODE", 51, ParametersG2, surftrak_mode, (uint8_t)Copter::SurfaceTracking::Surface::GROUND),
 
+    // @Param: FS_DR_ENABLE
+    // @DisplayName: DeadReckon Failsafe Action
+    // @Description: Failsafe action taken immediately as deadreckoning starts. Deadreckoning starts when EKF loses position and velocity source and relies only on wind estimates
+    // @Values: 0:Disabled/NoAction,1:Land, 2:RTL, 3:SmartRTL or RTL, 4:SmartRTL or Land, 6:Auto DO_LAND_START or RTL
+    // @User: Standard
+    AP_GROUPINFO("FS_DR_ENABLE", 52, ParametersG2, failsafe_dr_enable, (uint8_t)Copter::FailsafeAction::RTL),
+
+    // @Param: FS_DR_TIMEOUT
+    // @DisplayName: DeadReckon Failsafe Timeout
+    // @Description: DeadReckoning is available for this many seconds after losing position and/or velocity source.  After this timeout elapses the EKF failsafe will trigger in modes requiring a position estimate
+    // @Range: 0 120
+    // @User: Standard
+    AP_GROUPINFO("FS_DR_TIMEOUT", 53, ParametersG2, failsafe_dr_timeout, 30),
+
     AP_GROUPEND
 };
 

ArduCopter/Parameters.h

@@ -667,6 +667,8 @@ public:
     AP_Float                pilot_y_rate;
     AP_Float                pilot_y_expo;
     AP_Int8                 surftrak_mode;
+    AP_Int8                 failsafe_dr_enable;
+    AP_Int16                failsafe_dr_timeout;
 };
 
 extern const AP_Param::Info        var_info[];

ArduCopter/events.cpp

@@ -309,6 +309,66 @@ void Copter::gpsglitch_check()
     }
 }
 
+// dead reckoning alert and failsafe
+void Copter::failsafe_deadreckon_check()
+{
+    // update dead reckoning state
+    const char* dr_prefix_str = "Dead Reckoning";
+
+    // get EKF filter status
+    bool ekf_dead_reckoning = inertial_nav.get_filter_status().flags.dead_reckoning;
+
+    // alert user to start or stop of dead reckoning
+    const uint32_t now_ms = AP_HAL::millis();
+    if (dead_reckoning.active != ekf_dead_reckoning) {
+        dead_reckoning.active = ekf_dead_reckoning;
+        if (dead_reckoning.active) {
+            dead_reckoning.start_ms = now_ms;
+            gcs().send_text(MAV_SEVERITY_CRITICAL,"%s started", dr_prefix_str);
+        } else {
+            dead_reckoning.start_ms = 0;
+            dead_reckoning.timeout = false;
+            gcs().send_text(MAV_SEVERITY_CRITICAL,"%s stopped", dr_prefix_str);
+        }
+    }
+
+    // check for timeout
+    if (dead_reckoning.active && !dead_reckoning.timeout) {
+        const uint32_t dr_timeout_ms = uint32_t(constrain_float(g2.failsafe_dr_timeout * 1000.0f, 0.0f, UINT32_MAX));
+        if (now_ms - dead_reckoning.start_ms > dr_timeout_ms) {
+            dead_reckoning.timeout = true;
+            gcs().send_text(MAV_SEVERITY_CRITICAL,"%s timeout", dr_prefix_str);
+        }
+    }
+
+    // exit immediately if deadreckon failsafe is disabled
+    if (g2.failsafe_dr_enable <= 0) {
+        failsafe.deadreckon = false;
+        return;
+    }
+
+    // check for failsafe action
+    if (failsafe.deadreckon != ekf_dead_reckoning) {
+        failsafe.deadreckon = ekf_dead_reckoning;
+
+        // only take action in modes requiring position estimate
+        if (failsafe.deadreckon && copter.flightmode->requires_GPS()) {
+
+            // log error
+            AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_DEADRECKON, LogErrorCode::FAILSAFE_OCCURRED);
+
+            // immediately disarm while landed
+            if (should_disarm_on_failsafe()) {
+                arming.disarm(AP_Arming::Method::DEADRECKON_FAILSAFE);
+                return;
+            }
+
+            // take user specified action
+            do_failsafe_action((FailsafeAction)g2.failsafe_dr_enable.get(), ModeReason::DEADRECKON_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(ModeReason reason)

ArduCopter/system.cpp

@@ -273,7 +273,7 @@ bool Copter::ekf_has_relative_position() const
         return false;
     }
 
-    // return immediately if neither optflow nor visual odometry nor wind estimatation is enabled
+    // return immediately if neither optflow nor visual odometry is enabled and dead reckoning is inactive
     bool enabled = false;
 #if AP_OPTICALFLOW_ENABLED
     if (optflow.enabled()) {
@@ -285,8 +285,7 @@ bool Copter::ekf_has_relative_position() const
         enabled = true;
     }
 #endif
-    Vector3f airspeed_vec_bf;
-    if (AP::ahrs().airspeed_vector_true(airspeed_vec_bf)) {
+    if (dead_reckoning.active && !dead_reckoning.timeout) {
         enabled = true;
     }
     if (!enabled) {