diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index fdbf5cc8d5..c445528a83 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/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 - 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) + 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 = 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 = [] diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py index 3f210d48a7..15c8bbfcc6 100644 --- a/selfdrive/car/toyota/tunes.py +++ b/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.] diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 32923b43ab..4b3f82fbc0 100644 --- a/selfdrive/car/toyota/values.py +++ b/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 diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 5fff750105..41f2296dec 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -e32613484a42234bf896f1205039e9becc91ea3b \ No newline at end of file +e0926a8b9f7cffc35808109a710648a7f57c0b71 \ No newline at end of file