Plane: basic automatic emergency parachute release at critical altitude error in AUTO.

ArduPlane/Parameters.cpp

@@ -1058,10 +1058,26 @@ const AP_Param::Info Plane::var_info[] = {
     GOBJECT(relay,                  "RELAY_", AP_Relay),
 
 #if PARACHUTE == ENABLED
-	// @Group: CHUTE_
+    // @Group: CHUTE_
     // @Path: ../libraries/AP_Parachute/AP_Parachute.cpp
     GOBJECT(parachute,		"CHUTE_", AP_Parachute),
 
+    // @Param: CHUTE_AUTO_ON
+    // @DisplayName: Parachute automatic emergency release
+    // @Description: Parachute automatic emergency release enabled or disabled.
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+    GSCALAR(parachute_auto_enabled,    "CHUTE_AUTO_ON",   0),
+
+    // @Param: CHUTE_AUTO_ERROR
+    // @DisplayName: Altitude deviation for parachute release
+    // @Description: Altitude deviation at which to release parachute if in AUTO and CHUTE_AUTO_ON.
+    // @Units: Meters
+    // @Range: 10 32767
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(parachute_auto_error,      "CHUTE_AUTO_ERROR",      20),
+
     // @Param: CHUTE_CHAN
     // @DisplayName: Parachute release channel
     // @Description: If set to a non-zero value then this is an RC input channel number to use for manually releasing the parachute. When this channel goes above 1700 the parachute will be released

ArduPlane/Parameters.h

@@ -146,6 +146,8 @@ class Parameters {
         k_param_parachute,
         k_param_arming = 100,
         k_param_parachute_channel,
+        k_param_parachute_auto_enabled,
+        k_param_parachute_auto_error,
         k_param_crash_accel_threshold,
         k_param_override_safety,
 
@@ -510,6 +512,8 @@ class Parameters {
 #endif
     AP_Int16 gcs_pid_mask;
     AP_Int8 parachute_channel;
+    AP_Int8 parachute_auto_enabled;
+    AP_Int16 parachute_auto_error;
 
     // RC channels
     RC_Channel rc_1;

ArduPlane/Plane.h

@@ -1021,6 +1021,7 @@ class Plane : public AP_HAL::HAL::Callbacks {
     void parachute_check();
     void parachute_release();
     bool parachute_manual_release();
+    void parachute_emergency_check();
     void accel_cal_update(void);
 
 public:

ArduPlane/parachute.cpp

@@ -3,12 +3,15 @@
 #include "Plane.h"
 
 
-/* 
+/*
    call parachute library update
 */
 void Plane::parachute_check()
 {
     parachute.update();
+
+    // check if there is an emergency situation
+    parachute_emergency_check();
 }
 
 /*
@@ -19,7 +22,7 @@ void Plane::parachute_release()
     if (parachute.released()) {
         return;
     }
-    
+
     // send message to gcs and dataflash
     gcs_send_text(MAV_SEVERITY_CRITICAL,"Parachute: Released");
 
@@ -56,3 +59,62 @@ bool Plane::parachute_manual_release()
 
     return true;
 }
+
+// Code to detect loss of control for ArduPlane
+#define PARACHUTE_CHECK_TRIGGER_SEC         1  // 1 second of loss of control triggers the parachute
+
+/*
+  parachute_emergency_check - trigger the release of the parachute
+  automatically if a critical situation is detected
+*/
+void Plane::parachute_emergency_check()
+{
+    static uint32_t control_loss_ms;  // time when we first lost control and never regained it
+    uint32_t now = millis();
+
+    // exit immediately if parachute or automatic release is not enabled, or already released
+    if (!parachute.enabled() || parachute.released() || g.parachute_auto_enabled == 0) {
+        control_loss_ms = 0;
+        return;
+    }
+
+    // only automatically release in AUTO mode
+    if (control_mode != AUTO) {
+        control_loss_ms = 0;
+        return;
+    }
+
+    // do not release if vehicle is not flying
+    if (!is_flying()) {
+        control_loss_ms = 0;
+        return;
+    }
+
+    // do not release if taking off or landing
+    if (auto_state.takeoff_complete == false || mission.get_current_nav_cmd().id == MAV_CMD_NAV_LAND) {
+        control_loss_ms = 0;
+        return;
+    }
+
+    if (relative_altitude() < parachute.alt_min()) {
+        control_loss_ms = 0;
+        return;
+    }
+
+    // do not release if we are flying within given error
+    if (altitude_error_cm < g.parachute_auto_error * 100.0f) {
+        control_loss_ms = 0;
+        return;
+    }
+
+    // at this point we consider control lost
+    if (control_loss_ms == 0) {
+        gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Crash: Starting to lose control, %d m error", altitude_error_cm / 100);
+        control_loss_ms = now;
+    } else if (now - control_loss_ms > PARACHUTE_CHECK_TRIGGER_SEC * 1000) {
+        // we have lost control for too long
+        gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Crash: Critical altitude error %d m", altitude_error_cm / 100);
+        parachute_release();
+        control_loss_ms = 0;
+    }
+}