Plane: zero attitude integrators on the ground

ArduPlane/ArduPlane.pde

@@ -1045,16 +1045,6 @@ static void update_current_flight_mode(void)
                 nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, takeoff_pitch_cd);
             }
 
-            float aspeed;
-            if (ahrs.airspeed_estimate(&aspeed)) {
-                // don't use a pitch/roll integrators during takeoff if we are
-                // below minimum speed
-                if (aspeed < g.flybywire_airspeed_min) {
-                    g.pitchController.reset_I();
-                    g.rollController.reset_I();
-                }
-            }
-
             // max throttle for takeoff
             g.channel_throttle.servo_out = g.throttle_max;
 

ArduPlane/Attitude.pde

@@ -245,6 +245,21 @@ static void stabilize()
         }
         stabilize_yaw(speed_scaler);
     }
+
+    /*
+      see if we should zero the attitude controller integrators. 
+     */
+    if (g.channel_throttle.control_in == 0 &&
+        abs(current_loc.alt - home.alt) < 500 && 
+        fabs(barometer.get_climb_rate()) < 0.5f &&
+        g_gps->ground_speed < 300) {
+        // we are low, with no climb rate, and zero throttle, and very
+        // low ground speed. Zero the attitude controller
+        // integrators. This prevents integrator buildup pre-takeoff.
+        g.rollController.reset_I();
+        g.pitchController.reset_I();
+        g.yawController.reset_I();
+    }
 }