sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Toyota: Only use gas interceptor under 19 mph (#21101)

Browse Source 
* 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 4 years ago committed by GitHub
parent 73ea28f7ef
commit ebc294296a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 58 additions and 44 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      RELEASES.md
  2. 39
      selfdrive/car/toyota/carcontroller.py
  3. 54
      selfdrive/car/toyota/interface.py
  4. 6
      selfdrive/car/toyota/values.py
  5. 2
      selfdrive/test/process_replay/ref_commit

1
RELEASES.md
Unescape View File

@ -2,6 +2,7 @@ Version 0.8.5 (2021-XX-XX)
======================== ========================
* NEOS update: improved reliability and stability * NEOS update: improved reliability and stability
* Smart model-based Forward Collision Warning * Smart model-based Forward Collision Warning
* Improved longitudinal control on Toyotas with a comma pedal
* Hyundai Elantra 2021 support thanks to CruiseBrantley! * Hyundai Elantra 2021 support thanks to CruiseBrantley!
* Lexus UX Hybrid 2019-2020 support thanks to brianhaugen2! * Lexus UX Hybrid 2019-2020 support thanks to brianhaugen2!
* Toyota Avalon Hybrid 2019 support thanks to jbates9011! * Toyota Avalon Hybrid 2019 support thanks to jbates9011!

39
selfdrive/car/toyota/carcontroller.py
Unescape View File

@ -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, \ from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
create_accel_command, create_acc_cancel_command, \ create_accel_command, create_acc_cancel_command, \
create_fcw_command, create_lta_steer_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 from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert VisualAlert = car.CarControl.HUDControl.VisualAlert
@ -32,8 +33,8 @@ class CarController():
self.alert_active = False self.alert_active = False
self.last_standstill = False self.last_standstill = False
self.standstill_req = False self.standstill_req = False
self.steer_rate_limited = False self.steer_rate_limited = False
self.use_interceptor = False
self.fake_ecus = set() self.fake_ecus = set()
if CP.enableCamera: if CP.enableCamera:
@ -49,18 +50,24 @@ class CarController():
# *** compute control surfaces *** # *** compute control surfaces ***
# gas and brake # gas and brake
interceptor_gas_cmd = 0.
apply_gas = clip(actuators.gas, 0., 1.) pcm_accel_cmd = actuators.gas - actuators.brake
if CS.CP.enableGasInterceptor: if CS.CP.enableGasInterceptor:
# send only negative accel if interceptor is detected. otherwise, send the regular value # handle hysteresis when around the minimum acc speed
# +0.06 offset to reduce ABS pump usage when OP is engaged if CS.out.vEgo < MIN_ACC_SPEED:
apply_accel = 0.06 - actuators.brake self.use_interceptor = True
else: elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP:
apply_accel = actuators.gas - actuators.brake 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) pcm_accel_cmd, self.accel_steady = accel_hysteresis(pcm_accel_cmd, self.accel_steady, enabled)
apply_accel = clip(apply_accel * CarControllerParams.ACCEL_SCALE, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX) pcm_accel_cmd = clip(pcm_accel_cmd * CarControllerParams.ACCEL_SCALE, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
# steer torque # steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@ -86,7 +93,7 @@ class CarController():
self.standstill_req = False self.standstill_req = False
self.last_steer = apply_steer self.last_steer = apply_steer
self.last_accel = apply_accel self.last_accel = pcm_accel_cmd
self.last_standstill = CS.out.standstill self.last_standstill = CS.out.standstill
can_sends = [] can_sends = []
@ -115,14 +122,14 @@ class CarController():
if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS: if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
can_sends.append(create_acc_cancel_command(self.packer)) can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl: 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: else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead)) can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
if (frame % 2 == 0) and (CS.CP.enableGasInterceptor): 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. # 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 # 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: # ui mesg is at 100Hz but we send asap if:
# - there is something to display # - there is something to display

54
selfdrive/car/toyota/interface.py
Unescape View File

@ -1,7 +1,7 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
from cereal import car from cereal import car
from selfdrive.config import Conversions as CV 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.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
from selfdrive.swaglog import cloudlog from selfdrive.swaglog import cloudlog
from selfdrive.car.interfaces import CarInterfaceBase from selfdrive.car.interfaces import CarInterfaceBase
@ -24,26 +24,6 @@ class CarInterface(CarInterfaceBase):
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay
ret.steerLimitTimer = 0.4 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 if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI
ret.lateralTuning.init('pid') ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] 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 # 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. # 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 # 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 # intercepting the DSU is a community feature since it requires unofficial hardware
ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu
if ret.enableGasInterceptor: if ret.enableGasInterceptor:
ret.gasMaxBP = [0., 9., 35] # Transitions from original pedal tuning at MIN_ACC_SPEED to default tuning at MIN_ACC_SPEED + hysteresis gap
ret.gasMaxV = [0.2, 0.5, 0.7] ret.gasMaxBP = [0., MIN_ACC_SPEED]
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.gasMaxV = [0.2, 0.5]
ret.longitudinalTuning.kiV = [0.18, 0.12] 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 return ret

6
selfdrive/car/toyota/values.py
Unescape View File

@ -1,8 +1,12 @@
# flake8: noqa # flake8: noqa
from selfdrive.car import dbc_dict
from cereal import car from cereal import car
from selfdrive.car import dbc_dict
from selfdrive.config import Conversions as CV
Ecu = car.CarParams.Ecu Ecu = car.CarParams.Ecu
MIN_ACC_SPEED = 19. * CV.MPH_TO_MS
PEDAL_HYST_GAP = 3. * CV.MPH_TO_MS
class CarControllerParams: class CarControllerParams:
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
ea8e38e4189e5f39a91fb455701ae9b876c5bcd8 7ed0df84ad09e52f53311f038d7765623ba710b2
Write Preview
Loading…
Cancel
Save