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)