feat(pid_longitudinal_controller): improve FF gain constant calculation

Signed-off-by: Berkay Karaman <[email protected]>
brkay54 · Nov 10, 2023 · c8569db · c8569db
1 parent c12fe31
commit c8569db
Showing 1 changed file with 5 additions and 2 deletions.
diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -1011,7 +1011,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 &&
@@ -1040,7 +1041,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;