From ebe2278f5d40d9c7f67360f674cbc0f78cc986cd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 7 Jul 2019 10:16:47 +1000 Subject: [PATCH] Plane: reset rate controller integrators if not run for 2s this prevents a bug where integrator buildup can happen from a previous flight mode --- ArduPlane/Attitude.cpp | 12 +++++++++++- ArduPlane/Plane.h | 3 +++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index d5fd998c64..9cbd2a86bc 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -370,7 +370,17 @@ void Plane::stabilize() nav_pitch_cd = constrain_float((quadplane.tailsitter.transition_angle+5)*100, 5500, 8500), nav_roll_cd = 0; } - + + uint32_t now = AP_HAL::millis(); + if (now - last_stabilize_ms > 2000) { + // if we haven't run the rate controllers for 2 seconds then + // reset the integrators + rollController.reset_I(); + pitchController.reset_I(); + yawController.reset_I(); + } + last_stabilize_ms = now; + if (control_mode == &mode_training) { stabilize_training(speed_scaler); } else if (control_mode == &mode_acro) { diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 73c467d77d..3ca8b72ecf 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -349,6 +349,9 @@ private: // This is used to enable the inverted flight feature bool inverted_flight; + // last time we ran roll/pitch stabilization + uint32_t last_stabilize_ms; + // Failsafe struct { // Used to track if the value on channel 3 (throtttle) has fallen below the failsafe threshold