toyota sng

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

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