From b41664cfce05ec882128a42d5b1ad40db9cf41e8 Mon Sep 17 00:00:00 2001 From: infiniteCable2 Date: Fri, 15 Aug 2025 18:07:24 +0200 Subject: [PATCH] adapt to upstream --- common/pid_mu.py | 20 +++++++++++-------- .../controls/lib/latcontrol_curvature.py | 6 +++--- tinygrad_repo | 2 +- 3 files changed, 16 insertions(+), 12 deletions(-) diff --git a/common/pid_mu.py b/common/pid_mu.py index 193d46e64..49dd0a42c 100644 --- a/common/pid_mu.py +++ b/common/pid_mu.py @@ -2,27 +2,31 @@ from openpilot.common.pid import PIDController import numpy as np class MultiplicativeUnwindPID(PIDController): + def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100): + super().__init__(k_p, k_i, k_f=k_f, k_d=k_d, pos_limit=pos_limit, neg_limit=neg_limit, rate=rate) + + self.i_unwind_rate = 0.3 / rate + def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False): self.speed = speed - self.p = float(error) * self.k_p self.f = feedforward * self.k_f self.d = error_rate * self.k_d if override: - # Multiplikativer Abbau von i self.i *= (1.0 - self.i_unwind_rate) if abs(self.i) < 1e-10: self.i = 0.0 else: if not freeze_integrator: - self.i += error * self.k_i * self.i_rate + i = self.i + error * self.k_i * self.i_rate - # Clip i to prevent exceeding control limits - control_no_i = self.p + self.d + self.f - control_no_i = np.clip(control_no_i, self.neg_limit, self.pos_limit) - self.i = np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i) + # Don't allow windup if already clipping + test_control = self.p + i + self.d + self.f + i_upperbound = self.i if test_control > self.pos_limit else self.pos_limit + i_lowerbound = self.i if test_control < self.neg_limit else self.neg_limit + self.i = np.clip(i, i_lowerbound, i_upperbound) control = self.p + self.i + self.d + self.f self.control = np.clip(control, self.neg_limit, self.pos_limit) - return self.control + return self.control \ No newline at end of file diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index 1ad602c30..7f0a86453 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -20,7 +20,7 @@ class LatControlCurvature(LatControl): super().reset() self.pid.reset() - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited): pid_log = log.ControlsState.LateralCurvatureState.new_message() if not active: output_curvature = 0.0 @@ -37,7 +37,7 @@ class LatControlCurvature(LatControl): desired_curvature_corr = desired_curvature - roll_compensation pid_log.error = float(desired_curvature - actual_curvature) - freeze_integrator = steer_limited_by_controls or CS.vEgo < 5 or CS.steeringPressed + freeze_integrator = steer_limited_by_safety 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) @@ -51,6 +51,6 @@ class LatControlCurvature(LatControl): pid_log.output = float(output_curvature) 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.saturated = bool(self._check_saturation(steer_limited_by_safety, CS, False, curvature_limited)) return 0.0, 0.0, output_curvature, pid_log diff --git a/tinygrad_repo b/tinygrad_repo index d2bb1bcb9..7737cbb2a 160000 --- a/tinygrad_repo +++ b/tinygrad_repo @@ -1 +1 @@ -Subproject commit d2bb1bcb976f106a41928f2d66d354ab7afd6f59 +Subproject commit 7737cbb2a0635fce95a9085fb1b53d5bea1093f8