diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b5d5e3d8a2..1d92cd4fb2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -17,7 +17,7 @@ from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can from selfdrive.controls.lib.lateral_planner import CAMERA_OFFSET from selfdrive.controls.lib.drive_helpers import VCruiseHelper, get_lag_adjusted_curvature -from selfdrive.controls.lib.latcontrol import LatControl +from selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED from selfdrive.controls.lib.longcontrol import LongControl from selfdrive.controls.lib.latcontrol_pid import LatControlPID from selfdrive.controls.lib.latcontrol_indi import LatControlINDI @@ -569,9 +569,11 @@ class Controls: CC = car.CarControl.new_message() CC.enabled = self.enabled + # Check which actuators can be enabled + standstill = CS.vEgo <= max(self.CP.minSteerSpeed, MIN_LATERAL_CONTROL_SPEED) or CS.standstill CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \ - CS.vEgo > self.CP.minSteerSpeed and not CS.standstill + (not standstill or self.joystick_mode) CC.longActive = self.enabled and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl actuators = CC.actuators diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index cefbc1078c..d38959c560 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -3,7 +3,7 @@ from abc import abstractmethod, ABC from common.numpy_fast import clip from common.realtime import DT_CTRL -MIN_STEER_SPEED = 0.3 +MIN_LATERAL_CONTROL_SPEED = 0.3 # m/s class LatControl(ABC): diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index d692f80b4b..9ed140d38e 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -1,7 +1,7 @@ import math from cereal import log -from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.latcontrol import LatControl STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees @@ -14,7 +14,7 @@ class LatControlAngle(LatControl): def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): angle_log = log.ControlsState.LateralAngleState.new_message() - if CS.vEgo < MIN_STEER_SPEED or not active: + if not active: angle_log.active = False angle_steers_des = float(CS.steeringAngleDeg) else: diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 2bc3cef76b..dca82c6729 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -5,7 +5,7 @@ from cereal import log from common.filter_simple import FirstOrderFilter from common.numpy_fast import clip, interp from common.realtime import DT_CTRL -from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.latcontrol import LatControl class LatControlINDI(LatControl): @@ -82,7 +82,7 @@ class LatControlINDI(LatControl): rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0) indi_log.steeringRateDesiredDeg = math.degrees(rate_des) - if CS.vEgo < MIN_STEER_SPEED or not active: + if not active: indi_log.active = False self.steer_filter.x = 0.0 output_steer = 0 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 6bd678073e..6696d2e304 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -1,7 +1,7 @@ import math from cereal import log -from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.latcontrol import LatControl from selfdrive.controls.lib.pid import PIDController @@ -28,7 +28,7 @@ class LatControlPID(LatControl): pid_log.steeringAngleDesiredDeg = angle_steers_des pid_log.angleError = error - if CS.vEgo < MIN_STEER_SPEED or not active: + if not active: output_steer = 0.0 pid_log.active = False self.pid.reset() diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index d10d39d945..2f56094379 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -2,7 +2,7 @@ import math from cereal import log from common.numpy_fast import interp -from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +from selfdrive.controls.lib.latcontrol import LatControl from selfdrive.controls.lib.pid import PIDController from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY @@ -39,7 +39,7 @@ class LatControlTorque(LatControl): def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk): pid_log = log.ControlsState.LateralTorqueState.new_message() - if CS.vEgo < MIN_STEER_SPEED or not active: + if not active: output_torque = 0.0 pid_log.active = False else: