From c35530c362f6b1dab17786099bcd94737aaa6147 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 22 Aug 2012 17:34:01 +1000
Subject: [PATCH] APM: fixed wheeled takeoff with Jons new controllers

---
 ArduPlane/Attitude.pde | 19 ++++++++++---------
 1 file changed, 10 insertions(+), 9 deletions(-)

diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde
index fa6768e3bb..69cb681cec 100644
--- a/ArduPlane/Attitude.pde
+++ b/ArduPlane/Attitude.pde
@@ -173,20 +173,21 @@ static void calc_throttle()
 // ----------------------------------------------------------------------------------------
 static void calc_nav_yaw(float speed_scaler, float ch4_inf)
 {
+    if (hold_course != -1) {
+        // steering on or close to ground
+        g.channel_rudder.servo_out = g.pidWheelSteer.get_pid(bearing_error_cd) + g.kff_rudder_mix * g.channel_roll.servo_out;
+        return;
+    }
+
 #if APM_CONTROL == DISABLED
     // always do rudder mixing from roll
     g.channel_rudder.servo_out = g.kff_rudder_mix * g.channel_roll.servo_out;
 
-    if (hold_course != -1) {
-        // steering on or close to ground
-        g.channel_rudder.servo_out += g.pidWheelSteer.get_pid(bearing_error_cd);
-    } else {
-        // a PID to coordinate the turn (drive y axis accel to zero)
-        Vector3f temp = imu.get_accel();
-        int32_t error = -temp.y*100.0;
+    // a PID to coordinate the turn (drive y axis accel to zero)
+    Vector3f temp = imu.get_accel();
+    int32_t error = -temp.y*100.0;
 
-        g.channel_rudder.servo_out += g.pidServoRudder.get_pid(error, speed_scaler);
-    }
+    g.channel_rudder.servo_out += g.pidServoRudder.get_pid(error, speed_scaler);
 #else // APM_CONTROL == ENABLED
     // use the new APM_Control library
 	g.channel_rudder.servo_out = g.yawController.get_servo_out(speed_scaler, ch4_inf < 0.25) + g.channel_roll.servo_out * g.kff_rudder_mix;