sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Move all CarControllerParams into values.py (#19663)

Browse Source 
* toyota

* use scale from values.py

* nissan

* subaru

* honda

* gm

* toyota combine into CarControllerParams

* nissan refactor

* chrysler refactor

* mazda refactor

* hyundai refactor

* subaru refactor
pull/19657/head^2
Willem Melching 4 years ago 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
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      selfdrive/car/chrysler/carcontroller.py
  2. 2
      selfdrive/car/chrysler/values.py
  3. 30
      selfdrive/car/gm/carcontroller.py
  4. 27
      selfdrive/car/gm/values.py
  5. 11
      selfdrive/car/honda/carcontroller.py
  6. 14
      selfdrive/car/honda/values.py
  7. 4
      selfdrive/car/hyundai/carcontroller.py
  8. 4
      selfdrive/car/hyundai/values.py
  9. 6
      selfdrive/car/mazda/carcontroller.py
  10. 2
      selfdrive/car/mazda/values.py
  11. 17
      selfdrive/car/nissan/carcontroller.py
  12. 4
      selfdrive/car/nissan/carstate.py
  13. 8
      selfdrive/car/nissan/values.py
  14. 24
      selfdrive/car/subaru/carcontroller.py
  15. 9
      selfdrive/car/subaru/values.py
  16. 21
      selfdrive/car/toyota/carcontroller.py
  17. 4
      selfdrive/car/toyota/interface.py
  18. 10
      selfdrive/car/toyota/values.py
  19. 6
      selfdrive/controls/lib/latcontrol_indi.py

6
selfdrive/car/chrysler/carcontroller.py
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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

Write Preview
Loading…
Cancel
Save