From 2fe17a6e64136e6b4ae9f973e3c9ae4e748ae49b Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Sat, 7 Jun 2025 01:17:00 +0200 Subject: [PATCH 1/5] Update latcontrol_curvature.py --- selfdrive/controls/lib/latcontrol_curvature.py | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index 3f8f84e77..8ea159f75 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -25,11 +25,13 @@ class LatControlCurvature(LatControl): assert calibrated_pose is not None actual_curvature_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose]) - - pid_log.error = float(desired_curvature - (actual_curvature + roll_compensation)) - + + gravitiy_adjusted_curvature = desired_curvature - roll_compensation + + pid_log.error = float(gravitiy_adjusted_curvature - actual_curvature) freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5 - output_curvature = self.pid.update(pid_log.error, feedforward=desired_curvature, speed=CS.vEgo, freeze_integrator=freeze_integrator) + + output_curvature = self.pid.update(pid_log.error, feedforward=gravitiy_adjusted_curvature, speed=CS.vEgo, freeze_integrator=freeze_integrator) pid_log.active = True pid_log.p = float(self.pid.p) From 31cb0dc202c83827d710a5ddec1bd3581814435d Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Sat, 7 Jun 2025 01:18:55 +0200 Subject: [PATCH 2/5] Update latcontrol_curvature.py --- selfdrive/controls/lib/latcontrol_curvature.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index 8ea159f75..7325fedfb 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -20,12 +20,11 @@ class LatControlCurvature(LatControl): pid_log.active = False else: actual_curvature_vm = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) - roll_compensation = -VM.roll_compensation(params.roll, CS.vEgo) - assert calibrated_pose is not None actual_curvature_pose = calibrated_pose.angular_velocity.yaw / CS.vEgo actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose]) - + + roll_compensation = -VM.roll_compensation(params.roll, CS.vEgo) gravitiy_adjusted_curvature = desired_curvature - roll_compensation pid_log.error = float(gravitiy_adjusted_curvature - actual_curvature) From aa822122cc03f2e2bf95b9d2e842fe7e99ae767c Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Sat, 7 Jun 2025 01:21:33 +0200 Subject: [PATCH 3/5] Update latcontrol_curvature.py --- selfdrive/controls/lib/latcontrol_curvature.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index 7325fedfb..c646a52f5 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -25,12 +25,12 @@ class LatControlCurvature(LatControl): actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose]) roll_compensation = -VM.roll_compensation(params.roll, CS.vEgo) - gravitiy_adjusted_curvature = desired_curvature - roll_compensation + gravity_adjusted_curvature = desired_curvature - roll_compensation - pid_log.error = float(gravitiy_adjusted_curvature - actual_curvature) + pid_log.error = float(gravity_adjusted_curvature - actual_curvature) freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5 - output_curvature = self.pid.update(pid_log.error, feedforward=gravitiy_adjusted_curvature, speed=CS.vEgo, freeze_integrator=freeze_integrator) + output_curvature = self.pid.update(pid_log.error, feedforward=gravity_adjusted_curvature, speed=CS.vEgo, freeze_integrator=freeze_integrator) pid_log.active = True pid_log.p = float(self.pid.p) @@ -38,7 +38,7 @@ class LatControlCurvature(LatControl): pid_log.f = float(self.pid.f) pid_log.output = float(output_curvature) pid_log.actualCurvature = float(actual_curvature) - pid_log.desiredCurvature = float(desired_curvature) + pid_log.desiredCurvature = float(gravity_adjusted_curvature) pid_log.saturated = bool(self._check_saturation(self.curvature_max - abs(output_curvature) < 1e-5, CS, steer_limited_by_controls, curvature_limited)) return 0.0, 0.0, output_curvature, pid_log From 153cebce1e082a912847976dece630083bf2126b Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Sat, 7 Jun 2025 01:37:53 +0200 Subject: [PATCH 4/5] Update latcontrol_curvature.py --- selfdrive/controls/lib/latcontrol_curvature.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index c646a52f5..9556b42be 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -27,7 +27,7 @@ class LatControlCurvature(LatControl): roll_compensation = -VM.roll_compensation(params.roll, CS.vEgo) gravity_adjusted_curvature = desired_curvature - roll_compensation - pid_log.error = float(gravity_adjusted_curvature - actual_curvature) + pid_log.error = float(desired_curvature - actual_curvature) freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5 output_curvature = self.pid.update(pid_log.error, feedforward=gravity_adjusted_curvature, speed=CS.vEgo, freeze_integrator=freeze_integrator) @@ -38,7 +38,7 @@ class LatControlCurvature(LatControl): pid_log.f = float(self.pid.f) pid_log.output = float(output_curvature) pid_log.actualCurvature = float(actual_curvature) - pid_log.desiredCurvature = float(gravity_adjusted_curvature) + pid_log.desiredCurvature = float(desired_curvature) pid_log.saturated = bool(self._check_saturation(self.curvature_max - abs(output_curvature) < 1e-5, CS, steer_limited_by_controls, curvature_limited)) return 0.0, 0.0, output_curvature, pid_log From 252e1b07255c6433abedce2a2e9a06074d79c84e Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Sat, 7 Jun 2025 12:46:02 +0200 Subject: [PATCH 5/5] Update latcontrol_curvature.py --- selfdrive/controls/lib/latcontrol_curvature.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_curvature.py b/selfdrive/controls/lib/latcontrol_curvature.py index 9556b42be..f52dc1d94 100644 --- a/selfdrive/controls/lib/latcontrol_curvature.py +++ b/selfdrive/controls/lib/latcontrol_curvature.py @@ -7,8 +7,8 @@ from openpilot.common.pid import PIDController class LatControlCurvature(LatControl): - def __init__(self, CP, CI): - super().__init__(CP, CI) + def __init__(self, CP, CP_SP, CI): + super().__init__(CP, CP_SP, CI) self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), k_f=CP.lateralTuning.pid.kf, pos_limit=self.curvature_max, neg_limit=-self.curvature_max)