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

Revert "Update controlsd.py"

Browse Source 
This reverts commit f04149e48a.
This commit is contained in:
infiniteCable2
2025-04-22 14:41:21 +02:00
parent 0497b1a2f1
commit 1746ef7e2f
1 changed files with 12 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/controlsd.py
+12 -1
View File
@@ -52,9 +52,12 @@ class Controls:
self.steer_limited_by_controls = False
self.curvature = 0.0
self.curvature_3dof = 0.0
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.enable_smooth_steer = self.params.get_bool("EnableSmoothSteer")
self.smooth_steer = PT2Filter(46.0, 1.0, DT_CTRL)
@@ -82,6 +85,7 @@ class Controls:
self.param_counter += 1
if self.param_counter >= 100:
self.param_counter = 0
self.enable_disturbance_correction = self.params.get_bool("EnableDisturbanceCorrection")
self.enable_smooth_steer = self.params.get_bool("EnableSmoothSteer")
def state_control(self):
@@ -95,6 +99,10 @@ class Controls:
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - lp.angleOffsetDeg)
self.curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, lp.roll)
if self.calibrated_pose is not None:
self.curvature_3dof = -self.VM.calc_curvature_3dof(self.calibrated_pose.acceleration.y, self.calibrated_pose.acceleration.x,
self.calibrated_pose.angular_velocity.yaw, CS.vEgo, steer_angle_without_offset,
lp.roll)
self.roll = lp.roll
# Update Torque Params
@@ -150,6 +158,8 @@ class Controls:
new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
if self.enable_smooth_steer:
new_desired_curvature = self.smooth_steer.update(new_desired_curvature)
if self.enable_disturbance_correction:
new_desired_curvature = self.disturbance_controller.compensate(CS, self.VM, lp, self.calibrated_pose, new_desired_curvature, self.curvature_3dof)
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)
actuators.curvature = self.desired_curvature
@@ -177,7 +187,8 @@ class Controls:
CS = self.sm['carState']
CC.currentCurvature = self.curvature
CC.rollDEPRECATED = self.roll # for lateral iso limit calculation
CC.pitch = float(self.curvature_3dof) # debugging purposes
CC.roll = self.roll # for lateral iso limit calculation
# Orientation and angle rates can be useful for carcontroller
# Only calibrated (car) frame is relevant for the carcontroller