From 92bcd47f559939dbc15ccf0de40664e037d11275 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Mon, 18 Sep 2023 15:00:51 +0300 Subject: [PATCH] change ff constant calculation ds --- .../src/pid_longitudinal_controller.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 9b5ffbd403a22..bda36cae88165 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -971,6 +971,8 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont const double current_vel_abs = std::fabs(control_data.current_motion.vel); const double target_vel_abs = std::fabs( 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 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; @@ -987,8 +989,11 @@ 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 - abs(control_data.current_motion.vel)) / + 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;