retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Move all CarControllerParams into values.py (#19663) 

* toyota

* use scale from values.py

* nissan

* subaru

* honda

* gm

* toyota combine into CarControllerParams

* nissan refactor

* chrysler refactor

* mazda refactor

* hyundai refactor

* subaru refactor
albatross
Willem Melching 2021-01-06 11:20:44 +01:00 committed by GitHub
parent d0ba19fedb
commit e4cf43c6fc
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
19 changed files with 103 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
selfdrive/car/chrysler/carcontroller.py
View File

@ -1,7 +1,7 @@
from selfdrive.car import apply_toyota_steer_torque_limits
from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \
create_wheel_buttons
from selfdrive.car.chrysler.values import CAR, SteerLimitParams
from selfdrive.car.chrysler.values import CAR, CarControllerParams
from opendbc.can.packer import CANPacker
class CarController():
@ -24,9 +24,9 @@ class CarController():
# *** compute control surfaces ***
# steer torque
new_steer = actuators.steer * SteerLimitParams.STEER_MAX
new_steer = actuators.steer * CarControllerParams.STEER_MAX
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last,
CS.out.steeringTorqueEps, SteerLimitParams)
CS.out.steeringTorqueEps, CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message

2
selfdrive/car/chrysler/values.py
View File

@ -4,7 +4,7 @@ from selfdrive.car import dbc_dict
from cereal import car
Ecu = car.CarParams.Ecu
class SteerLimitParams:
class CarControllerParams:
STEER_MAX = 261 # 262 faults
STEER_DELTA_UP = 3 # 3 is stock. 100 is fine. 200 is too much it seems
STEER_DELTA_DOWN = 3 # no faults on the way down it seems

30
selfdrive/car/gm/carcontroller.py
View File

@ -4,40 +4,12 @@ from common.numpy_fast import interp
from selfdrive.config import Conversions as CV
from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.car.gm import gmcan
from selfdrive.car.gm.values import DBC, CanBus
from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams
from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarControllerParams():
def __init__(self):
self.STEER_MAX = 300
self.STEER_STEP = 2 # how often we update the steer cmd
self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s)
self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero
self.MIN_STEER_SPEED = 3.
self.STEER_DRIVER_ALLOWANCE = 50 # allowed driver torque before start limiting
self.STEER_DRIVER_MULTIPLIER = 4 # weight driver torque heavily
self.STEER_DRIVER_FACTOR = 100 # from dbc
self.NEAR_STOP_BRAKE_PHASE = 0.5 # m/s, more aggressive braking near full stop
# Takes case of "Service Adaptive Cruise" and "Service Front Camera"
# dashboard messages.
self.ADAS_KEEPALIVE_STEP = 100
self.CAMERA_KEEPALIVE_STEP = 100
# pedal lookups, only for Volt
MAX_GAS = 3072 # Only a safety limit
ZERO_GAS = 2048
MAX_BRAKE = 350 # Should be around 3.5m/s^2, including regen
self.MAX_ACC_REGEN = 1404 # ACC Regen braking is slightly less powerful than max regen paddle
self.GAS_LOOKUP_BP = [-0.25, 0., 0.5]
self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS]
self.BRAKE_LOOKUP_BP = [-1., -0.25]
self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0]
class CarController():
def __init__(self, dbc_name, CP, VM):
self.start_time = 0.

27
selfdrive/car/gm/values.py
View File

