From 664c92fb81acf887f6efdf45c393fabf5b71614e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 16 Apr 2015 23:30:32 +1000
Subject: [PATCH] Plane: fixed RUDDER_ONLY to not combine direct rudder output

this prevents us over-rolling in FBWA
---
 ArduPlane/ArduPlane.pde  |  5 ++++-
 ArduPlane/Attitude.pde   | 13 ++++++++-----
 ArduPlane/navigation.pde |  2 +-
 ArduPlane/radio.pde      | 10 ++++++++++
 4 files changed, 23 insertions(+), 7 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index 7a85a3973d..db4f83b1ba 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -596,6 +596,9 @@ static int32_t nav_roll_cd;
 // The instantaneous desired pitch angle.  Hundredths of a degree
 static int32_t nav_pitch_cd;
 
+// we separate out rudder input to allow for RUDDER_ONLY=1
+static int16_t rudder_input;
+
 // the aerodymamic load factor. This is calculated from the demanded
 // roll before the roll is clipped, using 1/sqrt(cos(nav_roll))
 static float aerodynamic_load_factor = 1.0f;
@@ -1362,7 +1365,7 @@ static void update_flight_mode(void)
           any aileron or rudder input
         */
         if ((channel_roll->control_in != 0 ||
-             channel_rudder->control_in != 0)) {                
+             rudder_input != 0)) {                
             cruise_state.locked_heading = false;
             cruise_state.lock_timer_ms = 0;
         }                 
diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde
index c757b202b4..d352cc6226 100644
--- a/ArduPlane/Attitude.pde
+++ b/ArduPlane/Attitude.pde
@@ -339,7 +339,7 @@ static void stabilize_acro(float speed_scaler)
     /*
       manual rudder for now
      */
-    steering_control.steering = steering_control.rudder = channel_rudder->control_in;
+    steering_control.steering = steering_control.rudder = rudder_input;
 }
 
 /*
@@ -414,14 +414,14 @@ static void calc_throttle()
 static void calc_nav_yaw_coordinated(float speed_scaler)
 {
     bool disable_integrator = false;
-    if (control_mode == STABILIZE && channel_rudder->control_in != 0) {
+    if (control_mode == STABILIZE && rudder_input != 0) {
         disable_integrator = true;
     }
     steering_control.rudder = yawController.get_servo_out(speed_scaler, disable_integrator);
 
     // add in rudder mixing from roll
     steering_control.rudder += channel_roll->servo_out * g.kff_rudder_mix;
-    steering_control.rudder += channel_rudder->control_in;
+    steering_control.rudder += rudder_input;
     steering_control.rudder = constrain_int16(steering_control.rudder, -4500, 4500);
 }
 
@@ -451,11 +451,11 @@ static void calc_nav_yaw_ground(void)
         // manual rudder control while still
         steer_state.locked_course = false;
         steer_state.locked_course_err = 0;
-        steering_control.steering = channel_rudder->control_in;
+        steering_control.steering = rudder_input;
         return;
     }
 
-    float steer_rate = (channel_rudder->control_in/4500.0f) * g.ground_steer_dps;
+    float steer_rate = (rudder_input/4500.0f) * g.ground_steer_dps;
     if (flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) {
         steer_rate = 0;
     }
@@ -969,6 +969,9 @@ static void set_servos(void)
     // send values to the PWM timers for output
     // ----------------------------------------
     if (g.rudder_only == 0) {
+        // when we RUDDER_ONLY mode we don't send the channel_roll
+        // output and instead rely on KFF_RDDRMIX. That allows the yaw
+        // damper to operate.
         channel_roll->output();
     }
     channel_pitch->output();
diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde
index 8bdd5c5305..bdbc5fa0cf 100644
--- a/ArduPlane/navigation.pde
+++ b/ArduPlane/navigation.pde
@@ -138,7 +138,7 @@ static void update_cruise()
 {
     if (!cruise_state.locked_heading &&
         channel_roll->control_in == 0 &&
-        channel_rudder->control_in == 0 &&
+        rudder_input == 0 &&
         gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
         gps.ground_speed() >= 3 &&
         cruise_state.lock_timer_ms == 0) {
diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde
index cfb11460a5..f1c65a6254 100644
--- a/ArduPlane/radio.pde
+++ b/ArduPlane/radio.pde
@@ -9,6 +9,8 @@
 static void set_control_channels(void)
 {
     if (g.rudder_only) {
+        // in rudder only mode the roll and rudder channels are the
+        // same.
         channel_roll     = RC_Channel::rc_channel(rcmap.yaw()-1);
     } else {
         channel_roll     = RC_Channel::rc_channel(rcmap.roll()-1);
@@ -174,6 +176,14 @@ static void read_radio()
     }
 
     rudder_arm_check();
+
+    if (g.rudder_only != 0) {
+        // in rudder only mode we discard rudder input and get target
+        // attitude from the roll channel.
+        rudder_input = 0;
+    } else {
+        rudder_input = channel_rudder->control_in;
+    }
 }
 
 static void control_failsafe(uint16_t pwm)