diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 1b249a3d1a..f94dca57c1 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -10,6 +10,7 @@ class LatControlAngle(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) self.sat_check_min_speed = 5. + self.use_steer_limited_by_controls = CP.brand == "tesla" def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): angle_log = log.ControlsState.LateralAngleState.new_message() @@ -22,7 +23,13 @@ class LatControlAngle(LatControl): angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) angle_steers_des += params.angleOffsetDeg - angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD + if self.use_steer_limited_by_controls: + # these cars' carcontrolers calculate max lateral accel and jerk, so we can rely on carOutput for saturation + angle_control_saturated = steer_limited_by_controls + else: + # for cars which use a method of limiting torque such as a torque signal (Nissan and Toyota) + # or relying on EPS (Ford Q3), carOutput does not capture maxing out torque # TODO: this can be improved + angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited)) angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) angle_log.steeringAngleDesiredDeg = angle_steers_des