From cbb871e7bb4d94c5a840f98760c44be882f1a165 Mon Sep 17 00:00:00 2001
From: IamPete1 <33176108+IamPete1@users.noreply.github.com>
Date: Mon, 21 Jan 2019 01:36:10 +0000
Subject: [PATCH] SRV_Channel: add global E-stop

---
 libraries/SRV_Channel/SRV_Channel.cpp  | 16 ++++++++++++++++
 libraries/SRV_Channel/SRV_Channel.h    | 15 ++++++++++++++-
 libraries/SRV_Channel/SRV_Channels.cpp |  1 +
 3 files changed, 31 insertions(+), 1 deletion(-)

diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp
index 3eeb114c29..e9030d5519 100644
--- a/libraries/SRV_Channel/SRV_Channel.cpp
+++ b/libraries/SRV_Channel/SRV_Channel.cpp
@@ -112,6 +112,12 @@ void SRV_Channel::calc_pwm(int16_t output_scaled)
     if (have_pwm_mask & (1U<<ch_num)) {
         return;
     }
+
+    // check for E - stop
+    if (SRV_Channel::should_e_stop(get_function()) && SRV_Channels::emergency_stop) {
+        output_scaled = 0;
+    }
+
     uint16_t pwm;
     if (type_angle) {
         pwm = pwm_from_angle(output_scaled);
@@ -187,3 +193,13 @@ bool SRV_Channel::is_motor(SRV_Channel::Aux_servo_function_t function)
     return ((function >= SRV_Channel::k_motor1 && function <= SRV_Channel::k_motor8) ||
             (function >= SRV_Channel::k_motor9 && function <= SRV_Channel::k_motor12));
 }
+
+// return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous
+bool SRV_Channel::should_e_stop(SRV_Channel::Aux_servo_function_t function)
+{
+    return ((function >= SRV_Channel::k_heli_rsc && function <= SRV_Channel::k_motor8) ||
+            function == SRV_Channel::k_starter || function == SRV_Channel::k_throttle ||
+            function == SRV_Channel::k_throttleLeft || function == SRV_Channel::k_throttleRight ||
+            (function >= SRV_Channel::k_boost_throttle && function <= SRV_Channel::k_motor12) ||
+            function == k_engine_run_enable);
+}
diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h
index dd2ea63ba2..c888425817 100644
--- a/libraries/SRV_Channel/SRV_Channel.h
+++ b/libraries/SRV_Channel/SRV_Channel.h
@@ -193,6 +193,9 @@ public:
     // return true if function is for a multicopter motor
     static bool is_motor(SRV_Channel::Aux_servo_function_t function);
 
+    // return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous
+    static bool should_e_stop(SRV_Channel::Aux_servo_function_t function);
+
     // return the function of a channel
     SRV_Channel::Aux_servo_function_t get_function(void) const {
         return (SRV_Channel::Aux_servo_function_t)function.get();
@@ -456,7 +459,15 @@ public:
     static void set_digital_mask(uint16_t mask) {
         digital_mask |= mask;
     }
-    
+
+    // Set E - stop
+    static void set_emergency_stop(bool state) {
+        emergency_stop = state;
+    }
+
+    // get E - stop
+    static bool get_emergency_stop() { return emergency_stop;}
+
 private:
     struct {
         bool k_throttle_reversible:1;
@@ -516,4 +527,6 @@ private:
     static bool passthrough_disabled(void) {
         return disabled_passthrough;
     }
+
+    static bool emergency_stop;
 };
diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp
index 60caf91e39..e74b2607d5 100644
--- a/libraries/SRV_Channel/SRV_Channels.cpp
+++ b/libraries/SRV_Channel/SRV_Channels.cpp
@@ -51,6 +51,7 @@ uint16_t SRV_Channels::reversible_mask;
 
 bool SRV_Channels::disabled_passthrough;
 bool SRV_Channels::initialised;
+bool SRV_Channels::emergency_stop;
 Bitmask SRV_Channels::function_mask{SRV_Channel::k_nr_aux_servo_functions};
 SRV_Channels::srv_function SRV_Channels::functions[SRV_Channel::k_nr_aux_servo_functions];