From 678fd8de1db7cf475b1c862a2f4b2744faea6403 Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Fri, 29 Nov 2019 15:31:50 -0300 Subject: [PATCH] AC_PosControl_Sub: do not reset accel_z integrator when relaxing Doing so caused the ROV to dive and slowly go back to the altitude setpoint --- libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp index ef0a9a125f..06924fbd19 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl_Sub.cpp @@ -100,7 +100,6 @@ void AC_PosControl_Sub::relax_alt_hold_controllers() _accel_desired.z = 0.0f; _accel_last_z_cms = 0.0f; _flags.reset_rate_to_accel_z = true; - _pid_accel_z.set_integrator(-_motors.get_throttle_hover() * 1000.0f); _accel_target.z = -(_ahrs.get_accel_ef_blended().z + GRAVITY_MSS) * 100.0f; _pid_accel_z.reset_filter(); }