feat(pid_longitudinal_controller): improve FF gain constant calculation

Signed-off-by: Berkay Karaman <[email protected]>

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;