mawei/openpilot-meb
1
0
Fork 0
Code Issues Pull Requests Actions 5 Packages Projects Releases Wiki Activity

Misc PID refactors (#35844)

Browse Source 
* 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
This commit is contained in:
Harald Schäfer
2025-08-11 14:25:29 -07:00
committed by GitHub
parent c78b302b93
commit 455a6a586a
8 changed files with 39 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
common/pid.py
View File

@@ -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

8
selfdrive/controls/controlsd.py
View File

@@ -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

6
selfdrive/controls/lib/latcontrol.py
View File

@@ -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

9
selfdrive/controls/lib/latcontrol_angle.py
View File

@@ -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

26
selfdrive/controls/lib/latcontrol_pid.py
View File

@@ -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

6
selfdrive/controls/lib/latcontrol_torque.py
View File

@@ -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

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
62b3e8590fe85f87494517b3177a0ccaad1e17e5
543bd2347fa35f8300478a3893fdd0a03a7c1fe6

4
tools/tuning/measure_steering_accuracy.py
View File

@@ -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