@ -4,6 +4,33 @@ from cereal import car
from selfdrive.car import dbc_dict
Ecu = car.CarParams.Ecu
class CarControllerParams():
def __init__(self):
self.STEER_MAX = 300
self.STEER_STEP = 2 # how often we update the steer cmd
self.STEER_DELTA_UP = 7 # ~0.75s time to peak torque (255/50hz/0.75s)
self.STEER_DELTA_DOWN = 17 # ~0.3s from peak torque to zero
self.MIN_STEER_SPEED = 3.
self.STEER_DRIVER_ALLOWANCE = 50 # allowed driver torque before start limiting
self.STEER_DRIVER_MULTIPLIER = 4 # weight driver torque heavily
self.STEER_DRIVER_FACTOR = 100 # from dbc
self.NEAR_STOP_BRAKE_PHASE = 0.5 # m/s, more aggressive braking near full stop
# Takes case of "Service Adaptive Cruise" and "Service Front Camera"
# dashboard messages.
self.ADAS_KEEPALIVE_STEP = 100
self.CAMERA_KEEPALIVE_STEP = 100
# pedal lookups, only for Volt
MAX_GAS = 3072 # Only a safety limit
ZERO_GAS = 2048
MAX_BRAKE = 350 # Should be around 3.5m/s^2, including regen
self.MAX_ACC_REGEN = 1404 # ACC Regen braking is slightly less powerful than max regen paddle
self.GAS_LOOKUP_BP = [-0.25, 0., 0.5]
self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS]
self.BRAKE_LOOKUP_BP = [-1., -0.25]
self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0]
class CAR:
HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017"
VOLT = "CHEVROLET VOLT PREMIER 2017"

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

@ -5,7 +5,7 @@ from selfdrive.controls.lib.drive_helpers import rate_limit
from common.numpy_fast import clip, interp
from selfdrive.car import create_gas_command
from selfdrive.car.honda import hondacan
from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD, HONDA_BOSCH
from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD, HONDA_BOSCH, CarControllerParams
from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
@ -74,15 +74,6 @@ HUDData = namedtuple("HUDData",
["pcm_accel", "v_cruise", "car",
"lanes", "fcw", "acc_alert", "steer_required"])
class CarControllerParams():
def __init__(self, CP):
self.BRAKE_MAX = 1024//4
self.STEER_MAX = CP.lateralParams.torqueBP[-1]
# mirror of list (assuming first item is zero) for interp of signed request values
assert(CP.lateralParams.torqueBP[0] == 0)
assert(CP.lateralParams.torqueBP[0] == 0)
self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP)
self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV)
class CarController():
def __init__(self, dbc_name, CP, VM):

14
selfdrive/car/honda/values.py
View File

@ -6,6 +6,16 @@ from selfdrive.car import dbc_dict
Ecu = car.CarParams.Ecu
VisualAlert = car.CarControl.HUDControl.VisualAlert
class CarControllerParams():
def __init__(self, CP):
self.BRAKE_MAX = 1024//4
self.STEER_MAX = CP.lateralParams.torqueBP[-1]
# mirror of list (assuming first item is zero) for interp of signed request values
assert(CP.lateralParams.torqueBP[0] == 0)
assert(CP.lateralParams.torqueBP[0] == 0)
self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP)
self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV)
# Car button codes
class CruiseButtons:
RES_ACCEL = 4
@ -441,7 +451,7 @@ FW_VERSIONS = {
b'37805-5AN-AK20\x00\x00',
b'37805-5AN-AR20\x00\x00',
b'37805-5AN-CH20\x00\x00',
b'37805-5AN-L840\x00\x00',
b'37805-5AN-L840\x00\x00',
b'37805-5AN-L940\x00\x00',
b'37805-5AN-LF20\x00\x00',
b'37805-5AN-LH20\x00\x00',
@ -1020,7 +1030,7 @@ FW_VERSIONS = {
(Ecu.combinationMeter, 0x18da60f1, None): [
b'78109-T3R-A120\x00\x00',
],
},
},
}
DBC = {

4
selfdrive/car/hyundai/carcontroller.py
View File

@ -2,7 +2,7 @@ from cereal import car
from common.realtime import DT_CTRL
from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfa_mfa
from selfdrive.car.hyundai.values import Buttons, SteerLimitParams, CAR
from selfdrive.car.hyundai.values import Buttons, CarControllerParams, CAR
from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
@ -34,7 +34,7 @@ def process_hud_alert(enabled, fingerprint, visual_alert, left_lane,
class CarController():
def __init__(self, dbc_name, CP, VM):
self.p = SteerLimitParams(CP)
self.p = CarControllerParams(CP)
self.packer = CANPacker(dbc_name)
self.apply_steer_last = 0

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

@ -5,7 +5,7 @@ from selfdrive.car import dbc_dict
Ecu = car.CarParams.Ecu
# Steer torque limits
class SteerLimitParams:
class CarControllerParams:
def __init__(self, CP):
if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70, CAR.IONIQ_EV_2020]:
self.STEER_MAX = 384
@ -123,7 +123,7 @@ FINGERPRINTS = {
}],
CAR.IONIQ_EV_2020: [{
127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 524: 8, 544: 7, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1164: 8, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8, 2013: 8
}],
}],
CAR.IONIQ_EV_LTD: [{
127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 7, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1407: 8, 1419: 8, 1425: 2, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1507: 8, 1535: 8
}],

