diff --git a/cereal b/cereal index 49a0ee919..5c3520d53 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 49a0ee9196ce013d422febdf61847d87f1f57627 +Subproject commit 5c3520d53fd86cb07ff5151e8951d9db3e2d750e diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 36afc225e..0f1166890 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -44,7 +44,7 @@ class CarInterface(CarInterfaceBase): ret.stoppingDecelRate = 2.0 ret.startAccel = 0.0 - ret.longitudinalActuatorDelay = 1.0 # s + ret.longitudinalActuatorDelayUpperBound = 1.0 # s if candidate == CAR.SANTA_FE: ret.lateralTuning.pid.kf = 0.00005 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index d438e98ee..c58576959 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -86,7 +86,8 @@ class CarInterfaceBase(): ret.longitudinalTuning.kpV = [1.] ret.longitudinalTuning.kiBP = [0.] ret.longitudinalTuning.kiV = [1.] - ret.longitudinalActuatorDelay = 0.15 + ret.longitudinalActuatorDelayLowerBound = 0.15 + ret.longitudinalActuatorDelayUpperBound = 0.15 return ret # returns a car.CarState, pass in car.CarControl diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 6f5571073..d44196e73 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -70,9 +70,16 @@ class LongControl(): # Interp control trajectory # TODO estimate car specific lag, use .15s for now if len(long_plan.speeds) == CONTROL_N: - v_target = interp(CP.longitudinalActuatorDelay, T_IDXS[:CONTROL_N], long_plan.speeds) + v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound, T_IDXS[:CONTROL_N], long_plan.speeds) + a_target_lower = 2 * (v_target_lower - long_plan.speeds[0])/CP.longitudinalActuatorDelayLowerBound - long_plan.accels[0] + + v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], long_plan.speeds) + a_target_upper = 2 * (v_target_upper - long_plan.speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0] + + v_target = min(v_target_lower, v_target_upper) + a_target = min(a_target_lower, a_target_upper) + v_target_future = long_plan.speeds[-1] - a_target = 2 * (v_target - long_plan.speeds[0])/CP.longitudinalActuatorDelay - long_plan.accels[0] else: v_target = 0.0 v_target_future = 0.0 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index d301d1fcc..624d5c965 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -eb150cfb34f825d0db2dc30c1077357b0a66952e \ No newline at end of file +c967d67902bd36e82c8b9e70c5538475b448ac49 \ No newline at end of file