From 56768a8d6191819d89a2d69297efc85a35d1f7d5 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 3 Apr 2014 09:56:30 +0900
Subject: [PATCH] Copter: add min alt check to parachute release

---
 ArduCopter/control_modes.pde |  4 ++--
 ArduCopter/crash_check.pde   | 32 ++++++++++++++++++++++++++++++++
 2 files changed, 34 insertions(+), 2 deletions(-)

diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde
index ceac84bcd9..98060217d2 100644
--- a/ArduCopter/control_modes.pde
+++ b/ArduCopter/control_modes.pde
@@ -378,7 +378,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
 
     case AUX_SWITCH_PARACHUTE_RELEASE:
         if (ch_flag == AUX_SWITCH_HIGH) {
-            parachute_release();
+            parachute_manual_release();
         }
         break;
 
@@ -393,7 +393,7 @@ static void do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
                 break;
             case AUX_SWITCH_HIGH:
                 parachute.enabled(true);
-                parachute_release();
+                parachute_manual_release();
                 break;
         }
 #endif
diff --git a/ArduCopter/crash_check.pde b/ArduCopter/crash_check.pde
index a58d4a7bd7..eb0535d032 100644
--- a/ArduCopter/crash_check.pde
+++ b/ArduCopter/crash_check.pde
@@ -88,6 +88,9 @@ void parachute_check()
         return;
     }
 
+    // call update to give parachute a chance to move servo or relay back to off position
+    parachute.update();
+
     // return immediately if motors are not armed or pilot's throttle is above zero
     if (!motors.armed()) {
         control_loss_count = 0;
@@ -100,6 +103,12 @@ void parachute_check()
         return;
     }
 
+    // ensure we are above the minimum altitude above home
+    if (baro_alt < parachute.alt_min_cm()) {
+        control_loss_count = 0;
+        return;
+    }
+
     // get desired lean angles
     const Vector3f& target_angle = attitude_control.angle_ef_targets();
 
@@ -108,6 +117,11 @@ void parachute_check()
         labs(ahrs.pitch_sensor - target_angle.y) > CRASH_CHECK_ANGLE_DEVIATION_CD) {
         control_loss_count++;
 
+        // don't let control_loss_count get too high
+        if (control_loss_count > PARACHUTE_CHECK_ITERATIONS_MAX) {
+            control_loss_count = PARACHUTE_CHECK_ITERATIONS_MAX;
+        }
+
         // record baro if we have just started losing control
         if (control_loss_count == 1) {
             baro_alt_start = baro_alt;
@@ -121,6 +135,9 @@ void parachute_check()
 
         // check if loss of control for at least 1 second
         }else if (control_loss_count >= PARACHUTE_CHECK_ITERATIONS_MAX) {
+            // reset control loss counter
+            control_loss_count = 0;
+            // release parachute
             parachute_release();
         }
     }else{
@@ -146,4 +163,19 @@ static void parachute_release()
     // release parachute
     parachute.release();
 }
+
+// parachute_manual_release - trigger the release of the parachute, after performing some checks for pilot error
+//   checks if the vehicle is landed 
+static void parachute_manual_release()
+{
+    // do not release if we are landed or below the minimum altitude above home
+    if (ap.land_complete || baro_alt < parachute.alt_min_cm()) {
+        // warn user of reason for failure
+        gcs_send_text_P(SEVERITY_HIGH,PSTR("Parachute: Not Released"));
+        return;
+    }
+
+    // if we get this far release parachute
+    parachute_release();
+}
 #endif // PARACHUTE == ENABLED
\ No newline at end of file