Toyota: Stop and Go hack implementation (commaai#290)

CHANGELOGS.md

@@ -11,6 +11,9 @@ sunnypilot - 0.9.5.1 (2023-09-27)
   * GAP - Driving Personality
   * DLP - Dynamic Lane Profile
   * SLC - Speed Limit Control
+* NEW❗: Toyota - Stop and Go hack
+  * Allow some Toyota/Lexus cars to auto resume during stop and go traffic
+  * Only applicable to certain models and model years
 * NEW❗: Wide camera view when the steering angle is more than 45°
 * NEW❗: Toyota: ZSS support thanks to dragonpilot-community and ErichMoraga!
 * openpilot Longitudianl Control capable cars only

common/params.cc

@@ -292,6 +292,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"TorqueDeadzoneDeg", PERSISTENT},
     {"TorqueFriction", PERSISTENT},
     {"TorqueMaxLatAccel", PERSISTENT},
+    {"ToyotaSnG", PERSISTENT},
     {"ToyotaTSS2Long", PERSISTENT},
     {"TrueVEgoUi", PERSISTENT},
     {"TurnSpeedControl", PERSISTENT},

selfdrive/car/toyota/carcontroller.py

@@ -44,6 +44,7 @@ def __init__(self, dbc_name, CP, VM):
 
     self.param_s = Params()
     self._reverse_acc_change = self.param_s.get_bool("ReverseAccChange")
+    self._sng_hack = self.param_s.get_bool("ToyotaSnG")
 
   def update(self, CC, CS, now_nanos):
     actuators = CC.actuators
@@ -119,7 +120,8 @@ def update(self, CC, CS, now_nanos):
       pcm_cancel_cmd = 1
 
     # on entering standstill, send standstill request
-    if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor):
+    if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor) and \
+      not self._sng_hack:
       self.standstill_req = True
     if CS.pcm_acc_status != 8:
       # pcm entered standstill or it's disabled

selfdrive/ui/qt/offroad/sunnypilot_settings.cc

@@ -592,10 +592,20 @@ SPVehiclesTogglesPanel::SPVehiclesTogglesPanel(SPVehiclesPanel *parent) : ListWi
   toyotaTss2LongTune->setConfirmation(true, false);
   addItem(toyotaTss2LongTune);
 
+  auto toyotaSngHack = new ParamControl(
+    "ToyotaSnG",
+    "Enable Stop and Go Hack",
+    "sunnypilot will allow some Toyota/Lexus cars to auto resume during stop and go traffic. This feature is only applicable to certain models. Use at your own risk.",
+    "../assets/offroad/icon_blank.png"
+  );
+  toyotaSngHack->setConfirmation(true, false);
+  addItem(toyotaSngHack);
+
   // trigger offroadTransition when going onroad/offroad
   connect(uiState(), &UIState::offroadTransition, [=](bool offroad) {
     hkgSmoothStop->setEnabled(offroad);
     toyotaTss2LongTune->setEnabled(offroad);
+    toyotaSngHack->setEnabled(offroad);
   });
 }