From 0e6a64fc146bcfa1f13cbab9653d9ba2ed7e7837 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 5 May 2013 22:03:13 +1000 Subject: [PATCH] Plane: reset roll/pitch integrators for APM_Control in takeoff when airspeed is below airspeed minimum. This is consistent with the previous controllers Pair-Programmed-With: Paul Riseborough --- ArduPlane/ArduPlane.pde | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 1eb285d43a..00d041b231 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -1048,15 +1048,15 @@ static void update_current_flight_mode(void) nav_pitch_cd = constrain_int32(nav_pitch_cd, 500, takeoff_pitch_cd); } - if (g.att_controller == ATT_CONTROL_PID) { - 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.pidServoPitch.reset_I(); - g.pidServoRoll.reset_I(); - } + 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.pidServoPitch.reset_I(); + g.pidServoRoll.reset_I(); + g.pitchController.reset_I(); + g.rollController.reset_I(); } }