From 95b8630a0920ab069b189fd35975df1bb5a5460e Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Wed, 9 Feb 2022 01:00:43 +0000
Subject: [PATCH] Copter: motor test: check E-stop

---
 ArduCopter/motor_test.cpp | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp
index d684f86e02..2248aa3e16 100644
--- a/ArduCopter/motor_test.cpp
+++ b/ArduCopter/motor_test.cpp
@@ -121,6 +121,12 @@ bool Copter::mavlink_motor_control_check(const GCS_MAVLINK &gcs_chan, bool check
         return false;
     }
 
+    // check E-Stop is not active
+    if (SRV_Channels::get_emergency_stop()) {
+        gcs_chan.send_text(MAV_SEVERITY_CRITICAL,"%s: Motor Emergency Stopped", mode);
+        return false;
+    }
+
     // if we got this far the check was successful and the motor test can continue
     return true;
 }
@@ -135,8 +141,6 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t
     }
     // if test has not started try to start it
     if (!ap.motor_test) {
-        gcs().send_text(MAV_SEVERITY_INFO, "starting motor test");
-
         /* perform checks that it is ok to start test
            The RC calibrated check can be skipped if direct pwm is
            supplied
@@ -145,6 +149,7 @@ MAV_RESULT Copter::mavlink_motor_test_start(const GCS_MAVLINK &gcs_chan, uint8_t
             return MAV_RESULT_FAILED;
         } else {
             // start test
+            gcs().send_text(MAV_SEVERITY_INFO, "starting motor test");
             ap.motor_test = true;
 
             EXPECT_DELAY_MS(3000);