Rover: Using a new method to check for throttle failsafe

This method checks for throttle reversal.

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);

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();

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();