Toyota: Improve comma pedal Stop-and-Go
Co-Authored-By: Irene <12470297+cydia2020@users.noreply.github.com>
This commit is contained in:
FrogAi
parent
64d3664bd8
commit
879ac310d1
|
|
@ -7,7 +7,7 @@ from openpilot.selfdrive.car.toyota import toyotacan
|
|||
from openpilot.selfdrive.car.toyota.interface import CarInterface
|
||||
from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
|
||||
MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams, ToyotaFlags, \
|
||||
UNSUPPORTED_DSU_CAR
|
||||
UNSUPPORTED_DSU_CAR, STOP_AND_GO_CAR
|
||||
from opendbc.can.packer import CANPacker
|
||||
|
||||
SteerControlType = car.CarParams.SteerControlType
|
||||
|
|
@ -124,12 +124,12 @@ class CarController:
|
|||
lta_active, self.frame // 2, torque_wind_down))
|
||||
|
||||
# *** 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
|
||||
# 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])
|
||||
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])
|
||||
else:
|
||||
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
|
||||
|
|
@ -137,6 +137,9 @@ class CarController:
|
|||
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)
|
||||
interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
|
||||
elif ((CC.longActive and actuators.accel > 0.) or (not self.CP.openpilotLongitudinalControl and CS.stock_resume_ready)) \
|
||||
and self.CP.carFingerprint in STOP_AND_GO_CAR and self.CP.enableGasInterceptor and CS.out.vEgo < 1e-3:
|
||||
interceptor_gas_cmd = 0.12
|
||||
else:
|
||||
interceptor_gas_cmd = 0.
|
||||
|
||||
|
|
|
|||
|
|
@ -174,6 +174,13 @@ class CarState(CarStateBase):
|
|||
if self.CP.carFingerprint != CAR.PRIUS_V:
|
||||
self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD"])
|
||||
|
||||
# if openpilot does not control longitudinal and we are running on a TSS-P car, it is assumed that
|
||||
# 0x343 will be present on the ADAS Bus. PCM wants to resume when:
|
||||
# 1) the car is no longer sending standstill
|
||||
# 2) the car is still in standstill
|
||||
if not self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in (TSS2_CAR, UNSUPPORTED_DSU_CAR):
|
||||
self.stock_resume_ready = cp.vl["ACC_CONTROL"]["RELEASE_STANDSTILL"] == 1
|
||||
|
||||
# Driving personalities function
|
||||
if frogpilot_variables.personalities_via_wheel and ret.cruiseState.available:
|
||||
# Need to subtract by 1 to comply with the personality profiles of "0", "1", and "2"
|
||||
|
|
@ -310,4 +317,7 @@ class CarState(CarStateBase):
|
|||
("PCS_HUD", 1),
|
||||
]
|
||||
|
||||
if not CP.openpilotLongitudinalControl and CP.carFingerprint not in (TSS2_CAR, UNSUPPORTED_DSU_CAR):
|
||||
messages.append(("ACC_CONTROL", 33))
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@ from panda import Panda
|
|||
from panda.python import uds
|
||||
from openpilot.common.numpy_fast import interp
|
||||
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 get_safety_config
|
||||
from openpilot.selfdrive.car.disable_ecu import disable_ecu
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
|
||||
|
|
@ -46,10 +46,7 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop
|
||||
|
||||
stop_and_go = candidate in TSS2_CAR
|
||||
|
||||
if candidate == CAR.PRIUS:
|
||||
stop_and_go = True
|
||||
ret.wheelbase = 2.70
|
||||
ret.steerRatio = 15.74 # unknown end-to-end spec
|
||||
ret.tireStiffnessFactor = 0.6371 # hand-tune
|
||||
|
|
@ -61,14 +58,12 @@ class CarInterface(CarInterfaceBase):
|
|||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
|
||||
|
||||
elif candidate == CAR.PRIUS_V:
|
||||
stop_and_go = True
|
||||
ret.wheelbase = 2.78
|
||||
ret.steerRatio = 17.4
|
||||
ret.tireStiffnessFactor = 0.5533
|
||||
ret.mass = 3340. * CV.LB_TO_KG
|
||||
|
||||
elif candidate in (CAR.RAV4, CAR.RAV4H):
|
||||
stop_and_go = True if (candidate in CAR.RAV4H) else False
|
||||
ret.wheelbase = 2.65
|
||||
ret.steerRatio = 16.88 # 14.5 is spec end-to-end
|
||||
ret.tireStiffnessFactor = 0.5533
|
||||
|
|
@ -81,7 +76,6 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.mass = 2860. * CV.LB_TO_KG # mean between normal and hybrid
|
||||
|
||||
elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2):
|
||||
stop_and_go = True
|
||||
ret.wheelbase = 2.79
|
||||
ret.steerRatio = 16. # 14.8 is spec end-to-end
|
||||
ret.wheelSpeedFactor = 1.035
|
||||
|
|
@ -89,14 +83,12 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.mass = 4481. * CV.LB_TO_KG # mean between min and max
|
||||
|
||||
elif candidate in (CAR.CHR, CAR.CHR_TSS2):
|
||||
stop_and_go = True
|
||||
ret.wheelbase = 2.63906
|
||||
ret.steerRatio = 13.6
|
||||
ret.tireStiffnessFactor = 0.7933
|
||||
ret.mass = 3300. * CV.LB_TO_KG
|
||||
|
||||
elif candidate in (CAR.CAMRY, CAR.CAMRY_TSS2):
|
||||
stop_and_go = True
|
||||
ret.wheelbase = 2.82448
|
||||
ret.steerRatio = 13.7
|
||||
ret.tireStiffnessFactor = 0.7933
|
||||
|
|
@ -104,7 +96,6 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDER_TSS2):
|
||||
# TODO: TSS-P models can do stop and go, but unclear if it requires sDSU or unplugging DSU
|
||||
stop_and_go = True
|
||||
ret.wheelbase = 2.8194 # average of 109.8 and 112.2 in
|
||||
ret.steerRatio = 16.0
|
||||
ret.tireStiffnessFactor = 0.8
|
||||
|
|
@ -113,7 +104,6 @@ class CarInterface(CarInterfaceBase):
|
|||
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
|
||||
ret.wheelbase = 2.82
|
||||
ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download
|
||||
ret.tireStiffnessFactor = 0.7983
|
||||
|
|
@ -153,7 +143,6 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.mass = 3677. * CV.LB_TO_KG # mean between min and max
|
||||
|
||||
elif candidate == CAR.SIENNA:
|
||||
stop_and_go = True
|
||||
ret.wheelbase = 3.03
|
||||
ret.steerRatio = 15.5
|
||||
ret.tireStiffnessFactor = 0.444
|
||||
|
|
@ -172,14 +161,12 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.mass = 4034. * CV.LB_TO_KG
|
||||
|
||||
elif candidate == CAR.LEXUS_CTH:
|
||||
stop_and_go = True
|
||||
ret.wheelbase = 2.60
|
||||
ret.steerRatio = 18.6
|
||||
ret.tireStiffnessFactor = 0.517
|
||||
ret.mass = 3108 * CV.LB_TO_KG # mean between min and max
|
||||
|
||||
elif candidate in (CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2):
|
||||
stop_and_go = True
|
||||
ret.wheelbase = 2.66
|
||||
ret.steerRatio = 14.7
|
||||
ret.tireStiffnessFactor = 0.444 # not optimized yet
|
||||
|
|
@ -198,7 +185,6 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.mass = 3115. * CV.LB_TO_KG
|
||||
|
||||
elif candidate == CAR.MIRAI:
|
||||
stop_and_go = True
|
||||
ret.wheelbase = 2.91
|
||||
ret.steerRatio = 14.8
|
||||
ret.tireStiffnessFactor = 0.8
|
||||
|
|
@ -263,7 +249,7 @@ 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 MIN_ACC_SPEED
|
||||
ret.minEnableSpeed = -1. if (candidate in STOP_AND_GO_CAR or ret.enableGasInterceptor) else MIN_ACC_SPEED
|
||||
|
||||
tune = ret.longitudinalTuning
|
||||
tune.deadzoneBP = [0., 9.]
|
||||
|
|
|
|||
|
|
@ -503,3 +503,7 @@ ANGLE_CONTROL_CAR = {CAR.RAV4_TSS2_2023}
|
|||
|
||||
# no resume button press required
|
||||
NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDER, CAR.SIENNA}
|
||||
|
||||
# stop and go
|
||||
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}
|
||||
|
|
|
|||
Loading…
Reference in New Issue