From 1e0cd266afa6c02302fbc544eda39f407565e99e Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Wed, 10 Apr 2024 19:24:03 -0700
Subject: [PATCH] Toyota stop and go hack

Added toggle to force stop and go for Toyota vehicles.
---
 common/params.cc                                      | 1 +
 selfdrive/car/toyota/carcontroller.py                 | 2 +-
 selfdrive/controls/controlsd.py                       | 2 ++
 selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc | 5 +++--
 selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h  | 2 +-
 5 files changed, 8 insertions(+), 4 deletions(-)

diff --git a/common/params.cc b/common/params.cc
index 281333cbb..d39eb55bd 100644
--- a/common/params.cc
+++ b/common/params.cc
@@ -415,6 +415,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"SmoothBraking", PERSISTENT},
     {"SmoothBrakingFarLead", PERSISTENT},
     {"SmoothBrakingJerk", PERSISTENT},
+    {"SNGHack", PERSISTENT},
     {"SpeedLimitController", PERSISTENT},
     {"SpeedLimitChangedAlert", PERSISTENT},
     {"StandardFollow", PERSISTENT},
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 715fda41e..fc65879d8 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -212,7 +212,7 @@ class CarController(CarControllerBase):
       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 frogpilot_variables.sng_hack:
       self.standstill_req = True
     if CS.pcm_acc_status != 8:
       # pcm entered standstill or it's disabled
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 2873f95c2..957c84eb4 100644
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -1146,6 +1146,8 @@ class Controls:
     self.frogpilot_variables.reverse_cruise_increase = quality_of_life and self.params.get_bool("ReverseCruise")
     self.frogpilot_variables.set_speed_offset = self.params.get_int("SetSpeedOffset") * (1 if self.is_metric else CV.MPH_TO_KPH) if quality_of_life else 0
 
+    self.frogpilot_variables.sng_hack = self.params.get_bool("SNGHack")
+
     self.speed_limit_controller = self.CP.openpilotLongitudinalControl and self.params.get_bool("SpeedLimitController")
     self.frogpilot_variables.force_mph_dashboard = self.speed_limit_controller and self.params.get_bool("ForceMPHDashboard")
     self.frogpilot_variables.set_speed_limit = self.speed_limit_controller and self.params.get_bool("SetSpeedLimit")
diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc
index f630b9583..635d41291 100644
--- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc
+++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.cc
@@ -119,6 +119,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil
 
     {"ToyotaDoors", tr("Automatically Lock/Unlock Doors"), tr("Automatically lock the doors when in drive and unlock when in park."), ""},
     {"LongitudinalTune", tr("Longitudinal Tune"), tr("Use a custom Toyota longitudinal tune.\n\nCydia = More focused on TSS-P vehicles but works for all Toyotas\n\nDragonPilot = Focused on TSS2 vehicles\n\nFrogPilot = Takes the best of both worlds with some personal tweaks focused around FrogsGoMoo's 2019 Lexus ES 350"), ""},
+    {"SNGHack", tr("Stop and Go Hack"), tr("Enable the 'Stop and Go' hack for vehicles without stock stop and go functionality."), ""},
   };
 
   for (const auto &[param, title, desc, icon] : vehicleToggles) {
@@ -259,8 +260,8 @@ void FrogPilotVehiclesPanel::hideToggles() {
   std::set<QString> evCarKeys = {};
   std::set<QString> gmTruckKeys = {"GasRegenCmd"};
   std::set<QString> imprezaKeys = {"CrosstrekTorque"};
-  std::set<QString> longitudinalKeys = {"GasRegenCmd", "LongitudinalTune", "LongPitch"};
-  std::set<QString> sngKeys = {};
+  std::set<QString> longitudinalKeys = {"GasRegenCmd", "LongitudinalTune", "LongPitch", "SNGHack"};
+  std::set<QString> sngKeys = {"SNGHack"};
 
   for (auto &[key, toggle] : toggles) {
     if (toggle) {
diff --git a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h
index a7eaeb9e7..a6ee0f27b 100644
--- a/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h
+++ b/selfdrive/frogpilot/ui/qt/offroad/vehicle_settings.h
@@ -31,7 +31,7 @@ private:
 
   std::set<QString> gmKeys = {"GasRegenCmd", "LongPitch"};
   std::set<QString> subaruKeys = {"CrosstrekTorque"};
-  std::set<QString> toyotaKeys = {"LongitudinalTune", "ToyotaDoors"};
+  std::set<QString> toyotaKeys = {"LongitudinalTune", "SNGHack", "ToyotaDoors"};
 
   std::map<std::string, ParamControl*> toggles;