6
selfdrive/car/mazda/carcontroller.py
View File

@ -1,5 +1,5 @@
from selfdrive.car.mazda import mazdacan
from selfdrive.car.mazda.values import SteerLimitParams, Buttons
from selfdrive.car.mazda.values import CarControllerParams, Buttons
from opendbc.can.packer import CANPacker
from selfdrive.car import apply_std_steer_torque_limits
@ -18,9 +18,9 @@ class CarController():
if enabled:
# calculate steer and also set limits due to driver torque
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last,
CS.out.steeringTorque, SteerLimitParams)
CS.out.steeringTorque, CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
if CS.out.standstill and frame % 20 == 0:

2
selfdrive/car/mazda/values.py
View File

@ -7,7 +7,7 @@ Ecu = car.CarParams.Ecu
# Steer torque limits
class SteerLimitParams:
class CarControllerParams:
STEER_MAX = 600 # max_steer 2048
STEER_STEP = 1 # how often we update the steer cmd
STEER_DELTA_UP = 10 # torque increase per refresh

17
selfdrive/car/nissan/carcontroller.py
View File

@ -2,13 +2,8 @@ from cereal import car
from common.numpy_fast import clip, interp
from selfdrive.car.nissan import nissancan
from opendbc.can.packer import CANPacker
from selfdrive.car.nissan.values import CAR, STEER_THRESHOLD
from selfdrive.car.nissan.values import CAR, CarControllerParams
# Steer angle limits
ANGLE_DELTA_BP = [0., 5., 15.]
ANGLE_DELTA_V = [5., .8, .15] # windup limit
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower
VisualAlert = car.CarControl.HUDControl.VisualAlert
@ -41,22 +36,22 @@ class CarController():
if enabled:
# # windup slower
if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle):
angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP, ANGLE_DELTA_V)
angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V)
else:
angle_rate_lim = interp(CS.out.vEgo, ANGLE_DELTA_BP, ANGLE_DELTA_VU)
angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_VU)
apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim)
# Max torque from driver before EPS will give up and not apply torque
if not bool(CS.out.steeringPressed):
self.lkas_max_torque = LKAS_MAX_TORQUE
self.lkas_max_torque = CarControllerParams.LKAS_MAX_TORQUE
else:
# Scale max torque based on how much torque the driver is applying to the wheel
self.lkas_max_torque = max(
# Scale max torque down to half LKAX_MAX_TORQUE as a minimum
LKAS_MAX_TORQUE * 0.5,
CarControllerParams.LKAS_MAX_TORQUE * 0.5,
# Start scaling torque at STEER_THRESHOLD
LKAS_MAX_TORQUE - 0.6 * max(0, abs(CS.out.steeringTorque) - STEER_THRESHOLD)
CarControllerParams.LKAS_MAX_TORQUE - 0.6 * max(0, abs(CS.out.steeringTorque) - CarControllerParams.STEER_THRESHOLD)
)
else:

4
selfdrive/car/nissan/carstate.py
View File

