diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 078da4dff4675..38fbe161e8dbe 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -364,6 +364,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double applyVelocityFeedback( const Motion target_motion, const double dt, const double current_vel, const Shift & shift); + double applySlidingDownFeedback(const double current_vel, const ControlData & control_data) const; + /** * @brief update variables for debugging about pitch * @param [in] pitch current pitch of the vehicle (filtered) diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index f088a3a5cee56..b0645a5805b3e 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -666,8 +666,12 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target); raw_ctrl_cmd.vel = target_motion.vel; - raw_ctrl_cmd.acc = + + const double vel_feedback_acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target, control_data.shift); + const double sliding_down_feedback_acc = applySlidingDownFeedback(current_vel, control_data); + raw_ctrl_cmd.acc = vel_feedback_acc + sliding_down_feedback_acc; + RCLCPP_DEBUG( node_->get_logger(), "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " @@ -982,6 +986,15 @@ double PidLongitudinalController::applyVelocityFeedback( return feedback_acc; } +double PidLongitudinalController::applySlidingDownFeedback( + const double current_vel, const ControlData & control_data) const +{ + if (control_data.shift != Shift::Forward || 0.0 < current_vel) { + return 0.0; + } + return 0.5; +} + void PidLongitudinalController::updatePitchDebugValues( const double pitch, const double traj_pitch, const double raw_pitch) {