From 882713b26e2027e9b321e0173bc89d77dd1dbafe Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Fri, 12 Jan 2024 22:39:30 -0700
Subject: [PATCH] TSS2 Tune

Added toggle to customize the driving dynamics of TTS2 vehicles.

Credit goes to DragonPilot!

https: //github.com/dragonpilot-community/dragonpilot
Co-Authored-By: eFini <16603033+efinilan@users.noreply.github.com>
Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com>
---
 common/params.cc                           |  1 +
 selfdrive/car/toyota/interface.py          | 14 +++++++++++++-
 selfdrive/frogpilot/ui/vehicle_settings.cc |  3 ++-
 3 files changed, 16 insertions(+), 2 deletions(-)

diff --git a/common/params.cc b/common/params.cc
index b7ec39b..c1f3ac5 100644
--- a/common/params.cc
+++ b/common/params.cc
@@ -341,6 +341,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"StandardJerk", PERSISTENT},
     {"StoppingDistance", PERSISTENT},
     {"TetheringEnabled", PERSISTENT},
+    {"TSS2Tune", PERSISTENT},
     {"UnlimitedLength", PERSISTENT},
     {"Updated", PERSISTENT},
     {"UpdateSchedule", PERSISTENT},
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 8dad552..3797d16 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -263,7 +263,19 @@ class CarInterface(CarInterfaceBase):
     tune = ret.longitudinalTuning
     tune.deadzoneBP = [0., 9.]
     tune.deadzoneV = [.0, .15]
-    if candidate in TSS2_CAR or ret.enableGasInterceptor:
+    if (candidate in TSS2_CAR or ret.enableGasInterceptor) and Params().get_bool("TSS2Tune"):
+      # TSS2 tune - Credit goes to the DragonPilot team!
+      tune.kpBP = [0., 5., 20.]
+      tune.kpV = [1.3, 1.0, 0.7]
+      # In MPH  = [ 0.,   2,    5,   11,   27,  45,  52,  67,   90] ​​
+      tune.kiBP = [ 0.,  1.,   2.,   5.,  12., 20., 23., 30.,  40.]
+      tune.kiV =  [.33, .33, .313, .245, .215, .17, .10, .01, .001]
+      if candidate in TSS2_CAR:
+        ret.vEgoStopping = 0.1         # car is near 0.1 to 0.2 when car starts requesting stopping accel
+        ret.vEgoStarting = 0.1         # needs to be > or == vEgoStopping
+        ret.stopAccel = -0.40          # Toyota requests -0.4 when stopped
+        ret.stoppingDecelRate = 0.5    # reach stopping target smoothly
+    elif candidate in TSS2_CAR or ret.enableGasInterceptor:
       tune.kpBP = [0., 5., 20.]
       tune.kpV = [1.3, 1.0, 0.7]
       tune.kiBP = [0., 5., 12., 20., 27.]
diff --git a/selfdrive/frogpilot/ui/vehicle_settings.cc b/selfdrive/frogpilot/ui/vehicle_settings.cc
index 63e8c81..691e39c 100644
--- a/selfdrive/frogpilot/ui/vehicle_settings.cc
+++ b/selfdrive/frogpilot/ui/vehicle_settings.cc
@@ -105,6 +105,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil
 
     {"LockDoors", "Lock Doors In Drive", "Automatically lock the doors when in drive and unlock when in park.", ""},
     {"SNGHack", "Stop and Go Hack", "Enable the 'Stop and Go' hack for vehicles without stock stop and go functionality.", ""},
+    {"TSS2Tune", "TSS2 Tune", "Tuning profile based on the tuning profile from DragonPilot for TSS2 vehicles.", ""}
   };
 
   for (auto &[param, title, desc, icon] : vehicleToggles) {
@@ -122,7 +123,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil
   gmKeys = {"GasRegenCmd", "LongPitch"};
   toyotaKeys = {};
 
-  std::set<std::string> rebootKeys = {"LowerVolt"};
+  std::set<std::string> rebootKeys = {"LowerVolt", "TSS2Tune"};
   for (const std::string &key : rebootKeys) {
     QObject::connect(toggles[key], &ToggleControl::toggleFlipped, [this]() {
       if (FrogPilotConfirmationDialog::toggle("Reboot required to take effect.", "Reboot Now", this)) {