From 455a6a586a1f70691c801b9ca5d5c5ae9b70b044 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Harald=20Sch=C3=A4fer?= Date: Mon, 11 Aug 2025 14:25:29 -0700 Subject: [PATCH] Misc PID refactors (#35844) * Misc PID refactors * dead * finish rename * unused import * whitespace * typo * fix fan controller * pid_log * whitespace * integral clipping in pid * update ref * cleaner * rm print * update ref * revert fan changes * forgot this --- common/pid.py | 25 +++++++------------- selfdrive/controls/controlsd.py | 8 +++---- selfdrive/controls/lib/latcontrol.py | 6 ++--- selfdrive/controls/lib/latcontrol_angle.py | 9 +++---- selfdrive/controls/lib/latcontrol_pid.py | 26 ++++++++++----------- selfdrive/controls/lib/latcontrol_torque.py | 6 ++--- selfdrive/test/process_replay/ref_commit | 2 +- tools/tuning/measure_steering_accuracy.py | 4 ++-- 8 files changed, 39 insertions(+), 47 deletions(-) diff --git a/common/pid.py b/common/pid.py index f2ab935f45..721a7c9d65 100644 --- a/common/pid.py +++ b/common/pid.py @@ -17,7 +17,6 @@ class PIDController: self.pos_limit = pos_limit self.neg_limit = neg_limit - self.i_unwind_rate = 0.3 / rate self.i_rate = 1.0 / rate self.speed = 0.0 @@ -35,10 +34,6 @@ class PIDController: def k_d(self): return np.interp(self.speed, self._k_d[0], self._k_d[1]) - @property - def error_integral(self): - return self.i/self.k_i - def reset(self): self.p = 0.0 self.i = 0.0 @@ -46,25 +41,21 @@ class PIDController: self.f = 0.0 self.control = 0 - def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False): + def update(self, error, error_rate=0.0, speed=0.0, 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: - self.i -= self.i_unwind_rate * float(np.sign(self.i)) - else: - if not freeze_integrator: - self.i = self.i + error * self.k_i * self.i_rate + if not freeze_integrator: + 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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index dd7968b732..029d16e59e 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -40,7 +40,7 @@ class Controls: 'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState') self.pm = messaging.PubMaster(['carControl', 'controlsState']) - self.steer_limited_by_controls = False + self.steer_limited_by_safety = False self.curvature = 0.0 self.desired_curvature = 0.0 @@ -120,7 +120,7 @@ class Controls: actuators.curvature = self.desired_curvature steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp, - self.steer_limited_by_controls, self.desired_curvature, + self.steer_limited_by_safety, self.desired_curvature, curvature_limited) # TODO what if not available actuators.torque = float(steer) actuators.steeringAngleDeg = float(steeringAngleDeg) @@ -167,10 +167,10 @@ class Controls: if self.sm['selfdriveState'].active: CO = self.sm['carOutput'] if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - self.steer_limited_by_controls = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ + self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ STEER_ANGLE_SATURATION_THRESHOLD else: - self.steer_limited_by_controls = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 + self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2 # TODO: both controlsState and carControl valids should be set by # sm.all_checks(), but this creates a circular dependency diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index d67a4aa959..2a8b873e2e 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -15,15 +15,15 @@ class LatControl(ABC): self.steer_max = 1.0 @abstractmethod - 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): pass def reset(self): self.sat_count = 0. - def _check_saturation(self, saturated, CS, steer_limited_by_controls, curvature_limited): + def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited): # Saturated only if control output is not being limited by car torque/angle rate limits - if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_controls and not CS.steeringPressed: + if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed: self.sat_count += self.sat_count_rate else: self.sat_count -= self.sat_count_rate diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 807161735c..ac35151487 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -3,6 +3,7 @@ import math from cereal import log from openpilot.selfdrive.controls.lib.latcontrol import LatControl +# TODO This is speed dependent STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees @@ -10,9 +11,9 @@ class LatControlAngle(LatControl): def __init__(self, CP, CI): super().__init__(CP, CI) self.sat_check_min_speed = 5. - self.use_steer_limited_by_controls = CP.brand == "tesla" + self.use_steer_limited_by_safety = CP.brand == "tesla" - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): angle_log = log.ControlsState.LateralAngleState.new_message() if not active: @@ -23,9 +24,9 @@ class LatControlAngle(LatControl): angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) angle_steers_des += params.angleOffsetDeg - if self.use_steer_limited_by_controls: + if self.use_steer_limited_by_safety: # these cars' carcontrollers calculate max lateral accel and jerk, so we can rely on carOutput for saturation - angle_control_saturated = steer_limited_by_controls + angle_control_saturated = steer_limited_by_safety else: # for cars which use a method of limiting torque such as a torque signal (Nissan and Toyota) # or relying on EPS (Ford Q3), carOutput does not capture maxing out torque # TODO: this can be improved diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 4e4e0772fe..00a083509f 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -13,11 +13,7 @@ class LatControlPID(LatControl): k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max) self.get_steer_feedforward = CI.get_steer_feedforward_function() - def reset(self): - super().reset() - self.pid.reset() - - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steeringAngleDeg = float(CS.steeringAngleDeg) pid_log.steeringRateDeg = float(CS.steeringRateDeg) @@ -29,20 +25,24 @@ class LatControlPID(LatControl): pid_log.steeringAngleDesiredDeg = angle_steers_des pid_log.angleError = error if not active: - output_steer = 0.0 + output_torque = 0.0 pid_log.active = False - self.pid.reset() + else: # offset does not contribute to resistive torque - steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) + ff = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo) + freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 + + output_torque = self.pid.update(error, + feedforward=ff, + speed=CS.vEgo, + freeze_integrator=freeze_integrator) - output_steer = self.pid.update(error, override=CS.steeringPressed, - feedforward=steer_feedforward, speed=CS.vEgo) pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) pid_log.f = float(self.pid.f) - pid_log.output = float(output_steer) - pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited_by_controls, curvature_limited)) + pid_log.output = float(output_torque) + pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) - return output_steer, angle_steers_des, pid_log + return output_torque, angle_steers_des, pid_log diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 991ac3439b..c368a5da43 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -37,7 +37,7 @@ class LatControlTorque(LatControl): self.torque_params.latAccelOffset = latAccelOffset self.torque_params.friction = friction - def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited): + def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited): pid_log = log.ControlsState.LateralTorqueState.new_message() if not active: output_torque = 0.0 @@ -66,7 +66,7 @@ class LatControlTorque(LatControl): gravity_adjusted=True) ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) - freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5 + freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(pid_log.error, feedforward=ff, speed=CS.vEgo, @@ -80,7 +80,7 @@ class LatControlTorque(LatControl): pid_log.output = float(-output_torque) pid_log.actualLateralAccel = float(actual_lateral_accel) pid_log.desiredLateralAccel = float(desired_lateral_accel) - pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_controls, curvature_limited)) + pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited)) # TODO left is positive in this convention return -output_torque, 0.0, pid_log diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 43f97e212c..54ff189358 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -62b3e8590fe85f87494517b3177a0ccaad1e17e5 \ No newline at end of file +543bd2347fa35f8300478a3893fdd0a03a7c1fe6 \ No newline at end of file diff --git a/tools/tuning/measure_steering_accuracy.py b/tools/tuning/measure_steering_accuracy.py index a2f437819f..7e4e975742 100755 --- a/tools/tuning/measure_steering_accuracy.py +++ b/tools/tuning/measure_steering_accuracy.py @@ -49,7 +49,7 @@ class SteeringAccuracyTool: active = sm['controlsState'].active steer = sm['carOutput'].actuatorsOutput.torque standstill = sm['carState'].standstill - steer_limited_by_controls = abs(sm['carControl'].actuators.torque - sm['carControl'].actuatorsOutput.torque) > 1e-2 + steer_limited_by_safety = abs(sm['carControl'].actuators.torque - sm['carControl'].actuatorsOutput.torque) > 1e-2 overriding = sm['carState'].steeringPressed changing_lanes = sm['modelV2'].meta.laneChangeState != 0 model_points = sm['modelV2'].position.y @@ -77,7 +77,7 @@ class SteeringAccuracyTool: self.speed_group_stats[group][angle_abs]["steer"] += abs(steer) if len(model_points): self.speed_group_stats[group][angle_abs]["dpp"] += abs(model_points[0]) - if steer_limited_by_controls: + if steer_limited_by_safety: self.speed_group_stats[group][angle_abs]["limited"] += 1 if control_state.saturated: self.speed_group_stats[group][angle_abs]["saturated"] += 1