From 44fc5c1f30719a4b821cc8bc4d1cd055f4682a0b Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Tue, 27 Feb 2024 16:34:47 -0700
Subject: [PATCH] DragonPilot's Toyota tune

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 |  1 +
 3 files changed, 16 insertions(+)

diff --git a/common/params.cc b/common/params.cc
index ddaaf1c..c5dd932 100644
--- a/common/params.cc
+++ b/common/params.cc
@@ -248,6 +248,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"DisableOnroadUploads", PERSISTENT},
     {"DisableOpenpilotLongitudinal", PERSISTENT},
     {"DisengageVolume", PERSISTENT},
+    {"DragonPilotTune", PERSISTENT},
     {"DriveStats", PERSISTENT},
     {"DynamicPathWidth", PERSISTENT},
     {"EngageVolume", PERSISTENT},
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 7c27b9e..13c1eb6 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -282,6 +282,20 @@ class CarInterface(CarInterfaceBase):
       tune.kpV = [0.8, 1.]
       tune.kiBP = [0., 5.]
       tune.kiV = [0.3, 1.]
+    elif (candidate in TSS2_CAR or ret.enableGasInterceptor) and params.get_bool("DragonPilotTune"):
+      # Credit goes to the DragonPilot team!
+      tune.deadzoneBP = [0., 16., 20., 30.]
+      tune.deadzoneV =  [0., .03, .06, .15]
+      tune.kpBP = [0., 5., 20.]
+      tune.kpV = [1.3, 1.0, 0.7]
+      # In MPH  = [  0,   27,   45,  60,  89]
+      tune.kiBP = [ 0.,  12.,  20., 27., 40.]
+      tune.kiV =  [.35, .215, .195, .10, .01]
+      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]
diff --git a/selfdrive/frogpilot/ui/vehicle_settings.cc b/selfdrive/frogpilot/ui/vehicle_settings.cc
index 28c9081..3369c24 100644
--- a/selfdrive/frogpilot/ui/vehicle_settings.cc
+++ b/selfdrive/frogpilot/ui/vehicle_settings.cc
@@ -23,6 +23,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil
       std::vector<std::pair<QString, QString>> tuneOptions{
         {"StockTune", tr("Stock")},
         {"CydiaTune", tr("Cydia's")},
+        {"DragonPilotTune", tr("DragonPilot's")},
       };
       toggle = new FrogPilotButtonsParamControl(param, title, desc, icon, tuneOptions);