diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 82a16ff3d1..6aac73a773 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -606,6 +606,7 @@ private: enum class FlightOptions { DISABLE_THRUST_LOSS_CHECK = (1<<0), // 1 DISABLE_YAW_IMBALANCE_WARNING = (1<<1), // 2 + RELEASE_GRIPPER_ON_THRUST_LOSS = (1<<2), // 4 }; static constexpr int8_t _failsafe_priorities[] = { diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index c8fe905a07..241f8f4097 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1042,7 +1042,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Param: FLIGHT_OPTIONS // @DisplayName: Flight mode options // @Description: Flight mode specific options - // @Bitmask: 0:Disable thrust loss check, 1:Disable yaw imbalance warning + // @Bitmask: 0:Disable thrust loss check, 1:Disable yaw imbalance warning, 2:Release gripper on thrust loss // @User: Advanced AP_GROUPINFO("FLIGHT_OPTIONS", 44, ParametersG2, flight_options, 0), diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index d000f01a7b..d5872e0239 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -163,6 +163,12 @@ void Copter::thrust_loss_check() // enable thrust loss handling motors->set_thrust_boost(true); // the motors library disables this when it is no longer needed to achieve the commanded output + +#if GRIPPER_ENABLED == ENABLED + if ((copter.g2.flight_options & uint32_t(FlightOptions::RELEASE_GRIPPER_ON_THRUST_LOSS)) != 0) { + copter.g2.gripper.release(); + } +#endif } }