From 571b4478fd5956a5cdcf346de4498ac099288fbd Mon Sep 17 00:00:00 2001
From: Grant Morphett <grant@gmorph.com>
Date: Mon, 7 Sep 2015 13:51:01 +1000
Subject: [PATCH] Rover: Using a new method to check for throttle failsafe

This method checks for throttle reversal.
---
 APMrover2/Rover.h   |  1 +
 APMrover2/radio.cpp | 19 +++++++++++++++++++
 APMrover2/test.cpp  |  2 +-
 3 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index e1d526b43e..c46bb5dca2 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -456,6 +456,7 @@ private:
     void init_rc_out();
     void read_radio();
     void control_failsafe(uint16_t pwm);
+    bool throttle_failsafe_active();
     void trim_control_surfaces();
     void trim_radio();
     void init_barometer(void);
diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp
index 3548bb3e29..0463e323a4 100644
--- a/APMrover2/radio.cpp
+++ b/APMrover2/radio.cpp
@@ -116,6 +116,25 @@ void Rover::control_failsafe(uint16_t pwm)
 	}
 }
 
+/*
+  return true if throttle level is below throttle failsafe threshold
+  or RC input is invalid
+ */
+bool Rover::throttle_failsafe_active(void)
+{
+    if (!g.fs_throttle_enabled) {
+        return false;
+    }
+    if (millis() - failsafe.last_valid_rc_ms > 1000) {
+        // we haven't had a valid RC frame for 1 seconds
+        return true;
+    }
+    if (channel_throttle->get_reverse()) {
+        return channel_throttle->radio_in >= g.fs_throttle_value;
+    }
+    return channel_throttle->radio_in <= g.fs_throttle_value;
+}
+
 void Rover::trim_control_surfaces()
 {
 	read_radio();
diff --git a/APMrover2/test.cpp b/APMrover2/test.cpp
index 1d2dc53b92..559bacf7c1 100644
--- a/APMrover2/test.cpp
+++ b/APMrover2/test.cpp
@@ -171,7 +171,7 @@ int8_t Rover::test_failsafe(uint8_t argc, const Menu::arg *argv)
 			fail_test++;
 		}
 
-        if(g.fs_throttle_enabled && (channel_throttle->radio_in < g.fs_throttle_value)) {
+        if(throttle_failsafe_active()) {
 			cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), channel_throttle->radio_in);
             print_mode(cliSerial, readSwitch());
             cliSerial->println();