Toyota pedal rewrite (#23067)

* pedal redo

* add offset to compensate for creep and windbrake

* offset in standard units

* wrong size

* better creep values

* update ref

selfdrive/car/toyota/carcontroller.py

@@ -5,35 +5,18 @@ 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, PEDAL_SCALE, CarControllerParams
+                                        MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams
 from opendbc.can.packer import CANPacker
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
 
-def accel_hysteresis(accel, accel_steady, enabled):
-
-  # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command
-  if not enabled:
-    # send 0 when disabled, otherwise acc faults
-    accel_steady = 0.
-  elif accel > accel_steady + CarControllerParams.ACCEL_HYST_GAP:
-    accel_steady = accel - CarControllerParams.ACCEL_HYST_GAP
-  elif accel < accel_steady - CarControllerParams.ACCEL_HYST_GAP:
-    accel_steady = accel + CarControllerParams.ACCEL_HYST_GAP
-  accel = accel_steady
-
-  return accel, accel_steady
-
-
 class CarController():
   def __init__(self, dbc_name, CP, VM):
     self.last_steer = 0
-    self.accel_steady = 0.
     self.alert_active = False
     self.last_standstill = False
     self.standstill_req = False
     self.steer_rate_limited = False
-    self.use_interceptor = False
 
     self.packer = CANPacker(dbc_name)
 
@@ -43,25 +26,22 @@ class CarController():
     # *** compute control surfaces ***
 
     # gas and brake
+    if CS.CP.enableGasInterceptor and enabled:
+      MAX_INTERCEPTOR_GAS = 0.5
+      # RAV4 has very sensitive has pedal
+      if CS.CP.carFingerprint in [CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH]:
+        PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
+      elif CS.CP.carFingerprint in [CAR.COROLLA]:
+        PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
+      else:
+        PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
+      # offset for creep and windbrake
+      pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
+      pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
+      interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
+    else:
       interceptor_gas_cmd = 0.
-    pcm_accel_cmd = actuators.accel
-
-    if CS.CP.enableGasInterceptor:
-      # handle hysteresis when around the minimum acc speed
-      if CS.out.vEgo < MIN_ACC_SPEED:
-        self.use_interceptor = True
-      elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP:
-        self.use_interceptor = False
-
-      if self.use_interceptor and active:
-        # only send negative accel when using interceptor. gas handles acceleration
-        # +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 / PEDAL_SCALE, 0., MAX_INTERCEPTOR_GAS)
-        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_MIN, CarControllerParams.ACCEL_MAX)
+    pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -88,7 +68,6 @@ class CarController():
       self.standstill_req = False
 
     self.last_steer = apply_steer
-    self.last_accel = pcm_accel_cmd
     self.last_standstill = CS.out.standstill
 
     can_sends = []

selfdrive/car/toyota/tunes.py

@@ -1,6 +1,5 @@
 #!/usr/bin/env python3
 from enum import Enum
-from selfdrive.car.toyota.values import MIN_ACC_SPEED, PEDAL_HYST_GAP
 
 
 class LongTunes(Enum):
@@ -29,15 +28,8 @@ class LatTunes(Enum):
 
 ###### LONG ######
 def set_long_tune(tune, name):
-  if name == LongTunes.PEDAL:
-    tune.deadzoneBP = [0.]
-    tune.deadzoneV = [0.]
-    tune.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
-    tune.kpV = [1.2, 0.8, 0.765, 2.255, 1.5]
-    tune.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
-    tune.kiV = [0.18, 0.165, 0.489, 0.36]
   # Improved longitudinal tune
-  elif name == LongTunes.TSS2:
+  if name == LongTunes.TSS2 or name == LongTunes.PEDAL:
     tune.deadzoneBP = [0., 8.05]
     tune.deadzoneV = [.0, .14]
     tune.kpBP = [0., 5., 20.]

selfdrive/car/toyota/values.py

@@ -6,9 +6,8 @@ from selfdrive.config import Conversions as CV
 
 Ecu = car.CarParams.Ecu
 MIN_ACC_SPEED = 19. * CV.MPH_TO_MS
+PEDAL_TRANSITION = 10. * CV.MPH_TO_MS
 
-PEDAL_HYST_GAP = 3. * CV.MPH_TO_MS
-PEDAL_SCALE = 3.0
 
 class CarControllerParams:
   ACCEL_HYST_GAP = 0.06  # don't change accel command for small oscilalitons within this value

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-e32613484a42234bf896f1205039e9becc91ea3b
+e0926a8b9f7cffc35808109a710648a7f57c0b71