SRV_Channel: allow DO_SET_SERVO commands while rc pass-thru

libraries/SRV_Channel/SRV_Channel.h

@@ -217,7 +217,11 @@ public:
     bool function_configured(void) const {
         return function.configured();
     }
-    
+
+    // specify that small rc input changes should be ignored during passthrough
+    // used by DO_SET_SERVO commands
+    void ignore_small_rcin_changes() { ign_small_rcin_changes = true; }
+
 private:
     AP_Int16 servo_min;
     AP_Int16 servo_max;
@@ -268,6 +272,13 @@ private:
     // mask of channels where we have a output_pwm value. Cleared when a
     // scaled value is written. 
     static servo_mask_t have_pwm_mask;
+
+    // previous radio_in during pass-thru
+    int16_t previous_radio_in;
+
+    // specify that small rcinput changes should be ignored during passthrough
+    // used by DO_SET_SERVO commands
+    bool ign_small_rcin_changes;
 };
 
 /*

libraries/SRV_Channel/SRV_Channel_aux.cpp

@@ -46,7 +46,17 @@ void SRV_Channel::output_ch(void)
             if (SRV_Channels::passthrough_disabled()) {
                 output_pwm = c->get_radio_trim();
             } else {
-                output_pwm = c->get_radio_in();
+                const int16_t radio_in = c->get_radio_in();
+                if (!ign_small_rcin_changes) {
+                    output_pwm = radio_in;
+                    previous_radio_in = radio_in;
+                } else {
+                    // check if rc input value has changed by more than the deadzone
+                    if (abs(radio_in - previous_radio_in) > c->get_dead_zone()) {
+                        output_pwm = radio_in;
+                        ign_small_rcin_changes = false;
+                    }
+                }
             }
         }
     }