From b72ea5435bed2dee60a4bc7d86486c30c3e159ca Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 20 Dec 2013 09:48:36 +1100
Subject: [PATCH] Rover: check for failed RC input

loss of RC for 2s is considered a throttle failsafe
---
 APMrover2/APMrover2.pde |  1 +
 APMrover2/radio.pde     | 13 ++++++++++++-
 2 files changed, 13 insertions(+), 1 deletion(-)

diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index d544260a0c..2903d37dd8 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -354,6 +354,7 @@ static struct {
     uint32_t rc_override_timer;
     uint32_t start_time;
     uint8_t triggered;
+    uint32_t last_valid_rc_ms;
 } failsafe;
 
 // notification object for LEDs, buzzers etc (parameter set to false disables external leds)
diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde
index 2de38d456d..eb7262a375 100644
--- a/APMrover2/radio.pde
+++ b/APMrover2/radio.pde
@@ -45,6 +45,13 @@ static void init_rc_out()
 
 static void read_radio()
 {
+    if (!hal.rcin->valid_channels()) {
+        control_failsafe(channel_throttle->radio_in);
+        return;
+    }
+
+    failsafe.last_valid_rc_ms = hal.scheduler->millis();
+
     for (uint8_t i=0; i<8; i++) {
         RC_Channel::rc_channel(i)->set_pwm(RC_Channel::rc_channel(i)->read());
     }
@@ -101,7 +108,11 @@ static void control_failsafe(uint16_t pwm)
 	if (rc_override_active) {
         failsafe_trigger(FAILSAFE_EVENT_RC, (millis() - failsafe.rc_override_timer) > 1500);
 	} else if (g.fs_throttle_enabled) {
-        failsafe_trigger(FAILSAFE_EVENT_THROTTLE, pwm < (uint16_t)g.fs_throttle_value);
+        bool failed = pwm < (uint16_t)g.fs_throttle_value;
+        if (hal.scheduler->millis() - failsafe.last_valid_rc_ms > 2000) {
+            failed = true;
+        }
+        failsafe_trigger(FAILSAFE_EVENT_THROTTLE, failed);
 	}
 }