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 <adeebshihadeh@gmail.com> * 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 <adeebshihadeh@gmail.com>pull/20657/head
ShaneSmiskol
GitHub
parent
73ea28f7ef
commit
ebc294296a
|
|
@ -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!
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
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)
|
||||
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
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
ea8e38e4189e5f39a91fb455701ae9b876c5bcd8
|
||||
7ed0df84ad09e52f53311f038d7765623ba710b2
|
||||
Loading…
Reference in New Issue