From 296ce5a4096fa012d5c33873cb418e63aca68b8f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 26 Aug 2019 12:57:39 +1000
Subject: [PATCH] Plane: fixed throttle failsafe for FS_SHORT_ACTN=3

this is a replacement for #12043
many thanks to @Jaaaky for finding this issue
---
 ArduPlane/Plane.h   |  1 +
 ArduPlane/radio.cpp | 29 +++++++++++++++++++++--------
 2 files changed, 22 insertions(+), 8 deletions(-)

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 62b33aecfb..ac8ace4875 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -937,6 +937,7 @@ private:
     int16_t rudder_input(void);
     void control_failsafe();
     bool trim_radio();
+    bool rc_throttle_value_ok(void) const;
     bool rc_failsafe_active(void) const;
     void read_rangefinder(void);
     void read_airspeed(void);
diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp
index 5b18e2bca8..727b66bc44 100644
--- a/ArduPlane/radio.cpp
+++ b/ArduPlane/radio.cpp
@@ -182,12 +182,14 @@ void Plane::read_radio()
         return;
     }
 
-    if(!failsafe.rc_failsafe)
+    if (!failsafe.rc_failsafe)
     {
         failsafe.AFS_last_valid_rc_ms = millis();
     }
 
-    failsafe.last_valid_rc_ms = millis();
+    if (rc_throttle_value_ok()) {
+        failsafe.last_valid_rc_ms = millis();
+    }
 
     if (control_mode == &mode_training) {
         // in training mode we don't want to use a deadzone, as we
@@ -366,21 +368,32 @@ bool Plane::trim_radio()
     return true;
 }
 
+/*
+  check if throttle value is within allowed range
+ */
+bool Plane::rc_throttle_value_ok(void) const
+{
+    if (!g.throttle_fs_enabled) {
+        return true;
+    }
+    if (channel_throttle->get_reverse()) {
+        return channel_throttle->get_radio_in() < g.throttle_fs_value;
+    }
+    return channel_throttle->get_radio_in() > g.throttle_fs_value;
+}
+
 /*
   return true if throttle level is below throttle failsafe threshold
   or RC input is invalid
  */
 bool Plane::rc_failsafe_active(void) const
 {
-    if (!g.throttle_fs_enabled) {
-        return false;
+    if (!rc_throttle_value_ok()) {
+        return true;
     }
     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->get_radio_in() >= g.throttle_fs_value;
-    }
-    return channel_throttle->get_radio_in() <= g.throttle_fs_value;
+    return false;
 }