mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-05 13:32:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

toyota: clean up accel command (#22145)

Browse Source 
* toyota: clean up accel command

* revert comment

* fix HYST_GAP
old-commit-hash: ec71aa0458d7f655be1204e1fa7f3060c5d264d3
This commit is contained in:
Willem Melching
2021-09-06 16:12:58 -07:00
committed by GitHub
parent b1cfdaa5d6
commit c0c308b34e
2 changed files with 9 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/car/toyota/carcontroller.py
+6 -8
View File
@@ -5,7 +5,7 @@ from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_comma
create_accel_command, create_acc_cancel_command, \
create_fcw_command, create_lta_steer_command
from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams
MIN_ACC_SPEED, PEDAL_HYST_GAP, PEDAL_SCALE, CarControllerParams
from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -44,9 +44,7 @@ class CarController():
# gas and brake
interceptor_gas_cmd = 0.
# TODO this is needed to preserve behavior, but doesn't make sense
# This can all be cleaned up
pcm_accel_cmd = actuators.accel / CarControllerParams.ACCEL_SCALE
pcm_accel_cmd = actuators.accel
if CS.CP.enableGasInterceptor:
# handle hysteresis when around the minimum acc speed
@@ -57,13 +55,13 @@ class CarController():
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
# +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged
MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED], [0.2, 0.5])
interceptor_gas_cmd = clip(actuators.accel/CarControllerParams.ACCEL_SCALE, 0., MAX_INTERCEPTOR_GAS)
pcm_accel_cmd = 0.06 - clip(-actuators.accel/CarControllerParams.ACCEL_SCALE, 0., 1.)
interceptor_gas_cmd = clip(actuators.accel / PEDAL_SCALE, 0., MAX_INTERCEPTOR_GAS)
pcm_accel_cmd = 0.18 - max(0, -actuators.accel)
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)
pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
selfdrive/car/toyota/values.py
+3 -2
View File
@@ -6,13 +6,14 @@ 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
PEDAL_SCALE = 3.0
class CarControllerParams:
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value
ACCEL_HYST_GAP = 0.06 # don't change accel command for small oscilalitons within this value
ACCEL_MAX = 1.5 # m/s2
ACCEL_MIN = -3.0 # m/s2
ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
STEER_MAX = 1500
STEER_DELTA_UP = 10 # 1.5s time to peak torque