From beca704c8f5718a1e73c6a41ceae12d3946a04db Mon Sep 17 00:00:00 2001 From: Bill Geyer Date: Thu, 3 Feb 2022 11:32:50 -0500 Subject: [PATCH] Copter: reset integrators when landed in auto modes in any spool state --- ArduCopter/mode.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 565393e942..d7d38df4fe 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -464,16 +464,15 @@ void Mode::make_safe_ground_handling(bool force_throttle_unlimited) motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); } - + // aircraft is landed, integrator terms must be reset regardless of spool state + attitude_control->reset_rate_controller_I_terms_smoothly(); + switch (motors->get_spool_state()) { - case AP_Motors::SpoolState::SHUT_DOWN: case AP_Motors::SpoolState::GROUND_IDLE: - // relax controllers during idle states - attitude_control->reset_rate_controller_I_terms_smoothly(); + // reset yaw targets and rates during idle states attitude_control->reset_yaw_target_and_rate(); break; - case AP_Motors::SpoolState::SPOOLING_UP: case AP_Motors::SpoolState::THROTTLE_UNLIMITED: case AP_Motors::SpoolState::SPOOLING_DOWN: