ilk pra eklence

brkay54 · Sep 18, 2023 · 0986686 · 0986686
1 parent 409850c
commit 0986686
Showing 1 changed file with 3 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
@@ -973,10 +973,11 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont
     control_data.interpolated_traj.points.at(control_data.target_idx).longitudinal_velocity_mps);
   const double current_target_vel_abs = std::fabs(
     control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps);
+  const double vel_after_delay_abs = std::fabs(control_data.state_after_delay.vel);
   const bool is_under_control = m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
   const bool enable_integration =
-    (current_vel_abs > m_current_vel_threshold_pid_integrate) && is_under_control;
-  const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs);
+    (vel_after_delay_abs > m_current_vel_threshold_pid_integrate) && is_under_control;
+  const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - vel_after_delay_abs);
 
   std::vector<double> pid_contributions(3);
   const double pid_acc =