@ -5,7 +5,7 @@ from opendbc.can.can_define import CANDefine
from selfdrive.car.interfaces import CarStateBase
from selfdrive.config import Conversions as CV
from opendbc.can.parser import CANParser
from selfdrive.car.nissan.values import CAR, DBC, STEER_THRESHOLD
from selfdrive.car.nissan.values import CAR, DBC, CarControllerParams
TORQUE_SAMPLES = 12
@ -65,7 +65,7 @@ class CarState(CarStateBase):
ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"]
self.steeringTorqueSamples.append(ret.steeringTorque)
# Filtering driver torque to prevent steeringPressed false positives
ret.steeringPressed = bool(abs(sum(self.steeringTorqueSamples) / TORQUE_SAMPLES) > STEER_THRESHOLD)
ret.steeringPressed = bool(abs(sum(self.steeringTorqueSamples) / TORQUE_SAMPLES) > CarControllerParams.STEER_THRESHOLD)
ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"]

8
selfdrive/car/nissan/values.py
View File

@ -2,7 +2,13 @@
from selfdrive.car import dbc_dict
STEER_THRESHOLD = 1.0
class CarControllerParams:
ANGLE_DELTA_BP = [0., 5., 15.]
ANGLE_DELTA_V = [5., .8, .15] # windup limit
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower
STEER_THRESHOLD = 1.0
class CAR:
XTRAIL = "NISSAN X-TRAIL 2017"

24
selfdrive/car/subaru/carcontroller.py
View File

@ -1,20 +1,9 @@
from selfdrive.car import apply_std_steer_torque_limits
from selfdrive.car.subaru import subarucan
from selfdrive.car.subaru.values import DBC, PREGLOBAL_CARS
from selfdrive.car.subaru.values import DBC, PREGLOBAL_CARS, CarControllerParams
from opendbc.can.packer import CANPacker
class CarControllerParams():
def __init__(self):
self.STEER_MAX = 2047 # max_steer 4095
self.STEER_STEP = 2 # how often we update the steer cmd
self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max
self.STEER_DELTA_DOWN = 70 # torque decrease per refresh
self.STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting
self.STEER_DRIVER_MULTIPLIER = 10 # weight driver torque heavily
self.STEER_DRIVER_FACTOR = 1 # from dbc
class CarController():
def __init__(self, dbc_name, CP, VM):
self.apply_steer_last = 0
@ -24,7 +13,6 @@ class CarController():
self.fake_button_prev = 0
self.steer_rate_limited = False
self.params = CarControllerParams()
self.packer = CANPacker(DBC[CP.carFingerprint]['pt'])
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line):
@ -32,23 +20,23 @@ class CarController():
can_sends = []
# *** steering ***
if (frame % self.params.STEER_STEP) == 0:
if (frame % CarControllerParams.STEER_STEP) == 0:
apply_steer = int(round(actuators.steer * self.params.STEER_MAX))
apply_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
# limits due to driver torque
new_steer = int(round(apply_steer))
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
if not enabled:
apply_steer = 0
if CS.CP.carFingerprint in PREGLOBAL_CARS:
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, frame, self.params.STEER_STEP))
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, frame, CarControllerParams.STEER_STEP))
else:
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, self.params.STEER_STEP))
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, CarControllerParams.STEER_STEP))
self.apply_steer_last = apply_steer

9
selfdrive/car/subaru/values.py
View File

@ -4,6 +4,15 @@ from selfdrive.car import dbc_dict
from cereal import car
Ecu = car.CarParams.Ecu
class CarControllerParams:
STEER_MAX = 2047 # max_steer 4095
STEER_STEP = 2 # how often we update the steer cmd
STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max
STEER_DELTA_DOWN = 70 # torque decrease per refresh
STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting
STEER_DRIVER_MULTIPLIER = 10 # weight driver torque heavily
STEER_DRIVER_FACTOR = 1 # from dbc
class CAR:
ASCENT = "SUBARU ASCENT LIMITED 2019"
IMPREZA = "SUBARU IMPREZA LIMITED 2019"

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

