Plane: Emergency Parachute Feature

ArduCopter/crash_check.cpp

@@ -201,6 +201,9 @@ void Copter::parachute_check()
         // release parachute
         parachute_release();
     }
+
+    // pass sink rate to parachute library
+    parachute.set_sink_rate(-inertial_nav.get_velocity_z() * 0.01);
 }
 
 // parachute_release - trigger the release of the parachute, disarm the motors and notify the user

ArduPlane/ArduPlane.cpp

@@ -108,7 +108,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
 #endif
 };
 
-constexpr int8_t Plane::_failsafe_priorities[6];
+constexpr int8_t Plane::_failsafe_priorities[7];
 
 void Plane::setup() 
 {
@@ -577,7 +577,9 @@ void Plane::update_alt()
 
     // low pass the sink rate to take some of the noise out
     auto_state.sink_rate = 0.8f * auto_state.sink_rate + 0.2f*sink_rate;
-
+#if PARACHUTE == ENABLED
+    parachute.set_sink_rate(auto_state.sink_rate);
+#endif
     geofence_check(true);
 
     update_flight_stage();

ArduPlane/Plane.h

@@ -1074,11 +1074,13 @@ class Plane : public AP_HAL::HAL::Callbacks {
         Failsafe_Action_Land      = 2,
         Failsafe_Action_Terminate = 3,
         Failsafe_Action_QLand     = 4,
+        Failsafe_Action_Parachute = 5
     };
 
     // list of priorities, highest priority first
     static constexpr int8_t _failsafe_priorities[] = {
                                                       Failsafe_Action_Terminate,
+                                                      Failsafe_Action_Parachute,
                                                       Failsafe_Action_QLand,
                                                       Failsafe_Action_Land,
                                                       Failsafe_Action_RTL,

ArduPlane/events.cpp

@@ -178,6 +178,10 @@ void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
 #endif
             break;
 
+        case Failsafe_Action_Parachute:
+            parachute_release();
+            break;
+
         case Failsafe_Action_None:
             // don't actually do anything, however we should still flag the system as having hit a failsafe
             // and ensure all appropriate flags are going off to the user

ArduPlane/is_flying.cpp

@@ -155,6 +155,9 @@ void Plane::update_is_flying_5Hz(void)
     }
     previous_is_flying = new_is_flying;
     adsb.set_is_flying(new_is_flying);
+#if PARACHUTE == ENABLED
+    parachute.set_is_flying(new_is_flying);
+#endif
 #if STATS_ENABLED == ENABLED
     g2.stats.set_flying(new_is_flying);
 #endif

ArduPlane/parachute.cpp

@@ -64,5 +64,4 @@ bool Plane::parachute_manual_release()
 #endif
     return true;    
 }
-
-#endif
+#endif

libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp

@@ -138,7 +138,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
     // @Param: FS_CRT_ACT
     // @DisplayName: Critical battery failsafe action
     // @Description: What action the vehicle should perform if it hits a critical battery failsafe
-    // @Values{Plane}: 0:None,1:RTL,2:Land,3:Terminate,4:QLand
+    // @Values{Plane}: 0:None,1:RTL,2:Land,3:Terminate,4:QLand,5:Parachute
     // @Values{Copter}: 0:None,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land,5:Terminate
     // @Values{Sub}: 0:None,2:Disarm,3:Enter surface mode
     // @Values{Rover}: 0:None,1:RTL,2:Hold,3:SmartRTL,4:SmartRTL or Hold,5:Terminate

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,19 @@ void AP_Parachute::update()
     if (_enabled <= 0) {
         return;
     }
-
+    // check if the plane is sinking too fast for more than a second and release parachute
+    uint32_t time = AP_HAL::millis();
+    if((_critical_sink > 0) && (_sink_rate > _critical_sink) && !_release_initiated && _is_flying) {
+        if(_sink_time == 0) {
+            _sink_time = AP_HAL::millis();
+        }
+        if((time - _sink_time) >= 1000) {
+            release();
+        }
+    } else {
+        _sink_time = 0;
+    }
+
     // 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 @@ class AP_Parachute {
 
     /// 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,13 +93,17 @@ class AP_Parachute {
     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.
     uint32_t    _release_time;  // system time that parachute is ordered to be released (actual release will happen 0.5 seconds later)
     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
+    uint32_t    _sink_time;              // time that the vehicle exceeded critical sink rate
 };
 
 namespace AP {