toyota: clean up accel command (#22145)

* toyota: clean up accel command

* revert comment

* fix HYST_GAP

selfdrive/car/toyota/carcontroller.py

@@ -5,7 +5,7 @@ from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_comma
                                            create_accel_command, create_acc_cancel_command, \
                                            create_fcw_command, create_lta_steer_command
 from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
-                                        MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams
+                                        MIN_ACC_SPEED, PEDAL_HYST_GAP, PEDAL_SCALE, CarControllerParams
 from opendbc.can.packer import CANPacker
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
@@ -44,9 +44,7 @@ class CarController():
 
     # gas and brake
     interceptor_gas_cmd = 0.
-    # TODO this is needed to preserve behavior, but doesn't make sense
+    pcm_accel_cmd = actuators.accel
-    # This can all be cleaned up
-    pcm_accel_cmd = actuators.accel / CarControllerParams.ACCEL_SCALE
 
     if CS.CP.enableGasInterceptor:
       # handle hysteresis when around the minimum acc speed
@@ -57,13 +55,13 @@ class CarController():
 
       if self.use_interceptor and enabled:
         # only send negative accel when using interceptor. gas handles acceleration
-        # +0.06 offset to reduce ABS pump usage when OP is engaged
+        # +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged
         MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED], [0.2, 0.5])
-        interceptor_gas_cmd = clip(actuators.accel/CarControllerParams.ACCEL_SCALE, 0., MAX_INTERCEPTOR_GAS)
+        interceptor_gas_cmd = clip(actuators.accel / PEDAL_SCALE, 0., MAX_INTERCEPTOR_GAS)
-        pcm_accel_cmd = 0.06 - clip(-actuators.accel/CarControllerParams.ACCEL_SCALE, 0., 1.)
+        pcm_accel_cmd = 0.18 - max(0, -actuators.accel)
 
     pcm_accel_cmd, self.accel_steady = accel_hysteresis(pcm_accel_cmd, self.accel_steady, enabled)
-    pcm_accel_cmd = clip(pcm_accel_cmd * CarControllerParams.ACCEL_SCALE, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+    pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))

selfdrive/car/toyota/values.py

@@ -6,13 +6,14 @@ from selfdrive.config import Conversions as CV
 
 Ecu = car.CarParams.Ecu
 MIN_ACC_SPEED = 19. * CV.MPH_TO_MS
+
 PEDAL_HYST_GAP = 3. * CV.MPH_TO_MS
+PEDAL_SCALE = 3.0
 
 class CarControllerParams:
-  ACCEL_HYST_GAP = 0.02  # don't change accel command for small oscilalitons within this value
+  ACCEL_HYST_GAP = 0.06  # don't change accel command for small oscilalitons within this value
   ACCEL_MAX = 1.5  # m/s2
   ACCEL_MIN = -3.0  # m/s2
-  ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
 
   STEER_MAX = 1500
   STEER_DELTA_UP = 10       # 1.5s time to peak torque