mawei/onepilot
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

latcontrol_torque: more descriptive variable names (#36422)

Browse Source
This commit is contained in:
felsager
2025-10-22 10:44:14 -07:00
committed by GitHub
parent 5289b08bcf
commit d2bb8fe537
1 changed files with 9 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -4,7 +4,6 @@ from collections import deque
from cereal import log
from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
from opendbc.car.tests.test_lateral_limits import MAX_LAT_JERK_UP
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.selfdrive.controls.lib.drive_helpers import MIN_SPEED
@@ -28,6 +27,8 @@ KP = 1.0
KI = 0.3
KD = 0.0
LP_FILTER_CUTOFF_HZ = 1.2
LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
class LatControlTorque(LatControl):
def __init__(self, CP, CI, dt):
@@ -35,13 +36,13 @@ class LatControlTorque(LatControl):
self.torque_params = CP.lateralTuning.torque.as_builder()
self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
self.pid = PIDController(KP, KI, k_d=KD, rate=1/self.dt)
self.pid = PIDController(KP, KI, KD, rate=1/self.dt)
self.update_limits()
self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES = int(1 / self.dt)
self.requested_lateral_accel_buffer = deque([0.] * self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES , maxlen=self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES)
self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
self.previous_measurement = 0.0
self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * (MAX_LAT_JERK_UP - 0.5)), self.dt)
self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
self.torque_params.latAccelFactor = latAccelFactor
@@ -64,11 +65,11 @@ class LatControlTorque(LatControl):
curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
delay_frames = int(np.clip(lat_delay / self.dt, 1, self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES))
expected_lateral_accel = self.requested_lateral_accel_buffer[-delay_frames]
delay_frames = int(np.clip(lat_delay / self.dt, 1, self.lat_accel_request_buffer_len))
expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
# TODO factor out lateral jerk from error to later replace it with delay independent alternative
future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
self.requested_lateral_accel_buffer.append(future_desired_lateral_accel)
self.lat_accel_request_buffer.append(future_desired_lateral_accel)
gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
desired_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / lat_delay