RC_Channel: input value watchdog

libraries/RC_Channel/RC_Channel.cpp

@@ -86,6 +86,14 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
     // @User: Standard
     AP_GROUPINFO_FRAME("OPTION",  6, RC_Channel, option, 0, AP_PARAM_FRAME_COPTER|AP_PARAM_FRAME_ROVER|AP_PARAM_FRAME_PLANE),
 
+    // @Param: TIMEOUT
+    // @DisplayName: RC value timeout
+    // @Description: If an incoming RC value did not change during the timeout then this interpreted as signal lost. 0 to disable the check.
+    // @Units: Seconds
+    // @Range: 0 255
+    // @User: Advanced
+    AP_GROUPINFO("TIMEOUT",   7, RC_Channel, rc_watchdog_timeout_sec, 0),
+
     AP_GROUPEND
 };
 
@@ -128,8 +136,15 @@ RC_Channel::update(void)
 {
     if (has_override() && !(*RC_Channels::options & RC_IGNORE_OVERRIDES)) {
         radio_in = override_value;
+        rc_healthy = true;
     } else if (!(*RC_Channels::options & RC_IGNORE_RECEIVER)) {
-        radio_in = hal.rcin->read(ch_in);
+        int16_t in_value = hal.rcin->read(ch_in);
+
+        rc_healthy = watchdog_check(in_value);
+        // TODO: && PWM Range check
+        // TODO: && Throttle FS PWM check
+
+        radio_in = in_value;
     } else {
         return false;
     }
@@ -144,6 +159,26 @@ RC_Channel::update(void)
     return true;
 }
 
+// Check new incoming RC value has not stuck
+bool
+RC_Channel::watchdog_check(int16_t new_rc_value) 
+{
+    if (rc_watchdog_timeout_sec > 0) {    
+        uint32_t now_sec = AP_HAL::millis() / 1000;
+
+        if (now_sec != rc_watchdog_valid_time_sec) {
+            if (radio_in == new_rc_value){
+                if (now_sec - rc_watchdog_valid_time_sec >= rc_watchdog_timeout_sec)
+                    return false;
+            }
+
+            rc_watchdog_valid_time_sec = now_sec;
+        }
+    }
+
+    return true;
+}
+
 // recompute control values with no deadzone
 // When done this way the control_in value can be used as servo_out
 // to give the same output as input

libraries/RC_Channel/RC_Channel.h

@@ -109,6 +109,8 @@ class RC_Channel {
     void init_aux();
     void read_aux();
 
+    bool healthy(void) const { return rc_healthy; }
+
     // Aux Switch enumeration
     enum aux_func {
         DO_NOTHING =           0, // aux switch disabled
@@ -226,7 +228,14 @@ class RC_Channel {
 
     // the input channel this corresponds to
     uint8_t     ch_in;
-
+
+	// rc input watchdog
+    AP_Int8     rc_watchdog_timeout_sec;
+    uint32_t    rc_watchdog_valid_time_sec;    
+    bool watchdog_check(int16_t new_rc_value);
+
+    bool rc_healthy;    
+
     // overrides
     uint16_t override_value;
     uint32_t last_override_time;

libraries/RC_Channel/RC_Channels.cpp

@@ -80,12 +80,15 @@ bool RC_Channels::read_input(void)
 
     has_new_overrides = false;
 
-    bool success = false;
+    bool any_updated = false;
+    bool all_healthy = true;
+
     for (uint8_t i=0; i<NUM_RC_CHANNELS; i++) {
-        success |= channel(i)->update();
+        any_updated |= channel(i)->update();
+        all_healthy &= channel(i)->healthy();
     }
 
-    return success;
+    return any_updated && all_healthy;
 }
 
 uint8_t RC_Channels::get_valid_channel_count(void)