Copter: add motor thrust lost check

ArduCopter/Copter.h

@@ -700,6 +700,7 @@ private:
 
     // crash_check.cpp
     void crash_check();
+    void thrust_loss_check();
     void parachute_check();
     void parachute_release();
     void parachute_manual_release();

ArduCopter/crash_check.cpp

@@ -5,6 +5,11 @@
 #define CRASH_CHECK_ANGLE_DEVIATION_DEG 30.0f   // 30 degrees beyond angle max is signal we are inverted
 #define CRASH_CHECK_ACCEL_MAX           3.0f    // vehicle must be accelerating less than 3m/s/s to be considered crashed
 
+// Code to detect a thrust loss main ArduCopter code
+#define THRUST_LOSS_CHECK_TRIGGER_SEC         1     // 1 second decent while level and high throttle indictes thrust loss
+#define THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD 150    // we can't expect to maintain altitude beyond 15 degrees on all aircraft
+#define THRUST_LOSS_CHECK_MINIMUM_THROTTLE 0.9f     // we can expect to maintain altitude above 90 % throttle
+
 // crash_check - disarms motors if a crash has been detected
 // crashes are detected by the vehicle being more than 20 degrees beyond it's angle limits continuously for more than 1 second
 // called at MAIN_LOOP_RATE
@@ -53,6 +58,65 @@ void Copter::crash_check()
     }
 }
 
+// crash_check - disarms motors if a crash has been detected
+// crashes are detected by the vehicle being more than 20 degrees beyond it's angle limits continuously for more than 1 second
+// called at MAIN_LOOP_RATE
+void Copter::thrust_loss_check()
+{
+    static uint16_t thrust_loss_counter;  // number of iterations vehicle may have been crashed
+
+    // exit immediately if motor failure handling already engaged
+    if (motors->get_thrust_boost()) {
+        return;
+    }
+
+    // return immediately if disarmed
+    if (!motors->armed() || ap.land_complete) {
+        thrust_loss_counter = 0;
+        return;
+    }
+
+    // check for desired angle over 15 degrees
+    // todo: add thrust angle to AC_AttitudeControl
+    const Vector3f angle_target =  attitude_control->get_att_target_euler_cd();
+    if ((fabs(angle_target.x) > THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD) || (fabs(angle_target.y) > THRUST_LOSS_CHECK_ANGLE_DEVIATION_CD)) {
+        thrust_loss_counter = 0;
+        return;
+    }
+
+    // check for throttle over 90 % or throttle saturation
+    if ((attitude_control->get_throttle_in() < THRUST_LOSS_CHECK_MINIMUM_THROTTLE) && (!motors->limit.throttle_upper)) {
+        thrust_loss_counter = 0;
+        return;
+    }
+
+    // check for throttle over 90 % or throttle saturation
+    if ((attitude_control->get_throttle_in() < 0.25f)) {
+        thrust_loss_counter = 0;
+        return;
+    }
+
+    // check for decent
+    if (inertial_nav.get_velocity_z() >= 0.0f) {
+        thrust_loss_counter = 0;
+        return;
+    }
+
+    // we may be crashing
+    thrust_loss_counter++;
+
+    // check if thrust loss for 1 second
+    if (thrust_loss_counter >= (THRUST_LOSS_CHECK_TRIGGER_SEC * scheduler.get_loop_rate_hz())) {
+        // log an error in the dataflash
+        Log_Write_Error(ERROR_SUBSYSTEM_THRUST_LOSS_CHECK, ERROR_CODE_FAILSAFE_OCCURRED);
+        // send message to gcs
+        //gcs().send_text(MAV_SEVERITY_EMERGENCY,"Potential Thrust Loss");
+        gcs().send_text(MAV_SEVERITY_EMERGENCY, "Potential Thrust Loss (%d)", (int)motors->get_lost_motor());
+        // enable thrust loss handling
+        motors->set_thrust_boost(true);
+    }
+}
+
 #if PARACHUTE == ENABLED
 
 // Code to detect a crash main ArduCopter code

ArduCopter/defines.h

@@ -360,6 +360,7 @@ enum LoggingParameters {
 #define ERROR_SUBSYSTEM_NAVIGATION          22
 #define ERROR_SUBSYSTEM_FAILSAFE_TERRAIN    23
 #define ERROR_SUBSYSTEM_EKF_PRIMARY         24
+#define ERROR_SUBSYSTEM_THRUST_LOSS_CHECK   25
 // general error codes
 #define ERROR_CODE_ERROR_RESOLVED           0
 #define ERROR_CODE_FAILED_TO_INITIALISE     1

ArduCopter/land_detector.cpp

@@ -26,6 +26,7 @@ void Copter::update_land_and_crash_detectors()
 #endif
 
     crash_check();
+    thrust_loss_check();
 }
 
 // update_land_detector - checks if we have landed and updates the ap.land_complete flag