mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-09-04 15:23:42 +08:00
Code Issues Packages Projects Releases Wiki Activity

remove curvature controller

Browse Source
This commit is contained in:
infiniteCable2
2025-09-03 17:25:55 +02:00
parent 6d00fec2ea
commit 085ebe148a
6 changed files with 7 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/pid_mu.py
-32
View File
@@ -1,32 +0,0 @@
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:
self.i *= (1.0 - self.i_unwind_rate)
if abs(self.i) < 1e-10:
self.i = 0.0
else:
if not freeze_integrator:
i = self.i + error * self.k_i * self.i_rate
# 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
selfdrive/controls/controlsd.py
+4 -11
View File
@@ -18,7 +18,6 @@ from openpilot.selfdrive.controls.lib.latcontrol import LatControl
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature, CURVATURE_SATURATION_THRESHOLD
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import get_T_FOLLOW
@@ -76,8 +75,6 @@ class Controls(ControlsExt, ModelStateBase):
self.LaC: LatControl
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP, self.CP_SP, self.CI)
elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED:
self.LaC = LatControlCurvature(self.CP, self.CP_SP, self.CI)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP, self.CP_SP, self.CI)
elif self.CP.lateralTuning.which() == 'torque':
@@ -169,10 +166,10 @@ class Controls(ControlsExt, ModelStateBase):
new_desired_curvature = self.smooth_steer.update(new_desired_curvature)
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)
steer, steeringAngleDeg, curvature, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.steer_limited_by_safety, self.desired_curvature,
self.calibrated_pose, curvature_limited) # TODO what if not available
actuators.curvature = float(curvature)
actuators.curvature = self.desired_curvature
steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.steer_limited_by_safety, self.desired_curvature,
self.calibrated_pose, curvature_limited) # TODO what if not available
actuators.torque = float(steer)
actuators.steeringAngleDeg = float(steeringAngleDeg)
# Ensure no NaNs/Infs
@@ -229,8 +226,6 @@ class Controls(ControlsExt, ModelStateBase):
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.steer_limited_by_safety = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \
STEER_ANGLE_SATURATION_THRESHOLD
elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED:
self.steer_limited_by_safety = abs(CC.actuators.curvature - CO.actuatorsOutput.curvature) > CURVATURE_SATURATION_THRESHOLD
else:
self.steer_limited_by_safety = abs(CC.actuators.torque - CO.actuatorsOutput.torque) > 1e-2
@@ -256,8 +251,6 @@ class Controls(ControlsExt, ModelStateBase):
lat_tuning = self.CP.lateralTuning.which()
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
cs.lateralControlState.angleState = lac_log
elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED:
cs.lateralControlState.curvatureStateDEPRECATED = lac_log
elif lat_tuning == 'pid':
cs.lateralControlState.pidState = lac_log
elif lat_tuning == 'torque':
selfdrive/controls/lib/latcontrol_angle.py
+1 -1
View File
@@ -34,4 +34,4 @@ class LatControlAngle(LatControl):
angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited))
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
angle_log.steeringAngleDesiredDeg = angle_steers_des
return 0, float(angle_steers_des), desired_curvature, angle_log
return 0, float(angle_steers_des), angle_log
selfdrive/controls/lib/latcontrol_curvature.py
-27
View File
@@ -1,27 +0,0 @@
import math
from cereal import log
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
CURVATURE_SATURATION_THRESHOLD = 5e-4 # rad/m
class LatControlCurvature(LatControl):
def __init__(self, CP, CP_SP, CI):
super().__init__(CP, CP_SP, CI)
def reset(self):
super().reset()
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited):
curvature_log = log.ControlsState.LateralCurvatureState.new_message()
actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
output_curvature = desired_curvature
curvature_log.active = active
curvature_log.output = float(output_curvature)
curvature_log.actualCurvature = float(actual_curvature)
curvature_log.desiredCurvature = float(output_curvature)
curvature_log.saturated = bool(self._check_saturation(steer_limited_by_safety, CS, False, curvature_limited)) if active else False
return 0.0, 0.0, output_curvature, curvature_log
selfdrive/controls/lib/latcontrol_pid.py
+1 -1
View File
@@ -45,4 +45,4 @@ class LatControlPID(LatControl):
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_torque, angle_steers_des, desired_curvature, pid_log
return output_torque, angle_steers_des, pid_log
selfdrive/controls/lib/latcontrol_torque.py
+1 -1
View File
@@ -97,4 +97,4 @@ class LatControlTorque(LatControl):
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, desired_curvature, pid_log
return -output_torque, 0.0, pid_log