From 85b5223be14db267e955e8f6523686db3a693283 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 7 Jul 2019 11:22:52 +1000 Subject: [PATCH] Plane: decay fw rate integrators when at low airspeed this prevents large fixed wing control surface integrators from causing issues when we have wind from behind in hover --- ArduPlane/Attitude.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 997f64f595..0ffb42b778 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -28,6 +28,12 @@ float Plane::get_speed_scaler(void) if (aspeed < threshold) { float new_scaler = linear_interpolate(0, g.scaling_speed / threshold, aspeed, 0, threshold); speed_scaler = MIN(speed_scaler, new_scaler); + + // we also decay the integrator to prevent an integrator from before + // we were at low speed persistint at high speed + rollController.decay_I(); + pitchController.decay_I(); + yawController.decay_I(); } } } else {