diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index a90f023ed1..58cde85817 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -73,7 +73,7 @@ def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None, body_dbc=None) -> Dict[str, st return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc, 'body': body_dbc} -def apply_std_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS): +def apply_driver_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS): # limits due to driver torque driver_max_torque = LIMITS.STEER_MAX + (LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER @@ -93,24 +93,32 @@ def apply_std_steer_torque_limits(apply_torque, apply_torque_last, driver_torque return int(round(float(apply_torque))) -def apply_toyota_steer_torque_limits(apply_torque, apply_torque_last, motor_torque, LIMITS): - # limits due to comparison of commanded torque VS motor reported torque - max_lim = min(max(motor_torque + LIMITS.STEER_ERROR_MAX, LIMITS.STEER_ERROR_MAX), LIMITS.STEER_MAX) - min_lim = max(min(motor_torque - LIMITS.STEER_ERROR_MAX, -LIMITS.STEER_ERROR_MAX), -LIMITS.STEER_MAX) +def apply_dist_to_meas_limits(val, val_last, val_meas, + STEER_DELTA_UP, STEER_DELTA_DOWN, + STEER_ERROR_MAX, STEER_MAX): + # limits due to comparison of commanded val VS measured val (torque/angle/curvature) + max_lim = min(max(val_meas + STEER_ERROR_MAX, STEER_ERROR_MAX), STEER_MAX) + min_lim = max(min(val_meas - STEER_ERROR_MAX, -STEER_ERROR_MAX), -STEER_MAX) - apply_torque = clip(apply_torque, min_lim, max_lim) + val = clip(val, min_lim, max_lim) - # slow rate if steer torque increases in magnitude - if apply_torque_last > 0: - apply_torque = clip(apply_torque, - max(apply_torque_last - LIMITS.STEER_DELTA_DOWN, -LIMITS.STEER_DELTA_UP), - apply_torque_last + LIMITS.STEER_DELTA_UP) + # slow rate if val increases in magnitude + if val_last > 0: + val = clip(val, + max(val_last - STEER_DELTA_DOWN, -STEER_DELTA_UP), + val_last + STEER_DELTA_UP) else: - apply_torque = clip(apply_torque, - apply_torque_last - LIMITS.STEER_DELTA_UP, - min(apply_torque_last + LIMITS.STEER_DELTA_DOWN, LIMITS.STEER_DELTA_UP)) + val = clip(val, + val_last - STEER_DELTA_UP, + min(val_last + STEER_DELTA_DOWN, STEER_DELTA_UP)) - return int(round(float(apply_torque))) + return float(val) + + +def apply_meas_steer_torque_limits(apply_torque, apply_torque_last, motor_torque, LIMITS): + return int(round(apply_dist_to_meas_limits(apply_torque, apply_torque_last, motor_torque, + LIMITS.STEER_DELTA_UP, LIMITS.STEER_DELTA_DOWN, + LIMITS.STEER_ERROR_MAX, LIMITS.STEER_MAX))) def apply_std_steer_angle_limits(apply_angle, apply_angle_last, v_ego, LIMITS): diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index 20a44bce21..b418179e0e 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -1,6 +1,6 @@ from opendbc.can.packer import CANPacker from common.realtime import DT_CTRL -from selfdrive.car import apply_toyota_steer_torque_limits +from selfdrive.car import apply_meas_steer_torque_limits from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, create_cruise_buttons from selfdrive.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags @@ -67,7 +67,7 @@ class CarController: # steer torque new_steer = int(round(CC.actuators.steer * self.params.STEER_MAX)) - apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorqueEps, self.params) + apply_steer = apply_meas_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorqueEps, self.params) if not lkas_active or not lkas_control_bit: apply_steer = 0 self.apply_steer_last = apply_steer diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 42eaf7e88a..2a996c0ff6 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -3,7 +3,7 @@ from common.conversions import Conversions as CV from common.numpy_fast import interp from common.realtime import DT_CTRL from opendbc.can.packer import CANPacker -from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.gm import gmcan from selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons @@ -73,7 +73,7 @@ class CarController: if CC.latActive: new_steer = int(round(actuators.steer * self.params.STEER_MAX)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) + apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) else: apply_steer = 0 diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 1e6f78af20..2572038492 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -3,7 +3,7 @@ from common.conversions import Conversions as CV from common.numpy_fast import clip from common.realtime import DT_CTRL from opendbc.can.packer import CANPacker -from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.hyundai import hyundaicanfd, hyundaican from selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR @@ -60,7 +60,7 @@ class CarController: # steering torque new_steer = int(round(actuators.steer * self.params.STEER_MAX)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) + apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params) if not CC.latActive: apply_steer = 0 diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 524a02a370..cb401a8abd 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -1,6 +1,6 @@ from cereal import car from opendbc.can.packer import CANPacker -from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.mazda import mazdacan from selfdrive.car.mazda.values import CarControllerParams, Buttons @@ -23,8 +23,8 @@ class CarController: if CC.latActive: # calculate steer and also set limits due to driver torque new_steer = int(round(CC.actuators.steer * CarControllerParams.STEER_MAX)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, - CS.out.steeringTorque, CarControllerParams) + apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, + CS.out.steeringTorque, CarControllerParams) if CC.cruiseControl.cancel: # If brake is pressed, let us wait >70ms before trying to disable crz to avoid diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 24d85877d7..a6dbf4a39e 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,5 +1,5 @@ from opendbc.can.packer import CANPacker -from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.subaru import subarucan from selfdrive.car.subaru.values import DBC, GLOBAL_GEN2, PREGLOBAL_CARS, CarControllerParams @@ -34,7 +34,7 @@ class CarController: # 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.p) + apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) if not CC.latActive: apply_steer = 0 diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 2e0d7009c8..8004ea9dca 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,6 +1,6 @@ from cereal import car from common.numpy_fast import clip, interp -from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg +from selfdrive.car import apply_meas_steer_torque_limits, create_gas_interceptor_command, make_can_msg from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ create_accel_command, create_acc_cancel_command, \ create_fcw_command, create_lta_steer_command @@ -60,7 +60,7 @@ class CarController: # steer torque new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX)) - apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits) + apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits) # Count up to MAX_STEER_RATE_FRAMES, at which point we need to cut torque to avoid a steering fault if lat_active and abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE: diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 5d00b5a52f..c17b632450 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -3,7 +3,7 @@ from opendbc.can.packer import CANPacker from common.numpy_fast import clip from common.conversions import Conversions as CV from common.realtime import DT_CTRL -from selfdrive.car import apply_std_steer_torque_limits +from selfdrive.car import apply_driver_steer_torque_limits from selfdrive.car.volkswagen import mqbcan, pqcan from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams @@ -44,7 +44,7 @@ class CarController: if CC.latActive: new_steer = int(round(actuators.steer * self.CCP.STEER_MAX)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.CCP) + apply_steer = apply_driver_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.CCP) if apply_steer == 0: hcaEnabled = False self.hcaEnabledFrameCount = 0