mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-09-05 07:43:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert "Update controlsd.py"

Browse Source 
This reverts commit a756987ca1.
This commit is contained in:
infiniteCable2
2025-04-10 19:58:57 +02:00
parent a4ffbba293
commit a4f07b065f
1 changed files with 8 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+8 -1
View File
@@ -19,6 +19,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
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
from selfdrive.controls.lib.disturbance_controller import DisturbanceController
State = log.SelfdriveState.OpenpilotState
LaneChangeState = log.LaneChangeState
@@ -51,6 +52,9 @@ class Controls:
self.desired_curvature = 0.0
self.roll = 0.0
self.enable_disturbance_correction = self.params.get_bool("EnableDisturbanceCorrection")
self.disturbance_controller = DisturbanceController(self.CP)
self.pose_calibrator = PoseCalibrator()
self.calibrated_pose: Pose | None = None
@@ -82,7 +86,7 @@ class Controls:
self.VM.update_params(x, sr)
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)
self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, 0)
self.roll = lp.roll
# Update Torque Params
@@ -123,6 +127,7 @@ class Controls:
if not CC.latActive:
self.LaC.reset()
self.disturbance_controller.reset()
if not CC.longActive:
self.LoC.reset()
@@ -133,6 +138,8 @@ class Controls:
# Steering PID loop and lateral MPC
# Reset desired curvature to current to avoid violating the limits on engage
new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
if self.enable_disturbance_correction:
new_desired_curvature = self.disturbance_controller.compensate(CS, self.VM, lp, self.calibrated_pose, new_desired_curvature)
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)
actuators.curvature = self.desired_curvature