From f5f5355ce4d3377dc539632a61c7cd9d43816f94 Mon Sep 17 00:00:00 2001 From: Jason Young Date: Sat, 22 Feb 2025 20:14:34 -0500 Subject: [PATCH] controls: publish current curvature in CarControl --- selfdrive/controls/controlsd.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index c215a42a6d..a5c63ccc8d 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -131,8 +131,13 @@ class Controls: def publish(self, CC, lac_log): CS = self.sm['carState'] + lp = self.sm['liveParameters'] + steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) + current_curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) + # Orientation and angle rates can be useful for carcontroller # Only calibrated (car) frame is relevant for the carcontroller + CC.currentCurvature = current_curvature if self.calibrated_pose is not None: CC.orientationNED = self.calibrated_pose.orientation.xyz.tolist() CC.angularVelocity = self.calibrated_pose.angular_velocity.xyz.tolist() @@ -174,10 +179,7 @@ class Controls: dat.valid = CS.canValid cs = dat.controlsState - lp = self.sm['liveParameters'] - steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) - cs.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) - + cs.curvature = current_curvature cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2'] cs.desiredCurvature = float(self.desired_curvature)