diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index b4d345a1c7..f34e4da33e 100644 --- a/ArduPlane/ArduPlane.pde +++ b/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; diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index 3f4cdca4f6..42016f768f 100644 --- a/ArduPlane/Attitude.pde +++ b/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(); + } }