From dcb3c9865c5bd58e667f971cb8d938fd3b4a22e3 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Tue, 5 Dec 2023 15:50:16 +0300 Subject: [PATCH] feat(pid_longitudinal_controller): improve FF gain constant calculation Signed-off-by: Berkay Karaman --- .../src/pid_longitudinal_controller.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 2ba7742aad4a0..42546970507e7 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -1038,6 +1038,8 @@ PidLongitudinalController::StateAfterDelay PidLongitudinalController::predictedS double PidLongitudinalController::applyVelocityFeedback(const ControlData & control_data) { // NOTE: Acceleration command is always positive even if the ego drives backward. + const double nearest_target_vel = + control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps; const double vel_sign = (control_data.shift == Shift::Forward) ? 1.0 : (control_data.shift == Shift::Reverse ? -1.0 : 0.0); @@ -1072,7 +1074,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_motion.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;