Skip to content

Commit

Permalink
VW PQ: fix EPB rate limit
Browse files Browse the repository at this point in the history
* update carAPI EMA (175 SMA eq)
  • Loading branch information
dkiiv committed Nov 8, 2024
1 parent b18c9ec commit 8c4c7fe
Showing 1 changed file with 4 additions and 2 deletions.
6 changes: 4 additions & 2 deletions selfdrive/car/volkswagen/carcontroller.py
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ def __init__(self, dbc_name, CP, VM):
self.accel_last = 0
self.motor2_frame = 0
self.EPB_brake = 0
self.EPB_brake_last = 0
self.EPB_enable = 0
self.EPB_counter = 0
self.accel_diff = 0
Expand Down Expand Up @@ -154,14 +155,15 @@ def update(self, CC, CS, now_nanos):
# SMA to EMA conversion: alpha = 2 / (n + 1) n = SMA-sample
self.accel_diff = (0.0019 * (accel - self.accel_last)) + (1 - 0.0019) * self.accel_diff # 1000 SMA equivalence
self.long_deviation = interp(abs(accel - self.accel_diff), [0, .2, .3], [.13, .1, 0]) # floating comfort band calculation
self.long_ratelimit = (0.019 * (clip(abs(accel), 0.7, 3))) + (1 - 0.019) * self.long_ratelimit # set jerk/rate limit based on accel
self.long_ratelimit = (0.011 * (clip(abs(accel), 0.7, 3))) + (1 - 0.011) * self.long_ratelimit # set jerk/rate limit based on accel

if self.CCS == pqcan and CC.longActive and actuators.accel <= 0 and CS.out.vEgoRaw <= 5:
if not self.EPB_enable:
self.EPB_counter = 0 # Reset frame counter when EPB_enable is first activated
self.EPB_brake = 0
else:
self.EPB_brake = clip(accel, self.accel_last - self.long_ratelimit, self.accel_last + self.long_ratelimit)
self.EPB_brake = clip(accel, self.EPB_brake_last - self.long_ratelimit, self.EPB_brake_last + self.long_ratelimit)
self.EPB_brake_last = self.EPB_brake
self.EPB_enable = 1
else:
acc_control = 0 if acc_control != 6 and self.EPB_enable else acc_control # Pulse ACC status to 0 for one frame
Expand Down

0 comments on commit 8c4c7fe

Please sign in to comment.