diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 96d219f1a0..d7128674dd 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -92,6 +92,31 @@ class CarController(CarControllerBase): self.doors_locked = False + def _update_standstill_request(self, CC, CS, actuators, starpilot_toggles): + # Older TSS-P platforms need a standstill latch pulse, then an explicit release to move again. + if self.CP.carFingerprint not in NO_STOP_TIMER_CAR: + if CS.out.standstill and not self.last_standstill and not starpilot_toggles.sng_hack: + self.standstill_req = True + + if CS.pcm_acc_status != 8 or CC.cruiseControl.resume or starpilot_toggles.sng_hack: + # Clear once the PCM has latched the stop, when planner wants to move, + # or when the SNG hack is forcing the latch open. + self.standstill_req = False + + else: + # if user engages at a stop with foot on brake, PCM starts in a special cruise standstill mode. on resume press, + # brakes can take a while to ramp up causing a lurch forward. prevent resume press until planner wants to move. + # don't use CC.cruiseControl.resume since it is gated on CS.cruiseState.standstill which goes false for 3s after resume press + # TODO: hybrids do not have this issue and can stay stopped after resume press, whitelist them + should_resume = actuators.accel > 0 or starpilot_toggles.sng_hack + if should_resume: + self.standstill_req = False + + if not should_resume and CS.out.cruiseState.standstill: + self.standstill_req = True + + self.last_standstill = CS.out.standstill + def update(self, CC, CS, now_nanos, starpilot_toggles): actuators = CC.actuators stopping = actuators.longControlState == LongCtrlState.stopping @@ -184,27 +209,7 @@ class CarController(CarControllerBase): # *** gas and brake *** - # on entering standstill, send standstill request for older TSS-P cars that aren't designed to stay engaged at a stop - if self.CP.carFingerprint not in NO_STOP_TIMER_CAR: - if CS.out.standstill and not self.last_standstill and not starpilot_toggles.sng_hack: - self.standstill_req = True - if CS.pcm_acc_status != 8: - # pcm entered standstill or it's disabled - self.standstill_req = False - - else: - # if user engages at a stop with foot on brake, PCM starts in a special cruise standstill mode. on resume press, - # brakes can take a while to ramp up causing a lurch forward. prevent resume press until planner wants to move. - # don't use CC.cruiseControl.resume since it is gated on CS.cruiseState.standstill which goes false for 3s after resume press - # TODO: hybrids do not have this issue and can stay stopped after resume press, whitelist them - should_resume = actuators.accel > 0 or starpilot_toggles.sng_hack - if should_resume: - self.standstill_req = False - - if not should_resume and CS.out.cruiseState.standstill: - self.standstill_req = True - - self.last_standstill = CS.out.standstill + self._update_standstill_request(CC, CS, actuators, starpilot_toggles) # handle UI messages fcw_alert = hud_control.visualAlert == VisualAlert.fcw diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index 5ab47f3565..14db5a13eb 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -1,8 +1,11 @@ +from types import SimpleNamespace + from hypothesis import given, settings, strategies as st from opendbc.car import Bus from opendbc.car.structs import CarParams from opendbc.car.fw_versions import build_fw_dict +from opendbc.car.toyota.carcontroller import CarController from opendbc.car.toyota.fingerprints import FW_VERSIONS 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, \ @@ -165,3 +168,67 @@ class TestToyotaFingerprint: platforms_with_shared_codes |= {str(platform), *matches} assert platforms_with_shared_codes == FUZZY_EXCLUDED_PLATFORMS, (len(platforms_with_shared_codes), len(FW_VERSIONS)) + + +class TestToyotaCarController: + @staticmethod + def _make_controller(*, standstill_req=False, last_standstill=False): + controller = CarController.__new__(CarController) + controller.CP = SimpleNamespace(carFingerprint=CAR.TOYOTA_PRIUS) + controller.standstill_req = standstill_req + controller.last_standstill = last_standstill + return controller + + @staticmethod + def _make_cc(*, resume=False): + return SimpleNamespace(cruiseControl=SimpleNamespace(resume=resume)) + + @staticmethod + def _make_cs(*, standstill=True, cruise_standstill=True, pcm_acc_status=8): + return SimpleNamespace( + out=SimpleNamespace( + standstill=standstill, + cruiseState=SimpleNamespace(standstill=cruise_standstill), + ), + pcm_acc_status=pcm_acc_status, + ) + + @staticmethod + def _make_toggles(*, sng_hack=False): + return SimpleNamespace(sng_hack=sng_hack) + + def test_prius_standstill_request_latches_on_entry(self): + controller = self._make_controller() + + controller._update_standstill_request( + self._make_cc(), + self._make_cs(), + SimpleNamespace(accel=0.0), + self._make_toggles(), + ) + + assert controller.standstill_req is True + + def test_prius_resume_request_releases_standstill_latch(self): + controller = self._make_controller(standstill_req=True, last_standstill=True) + + controller._update_standstill_request( + self._make_cc(resume=True), + self._make_cs(), + SimpleNamespace(accel=0.0), + self._make_toggles(), + ) + + assert controller.standstill_req is False + + def test_sng_hack_clears_existing_standstill_latch(self): + controller = self._make_controller(standstill_req=True, last_standstill=True) + + controller._update_standstill_request( + self._make_cc(), + self._make_cs(), + SimpleNamespace(accel=0.0), + self._make_toggles(sng_hack=True), + ) + + assert controller.standstill_req is False