From cc96523cc28b82e74018011e5a476be2e6f68d92 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 7 Jul 2023 19:14:06 -0700 Subject: [PATCH] maybe --- selfdrive/car/toyota/carcontroller.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 241d64b078..fb4f7b0939 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -79,14 +79,15 @@ class CarController: # EPS uses the torque sensor angle to control with, offset to compensate apply_angle = actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg - # If the EPS output torque is above the limit, force the requested - # angle to lower at the max allowed rate - angle_down_limit = interp(CS.out.vEgo, self.params.ANGLE_RATE_LIMIT_DOWN.speed_bp, - self.params.ANGLE_RATE_LIMIT_DOWN.angle_v) - max_torque_angle_mod = interp(abs(CS.out.steeringTorqueEps), - [MAX_STEER_TORQUE, MAX_STEER_TORQUE + 100], [0, -angle_down_limit]) - sign_of = 1 if apply_angle >= 0 else -1 - apply_angle += max_torque_angle_mod * sign_of + # If the EPS output torque is above the limit, wind down the last + # requested angle until it's under the limit again + if abs(CS.out.steeringTorqueEps) > MAX_STEER_TORQUE: + angle_down_limit = interp(CS.out.vEgo, self.params.ANGLE_RATE_LIMIT_DOWN.speed_bp, + self.params.ANGLE_RATE_LIMIT_DOWN.angle_v) + max_torque_angle_mod = interp(abs(CS.out.steeringTorqueEps), + [MAX_STEER_TORQUE, MAX_STEER_TORQUE + 100], [0, -angle_down_limit]) + sign_of = 1 if apply_angle >= 0 else -1 + apply_angle += max_torque_angle_mod * sign_of # Angular rate limit based on speed apply_angle = apply_std_steer_angle_limits(apply_angle, self.last_angle, CS.out.vEgo, self.params)