From d30dd7c3f1bfe5d006fc48bf0a81cdf330b5148b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 5 Jul 2013 14:55:22 +1000
Subject: [PATCH] Plane: only run TECS when in an auto-throttle mode

this prevents integrator buildup in user controlled modes
---
 ArduPlane/ArduPlane.pde | 15 ++++++---------
 ArduPlane/Attitude.pde  |  4 ++--
 ArduPlane/system.pde    |  4 ++++
 3 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index 93a9a64aad..2189f78c52 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -471,6 +471,10 @@ static int32_t takeoff_altitude_cm;
 // Minimum pitch to hold during takeoff command execution.  Hundredths of a degree
 static int16_t takeoff_pitch_cd;
 
+// true if we are in an auto-throttle mode, which means
+// we need to run the speed/height controller
+static bool auto_throttle_mode;
+
 // this controls throttle suppression in auto modes
 static bool throttle_suppressed;
 
@@ -766,14 +770,7 @@ static void fast_loop()
     if (g.log_bitmask & MASK_LOG_IMU)
         Log_Write_IMU();
 
-    // inertial navigation
-    // ------------------
-#if INERTIAL_NAVIGATION == ENABLED
-    // TODO: implement inertial nav function
-    inertialNavigation();
-#endif
-
-    if (g.alt_control_algorithm == ALT_CONTROL_TECS) {
+    if (g.alt_control_algorithm == ALT_CONTROL_TECS && auto_throttle_mode) {
         // Call TECS 50Hz update
         SpdHgt_Controller->update_50hz();
     }
@@ -1252,7 +1249,7 @@ static void update_alt()
     //	add_altitude_data(millis() / 100, g_gps->altitude / 10);
 
     // Update the speed & height controller states
-    if (g.alt_control_algorithm == ALT_CONTROL_TECS) {
+    if (g.alt_control_algorithm == ALT_CONTROL_TECS && auto_throttle_mode) {
         SpdHgt_Controller->update_pitch_throttle(target_altitude_cm - home.alt, target_airspeed_cm, 
                                                  (control_mode==AUTO && takeoff_complete == false), 
                                                  takeoff_pitch_cd);
diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde
index beeecaa3ce..f3171d8265 100644
--- a/ArduPlane/Attitude.pde
+++ b/ArduPlane/Attitude.pde
@@ -38,7 +38,7 @@ static float get_speed_scaler(void)
  */
 static bool stick_mixing_enabled(void)
 {
-    if (control_mode == CIRCLE || control_mode > FLY_BY_WIRE_B) {
+    if (auto_throttle_mode) {
         // we're in an auto mode. Check the stick mixing flag
         if (g.stick_mixing != STICK_MIXING_DISABLED &&
             geofence_stickmixing() &&
@@ -490,7 +490,7 @@ static bool suppress_throttle(void)
         // we've previously met a condition for unsupressing the throttle
         return false;
     }
-    if (control_mode != CIRCLE && control_mode <= FLY_BY_WIRE_A) {
+    if (!auto_throttle_mode) {
         // the user controls the throttle
         throttle_suppressed = false;
         return false;
diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde
index 5dc27f6028..49c83b3514 100644
--- a/ArduPlane/system.pde
+++ b/ArduPlane/system.pde
@@ -379,7 +379,11 @@ static void set_mode(enum FlightMode mode)
     // if in an auto-throttle mode, start with throttle suppressed for
     // safety. suppress_throttle() will unsupress it when appropriate
     if (control_mode == CIRCLE || control_mode >= FLY_BY_WIRE_B) {
+        auto_throttle_mode = true;
         throttle_suppressed = true;
+    } else {
+        auto_throttle_mode = false;        
+        throttle_suppressed = false;
     }
 
     if (g.log_bitmask & MASK_LOG_MODE)