diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 89aacc1151f442..b8f9da4bf8066d 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -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 @@ -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