From 0d53ce4cd7f9acfe41ebe8ff847b6fb91964d402 Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sun, 7 Jun 2020 21:04:56 +0100
Subject: [PATCH] SRV_Channel: re-work timeout

---
 libraries/SRV_Channel/SRV_Channel.cpp     | 12 ++++++++----
 libraries/SRV_Channel/SRV_Channel.h       |  7 ++++++-
 libraries/SRV_Channel/SRV_Channel_aux.cpp | 10 +++++-----
 libraries/SRV_Channel/SRV_Channels.cpp    |  7 +++++--
 4 files changed, 24 insertions(+), 12 deletions(-)

diff --git a/libraries/SRV_Channel/SRV_Channel.cpp b/libraries/SRV_Channel/SRV_Channel.cpp
index bd3e866acd..46a779d299 100644
--- a/libraries/SRV_Channel/SRV_Channel.cpp
+++ b/libraries/SRV_Channel/SRV_Channel.cpp
@@ -114,8 +114,10 @@ void SRV_Channel::calc_pwm(int16_t output_scaled)
     }
 
     // check for E - stop
+    bool force = false;
     if (SRV_Channel::should_e_stop(get_function()) && SRV_Channels::emergency_stop) {
         output_scaled = 0;
+        force = true;
     }
 
     uint16_t pwm;
@@ -124,13 +126,15 @@ void SRV_Channel::calc_pwm(int16_t output_scaled)
     } else {
         pwm = pwm_from_range(output_scaled);
     }
-    set_output_pwm(pwm);
+    set_output_pwm(pwm,force);
 }
 
-void SRV_Channel::set_output_pwm(uint16_t pwm)
+void SRV_Channel::set_output_pwm(uint16_t pwm, bool force)
 {
-    output_pwm = pwm;
-    have_pwm_mask |= (1U<<ch_num);
+    if (!override_active || force) {
+        output_pwm = pwm;
+        have_pwm_mask |= (1U<<ch_num);
+    }
 }
 
 // set angular range of scaled output
diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h
index 03651721d2..f96c10ae24 100644
--- a/libraries/SRV_Channel/SRV_Channel.h
+++ b/libraries/SRV_Channel/SRV_Channel.h
@@ -167,7 +167,7 @@ public:
     };
 
     // set the output value as a pwm value
-    void set_output_pwm(uint16_t pwm);
+    void set_output_pwm(uint16_t pwm, bool force = false);
 
     // get the output value as a pwm value
     uint16_t get_output_pwm(void) const { return output_pwm; }
@@ -291,6 +291,11 @@ private:
     // specify that small rcinput changes should be ignored during passthrough
     // used by DO_SET_SERVO commands
     bool ign_small_rcin_changes;
+
+    // if true we should ignore all imputs on this channel
+    bool override_active;
+
+    void set_override(bool b) {override_active = b;};
 };
 
 /*
diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp
index 7cadf206b4..eb1b3b0334 100644
--- a/libraries/SRV_Channel/SRV_Channel_aux.cpp
+++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp
@@ -269,7 +269,7 @@ SRV_Channels::set_trim_to_servo_out_for(SRV_Channel::Aux_servo_function_t functi
     }
     for (uint8_t i = 0; i < NUM_SERVO_CHANNELS; i++) {
         if (channels[i].function.get() == function) {
-            channels[i].servo_trim.set_and_save_ifchanged(channels[i].output_pwm);
+            channels[i].servo_trim.set_and_save_ifchanged(channels[i].get_output_pwm());
         }
     }
 }
@@ -602,7 +602,7 @@ bool SRV_Channels::get_output_pwm(SRV_Channel::Aux_servo_function_t function, ui
         return false;
     }
     channels[chan].calc_pwm(functions[function].output_scaled);
-    value = channels[chan].output_pwm;
+    value = channels[chan].get_output_pwm();
     return true;
 }
 
@@ -656,7 +656,7 @@ void SRV_Channels::limit_slew_rate(SRV_Channel::Aux_servo_function_t function, f
         if (c.function == function) {
             c.calc_pwm(functions[function].output_scaled);
             uint16_t last_pwm = hal.rcout->read_last_sent(c.ch_num);
-            if (last_pwm == c.output_pwm) {
+            if (last_pwm == c.get_output_pwm()) {
                 continue;
             }
             uint16_t max_change = (c.get_output_max() - c.get_output_min()) * slew_rate * dt * 0.01f;
@@ -666,7 +666,7 @@ void SRV_Channels::limit_slew_rate(SRV_Channel::Aux_servo_function_t function, f
                 // change for this loop
                 max_change = 1;
             }
-            c.output_pwm = constrain_int16(c.output_pwm, last_pwm-max_change, last_pwm+max_change);
+            c.set_output_pwm(constrain_int16(c.get_output_pwm(), last_pwm-max_change, last_pwm+max_change));
         }
     }
 }
@@ -708,7 +708,7 @@ void SRV_Channels::constrain_pwm(SRV_Channel::Aux_servo_function_t function)
     for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
         SRV_Channel &c = channels[i];
         if (c.function == function) {
-            c.output_pwm = constrain_int16(c.output_pwm, c.servo_min, c.servo_max);
+            c.set_output_pwm(constrain_int16(c.output_pwm, c.servo_min, c.servo_max));
         }
     }
 }
diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp
index bcb8679a9b..6894f284c2 100644
--- a/libraries/SRV_Channel/SRV_Channels.cpp
+++ b/libraries/SRV_Channel/SRV_Channels.cpp
@@ -215,10 +215,12 @@ void SRV_Channels::calc_pwm(void)
         // check if channel has been locked out for this loop
         // if it has, decrement the loop count for that channel
         if (override_counter[i] == 0) {
-            channels[i].calc_pwm(functions[channels[i].function].output_scaled);
+            channels[i].set_override(false);
         } else {
+            channels[i].set_override(true);
             override_counter[i]--;
         }
+        channels[i].calc_pwm(functions[channels[i].function].output_scaled);
     }
 }
 
@@ -242,7 +244,8 @@ void SRV_Channels::set_output_pwm_chan_timeout(uint8_t chan, uint16_t value, uin
         // round up so any non-zero requested value will result in at least one loop
         const uint32_t loop_count = ((timeout_ms * 1000U) + (loop_period_us - 1U)) / loop_period_us;
         override_counter[chan] = constrain_int32(loop_count, 0, UINT16_MAX);
-        SRV_Channels::set_output_pwm_chan(chan, value);
+        channels[chan].set_override(true);
+        channels[chan].set_output_pwm(value,true);
     }
 }