global variable for min control speed

commaai · Aug 19, 2021 · 3d50211 · 3d50211
1 parent 3d04b21
commit 3d50211
Show file tree

Hide file tree

Showing 5 changed files with 10 additions and 8 deletions.
diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py
@@ -1,6 +1,8 @@
 from common.numpy_fast import clip
 from common.realtime import DT_CTRL
 
+MIN_CTRL_SPEED = 0.3
+
 
 class LatControl:
   def __init__(self, CP):

diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py
@@ -1,13 +1,13 @@
 import math
 from cereal import log
-from selfdrive.controls.lib.latcontrol import LatControl
+from selfdrive.controls.lib.latcontrol import LatControl, MIN_CTRL_SPEED
 
 
 class LatControlAngle(LatControl):
   def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvature_rate):
     angle_log = log.ControlsState.LateralAngleState.new_message()
 
-    if CS.vEgo < 0.3 or not active:
+    if CS.vEgo < MIN_CTRL_SPEED or 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
@@ -8,7 +8,7 @@
 from selfdrive.car import apply_toyota_steer_torque_limits
 from selfdrive.car.toyota.values import CarControllerParams
 from selfdrive.controls.lib.drive_helpers import get_steer_max
-from selfdrive.controls.lib.latcontrol import LatControl
+from selfdrive.controls.lib.latcontrol import LatControl, MIN_CTRL_SPEED
 
 
 class LatControlINDI(LatControl):
@@ -83,7 +83,7 @@ def update(self, active, CS, CP, VM, params, curvature, curvature_rate):
 
     steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo)
     steers_des += math.radians(params.angleOffsetDeg)
-    if CS.vEgo < 0.3 or not active:
+    if CS.vEgo < MIN_CTRL_SPEED or not active:
       indi_log.active = False
       self.output_steer = 0.0
       self.steer_filter.x = 0.0

diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py
@@ -5,7 +5,7 @@
 from common.realtime import DT_CTRL
 from cereal import log
 from selfdrive.controls.lib.drive_helpers import get_steer_max
-from selfdrive.controls.lib.latcontrol import LatControl
+from selfdrive.controls.lib.latcontrol import LatControl, MIN_CTRL_SPEED
 
 
 class LatControlLQR(LatControl):
@@ -50,7 +50,7 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur
     e = steering_angle_no_offset - angle_steers_k
     self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
 
-    if CS.vEgo < 0.3 or not active:
+    if CS.vEgo < MIN_CTRL_SPEED or not active:
       lqr_log.active = False
       lqr_output = 0.
       output_steer = 0.

diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py
@@ -2,7 +2,7 @@
 
 from selfdrive.controls.lib.pid import PIController
 from selfdrive.controls.lib.drive_helpers import get_steer_max
-from selfdrive.controls.lib.latcontrol import LatControl
+from selfdrive.controls.lib.latcontrol import LatControl, MIN_CTRL_SPEED
 from cereal import log
 
 
@@ -25,7 +25,7 @@ def update(self, active, CS, CP, VM, params, desired_curvature, desired_curvatur
     angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
     angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
 
-    if CS.vEgo < 0.3 or not active:
+    if CS.vEgo < MIN_CTRL_SPEED or not active:
       output_steer = 0.0
       pid_log.active = False
       self.pid.reset()