diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 3840d6e0de..59b6b6c6a0 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -21,7 +21,7 @@ from selfdrive.controls.lib.latcontrol import LatControl from selfdrive.controls.lib.longcontrol import LongControl from selfdrive.controls.lib.latcontrol_pid import LatControlPID from selfdrive.controls.lib.latcontrol_indi import LatControlINDI -from selfdrive.controls.lib.latcontrol_angle import LatControlAngle +from selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD from selfdrive.controls.lib.latcontrol_torque import LatControlTorque from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert @@ -722,7 +722,11 @@ class Controls: self.last_actuators, can_sends = self.CI.apply(CC) self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) CC.actuatorsOutput = self.last_actuators - self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2 + if self.CP.steerControlType == car.CarParams.SteerControlType.angle: + self.steer_limited = abs(CC.actuators.steeringAngleDeg - CC.actuatorsOutput.steeringAngleDeg) > \ + STEER_ANGLE_SATURATION_THRESHOLD + else: + self.steer_limited = abs(CC.actuators.steer - CC.actuatorsOutput.steer) > 1e-2 force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \ (self.state == State.softDisabling) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 0e5be4a977..1b74fd4186 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -19,7 +19,7 @@ class LatControlAngle(LatControl): angle_steers_des += params.angleOffsetDeg angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD - angle_log.saturated = self._check_saturation(angle_control_saturated, CS, steer_limited) + angle_log.saturated = self._check_saturation(angle_control_saturated, CS, False) angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) angle_log.steeringAngleDesiredDeg = angle_steers_des return 0, float(angle_steers_des), angle_log