Copter: add flight option to relase gripper on thrust loss
This commit is contained in:
parent
da79a5ea78
commit
f4c7760819
@ -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[] = {
|
||||
|
@ -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),
|
||||
|
||||
|
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user