From 10de321e994b4c9ff1a6f71c2328eaf3ba72b781 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Wed, 24 Jun 2026 20:52:50 +0200 Subject: [PATCH] Reimplement Multiplicative Unwind PID (#14) * Multiplicative PID, Saturation lat accel based * safer long mpc stopping * Fix roll * fix * Update opendbc_repo * Update latcontrol_curvature.py --- opendbc_repo | 2 +- .../controls/lib/latcontrol_curvature.py | 32 +++++++++++-------- .../lib/longitudinal_mpc_lib/long_mpc.py | 2 +- 3 files changed, 20 insertions(+), 16 deletions(-) diff --git a/opendbc_repo b/opendbc_repo index 841e8f403..3b5b192cd 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 841e8f4031ace9ed2cd1a2b9a5b6162af59b5777 +Subproject commit 3b5b192cdd4c235165f8ce1de3fe70f7e2d2913c diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index dafe5b3e1..9c4582730 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -2,11 +2,11 @@ import math import numpy as np from cereal import log -from openpilot.common.pid import PIDController +from openpilot.common.pid import MultiplicativeUnwindPID from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE -CURVATURE_SATURATION_THRESHOLD = 1e-3 # 1/m +LAT_ACCEL_SATURATION_THRESHOLD = 0.2 # m/s^2 class LatControlCurvature(LatControl): @@ -17,8 +17,10 @@ class LatControlCurvature(LatControl): self.curvature_correction = 0.0 if CP.lateralTuning.which() == 'pid': ct = CP.lateralTuning.pid - self.pid = PIDController((ct.kpBP, ct.kpV), (ct.kiBP, ct.kiV), - pos_limit=MAX_CURVATURE, neg_limit=-MAX_CURVATURE, rate=1 / dt) + self.pid = MultiplicativeUnwindPID((ct.kpBP, ct.kpV), (ct.kiBP, ct.kiV), + k_f=ct.kf, + pos_limit=MAX_CURVATURE, neg_limit=-MAX_CURVATURE, + rate=1 / dt, min_cmd=1e-6, ki_red_time=0.1) self.kf = ct.kf else: self.pid = None @@ -37,25 +39,26 @@ class LatControlCurvature(LatControl): def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited, lat_delay): curvature_log = log.ControlsState.LateralCurvatureState.new_message() + roll_compensation = -VM.roll_compensation(params.roll, CS.vEgo) actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) - if 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, actual_curvature_pose]) error = desired_curvature - actual_curvature + feedforward = self.kf * desired_curvature if not active: output_curvature = 0.0 curvature_log.active = False if self.pid is not None: - self.pid.reset() - elif self.pid is None or CS.steeringPressed or not self.enable_pid: - # no PID or override: feedforward only + self.pid.reset() + elif self.pid is None or not self.enable_pid: if self.pid is not None: self.pid.reset() - output_curvature = self.kf * desired_curvature + output_curvature = feedforward curvature_log.active = True else: - output_curvature = self.pid.update(error, speed=CS.vEgo, feedforward=self.kf * desired_curvature) + freeze_integrator = steer_limited_by_safety or CS.vEgo < 5 + output_curvature = self.pid.update(error, speed=CS.vEgo, feedforward=feedforward, + override=CS.steeringPressed, + freeze_integrator=freeze_integrator) curvature_log.p = float(self.pid.p) curvature_log.i = float(self.pid.i) curvature_log.f = float(self.pid.f) @@ -64,8 +67,9 @@ class LatControlCurvature(LatControl): curvature_log.error = float(error) curvature_log.actualCurvature = float(actual_curvature) curvature_log.desiredCurvature = float(desired_curvature) - output_curvature = output_curvature + self.curvature_correction + output_curvature = output_curvature - roll_compensation + self.curvature_correction curvature_log.output = float(output_curvature) - curvature_log.saturated = bool(self._check_saturation(abs(error) > CURVATURE_SATURATION_THRESHOLD, CS, + lat_accel_error = error * CS.vEgo ** 2 + curvature_log.saturated = bool(self._check_saturation(abs(lat_accel_error) > LAT_ACCEL_SATURATION_THRESHOLD, CS, False, curvature_limited)) return 0.0, 0.0, float(output_curvature), curvature_log diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index deae41648..8f93ef2df 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -40,7 +40,7 @@ J_EGO_COST = 5. A_CHANGE_COST = 200. DANGER_ZONE_COST = 100. CRASH_DISTANCE = .25 -LEAD_DANGER_FACTOR = 0.75 +LEAD_DANGER_FACTOR = 0.775 LIMIT_COST = 1e6 ACADOS_SOLVER_TYPE = 'SQP_RTI'