From 54a2626e4449b69e08a716360493d71062d9a8a0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 4 Sep 2024 16:29:04 -0700 Subject: [PATCH] Joystick: improve angle control (#33475) * calculate max angle/curvature with speed * don't inflate to inf * screw radians * up limit a bit --- selfdrive/controls/controlsd.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c3f1b9ba7e..27cf7c2134 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -38,6 +38,8 @@ LDW_MIN_SPEED = 31 * CV.MPH_TO_MS LANE_DEPARTURE_THRESHOLD = 0.1 CAMERA_OFFSET = 0.04 +JOYSTICK_MAX_LAT_ACCEL = 2.5 # m/s^2 + REPLAY = "REPLAY" in os.environ SIMULATION = "SIMULATION" in os.environ TESTING_CLOSET = "TESTING_CLOSET" in os.environ @@ -591,9 +593,11 @@ class Controls: actuators.accel = 4.0*clip(joystick_axes[0], -1, 1) if CC.latActive: + max_curvature = JOYSTICK_MAX_LAT_ACCEL / max(CS.vEgo ** 2, 1) + max_angle = math.degrees(self.VM.get_steer_from_curvature(max_curvature, CS.vEgo, lp.roll)) + steer = clip(joystick_axes[1], -1, 1) - # max angle is 45 for angle-based cars, max curvature is 0.02 - actuators.steer, actuators.steeringAngleDeg, actuators.curvature = steer, steer * 90., steer * -0.02 + actuators.steer, actuators.steeringAngleDeg, actuators.curvature = steer, steer * max_angle, steer * -max_curvature lac_log.active = self.active lac_log.steeringAngleDeg = CS.steeringAngleDeg