From c9efebac76309f9bdfc53a25fcdaa3ac9d6dd41f Mon Sep 17 00:00:00 2001 From: Daniel Sanchez Date: Fri, 10 Nov 2023 15:05:53 +0900 Subject: [PATCH] Take out velocity requirement to always use integrated error in PID Signed-off-by: Daniel Sanchez --- .../src/pid_longitudinal_controller.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index d13e628f2e1d4..64f3c164b14b7 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -954,13 +954,10 @@ double PidLongitudinalController::predictedVelocityInTargetPoint( double PidLongitudinalController::applyVelocityFeedback( const Motion target_motion, const double dt, const double current_vel, const Shift & shift) { - // NOTE: Acceleration command is always positive even if the ego drives backward. const double vel_sign = (shift == Shift::Forward) ? 1.0 : (shift == Shift::Reverse ? -1.0 : 0.0); const double diff_vel = (target_motion.vel - current_vel) * vel_sign; - const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && - m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; - const bool enable_integration = - (std::abs(current_vel) > m_current_vel_threshold_pid_integrate) && is_under_control; + const bool enable_integration = m_current_operation_mode.is_autoware_control_enabled && + m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; const double error_vel_filtered = m_lpf_vel_error->filter(diff_vel); std::vector pid_contributions(3);