From a68e5ff6d82e471a0a998b4d72007ff67abd00b6 Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Thu, 24 Jul 2025 17:45:39 +0200 Subject: [PATCH] Update latcontrol_curvature.py --- selfdrive/controls/lib/latcontrol_curvature.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index 9fce93878..8e58e2413 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -3,7 +3,7 @@ import numpy as np from cereal import log from openpilot.selfdrive.controls.lib.latcontrol import LatControl -from openpilot.common.pid import PIDController +from openpilot.common.pid_mu import MultiplicativeUnwindPID CURVATURE_SATURATION_THRESHOLD = 5e-4 # rad/m @@ -12,9 +12,9 @@ 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) + self.pid = MultiplicativeUnwindPID((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) def reset(self): super().reset() @@ -40,7 +40,7 @@ class LatControlCurvature(LatControl): freeze_integrator = steer_limited_by_controls or CS.vEgo < 5 or CS.steeringPressed pid_curvature = self.pid.update(pid_log.error, feedforward=desired_curvature_corr, speed=CS.vEgo, - freeze_integrator=freeze_integrator) #, override=CS.steeringPressed) + freeze_integrator=freeze_integrator, override=CS.steeringPressed) output_curvature = pid_curvature + (CS.curvature - actual_curvature_vm_no_roll) if self.useCarCurvature else pid_curvature