set curvature in joystickd (#34668)

* set curvature in joystickd * Update joystickd.py * Update joystickd.py
2 months ago · e9aa331f0f
parent a8e416abbe
commit e9aa331f0f
1 changed files with 5 additions and 0 deletions
--- 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()