From cfa24696d7583fd2bedf7acffa1ba228a3387e79 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Wed, 26 Mar 2025 22:23:07 -0700 Subject: [PATCH] duplicate line --- selfdrive/controls/controlsd.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f5b0573e5f..20ac896dc8 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -74,6 +74,9 @@ class Controls: sr = max(lp.steerRatio, 0.1) self.VM.update_params(x, sr) + steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg) + self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll) + # Update Torque Params if self.CP.lateralTuning.which() == 'torque': torque_params = self.sm['liveTorqueParameters'] @@ -111,14 +114,10 @@ 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 - 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) - - # Reset desired curvature to current to avoid violating the limits on engage if CC.latActive: self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, model_v2.action.desiredCurvature, lp.roll) else: + # Reset desired curvature to current to avoid violating the limits on engage self.desired_curvature, curvature_limited = self.curvature, False actuators.curvature = self.desired_curvature