diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index f30ddb601..0495aabeb 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -127,15 +127,19 @@ def limit_interceptor_pcm_accel(pcm_accel_cmd: float, target_accel: float, stopp return limited -def limit_interceptor_stopping_accel(pcm_accel_cmd: float, stopping: bool, v_ego: float, lead_visible: bool) -> float: - if not stopping or lead_visible or pcm_accel_cmd >= 0.0 or v_ego >= 1.5: +def limit_interceptor_stopping_accel(pcm_accel_cmd: float, target_accel: float, stopping: bool, v_ego: float, lead_visible: bool) -> float: + if not stopping or lead_visible or pcm_accel_cmd >= 0.0 or v_ego >= 2.5: return pcm_accel_cmd # Pedal/SDSU Toyotas can feel abrupt in the last few feet of a no-lead stop - # because stopping state holds onto a relatively strong negative accel. Keep - # real lead stops untouched, but soften the final crawl into standstill. - stop_floor = float(np.interp(v_ego, [0.0, 0.2, 0.5, 0.9, 1.5], [-0.90, -0.95, -1.05, -1.15, -1.30])) - return max(pcm_accel_cmd, stop_floor) + # because stopping state can hold onto a stale strong negative command even + # after the planner target has already softened. Keep real lead stops + # untouched, but let the last 2-3 mph of a no-lead stop unwind toward the + # current planner target instead of shoving through zero. + stop_floor = float(np.interp(v_ego, [0.0, 0.2, 0.5, 0.9, 1.5, 2.5], [-0.72, -0.76, -0.82, -0.92, -1.05, -1.20])) + target_buffer = float(np.interp(v_ego, [0.0, 0.5, 1.5, 2.5], [0.08, 0.10, 0.15, 0.20])) + planner_floor = float(target_accel) - target_buffer + return max(pcm_accel_cmd, max(stop_floor, planner_floor)) class CarController(CarControllerBase): @@ -405,7 +409,7 @@ class CarController(CarControllerBase): if self.CP.enableGasInterceptorDEPRECATED: pcm_accel_cmd = limit_interceptor_pcm_accel(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo) - pcm_accel_cmd = limit_interceptor_stopping_accel(pcm_accel_cmd, stopping, CS.out.vEgo, bool(hud_control.leadVisible)) + pcm_accel_cmd = limit_interceptor_stopping_accel(pcm_accel_cmd, actuators.accel, stopping, CS.out.vEgo, bool(hud_control.leadVisible)) pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)) diff --git a/opendbc_repo/opendbc/car/toyota/interface.py b/opendbc_repo/opendbc/car/toyota/interface.py index 423128b77..d7da4c159 100644 --- a/opendbc_repo/opendbc/car/toyota/interface.py +++ b/opendbc_repo/opendbc/car/toyota/interface.py @@ -158,6 +158,7 @@ class CarInterface(CarInterfaceBase): if ret.enableGasInterceptorDEPRECATED: # Pedal/SDSU Toyotas feel best with a softer final stop clamp. + ret.longitudinalActuatorDelay = max(ret.longitudinalActuatorDelay, 0.2) ret.stopAccel = -1.5 return ret diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index b249e01b8..dbc0bcc6b 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -10,6 +10,7 @@ from opendbc.car.toyota import toyotacan from opendbc.car.toyota.carcontroller import CarController, limit_interceptor_pcm_accel, limit_interceptor_stopping_accel, update_permit_braking from opendbc.car.toyota.carstate import calculate_interceptor_gas_pressed from opendbc.car.toyota.fingerprints import FW_VERSIONS +from opendbc.car.toyota.interface import CarInterface from opendbc.car.toyota.values import CAR, DBC, TSS2_CAR, ANGLE_CONTROL_CAR, RADAR_ACC_CAR, SECOC_CAR, \ FW_QUERY_CONFIG, PLATFORM_CODE_ECUS, FUZZY_EXCLUDED_PLATFORMS, \ get_platform_codes @@ -353,21 +354,47 @@ class TestToyotaCarController: assert limited <= 0.0 def test_interceptor_stopping_limit_softens_no_lead_final_crawl(self): - limited = limit_interceptor_stopping_accel(-1.48, True, 0.5, False) + limited = limit_interceptor_stopping_accel(-1.48, -1.48, True, 0.5, False) assert limited > -1.48 - assert limited == -1.05 + assert limited == -0.82 + + def test_interceptor_stopping_limit_tracks_softer_target_near_standstill(self): + limited = limit_interceptor_stopping_accel(-1.86, -0.63, True, 0.5, False) + + assert limited > -1.0 + assert limited == -0.73 def test_interceptor_stopping_limit_keeps_visible_lead_stop_untouched(self): - limited = limit_interceptor_stopping_accel(-1.48, True, 0.5, True) + limited = limit_interceptor_stopping_accel(-1.48, -0.63, True, 0.5, True) assert limited == -1.48 def test_interceptor_stopping_limit_keeps_higher_speed_stop_untouched(self): - limited = limit_interceptor_stopping_accel(-1.48, True, 2.0, False) + limited = limit_interceptor_stopping_accel(-1.48, -0.63, True, 3.0, False) assert limited == -1.48 + def test_interceptor_stopping_limit_softens_low_speed_no_lead_stop_before_final_crawl(self): + limited = limit_interceptor_stopping_accel(-1.55, -0.66, True, 1.45, False) + + assert abs(limited - (-0.8075)) < 1e-6 + + def test_avalon_pedal_params_raise_delay_and_soften_stop(self): + CP = CarInterface.get_params( + CAR.TOYOTA_AVALON_2019, + {0: {0x2FF: 8, 0x201: 8}}, + [], + True, + False, + False, + None, + ) + + assert CP.enableGasInterceptorDEPRECATED + assert abs(CP.longitudinalActuatorDelay - 0.2) < 1e-6 + assert CP.stopAccel == -1.5 + class TestToyotaCarState: def test_interceptor_gas_pressed_threshold(self):