diff --git a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp index dc8f2ae136..85290a2299 100644 --- a/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp +++ b/src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp @@ -67,9 +67,12 @@ void AckermannPosControl::updatePosControl() if (distance_to_target > _acceptance_radius || _arrival_speed > FLT_EPSILON) { - float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), - _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)); - speed_setpoint = math::min(speed_setpoint, _cruising_speed); + float speed_setpoint{_cruising_speed}; + + if (_param_ro_decel_limit.get() > FLT_EPSILON && _param_ro_jerk_limit.get() > FLT_EPSILON) { + speed_setpoint = math::min(math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)), _cruising_speed); + } pure_pursuit_status_s pure_pursuit_status{}; pure_pursuit_status.timestamp = timestamp; diff --git a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp index d7807c2e8d..125e8c1c89 100644 --- a/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp +++ b/src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp @@ -65,9 +65,12 @@ void DifferentialPosControl::updatePosControl() } if (distance_to_target > _param_nav_acc_rad.get() || _arrival_speed > FLT_EPSILON) { - float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), - _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)); - speed_setpoint = math::min(speed_setpoint, _cruising_speed); + float speed_setpoint{_cruising_speed}; + + if (_param_ro_decel_limit.get() > FLT_EPSILON && _param_ro_jerk_limit.get() > FLT_EPSILON) { + speed_setpoint = math::min(math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)), _cruising_speed); + } pure_pursuit_status_s pure_pursuit_status{}; pure_pursuit_status.timestamp = timestamp; diff --git a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp index 969bb28b30..68c31d141e 100644 --- a/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp +++ b/src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp @@ -66,9 +66,12 @@ void MecanumPosControl::updatePosControl() if (distance_to_target > _param_nav_acc_rad.get() || _arrival_speed > FLT_EPSILON) { - float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), - _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)); - speed_setpoint = math::min(speed_setpoint, _cruising_speed); + float speed_setpoint{_cruising_speed}; + + if (_param_ro_decel_limit.get() > FLT_EPSILON && _param_ro_jerk_limit.get() > FLT_EPSILON) { + speed_setpoint = math::min(math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), + _param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed)), _cruising_speed); + } pure_pursuit_status_s pure_pursuit_status{}; pure_pursuit_status.timestamp = timestamp;