From 5eb269cc6f6a67813d1cc66ae07c5c4926ea033d Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 26 Mar 2025 14:49:07 -0700 Subject: [PATCH] reset curvature while disabled --- selfdrive/controls/controlsd.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index ae5db046e1..8bace6c923 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -41,6 +41,7 @@ class Controls: self.pm = messaging.PubMaster(['carControl', 'controlsState']) self.steer_limited_by_controls = False + self.curvature = 0.0 self.desired_curvature = 0.0 self.pose_calibrator = PoseCalibrator() @@ -110,7 +111,15 @@ class Controls: actuators.accel = float(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits)) # Steering PID loop and lateral MPC - self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature, lp.roll) + lp = self.sm['liveParameters'] + steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) + self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) + + if CC.latActive: + self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature, lp.roll) + else: + self.desired_curvature, curvature_limited = self.curvature, False + actuators.curvature = self.desired_curvature steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, self.steer_limited_by_controls, self.desired_curvature, @@ -175,10 +184,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 = self.curvature cs.longitudinalPlanMonoTime = self.sm.logMonoTime['longitudinalPlan'] cs.lateralPlanMonoTime = self.sm.logMonoTime['modelV2'] cs.desiredCurvature = self.desired_curvature