@ -4,16 +4,11 @@ from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command,
from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
create_accel_command, create_acc_cancel_command, \
create_fcw_command
from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, NO_STOP_TIMER_CAR, SteerLimitParams
from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, NO_STOP_TIMER_CAR, CarControllerParams
from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
# Accel limits
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value
ACCEL_MAX = 1.5 # 1.5 m/s2
ACCEL_MIN = -3.0 # 3 m/s2
ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
def accel_hysteresis(accel, accel_steady, enabled):
@ -21,10 +16,10 @@ def accel_hysteresis(accel, accel_steady, enabled):
if not enabled:
# send 0 when disabled, otherwise acc faults
accel_steady = 0.
elif accel > accel_steady + ACCEL_HYST_GAP:
accel_steady = accel - ACCEL_HYST_GAP
elif accel < accel_steady - ACCEL_HYST_GAP:
accel_steady = accel + ACCEL_HYST_GAP
elif accel > accel_steady + CarControllerParams.ACCEL_HYST_GAP:
accel_steady = accel - CarControllerParams.ACCEL_HYST_GAP
elif accel < accel_steady - CarControllerParams.ACCEL_HYST_GAP:
accel_steady = accel + CarControllerParams.ACCEL_HYST_GAP
accel = accel_steady
return accel, accel_steady
@ -65,11 +60,11 @@ class CarController():
apply_accel = actuators.gas - actuators.brake
apply_accel, self.accel_steady = accel_hysteresis(apply_accel, self.accel_steady, enabled)
apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX)
apply_accel = clip(apply_accel * CarControllerParams.ACCEL_SCALE, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams)
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, CarControllerParams)
self.steer_rate_limited = new_steer != apply_steer
# Cut steering while we're in a known fault state (2s)

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

@ -1,7 +1,7 @@
#!/usr/bin/env python3
from cereal import car
from selfdrive.config import Conversions as CV
from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, TSS2_CAR, FINGERPRINTS
from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, TSS2_CAR, FINGERPRINTS, CarControllerParams
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint
from selfdrive.swaglog import cloudlog
from selfdrive.car.interfaces import CarInterfaceBase
@ -12,7 +12,7 @@ EventName = car.CarEvent.EventName
class CarInterface(CarInterfaceBase):
@staticmethod
def compute_gb(accel, speed):
return float(accel) / 3.0
return float(accel) / CarControllerParams.ACCEL_SCALE
@staticmethod
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]): # pylint: disable=dangerous-default-value

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

@ -4,8 +4,12 @@ from selfdrive.car import dbc_dict
from cereal import car
Ecu = car.CarParams.Ecu
# Steer torque limits
class SteerLimitParams:
class CarControllerParams:
ACCEL_HYST_GAP = 0.02 # don't change accel command for small oscilalitons within this value
ACCEL_MAX = 1.5 # 1.5 m/s2
ACCEL_MIN = -3.0 # 3 m/s2
ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
STEER_MAX = 1500
STEER_DELTA_UP = 10 # 1.5s time to peak torque
STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
@ -484,7 +488,7 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x750, 109): [
b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
],
},
},
CAR.CHR: {
(Ecu.engine, 0x700, None): [
b'\x01896631017100\x00\x00\x00\x00',

6
selfdrive/controls/lib/latcontrol_indi.py
View File

@ -4,7 +4,7 @@ import numpy as np
from cereal import log
from common.realtime import DT_CTRL
from common.numpy_fast import clip
from selfdrive.car.toyota.values import SteerLimitParams
from selfdrive.car.toyota.values import CarControllerParams
from selfdrive.car import apply_toyota_steer_torque_limits
from selfdrive.controls.lib.drive_helpers import get_steer_max
@ -97,10 +97,10 @@ class LatControlINDI():
# Enforce rate limit
if self.enforce_rate_limit:
steer_max = float(SteerLimitParams.STEER_MAX)
steer_max = float(CarControllerParams.STEER_MAX)
new_output_steer_cmd = steer_max * (self.delayed_output + delta_u)
prev_output_steer_cmd = steer_max * self.output_steer
new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, SteerLimitParams)
new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, CarControllerParams)
self.output_steer = new_output_steer_cmd / steer_max
else:
self.output_steer = self.delayed_output + delta_u