mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-02 12:02:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert "Test Lagd"

Browse Source 
This reverts commit b87ba63846.
This commit is contained in:
firestar5683
2025-11-29 11:05:32 -06:00
parent b87ba63846
commit caeeb30cc3
1 changed files with 5 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+5 -10
View File
@@ -46,7 +46,6 @@ class LatControlTorque(LatControl):
self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
self.lookahead_frames = int(JERK_LOOKAHEAD_SECONDS / self.dt)
self.jerk_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
self.setpoint_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
@@ -70,18 +69,14 @@ 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.lat_accel_request_buffer_len))
expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
lookahead_idx = int(np.clip(-delay_frames + self.lookahead_frames, -self.lat_accel_request_buffer_len+1, -2))
raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx+1] - self.lat_accel_request_buffer[lookahead_idx-1]) / (2 * self.dt)
desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk)
future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
self.lat_accel_request_buffer.append(future_desired_lateral_accel)
gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
expected_lateral_accel = self.setpoint_filter.update(self.lat_accel_request_buffer[-1])
delay_frames = int(np.clip(lat_delay / self.dt, 0, self.lat_accel_request_buffer_len - 1))
center_idx = max(-1 - delay_frames, -self.lat_accel_request_buffer_len + 2)
lookahead_offset = min(self.lookahead_frames, self.lat_accel_request_buffer_len + center_idx - 1)
lookahead_idx = int(np.clip(center_idx - lookahead_offset, -self.lat_accel_request_buffer_len + 1, -2))
raw_lateral_jerk = (self.lat_accel_request_buffer[lookahead_idx + 1] - self.lat_accel_request_buffer[lookahead_idx - 1]) / (2 * self.dt)
desired_lateral_jerk = self.jerk_filter.update(raw_lateral_jerk)
setpoint = expected_lateral_accel
measurement = measured_curvature * CS.vEgo ** 2