AP_Parachute: Added parachute release at critical sink rate

Signed-off-by: Vinicius Knabben <viniciusknabben@hotmail.com>

libraries/AP_Parachute/AP_Parachute.cpp

@@ -62,6 +62,16 @@ const AP_Param::GroupInfo AP_Parachute::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("DELAY_MS", 5, AP_Parachute, _delay_ms, AP_PARACHUTE_RELEASE_DELAY_MS),
     
+    // @Param: CRT_SINK
+    // @DisplayName: Critical sink speed rate in m/s to trigger emergency parachute
+    // @Description: Release parachute when critical sink rate is reached
+    // @Range: 0 15
+    // @Units: m/s
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("CRT_SINK", 6, AP_Parachute, _critical_sink, AP_PARACHUTE_CRITICAL_SINK_DEFAULT),
+    
+    
     AP_GROUPEND
 };
 
@@ -105,7 +115,10 @@ void AP_Parachute::update()
     if (_enabled <= 0) {
         return;
     }
-
+    // check if the plane is sinking too fast and release parachute
+    if((_critical_sink > 0) && (_sink_rate > _critical_sink) && !_release_initiated && _is_flying) {
+        release();
+    }
     // calc time since release
     uint32_t time_diff = AP_HAL::millis() - _release_time;
     uint32_t delay_ms = _delay_ms<=0 ? 0: (uint32_t)_delay_ms;

libraries/AP_Parachute/AP_Parachute.h

@@ -20,6 +20,8 @@
 
 #define AP_PARACHUTE_ALT_MIN_DEFAULT            10     // default min altitude the vehicle should have before parachute is released
 
+#define AP_PARACHUTE_CRITICAL_SINK_DEFAULT      0    // default critical sink speed in m/s to trigger emergency parachute
+
 /// @class	AP_Parachute
 /// @brief	Class managing the release of a parachute
 class AP_Parachute {
@@ -63,11 +65,20 @@ public:
     
     /// update - shuts off the trigger should be called at about 10hz
     void update();
+    
+    /// critical_sink - returns the configured maximum sink rate to trigger emergency release
+    float critical_sink() const { return _critical_sink; }
 
     /// alt_min - returns the min altitude above home the vehicle should have before parachute is released
     ///   0 = altitude check disabled
     int16_t alt_min() const { return _alt_min; }
 
+    /// set_is_flying - accessor to the is_flying flag
+    void set_is_flying(const bool is_flying) { _is_flying = is_flying; }
+
+    // set_sink_rate - set vehicle sink rate
+    void set_sink_rate(float sink_rate) { _sink_rate = sink_rate; }
+
     static const struct AP_Param::GroupInfo        var_info[];
 
     // get singleton instance
@@ -82,6 +93,7 @@ private:
     AP_Int16    _servo_off_pwm; // PWM value to move servo to when shutter is deactivated
     AP_Int16    _alt_min;       // min altitude the vehicle should have before parachute is released
     AP_Int16    _delay_ms;      // delay before chute release for motors to stop
+    AP_Float    _critical_sink;      // critical sink rate to trigger emergency parachute
 
     // internal variables
     AP_Relay   &_relay;         // pointer to relay object from the base class Relay.
@@ -89,6 +101,8 @@ private:
     bool        _release_initiated:1;    // true if the parachute release initiated (may still be waiting for engine to be suppressed etc.)
     bool        _release_in_progress:1;  // true if the parachute release is in progress
     bool        _released:1;             // true if the parachute has been released
+    bool        _is_flying:1;            // true if the vehicle is flying
+    float       _sink_rate;              // vehicle sink rate in m/s
 };
 
 namespace AP {