diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index fcefd307607b6..2ffb6bc7309b0 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -972,6 +972,8 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont const double target_vel_abs = std::fabs( control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps); const double vel_after_delay_abs = std::fabs(control_data.state_after_delay.vel); + const double current_target_vel_abs = std::fabs( + control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps); const bool is_under_control = m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; const bool enable_integration = (vel_after_delay_abs > m_current_vel_threshold_pid_integrate) && is_under_control; @@ -988,8 +990,10 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont // deviation will be bigger. constexpr double ff_scale_max = 2.0; // for safety constexpr double ff_scale_min = 0.5; // for safety - const double ff_scale = - std::clamp(current_vel_abs / std::max(target_vel_abs, 0.1), ff_scale_min, ff_scale_max); + const double ff_scale = std::clamp( + 1.0 + (abs(current_target_vel_abs - current_vel_abs) / std::max(current_target_vel_abs, 0.1)), + ff_scale_min, ff_scale_max); + const double ff_acc = control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2 * ff_scale;