Plane: Added parachute release on critcal sink rate and battery failsafe actions

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

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 @@ private:
         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