From 4f9b8e1b6c9ee787f209c16c45c1f5fb04a5dacf Mon Sep 17 00:00:00 2001
From: IamPete1 <33176108+IamPete1@users.noreply.github.com>
Date: Fri, 25 Jan 2019 18:58:50 +0000
Subject: [PATCH] Rover: add E-stop

---
 APMrover2/APMrover2.cpp | 6 ++++++
 APMrover2/AP_Arming.cpp | 7 +++++++
 2 files changed, 13 insertions(+)

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)