controlsd: set latActive with max minimum steer speed (#26805)

* refactor minimum lateral speed handling

* rename for clarity

* simplify without joystick at standstill

* intermediate standstill variable, check notCar

* check joystick for now

* cmt

Co-authored-by: Shane Smiskol <shane@smiskol.com>

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

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):

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:

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

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()

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: