brianhansonxyz/clearpilot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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-02-27 16:34:47 -07:00
parent 075e81ce44
commit 6fe645e21c
4 changed files with 23 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,7 +6,7 @@ from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_st
from openpilot.selfdrive.car.toyota import toyotacan
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
@@ -123,12 +123,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])
@@ -136,6 +136,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.

11
selfdrive/car/toyota/carstate.py
View File

@@ -175,6 +175,12 @@ 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, in this case, assume 0x343 is on bus0
# 1) the car is no longer sending standstill
# 2) the car is still in standstill
if not self.CP.openpilotLongitudinalControl:
self.stock_resume_ready = cp.vl["ACC_CONTROL"]["RELEASE_STANDSTILL"] == 1
# FrogPilot functions
if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER):
# distance button is wired to the ACC module (camera or radar)
@@ -303,7 +309,7 @@ class CarState(CarStateBase):
if CP.enableBsm:
messages.append(("BSM", 1))
if CP.carFingerprint in RADAR_ACC_CAR and not CP.flags & ToyotaFlags.DISABLE_RADAR.value:
if not CP.openpilotLongitudinalControl or (CP.carFingerprint in RADAR_ACC_CAR and not CP.flags & ToyotaFlags.DISABLE_RADAR.value):
if not CP.flags & ToyotaFlags.SMART_DSU.value:
messages += [
("ACC_CONTROL", 33),
@@ -345,4 +351,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)

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

@@ -3,7 +3,7 @@ from openpilot.common.conversions import Conversions as CV
from panda import Panda
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, \
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
@@ -56,7 +56,6 @@ class CarInterface(CarInterfaceBase):
ret.flags |= ToyotaFlags.RADAR_CAN_FILTER.value
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
@@ -68,14 +67,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
@@ -88,7 +85,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
@@ -96,14 +92,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
@@ -111,7 +105,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
@@ -120,7 +113,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
@@ -160,7 +152,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
@@ -179,14 +170,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
@@ -205,7 +194,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
@@ -271,7 +259,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.]

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

@@ -501,3 +501,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}