From 2321cefdc26cb32c3edc6999b20d4a22630f7935 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 19 Jul 2017 17:00:13 +0900
Subject: [PATCH] Rover: move manual mode failsafe handling to mode class

---
 APMrover2/Steering.cpp    | 11 ++---------
 APMrover2/mode_manual.cpp | 12 +++++++++---
 2 files changed, 11 insertions(+), 12 deletions(-)

diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp
index b6e0db6287..32c7e308f0 100644
--- a/APMrover2/Steering.cpp
+++ b/APMrover2/Steering.cpp
@@ -51,15 +51,8 @@ bool Rover::in_stationary_loiter()
 /*****************************************
     Set the flight control servos based on the current calculated values
 *****************************************/
-void Rover::set_servos(void) {
-    // Apply slew rate limit on non Manual modes
-    if (control_mode == &mode_manual || control_mode == &mode_learning) {
-        if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
-            g2.motors.set_throttle(0.0f);
-            g2.motors.set_steering(0.0f);
-        }
-    }
-
+void Rover::set_servos(void)
+{
     // send output signals to motors
     if (motor_test) {
         g2.motors.slew_limit_throttle(false);
diff --git a/APMrover2/mode_manual.cpp b/APMrover2/mode_manual.cpp
index 9fcedd7d30..8e2d18db4d 100644
--- a/APMrover2/mode_manual.cpp
+++ b/APMrover2/mode_manual.cpp
@@ -3,9 +3,15 @@
 
 void ModeManual::update()
 {
-    // copy RC scaled inputs to outputs
-    g2.motors.set_throttle(channel_throttle->get_control_in());
-    g2.motors.set_steering(channel_steer->get_control_in());
+    // check for radio failsafe
+    if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
+        g2.motors.set_throttle(0.0f);
+        g2.motors.set_steering(0.0f);
+    } else {
+        // copy RC scaled inputs to outputs
+        g2.motors.set_throttle(channel_throttle->get_control_in());
+        g2.motors.set_steering(channel_steer->get_control_in());
+    }
 
     // mark us as in_reverse when using a negative throttle to stop AHRS getting off
     rover.set_reverse(is_negative(g2.motors.get_throttle()));