diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index d2372592e3..f5faded24e 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -320,6 +320,12 @@ void Rover::update_GPS(void) void Rover::update_current_mode(void) { + // check for emergency stop + if (SRV_Channels::get_emergency_stop()) { + // relax controllers, motor stopping done at output level + g2.attitude_control.relax_I(); + } + control_mode->update(); } diff --git a/APMrover2/AP_Arming.cpp b/APMrover2/AP_Arming.cpp index 4a69bbbc23..1287f7064d 100644 --- a/APMrover2/AP_Arming.cpp +++ b/APMrover2/AP_Arming.cpp @@ -71,6 +71,13 @@ bool AP_Arming_Rover::gps_checks(bool display_failure) bool AP_Arming_Rover::pre_arm_checks(bool report) { + if (SRV_Channels::get_emergency_stop()) { + if (report) { + gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motors Emergency Stopped"); + } + return false; + } + return (AP_Arming::pre_arm_checks(report) & rover.g2.motors.pre_arm_check(report) & fence_checks(report)