diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 9facd665c0..f327b376fa 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/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(); diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 9a397cba1a..8169a0118e 100644 --- a/ArduPlane/Plane.h +++ b/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, diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index 1528900b33..9c172284f0 100644 --- a/ArduPlane/events.cpp +++ b/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 diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 16133df8fe..b93b22d4a2 100644 --- a/ArduPlane/is_flying.cpp +++ b/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 diff --git a/ArduPlane/parachute.cpp b/ArduPlane/parachute.cpp index 0ca87965fb..c7005d3032 100644 --- a/ArduPlane/parachute.cpp +++ b/ArduPlane/parachute.cpp @@ -64,5 +64,4 @@ bool Plane::parachute_manual_release() #endif return true; } - -#endif +#endif \ No newline at end of file