From a830c71a03914cf594ad6817c36a633dbb1d37fa Mon Sep 17 00:00:00 2001 From: ShaneSmiskol Date: Thu, 3 Jun 2021 15:11:10 -0700 Subject: [PATCH] Toyota: Only use gas interceptor under 19 mph (#21101) * pedal improvements * pedal tuning under 19 mph, stock tuning above * Correctly set tuning, and a temporary reference script * looking good * use minEnableSpeed * minEnableSpeed will always be -1 if pedal since it's used for lockout * add hysteresis around 19 mph for enabling pedal use * parameterize minAccSpeed (enable speed is -1 when stop and go to allow engage, acc is min limit for acc commands) * fix Prius later, 5 mph gap around switch speed * define min_acc_speed in interface and CC * use pedal gains up to 22 mph, and correctly use hysteresis above the switch speed * only check this if pedal * consolidate comment * Change BP to switch tuning at 19 mph, define global constant * Start at interceptor status Co-authored-by: Adeeb Shihadeh * update releases * typo * transition tuning from 19 to +3 mph * change cmd names and update tuning comment * Update RELEASES.md * calculate new pedal gains * little more clear * even clearer * same comment convention and hardcode new pedal tune * bump cereal * weird, got added in with the rebase * three possible long tunings: pedal, improved non-pedal, and default non-pedal * update refs * future note: don't add submodules when rebasing! * update refs * update refs Co-authored-by: Adeeb Shihadeh old-commit-hash: ebc294296aa17a5aabeb9d1bb6f9cc38970173ba --- RELEASES.md | 1 + selfdrive/car/toyota/carcontroller.py | 39 ++++++++++------- selfdrive/car/toyota/interface.py | 54 ++++++++++++------------ selfdrive/car/toyota/values.py | 6 ++- selfdrive/test/process_replay/ref_commit | 2 +- 5 files changed, 58 insertions(+), 44 deletions(-) diff --git a/RELEASES.md b/RELEASES.md index 07a9493579..03084e5a0e 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -2,6 +2,7 @@ Version 0.8.5 (2021-XX-XX) ======================== * NEOS update: improved reliability and stability * Smart model-based Forward Collision Warning + * Improved longitudinal control on Toyotas with a comma pedal * Hyundai Elantra 2021 support thanks to CruiseBrantley! * Lexus UX Hybrid 2019-2020 support thanks to brianhaugen2! * Toyota Avalon Hybrid 2019 support thanks to jbates9011! diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index c961288aaf..1bc18efb74 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -4,7 +4,8 @@ from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ create_accel_command, create_acc_cancel_command, \ create_fcw_command, create_lta_steer_command -from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, CarControllerParams +from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ + MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -32,8 +33,8 @@ class CarController(): self.alert_active = False self.last_standstill = False self.standstill_req = False - self.steer_rate_limited = False + self.use_interceptor = False self.fake_ecus = set() if CP.enableCamera: @@ -49,18 +50,24 @@ class CarController(): # *** compute control surfaces *** # gas and brake - - apply_gas = clip(actuators.gas, 0., 1.) + interceptor_gas_cmd = 0. + pcm_accel_cmd = actuators.gas - actuators.brake if CS.CP.enableGasInterceptor: - # send only negative accel if interceptor is detected. otherwise, send the regular value - # +0.06 offset to reduce ABS pump usage when OP is engaged - apply_accel = 0.06 - actuators.brake - else: - apply_accel = actuators.gas - actuators.brake + # 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 enabled: + # only send negative accel when using interceptor. gas handles acceleration + # +0.06 offset to reduce ABS pump usage when OP is engaged + interceptor_gas_cmd = clip(actuators.gas, 0., 1.) + pcm_accel_cmd = 0.06 - actuators.brake - apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled) - apply_accel = clip(apply_accel * CarControllerParams.ACCEL_SCALE, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) + 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) # steer torque new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) @@ -86,7 +93,7 @@ class CarController(): self.standstill_req = False self.last_steer = apply_steer - self.last_accel = apply_accel + self.last_accel = pcm_accel_cmd self.last_standstill = CS.out.standstill can_sends = [] @@ -115,14 +122,14 @@ class CarController(): if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS: can_sends.append(create_acc_cancel_command(self.packer)) elif CS.CP.openpilotLongitudinalControl: - can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead)) + can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead)) else: can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead)) - if (frame % 2 == 0) and (CS.CP.enableGasInterceptor): - # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas. + if frame % 2 == 0 and CS.CP.enableGasInterceptor: + # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd. # This prevents unexpected pedal range rescaling - can_sends.append(create_gas_command(self.packer, apply_gas, frame//2)) + can_sends.append(create_gas_command(self.packer, interceptor_gas_cmd, frame // 2)) # ui mesg is at 100Hz but we send asap if: # - there is something to display diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 40b8af903c..6408ad01ac 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from cereal import car from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, CarControllerParams +from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.swaglog import cloudlog from selfdrive.car.interfaces import CarInterfaceBase @@ -24,26 +24,6 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 - # Improved longitudinal tune - if candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2, CAR.LEXUS_ESH_TSS2]: - ret.longitudinalTuning.deadzoneBP = [0., 8.05] - ret.longitudinalTuning.deadzoneV = [.0, .14] - ret.longitudinalTuning.kpBP = [0., 5., 20.] - ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7] - ret.longitudinalTuning.kiBP = [0., 5., 12., 20., 27.] - ret.longitudinalTuning.kiV = [.35, .23, .20, .17, .1] - ret.stoppingBrakeRate = 0.1 # reach stopping target smoothly - ret.startingBrakeRate = 2.0 # release brakes fast - ret.startAccel = 1.2 # Accelerate from 0 faster - else: - # Default longitudinal tune - ret.longitudinalTuning.deadzoneBP = [0., 9.] - ret.longitudinalTuning.deadzoneV = [0., .15] - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kiBP = [0., 35.] - ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] - ret.longitudinalTuning.kiV = [0.54, 0.36] - if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI ret.lateralTuning.init('pid') ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] @@ -335,17 +315,39 @@ class CarInterface(CarInterfaceBase): # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. - ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS + ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED # removing the DSU disables AEB and it's considered a community maintained feature # intercepting the DSU is a community feature since it requires unofficial hardware ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu if ret.enableGasInterceptor: - ret.gasMaxBP = [0., 9., 35] - ret.gasMaxV = [0.2, 0.5, 0.7] - ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] - ret.longitudinalTuning.kiV = [0.18, 0.12] + # Transitions from original pedal tuning at MIN_ACC_SPEED to default tuning at MIN_ACC_SPEED + hysteresis gap + ret.gasMaxBP = [0., MIN_ACC_SPEED] + ret.gasMaxV = [0.2, 0.5] + ret.longitudinalTuning.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.765, 2.255, 1.5] + ret.longitudinalTuning.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.] + ret.longitudinalTuning.kiV = [0.18, 0.165, 0.489, 0.36] + elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]: + # Improved longitudinal tune + ret.longitudinalTuning.deadzoneBP = [0., 8.05] + ret.longitudinalTuning.deadzoneV = [.0, .14] + ret.longitudinalTuning.kpBP = [0., 5., 20.] + ret.longitudinalTuning.kpV = [1.3, 1.0, 0.7] + ret.longitudinalTuning.kiBP = [0., 5., 12., 20., 27.] + ret.longitudinalTuning.kiV = [.35, .23, .20, .17, .1] + ret.stoppingBrakeRate = 0.1 # reach stopping target smoothly + ret.startingBrakeRate = 2.0 # release brakes fast + ret.startAccel = 1.2 # Accelerate from 0 faster + else: + # Default longitudinal tune + ret.longitudinalTuning.deadzoneBP = [0., 9.] + ret.longitudinalTuning.deadzoneV = [0., .15] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] + ret.longitudinalTuning.kiV = [0.54, 0.36] return ret diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 19cab0c3c0..696dc63233 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -1,8 +1,12 @@ # flake8: noqa -from selfdrive.car import dbc_dict from cereal import car +from selfdrive.car import dbc_dict +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 class CarControllerParams: ACCEL_HYST_GAP = 0.02 # 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 8c0287d207..373d09c7be 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -ea8e38e4189e5f39a91fb455701ae9b876c5bcd8 \ No newline at end of file +7ed0df84ad09e52f53311f038d7765623ba710b2 \ No newline at end of file