From 4bcf4a13501366efe7e77e453a6d52ab491c6116 Mon Sep 17 00:00:00 2001 From: MoreTore Date: Sat, 26 Apr 2025 12:35:54 -0500 Subject: [PATCH] apply_ti_steer_torque_limits --- selfdrive/car/__init__.py | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index b918b684a..a7289a81e 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -108,6 +108,28 @@ def apply_driver_steer_torque_limits(apply_torque, apply_torque_last, driver_tor return int(round(float(apply_torque))) +#alternate settings when using torque interceptor. May or may not be useful to some users/branches. +def apply_ti_steer_torque_limits(apply_torque, apply_torque_last, driver_torque, LIMITS): + + # limits due to driver torque + driver_max_torque = LIMITS.TI_STEER_MAX + (LIMITS.TI_STEER_DRIVER_ALLOWANCE + + driver_torque * LIMITS.TI_STEER_DRIVER_FACTOR) * LIMITS.TI_STEER_DRIVER_MULTIPLIER + driver_min_torque = -LIMITS.TI_STEER_MAX + (-LIMITS.TI_STEER_DRIVER_ALLOWANCE + driver_torque * + LIMITS.TI_STEER_DRIVER_FACTOR) * LIMITS.TI_STEER_DRIVER_MULTIPLIER + max_steer_allowed = max(min(LIMITS.TI_STEER_MAX, driver_max_torque), 0) + min_steer_allowed = min(max(-LIMITS.TI_STEER_MAX, driver_min_torque), 0) + apply_torque = clip(apply_torque, min_steer_allowed, max_steer_allowed) + + # slow rate if steer torque increases in magnitude + if apply_torque_last > 0: + apply_torque = clip(apply_torque, max(apply_torque_last - LIMITS.TI_STEER_DELTA_DOWN, -LIMITS.TI_STEER_DELTA_UP), + apply_torque_last + LIMITS.TI_STEER_DELTA_UP) + else: + apply_torque = clip(apply_torque, apply_torque_last - LIMITS.TI_STEER_DELTA_UP, + min(apply_torque_last + LIMITS.TI_STEER_DELTA_DOWN, LIMITS.TI_STEER_DELTA_UP)) + + return int(round(float(apply_torque))) + def apply_dist_to_meas_limits(val, val_last, val_meas, STEER_DELTA_UP, STEER_DELTA_DOWN,