|
|
|
|
# functions common among cars
|
|
|
|
|
from common.numpy_fast import clip
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def dbc_dict(pt_dbc, radar_dbc, chassis_dbc=None):
|
|
|
|
|
return {'pt': pt_dbc, 'radar': radar_dbc, 'chassis': chassis_dbc}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
def apply_std_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
|
|
|
|
|
driver_min_torque = -LIMITS.STEER_MAX + (-LIMITS.STEER_DRIVER_ALLOWANCE + driver_torque * LIMITS.STEER_DRIVER_FACTOR) * LIMITS.STEER_DRIVER_MULTIPLIER
|
|
|
|
|
max_steer_allowed = max(min(LIMITS.STEER_MAX, driver_max_torque), 0)
|
|
|
|
|
min_steer_allowed = min(max(-LIMITS.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.STEER_DELTA_DOWN, -LIMITS.STEER_DELTA_UP),
|
|
|
|
|
apply_torque_last + LIMITS.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))
|
|
|
|
|
|
|
|
|
|
return int(round(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)
|
|
|
|
|
|
|
|
|
|
apply_torque = clip(apply_torque, 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)
|
|
|
|
|
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))
|
|
|
|
|
|
|
|
|
|
return int(round(apply_torque))
|
