readd from openpilot.selfdrive.controls.lib.pid import PIDController

selfdrive/car/toyota/carcontroller.py

@@ -1,4 +1,7 @@
 from cereal import car
+import math
+from openpilot.common.params import Params
+from openpilot.selfdrive.controls.lib.pid import PIDController
 from common.conversions import Conversions as CV
 from openpilot.common.numpy_fast import clip, interp
 from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_can_msg, make_tester_present_msg, \
@@ -13,6 +16,7 @@ from opendbc.can.packer import CANPacker
 GearShifter = car.CarState.GearShifter
 SteerControlType = car.CarParams.SteerControlType
 VisualAlert = car.CarControl.HUDControl.VisualAlert
+LongCtrlState = car.CarControl.Actuators.LongControlState
 
 # LKA limits
 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
@@ -44,8 +48,11 @@ class CarController(CarControllerBase):
     self.last_standstill = False
     self.standstill_req = False
     self.steer_rate_counter = 0
+    self.pcm_accel_comp = 0
     self.distance_button = 0
 
+    self.pid = PIDController(k_p=0.5, k_i=0.25, k_f=0)
+
     self.packer = CANPacker(dbc_name)
     self.gas = 0
     self.accel = 0
@@ -140,6 +147,38 @@ class CarController(CarControllerBase):
                                                           lta_active, self.frame // 2, torque_wind_down))
 
     # *** gas and brake ***
+    sp_tss2_long_tune = Params().get_bool("ToyotaTSS2Long")
+
+    # When sp_tss2_long_tune is True and CC.longActive
+    if sp_tss2_long_tune:
+      # we will throw out PCM's compensations, but that may be a good thing. for example:
+      # we lose things like pitch compensation, gas to maintain speed, brake to compensate for creeping, etc.
+      # but also remove undesirable "snap to standstill" behavior when not requesting enough accel at low speeds,
+      # lag to start moving, lag to start braking, etc.
+      # PI should compensate for lack of the desirable behaviors, but might be worse than the PCM doing them
+
+      # FIXME? neutral force will only be positive under ~5 mph, which messes up stopping control considerably
+      # not sure why this isn't captured in the PCM accel net, maybe that just ignores creep force + high speed deceleration
+      # it also doesn't seem to capture slightly more braking on downhills (VSC1S07->ASLP (pitch, deg.) might have some clues)
+      offset = min(CS.pcm_neutral_force / self.CP.mass, 0.0)
+      pitch_offset = math.sin(math.radians(CS.vsc_slope_angle)) * 9.81  # downhill is negative
+      # TODO: these limits are too slow to prevent a jerk when engaging, ramp down on engage?
+      # self.pcm_accel_comp = clip(actuators.accel - CS.pcm_accel_net, self.pcm_accel_comp - 0.05, self.pcm_accel_comp + 0.05)
+      pcm_accel_comp = self.pid.update(actuators.accel - CS.pcm_true_accel_net)
+      self.pcm_accel_comp = clip(pcm_accel_comp, self.pcm_accel_comp - 0.005, self.pcm_accel_comp + 0.005)
+      if CS.out.cruiseState.standstill or actuators.longControlState == LongCtrlState.stopping:
+        self.pcm_accel_comp = 0.0
+        self.pid.reset()
+      # TODO: just set kp to 1 and remove *2 here
+      pcm_accel_cmd = actuators.accel + self.pcm_accel_comp * 2  # + offset
+      # pcm_accel_cmd = actuators.accel - pitch_offset
+
+      if not CC.longActive:
+        self.pid.reset()
+        self.pcm_accel_comp = 0.0
+        pcm_accel_cmd = 0.0
+
+
     if self.CP.enableGasInterceptorDEPRECATED and CC.longActive:
       MAX_INTERCEPTOR_GAS = 0.5
       # RAV4 has very sensitive gas pedal
@@ -155,7 +194,10 @@ class CarController(CarControllerBase):
       interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
     else:
       interceptor_gas_cmd = 0.
-    pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
+    if sp_tss2_long_tune:
+      pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
+    else:
+      pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
 
     # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
     # than CS.cruiseState.enabled. confirm they're not meaningfully different

selfdrive/car/toyota/carstate.py

@@ -56,6 +56,10 @@ class CarState(CarStateBase):
     self.low_speed_lockout = False
     self.acc_type = 1
     self.lkas_hud = {}
+    self.pcm_accel_net = 0.0
+    self.pcm_true_accel_net = 0.0
+    self.pcm_neutral_force = 0.0
+    self.vsc_slope_angle = 0.0
 
     self.lkas_enabled = None
     self.prev_lkas_enabled = None
@@ -117,6 +121,12 @@ class CarState(CarStateBase):
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
     ret.vEgoCluster = ret.vEgo * 1.015  # minimum of all the cars
 
+    # thought to be the gas/brake as issued by the pcm (0=coasting)
+    self.pcm_accel_net = cp.vl["PCM_CRUISE"]["ACCEL_NET"]
+    self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"]
+    self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
+    self.vsc_slope_angle = cp.vl["VSC1S07"]["ASLP"]
+
     ret.standstill = abs(ret.vEgoRaw) < 1e-3
 
     if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
@@ -402,7 +412,9 @@ class CarState(CarStateBase):
       ("STEER_ANGLE_SENSOR", 80),
       ("PCM_CRUISE", 33),
       ("PCM_CRUISE_SM", 1),
+      ("VSC1S07", 20),
       ("STEER_TORQUE_SENSOR", 50),
+      ("CLUTCH", 16),
     ]
 
     if CP.carFingerprint != CAR.TOYOTA_MIRAI:

selfdrive/car/toyota/interface.py

@@ -162,14 +162,14 @@ class CarInterface(CarInterfaceBase):
 
     # hand tuned (August 12, 2024)
     def custom_tss2_longitudinal_tuning():
-      ret.vEgoStopping = 0.10
-      ret.vEgoStarting = 0.01
-      ret.stoppingDecelRate = 0.2
+      ret.vEgoStopping = 0.25
+      ret.vEgoStarting = 0.25
+      ret.stoppingDecelRate = 0.6  # reach stopping target smoothly
 
     def default_tss2_longitudinal_tuning():
-      ret.vEgoStopping = 0.01
-      ret.vEgoStarting = 0.01
-      ret.stoppingDecelRate = 0.1  # reach stopping target smoothly
+      ret.vEgoStopping = 0.25
+      ret.vEgoStarting = 0.25
+      ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
 
     def default_longitudinal_tuning():
       tune.kiBP = [0., 5., 35.]
@@ -178,8 +178,10 @@ class CarInterface(CarInterfaceBase):
     tune = ret.longitudinalTuning
     if candidate in TSS2_CAR or ret.enableGasInterceptorDEPRECATED:
       if sp_tss2_long_tune:
-        tune.kiBP = [0.,   5.,   12.,   20.,  27.,  40.]
-        tune.kiV = [.35,   .2286,  .2086,  .1716, .10, .06]
+        tune.kiBP = [5., 35.]
+        tune.kiV = [0.5, 0.5]
+        #tune.kiBP = [0.,   5.,   12.,   20.,  27.,  40.]
+        #tune.kiV = [.35,   .2286,  .2086,  .1716, .10, .06]
         #tune.kiBP = [0.,   1.,    8.,     12.,  20.,  27.,  40.]
         #tune.kiV =  [.35,  .3168, .1965,  .1965,  .184, .101, .06]
         custom_tss2_longitudinal_tuning()