diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp index c39a9f7fdd..a74d50b2ba 100644 --- a/libraries/AP_Arming/AP_Arming.cpp +++ b/libraries/AP_Arming/AP_Arming.cpp @@ -1466,6 +1466,26 @@ bool AP_Arming::serial_protocol_checks(bool display_failure) return true; } +//Check for estop +bool AP_Arming::estop_checks(bool display_failure) +{ + if (!SRV_Channels::get_emergency_stop()) { + // not emergency-stopped, so no prearm failure: + return true; + } + // vehicle is emergency-stopped; if this *appears* to have been done via switch then we do not fail prearms: + const RC_Channel *chan = rc().find_channel_for_option(RC_Channel::AUX_FUNC::ARM_EMERGENCY_STOP); + if (chan != nullptr) { + // an RC channel is configured for arm_emergency_stop option, so estop maybe activated via this switch + if (chan->get_aux_switch_pos() == RC_Channel::AuxSwitchPos::LOW) { + // switch is configured and is in estop position, so likely the reason we are estopped, so no prearm failure + return true; // no prearm failure + } + } + check_failed(display_failure,"Motors Emergency Stopped"); + return false; +} + bool AP_Arming::pre_arm_checks(bool report) { #if !APM_BUILD_COPTER_OR_HELI @@ -1507,7 +1527,8 @@ bool AP_Arming::pre_arm_checks(bool report) & disarm_switch_checks(report) & fence_checks(report) & opendroneid_checks(report) - & serial_protocol_checks(report); + & serial_protocol_checks(report) + & estop_checks(report); } bool AP_Arming::arm_checks(AP_Arming::Method method) diff --git a/libraries/AP_Arming/AP_Arming.h b/libraries/AP_Arming/AP_Arming.h index 98e531e127..8cb1c5f060 100644 --- a/libraries/AP_Arming/AP_Arming.h +++ b/libraries/AP_Arming/AP_Arming.h @@ -213,6 +213,8 @@ protected: bool opendroneid_checks(bool display_failure); bool serial_protocol_checks(bool display_failure); + + bool estop_checks(bool display_failure); virtual bool system_checks(bool report);