From 4236acd9e120c64d26788745c4488b8f7d11a3b5 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Fri, 7 Jul 2023 19:07:30 -0700 Subject: [PATCH] Revert "there was a bug, need to subtract from last_angle essentially" This reverts commit a595e4af64523665519fd450dbeb2fefd293ce2b. --- selfdrive/car/toyota/carcontroller.py | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 08b44d4ec4..241d64b078 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -77,18 +77,16 @@ class CarController: apply_steer_req = 0 if self.frame % 2 == 0: # EPS uses the torque sensor angle to control with, offset to compensate - new_angle = actuators.steeringAngleDeg + CS.out.steeringAngleOffsetDeg - angle_diff = new_angle - self.last_angle + 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) - sign_of = 1 if new_angle >= 0 else -1 - angle_diff = interp(abs(CS.out.steeringTorqueEps), - [MAX_STEER_TORQUE, MAX_STEER_TORQUE + 100], - [angle_diff, -angle_down_limit * sign_of]) - apply_angle = self.last_angle + angle_diff + 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)