zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

FrogPilot community - Toyota: Improve comma pedal Stop-and-Go

Browse Source 
Co-Authored-By: Irene <12470297+cydia2020@users.noreply.github.com>
This commit is contained in:
FrogAi 2024-05-09 22:45:33 -07:00
parent 19cf926c35
commit 5dbbaf4f6b
3 changed files with 11 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
selfdrive/car/toyota/carcontroller.py
View File

@ -7,7 +7,7 @@ from openpilot.selfdrive.car.interfaces import CarControllerBase
from openpilot.selfdrive.car.toyota import toyotacan from openpilot.selfdrive.car.toyota import toyotacan
from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \ MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \
UNSUPPORTED_DSU_CAR UNSUPPORTED_DSU_CAR, STOP_AND_GO_CAR
from opendbc.can.packer import CANPacker from opendbc.can.packer import CANPacker
SteerControlType = car.CarParams.SteerControlType SteerControlType = car.CarParams.SteerControlType
@ -105,12 +105,12 @@ class CarController(CarControllerBase):
lta_active, self.frame // 2, torque_wind_down)) lta_active, self.frame // 2, torque_wind_down))
# *** gas and brake *** # *** gas and brake ***
if self.CP.enableGasInterceptor and CC.longActive: if self.CP.enableGasInterceptor and CC.longActive and self.CP.carFingerprint not in STOP_AND_GO_CAR:
MAX_INTERCEPTOR_GAS = 0.5 MAX_INTERCEPTOR_GAS = 0.5
# RAV4 has very sensitive gas pedal # RAV4 has very sensitive gas pedal
if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER): if self.CP.carFingerprint == CAR.RAV4:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0]) PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
elif self.CP.carFingerprint in (CAR.COROLLA,): elif self.CP.carFingerprint == CAR.COROLLA:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0]) PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
else: else:
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0]) PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
@ -118,6 +118,8 @@ class CarController(CarControllerBase):
pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2]) 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) pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS) interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
elif self.CP.enableGasInterceptor and CC.longActive and self.CP.carFingerprint in STOP_AND_GO_CAR and actuators.accel > 0.0:
interceptor_gas_cmd = 0.12 if CS.out.standstill else 0.
else: else:
interceptor_gas_cmd = 0. interceptor_gas_cmd = 0.
pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX) pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)

22
selfdrive/car/toyota/interface.py
View File

@ -2,7 +2,7 @@ from cereal import car, custom
from panda import Panda from panda import Panda
from panda.python import uds from panda.python import uds
from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \ from openpilot.selfdrive.car.toyota.values import Ecu, CAR, DBC, ToyotaFlags, CarControllerParams, TSS2_CAR, RADAR_ACC_CAR, NO_DSU_CAR, \
MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR MIN_ACC_SPEED, EPS_SCALE, UNSUPPORTED_DSU_CAR, NO_STOP_TIMER_CAR, ANGLE_CONTROL_CAR, STOP_AND_GO_CAR
from openpilot.selfdrive.car import create_button_events, get_safety_config from openpilot.selfdrive.car import create_button_events, get_safety_config
from openpilot.selfdrive.car.disable_ecu import disable_ecu from openpilot.selfdrive.car.disable_ecu import disable_ecu
from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.car.interfaces import CarInterfaceBase
@ -46,8 +46,6 @@ class CarInterface(CarInterfaceBase):
ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop
stop_and_go = candidate in TSS2_CAR
# Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it # Detect smartDSU, which intercepts ACC_CMD from the DSU (or radar) allowing openpilot to send it
# 0x2AA is sent by a similar device which intercepts the radar instead of DSU on NO_DSU_CARs # 0x2AA is sent by a similar device which intercepts the radar instead of DSU on NO_DSU_CARs
if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR): if 0x2FF in fingerprint[0] or (0x2AA in fingerprint[0] and candidate in NO_DSU_CAR):
@ -67,7 +65,6 @@ class CarInterface(CarInterfaceBase):
and not (ret.flags & ToyotaFlags.SMART_DSU) and not (ret.flags & ToyotaFlags.SMART_DSU)
if candidate == CAR.PRIUS: if candidate == CAR.PRIUS:
stop_and_go = True
# Only give steer angle deadzone to for bad angle sensor prius # Only give steer angle deadzone to for bad angle sensor prius
for fw in car_fw: for fw in car_fw:
if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00': if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
@ -75,14 +72,8 @@ class CarInterface(CarInterfaceBase):
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2) CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2): elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2):
stop_and_go = True
ret.wheelSpeedFactor = 1.035 ret.wheelSpeedFactor = 1.035
elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALON_TSS2):
# starting from 2019, all Avalon variants have stop and go
# https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf
stop_and_go = candidate != CAR.AVALON
elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023): elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4_TSS2_2023):
ret.lateralTuning.init('pid') ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kiBP = [0.0] ret.lateralTuning.pid.kiBP = [0.0]
@ -100,15 +91,6 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kf = 0.00004 ret.lateralTuning.pid.kf = 0.00004
break break
elif candidate in (CAR.RAV4H, CAR.CHR, CAR.CAMRY, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_NX):
# TODO: Some of these platforms are not advertised to have full range ACC, are they similar to SNG_WITHOUT_DSU cars?
stop_and_go = True
# TODO: these models can do stop and go, but unclear if it requires sDSU or unplugging DSU.
# For now, don't list stop and go functionality in the docs
if ret.flags & ToyotaFlags.SNG_WITHOUT_DSU:
stop_and_go = stop_and_go or bool(ret.flags & ToyotaFlags.SMART_DSU.value) or (ret.enableDsu and not docs)
ret.centerToFront = ret.wheelbase * 0.44 ret.centerToFront = ret.wheelbase * 0.44
# TODO: Some TSS-P platforms have BSM, but are flipped based on region or driving direction. # TODO: Some TSS-P platforms have BSM, but are flipped based on region or driving direction.
@ -153,7 +135,7 @@ 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 MIN_ACC_SPEED ret.minEnableSpeed = -1. if (candidate in STOP_AND_GO_CAR or ret.enableGasInterceptor) else MIN_ACC_SPEED
tune = ret.longitudinalTuning tune = ret.longitudinalTuning
tune.deadzoneBP = [0., 9.] tune.deadzoneBP = [0., 9.]

3
selfdrive/car/toyota/values.py
View File

@ -612,4 +612,7 @@ ANGLE_CONTROL_CAR = CAR.with_flags(ToyotaFlags.ANGLE_CONTROL)
# no resume button press required # no resume button press required
NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER) NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER)
STOP_AND_GO_CAR = TSS2_CAR | {CAR.PRIUS, CAR.PRIUS_V, CAR.RAV4H, CAR.LEXUS_RX, CAR.CHR, CAR.CAMRY, CAR.HIGHLANDER,
CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_NX, CAR.MIRAI, CAR.AVALON_2019}
DBC = CAR.create_dbc_map() DBC = CAR.create_dbc_map()