From 1dcf90c16111cd8f875054fa8cd401ae970cd816 Mon Sep 17 00:00:00 2001
From: Pierre Kancir <pierre.kancir.emn@gmail.com>
Date: Tue, 20 Jun 2017 14:30:55 +0200
Subject: [PATCH] Rover: use scaled input in Manual modes

---
 APMrover2/Steering.cpp | 54 ++++++++++++++++--------------------------
 APMrover2/radio.cpp    |  3 ++-
 2 files changed, 22 insertions(+), 35 deletions(-)

diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp
index 7f3947a531..e006ac1620 100644
--- a/APMrover2/Steering.cpp
+++ b/APMrover2/Steering.cpp
@@ -278,44 +278,30 @@ void Rover::mix_skid_steering(void)
     Set the flight control servos based on the current calculated values
 *****************************************/
 void Rover::set_servos(void) {
-    if (control_mode == MANUAL || control_mode == LEARNING) {
-        // do a direct pass through of radio values
-        SRV_Channels::set_output_pwm(SRV_Channel::k_steering, channel_steer->read());
-        SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, channel_throttle->read());
-        if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
-            // suppress throttle if in failsafe and manual
-            SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
-            // suppress steer if in failsafe and manual and skid steer mode
-            if (g.skid_steer_out) {
-                SRV_Channels::set_output_limit(SRV_Channel::k_steering, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
-            }
-        }
+    if (in_reverse) {
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
+                                                                                 -g.throttle_max,
+                                                                                 -g.throttle_min));
     } else {
-        if (in_reverse) {
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
-                                          -g.throttle_max,
-                                          -g.throttle_min));
-        } else {
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
-                                          g.throttle_min.get(),
-                                          g.throttle_max.get()));
-        }
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle),
+                                                                                 g.throttle_min,
+                                                                                 g.throttle_max));
+    }
 
-        if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
-            // suppress throttle if in failsafe
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
-            // suppress steer if in failsafe and skid steer mode
-            if (have_skid_steering()) {
-                SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
-            }
+    if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
+        // suppress throttle if in failsafe
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
+        // suppress steer if in failsafe and skid steer mode
+        if (have_skid_steering()) {
+            SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
         }
+    }
 
-        if (!hal.util->get_soft_armed()) {
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
-            // suppress steer if in failsafe and skid steer mode
-            if (have_skid_steering()) {
-                SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
-            }
+    if (!hal.util->get_soft_armed()) {
+        SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0);
+        // suppress steer if in failsafe and skid steer mode
+        if (have_skid_steering()) {
+            SRV_Channels::set_output_scaled(SRV_Channel::k_steering, 0);
         }
     }
 
diff --git a/APMrover2/radio.cpp b/APMrover2/radio.cpp
index 74dc116289..373969ed42 100644
--- a/APMrover2/radio.cpp
+++ b/APMrover2/radio.cpp
@@ -137,8 +137,9 @@ void Rover::read_radio()
     RC_Channels::set_pwm_all();
     // check that RC value are valid
     control_failsafe(channel_throttle->get_radio_in());
-    // copy RC throttle input to throttle output
+    // copy RC scaled inputs to outputs
     SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, channel_throttle->get_control_in());
+    SRV_Channels::set_output_scaled(SRV_Channel::k_steering, channel_steer->get_control_in());
 
     // Check if the throttle value is above 50% and we need to nudge
     // Make sure its above 50% in the direction we are travelling