From c0c308b34e76c2eeb233aed5107f7240534e943f Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Mon, 6 Sep 2021 16:12:58 -0700 Subject: [PATCH] toyota: clean up accel command (#22145) * toyota: clean up accel command * revert comment * fix HYST_GAP old-commit-hash: ec71aa0458d7f655be1204e1fa7f3060c5d264d3 --- selfdrive/car/toyota/carcontroller.py | 14 ++++++-------- selfdrive/car/toyota/values.py | 5 +++-- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 94cc81406e..7a60ebc525 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/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 - # This can all be cleaned up - pcm_accel_cmd = actuators.accel / CarControllerParams.ACCEL_SCALE + pcm_accel_cmd = actuators.accel 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) - pcm_accel_cmd = 0.06 - clip(-actuators.accel/CarControllerParams.ACCEL_SCALE, 0., 1.) + 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_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)) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 8d4ef376d1..f0fbbb3bd9 100644 --- a/selfdrive/car/toyota/values.py +++ b/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