diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index f81cd0c40c..e74be7199e 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -13,6 +13,7 @@ V_CRUISE_MIN = 8 # kph V_CRUISE_ENABLE_MIN = 40 # kph V_CRUISE_INITIAL = 255 # kph +MIN_SPEED = 1.0 LAT_MPC_N = 16 LON_MPC_N = 32 CONTROL_N = 17 @@ -102,7 +103,7 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, curvature_rates): psis = [0.0]*CONTROL_N curvatures = [0.0]*CONTROL_N curvature_rates = [0.0]*CONTROL_N - v_ego = max(v_ego, 0.1) + v_ego = max(MIN_SPEED, v_ego) # TODO this needs more thought, use .2s extra for now to estimate other delays delay = CP.steerActuatorDelay + .2 diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 29137defd3..932ad49535 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -4,7 +4,7 @@ from common.numpy_fast import interp from system.swaglog import cloudlog from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N -from selfdrive.controls.lib.drive_helpers import CONTROL_N +from selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED from selfdrive.controls.lib.desire_helper import DesireHelper import cereal.messaging as messaging from cereal import log @@ -22,7 +22,6 @@ LATERAL_JERK_COST = 0.05 # TODO this cost should be lowered when low # speed lateral control is stable on all cars STEERING_RATE_COST = 800.0 -MIN_SPEED = 1.5 class LateralPlanner: diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 84658effee..230e81d949 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -1f41cd8bbf2431ae89c489a81698120d14a44145 \ No newline at end of file +f5a352666728344ca5065bb44c47c1f5650b4243