From 94e8d564459517eb3192cd5d3214980001af10dd 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] Subaru Crosstrek Torque increase

Add toggle to increase the torque for the Subaru Crosstrek.
---
 common/params.cc                           | 1 +
 panda/board/safety/safety_subaru.h         | 2 +-
 selfdrive/car/subaru/interface.py          | 6 ++++--
 selfdrive/car/subaru/values.py             | 2 ++
 selfdrive/frogpilot/ui/vehicle_settings.cc | 4 +++-
 selfdrive/frogpilot/ui/vehicle_settings.h  | 2 +-
 6 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/common/params.cc b/common/params.cc
index 1680460..383d3ef 100644
--- a/common/params.cc
+++ b/common/params.cc
@@ -243,6 +243,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CEStopLightsLead", PERSISTENT},
     {"Compass", PERSISTENT},
     {"ConditionalExperimental", PERSISTENT},
+    {"CrosstrekTorque", PERSISTENT},
     {"CurrentHolidayTheme", PERSISTENT},
     {"CustomAlerts", PERSISTENT},
     {"CustomColors", PERSISTENT},
diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h
index 924b767..7c887a0 100644
--- a/panda/board/safety/safety_subaru.h
+++ b/panda/board/safety/safety_subaru.h
@@ -17,7 +17,7 @@
   }
 
 
-const SteeringLimits SUBARU_STEERING_LIMITS      = SUBARU_STEERING_LIMITS_GENERATOR(2047, 50, 70);
+const SteeringLimits SUBARU_STEERING_LIMITS      = SUBARU_STEERING_LIMITS_GENERATOR(3071, 50, 70);
 const SteeringLimits SUBARU_GEN2_STEERING_LIMITS = SUBARU_STEERING_LIMITS_GENERATOR(1000, 40, 40);
 
 
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
index e1113fc..e1c53ab 100644
--- a/selfdrive/car/subaru/interface.py
+++ b/selfdrive/car/subaru/interface.py
@@ -10,6 +10,8 @@ class CarInterface(CarInterfaceBase):
 
   @staticmethod
   def _get_params(ret, params, candidate, fingerprint, car_fw, experimental_long, docs):
+    crosstrek_torque_increase = params.get_bool("CrosstrekTorque")
+
     ret.carName = "subaru"
     ret.radarUnavailable = True
     # for HYBRID CARS to be upstreamed, we need:
@@ -47,9 +49,9 @@ class CarInterface(CarInterfaceBase):
       ret.steerRatio = 13.5
       ret.steerActuatorDelay = 0.3   # end-to-end angle controller
       ret.lateralTuning.init('pid')
-      ret.lateralTuning.pid.kf = 0.00003
+      ret.lateralTuning.pid.kf = 0.00003333 if crosstrek_torque_increase else 0.00003
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]]
-      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.0025, 0.1], [0.00025, 0.01]]
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.133, 0.2], [0.0133, 0.02]] if crosstrek_torque_increase else [[0.0025, 0.1], [0.00025, 0.01]]
 
     elif candidate == CAR.IMPREZA:
       ret.mass = 1568.
diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py
index 02b0498..bda859c 100644
--- a/selfdrive/car/subaru/values.py
+++ b/selfdrive/car/subaru/values.py
@@ -27,6 +27,8 @@ class CarControllerParams:
       self.STEER_DELTA_DOWN = 40
     elif CP.carFingerprint == CAR.IMPREZA_2020:
       self.STEER_MAX = 1439
+    elif CP.carFingerprint == CAR.IMPREZA and Params().get_bool("CrosstrekTorque"):
+      self.STEER_MAX = 3071
     else:
       self.STEER_MAX = 2047
 
diff --git a/selfdrive/frogpilot/ui/vehicle_settings.cc b/selfdrive/frogpilot/ui/vehicle_settings.cc
index 525e80e..5860166 100644
--- a/selfdrive/frogpilot/ui/vehicle_settings.cc
+++ b/selfdrive/frogpilot/ui/vehicle_settings.cc
@@ -114,6 +114,8 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil
     {"LongPitch", "Long Pitch Compensation", "Reduce speed and acceleration error for greater passenger comfort and improved vehicle efficiency.", ""},
     {"LowerVolt", "Lower Volt Enable Speed", "Lower the Volt's minimum enable speed to enable openpilot at any speed.", ""},
 
+    {"CrosstrekTorque", "Subaru Crosstrek Torque Increase", "Increases the maximum allowed torque for the Subaru Crosstrek.", ""},
+
     {"LockDoors", "Lock Doors In Drive", "Automatically lock the doors when in drive and unlock when in park.", ""},
     {"LongitudinalTune", "Longitudinal Tune", "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 my 2019 Lexus ES 350", ""},
   };
@@ -155,7 +157,7 @@ FrogPilotVehiclesPanel::FrogPilotVehiclesPanel(SettingsWindow *parent) : FrogPil
     });
   }
 
-  std::set<std::string> rebootKeys = {"GasRegenCmd", "LowerVolt"};
+  std::set<std::string> rebootKeys = {"CrosstrekTorque", "GasRegenCmd", "LowerVolt"};
   for (const std::string &key : rebootKeys) {
     QObject::connect(toggles[key], &ToggleControl::toggleFlipped, [this]() {
       if (started) {
diff --git a/selfdrive/frogpilot/ui/vehicle_settings.h b/selfdrive/frogpilot/ui/vehicle_settings.h
index 7abf10d..873a7d6 100644
--- a/selfdrive/frogpilot/ui/vehicle_settings.h
+++ b/selfdrive/frogpilot/ui/vehicle_settings.h
@@ -29,7 +29,7 @@ private:
   std::map<std::string, ParamControl*> toggles;
 
   std::set<QString> gmKeys = {"GasRegenCmd", "LongPitch", "LowerVolt"};
-  std::set<QString> subaruKeys = {};
+  std::set<QString> subaruKeys = {"CrosstrekTorque"};
   std::set<QString> toyotaKeys = {"LockDoors", "LongitudinalTune"};
 
   Params params;