From 3cbb3622568af95296714cf2ec15d28d92c1c64b Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Tue, 26 Sep 2023 20:14:53 +0300 Subject: [PATCH] feat(pid_longitudinal_controller): improve FF gain constant calculation Signed-off-by: Berkay Karaman --- .../src/pid_longitudinal_controller.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index fd2965ef799c6..a4f28c0545419 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -1008,7 +1008,8 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont const double current_vel = control_data.current_motion.vel; const double target_vel = control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps; - + const double nearest_target_vel = + control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps; const double vel_after_delay = control_data.state_after_delay.vel; const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && @@ -1037,7 +1038,9 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont constexpr double ff_scale_max = 2.0; // for safety constexpr double ff_scale_min = 0.5; // for safety const double ff_scale = std::clamp( - std::abs(current_vel) / std::max(std::abs(target_vel), 0.1), ff_scale_min, ff_scale_max); + 1.0 + (abs(nearest_target_vel - current_vel) / std::max(abs(current_vel), 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;