From e9aa331f0fbfe89078ecb525fbee2c833c08c2ae Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sat, 22 Feb 2025 01:43:00 -0800 Subject: [PATCH] set curvature in joystickd (#34668) * set curvature in joystickd * Update joystickd.py * Update joystickd.py --- tools/joystick/joystickd.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/tools/joystick/joystickd.py b/tools/joystick/joystickd.py index d52e80af86..f76466dfce 100755 --- a/tools/joystick/joystickd.py +++ b/tools/joystick/joystickd.py @@ -62,6 +62,11 @@ def joystickd_thread(): cs_msg.valid = True controlsState = cs_msg.controlsState controlsState.lateralControlState.init('debugState') + + lp = sm['liveParameters'] + steer_angle_without_offset = math.radians(sm['carState'].steeringAngleDeg - lp.angleOffsetDeg) + controlsState.curvature = -VM.calc_curvature(steer_angle_without_offset, sm['carState'].vEgo, lp.roll) + pm.send('controlsState', cs_msg) rk.keep_time()