diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index b81526e9c0..3ab2ff842a 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -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 diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 88110f0530..46d33f58eb 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -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 diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 9b8e68fee4..dcd300c70c 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -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. diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index 5e50e6166e..9f8d9fa759 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -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" diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index b73af39a66..4d1c60d4ab 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -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): diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index e7e0efdb61..c89dce09aa 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -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 = { diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 69c388fe8e..dc14bfb342 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -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 diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 032502734e..ac6fed94c0 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -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 }], diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 20d80e519a..36a0495fd0 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -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: diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index 81c2b2b4a4..3060cef5f0 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -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 diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 5838d67ae3..6e8dc3d579 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -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: diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index f2076183d9..e3a418d3cf 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -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"] diff --git a/selfdrive/car/nissan/values.py b/selfdrive/car/nissan/values.py index ea4af219ea..8b5f77a746 100644 --- a/selfdrive/car/nissan/values.py +++ b/selfdrive/car/nissan/values.py @@ -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" diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 744fd0bcc9..a15e03b58b 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -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 diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 88c4ec6901..9b3ff060d2 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -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" diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index b76b66b3e9..a3a04dcc9a 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -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) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index abd3ef4765..a22d836010 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -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 diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index f0505c66a9..1ab37580d6 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -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', diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 51872f6d0f..0e685fd1a4 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -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