diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 45a6db2e90..6ec3762a94 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import math import numpy as np -from common.numpy_fast import interp +from common.numpy_fast import clip, interp import cereal.messaging as messaging from common.conversions import Conversions as CV @@ -106,15 +106,17 @@ class LongitudinalPlanner: # No change cost when user is controlling the speed, or when standstill prev_accel_constraint = not (reset_state or sm['carState'].standstill) + accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] + accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) + if reset_state: self.v_desired_filter.x = v_ego - self.a_desired = 0.0 + # Clip aEgo to cruise limits to prevent large accelerations when becoming active + self.a_desired = clip(sm['carState'].aEgo, accel_limits[0], accel_limits[1]) # Prevent divergence, smooth in current v_ego self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego)) - accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] - accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP) if force_slow_decel: # if required so, force a smooth deceleration accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 7485942244..1f459b1948 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -5d33199905cbf9d9b45ef722a40530b08d5cecf4 \ No newline at end of file +53079010a5db8105854212157b5ee90029df7b92 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 28134385d5..38ed0e07ad 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -18,7 +18,7 @@ from tools.lib.logreader import LogReader source_segments = [ ("BODY", "937ccb7243511b65|2022-05-24--16-03-09--1"), # COMMA.BODY ("HYUNDAI", "02c45f73a2e5c6e9|2021-01-01--19-08-22--1"), # HYUNDAI.SONATA - ("HYUNDAI2", "d824e27e8c60172c|2022-09-13--11-26-50--2"), # HYUNDAI.KIA_EV6 + ("HYUNDAI2", "d824e27e8c60172c|2022-09-13--11-26-50--2"), # HYUNDAI.KIA_EV6 ("TOYOTA", "0982d79ebb0de295|2021-01-04--17-13-21--13"), # TOYOTA.PRIUS (INDI) ("TOYOTA2", "0982d79ebb0de295|2021-01-03--20-03-36--6"), # TOYOTA.RAV4 (LQR) ("TOYOTA3", "f7d7e3538cda1a2a|2021-08-16--08-55-34--6"), # TOYOTA.COROLLA_TSS2