diff --git a/libraries/AP_Parachute/AP_Parachute.cpp b/libraries/AP_Parachute/AP_Parachute.cpp index c570f8bf6c..ad45fed687 100644 --- a/libraries/AP_Parachute/AP_Parachute.cpp +++ b/libraries/AP_Parachute/AP_Parachute.cpp @@ -10,6 +10,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -65,7 +66,7 @@ const AP_Param::GroupInfo AP_Parachute::var_info[] = { // @Increment: 1 // @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 @@ -78,9 +79,9 @@ const AP_Param::GroupInfo AP_Parachute::var_info[] = { // @Param: OPTIONS // @DisplayName: Parachute options // @Description: Optional behaviour for parachute - // @Bitmask: 0:hold open forever after release + // @Bitmask: 0:hold open forever after release,1:skip disarm before parachute release // @User: Standard - AP_GROUPINFO("OPTIONS", 7, AP_Parachute, _options, 0), + AP_GROUPINFO("OPTIONS", 7, AP_Parachute, _options, AP_PARACHUTE_OPTIONS_DEFAULT), AP_GROUPEND }; @@ -107,6 +108,12 @@ void AP_Parachute::release() GCS_SEND_TEXT(MAV_SEVERITY_INFO,"Parachute: Released"); LOGGER_WRITE_EVENT(LogEvent::PARACHUTE_RELEASED); + bool need_disarm = (_options.get() & uint32_t(Options::SkipDisarmBeforeParachuteRelease)) == 0; + if (need_disarm) { + // stop motors to avoid parachute tangling + AP::arming().disarm(AP_Arming::Method::PARACHUTE_RELEASE); + } + // set release time to current system time if (_release_time == 0) { _release_time = AP_HAL::millis(); diff --git a/libraries/AP_Parachute/AP_Parachute.h b/libraries/AP_Parachute/AP_Parachute.h index 30005e7543..b37f20051a 100644 --- a/libraries/AP_Parachute/AP_Parachute.h +++ b/libraries/AP_Parachute/AP_Parachute.h @@ -19,7 +19,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 +#define AP_PARACHUTE_CRITICAL_SINK_DEFAULT 0 // default critical sink speed in m/s to trigger emergency parachute +#define AP_PARACHUTE_OPTIONS_DEFAULT 0 // default parachute options: enabled disarm after parachute release #ifndef HAL_PARACHUTE_ENABLED // default to parachute enabled to match previous configs @@ -115,6 +116,7 @@ private: enum class Options : uint8_t { HoldOpen = (1U<<0), + SkipDisarmBeforeParachuteRelease = (1U<<1), }; AP_Int32 _options;