From 57466c2fc3a78436e1546e109cd97c16a97839f9 Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Thu, 24 Jul 2025 15:15:03 +0200 Subject: [PATCH] Update latcontrol_curvature.py --- selfdrive/controls/lib/latcontrol_curvature.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index 0c3ad3168..7ab0e1f91 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -11,6 +11,7 @@ CURVATURE_SATURATION_THRESHOLD = 5e-4 # rad/m class LatControlCurvature(LatControl): def __init__(self, CP, CP_SP, CI): super().__init__(CP, CP_SP, CI) + self.useCarCurvature = CP.lateralTuning.pid.carCurvatureCorrection self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), k_f=CP.lateralTuning.pid.kf, pos_limit=self.curvature_max, neg_limit=-self.curvature_max) @@ -25,20 +26,23 @@ class LatControlCurvature(LatControl): output_curvature = 0.0 pid_log.active = False self.pid.reset() - else: - actual_curvature_vm = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) + else: + roll_compensation = -VM.roll_compensation(params.roll, CS.vEgo) + actual_curvature_vm_no_roll = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, 0.) + actual_curvature_vm = actual_curvature_vm_no_roll - roll_compensation assert calibrated_pose is not None actual_curvature_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose]) - roll_compensation = -VM.roll_compensation(params.roll, CS.vEgo) desired_curvature_corr = desired_curvature - roll_compensation - pid_log.error = float(desired_curvature_corr - actual_curvature) + error = desired_curvature_corr - actual_curvature freeze_integrator = steer_limited_by_controls or CS.vEgo < 5 - output_curvature = self.pid.update(pid_log.error, feedforward=desired_curvature_corr, speed=CS.vEgo, - freeze_integrator=freeze_integrator, override=CS.steeringPressed) + pid_curvature = self.pid.update(error, feedforward=desired_curvature_corr, speed=CS.vEgo, + freeze_integrator=freeze_integrator, override=CS.steeringPressed) + + output_curvature = pid_curvature + (CS.curvature - actual_curvature_vm_no_roll) if self.useCarCurvature else pid_curvature pid_log.active = True pid_log.p = float(self.pid.p) @@ -48,5 +52,6 @@ class LatControlCurvature(LatControl): pid_log.actualCurvature = float(actual_curvature) pid_log.desiredCurvature = float(desired_curvature) pid_log.saturated = bool(self._check_saturation(steer_limited_by_controls, CS, False, curvature_limited)) + pid_log.error = float(error) return 0.0, 0.0, output_curvature, pid_log