Vehicles - Toyota - Longitudinal Tune - FrogPilot

selfdrive/car/toyota/carcontroller.py

@@ -6,7 +6,7 @@ from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.car.toyota import toyotacan
 from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
                                         MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \
-                                        UNSUPPORTED_DSU_CAR, STOP_AND_GO_CAR
+                                        UNSUPPORTED_DSU_CAR, STOP_AND_GO_CAR, TSS2_CAR
 from opendbc.can.packer import CANPacker
 
 LongCtrlState = car.CarControl.Actuators.LongControlState
@@ -38,6 +38,15 @@ PARK = car.CarState.GearShifter.park
 COMPENSATORY_CALCULATION_THRESHOLD_V = [-0.3, -0.25, 0.]  # m/s^2
 COMPENSATORY_CALCULATION_THRESHOLD_BP = [0., 11., 23.]  # m/s
 
+def compute_gb_toyota(accel, speed):
+  creep_brake = 0.0
+  creep_speed = 2.3
+  creep_brake_value = 0.15
+  if speed < creep_speed:
+    creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value
+  gb = accel - creep_brake
+  return gb
+
 class CarController(CarControllerBase):
   def __init__(self, dbc_name, CP, VM):
     self.CP = CP
@@ -60,9 +69,12 @@ class CarController(CarControllerBase):
     params = Params()
 
     self.cydia_tune = params.get_bool("CydiaTune")
+    self.frogs_go_moo_tune = params.get_bool("FrogsGoMooTune")
 
     self.doors_locked = False
 
+    self.pcm_accel_comp = 0
+
   def update(self, CC, CS, now_nanos, frogpilot_toggles):
     actuators = CC.actuators
     hud_control = CC.hudControl
@@ -150,17 +162,28 @@ class CarController(CarControllerBase):
       self.prohibit_neg_calculation = False
 
     # limit minimum to only positive until first positive is reached after engagement, don't calculate when long isn't active
-    if CC.longActive and not self.prohibit_neg_calculation and self.cydia_tune:
+    if CC.longActive and not self.prohibit_neg_calculation and (self.cydia_tune or self.frogs_go_moo_tune and self.CP.carFingerprint not in TSS2_CAR):
       accel_offset = CS.pcm_neutral_force / self.CP.mass
     else:
       accel_offset = 0.
 
+    if CC.longActive and self.frogs_go_moo_tune:
+      wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15])
+      gas_accel = compute_gb_toyota(actuators.accel, CS.out.vEgo) + wind_brake
+      self.pcm_accel_comp = clip(gas_accel - CS.pcm_accel_net, self.pcm_accel_comp - 0.03, self.pcm_accel_comp + 0.03)
+
     # only calculate pcm_accel_cmd when long is active to prevent disengagement from accelerator depression
     if CC.longActive:
       if frogpilot_toggles.sport_plus:
-        pcm_accel_cmd = clip(actuators.accel + accel_offset, self.params.ACCEL_MIN, self.params.ACCEL_MAX_PLUS)
+        if self.frogs_go_moo_tune:
+          pcm_accel_cmd = clip(gas_accel + self.pcm_accel_comp, self.params.ACCEL_MIN, self.params.ACCEL_MAX_PLUS)
+        else:
+          pcm_accel_cmd = clip(actuators.accel + accel_offset, self.params.ACCEL_MIN, self.params.ACCEL_MAX_PLUS)
       else:
-        pcm_accel_cmd = clip(actuators.accel + accel_offset, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
+        if self.frogs_go_moo_tune:
+          pcm_accel_cmd = clip(gas_accel + self.pcm_accel_comp, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
+        else:
+          pcm_accel_cmd = clip(actuators.accel + accel_offset, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
     else:
       pcm_accel_cmd = 0.
 

selfdrive/car/toyota/carstate.py

@@ -71,6 +71,8 @@ class CarState(CarStateBase):
     self.zss_compute = False
     self.zss_cruise_active_last = False
 
+    self.pcm_accel_net = 0
+    self.pcm_neutral_force = 0
     self.zss_angle_offset = 0
     self.zss_threshold_count = 0
 
@@ -222,6 +224,7 @@ class CarState(CarStateBase):
       message_keys = ["LDA_ON_MESSAGE", "SET_ME_X02"]
       self.lkas_enabled = any(self.lkas_hud.get(key) == 1 for key in message_keys)
 
+    self.pcm_accel_net = cp.vl["PCM_CRUISE"]["ACCEL_NET"]
     self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
 
     # ZSS Support - Credit goes to the DragonPilot team!

selfdrive/car/toyota/interface.py

@@ -136,14 +136,18 @@ class CarInterface(CarInterfaceBase):
     ret.minEnableSpeed = -1. if (candidate in STOP_AND_GO_CAR or ret.enableGasInterceptor) else MIN_ACC_SPEED
 
     tune = ret.longitudinalTuning
-    if params.get_bool("CydiaTune"):
+    if params.get_bool("CydiaTune") or params.get_bool("FrogsGoMooTune"):
       ret.stopAccel = -2.5             # on stock Toyota this is -2.5
       ret.stoppingDecelRate = 0.3      # reach stopping target smoothly
       if candidate in TSS2_CAR or ret.enableGasInterceptor:
         tune.kpV = [0.0]
         tune.kiV = [0.5]
-        ret.vEgoStopping = 0.25
-        ret.vEgoStarting = 0.25
+        if params.get_bool("FrogsGoMooTune"):
+          ret.vEgoStopping = 0.15
+          ret.vEgoStarting = 0.15
+        else:
+          ret.vEgoStopping = 0.25
+          ret.vEgoStarting = 0.25
       else:
         tune.kpV = [0.0]
         tune.kiV = [1.2]               # appears to produce minimal oscillation on TSS-P