From 91f216735a7ccd6394fac01330c5052eb415c6a1 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 7 Jul 2023 18:02:44 -0700 Subject: [PATCH] clean up --- selfdrive/car/toyota/carcontroller.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 1e3c3b773e..241d64b078 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -79,12 +79,12 @@ 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 rate limit - # if torque is above limit, force angle to lower - # TODO: tune this, also won't work well near 0. multiplier instead of offset? - max_down_limit = max(self.params.ANGLE_RATE_LIMIT_DOWN.angle_v) + # 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, -max_down_limit]) + [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