revert "Toyota: API is now gas/brake #1189"

selfdrive/car/toyota/carcontroller.py

@@ -1,6 +1,4 @@
 from cereal import car
-import math
-from openpilot.common.params import Params
 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, \
@@ -15,7 +13,6 @@ 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
@@ -47,7 +44,6 @@ 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.packer = CANPacker(dbc_name)
@@ -144,45 +140,6 @@ class CarController(CarControllerBase):
                                                           lta_active, self.frame // 2, torque_wind_down))
 
     # *** gas and brake ***
-
-    # 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)
-    sp_tss2_long_tune = Params().get_bool("ToyotaTSS2Long")
-
-    # When sp_tss2_long_tune is True and CC.longActive
-    if sp_tss2_long_tune and CC.longActive:
-      self.pcm_accel_comp = 0.0
-      pcm_accel_cmd = 0.0
-
-    # When sp_tss2_long_tune is False and CC.longActive
-    elif not sp_tss2_long_tune and CC.longActive:
-      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)
-
-      if CS.out.cruiseState.standstill or actuators.longControlState == LongCtrlState.stopping:
-        self.pcm_accel_comp = 0.0
-
-      # You can uncomment these lines as needed
-      pcm_accel_cmd = actuators.accel + self.pcm_accel_comp  # + offset
-      # pcm_accel_cmd = actuators.accel - pitch_offset
-
-    # In case CC.longActive is False
-    else:
-      self.pcm_accel_comp = 0.0
-      pcm_accel_cmd = 0.0
-
-    #pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
-
-
     if self.CP.enableGasInterceptorDEPRECATED and CC.longActive:
       MAX_INTERCEPTOR_GAS = 0.5
       # RAV4 has very sensitive gas pedal
@@ -198,10 +155,7 @@ class CarController(CarControllerBase):
       interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
     else:
       interceptor_gas_cmd = 0.
-    if not 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)
+    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,9 +56,6 @@ class CarState(CarStateBase):
     self.low_speed_lockout = False
     self.acc_type = 1
     self.lkas_hud = {}
-    self.pcm_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
@@ -120,11 +117,6 @@ 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_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
-    self.vsc_slope_angle = cp.vl["VSC1"]["SLOPE_ANGLE"]
-
     ret.standstill = abs(ret.vEgoRaw) < 1e-3
 
     if self.CP.carFingerprint != CAR.TOYOTA_PRIUS_V:
@@ -410,7 +402,6 @@ class CarState(CarStateBase):
       ("STEER_ANGLE_SENSOR", 80),
       ("PCM_CRUISE", 33),
       ("PCM_CRUISE_SM", 1),
-      ("VSC1", 20),
       ("STEER_TORQUE_SENSOR", 50),
     ]
 

selfdrive/car/toyota/interface.py

@@ -184,8 +184,8 @@ class CarInterface(CarInterfaceBase):
         #tune.kiV =  [.35,  .3168, .1965,  .1965,  .184, .101, .06]
         custom_tss2_longitudinal_tuning()
       else:
-        tune.kpBP = [0., 5., 20.]
-        tune.kpV = [2.3, 1.0, 0.7]
+        tune.kpV = [0.0]
+        tune.kiV = [0.5]
       if candidate in TSS2_CAR:
         default_tss2_longitudinal_tuning()
     else: