Skip to content

Commit 99e3e09

Browse files
committed
feat(pid_longitudinal_controller): improve FF gain constant calculation
Signed-off-by: Berkay Karaman <brkay54@gmail.com>
1 parent 6b1a459 commit 99e3e09

File tree

1 file changed

+5
-1
lines changed

1 file changed

+5
-1
lines changed

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

+5-1
Original file line numberDiff line numberDiff line change
@@ -1053,6 +1053,8 @@ PidLongitudinalController::StateAfterDelay PidLongitudinalController::predictedS
10531053
double PidLongitudinalController::applyVelocityFeedback(const ControlData & control_data)
10541054
{
10551055
// NOTE: Acceleration command is always positive even if the ego drives backward.
1056+
const double nearest_target_vel =
1057+
control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps;
10561058
const double vel_sign = (control_data.shift == Shift::Forward)
10571059
? 1.0
10581060
: (control_data.shift == Shift::Reverse ? -1.0 : 0.0);
@@ -1087,7 +1089,9 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont
10871089
constexpr double ff_scale_max = 2.0; // for safety
10881090
constexpr double ff_scale_min = 0.5; // for safety
10891091
const double ff_scale = std::clamp(
1090-
std::abs(current_vel) / std::max(std::abs(target_motion.vel), 0.1), ff_scale_min, ff_scale_max);
1092+
1.0 + (abs(nearest_target_vel - current_vel) / std::max(abs(current_vel), 0.1)), ff_scale_min,
1093+
ff_scale_max);
1094+
10911095
const double ff_acc =
10921096
control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2 * ff_scale;
10931097

0 commit comments

Comments
 (0)