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

@@ -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;