From e1fcc2e63bda2dec5627023c63b65b66cf50c317 Mon Sep 17 00:00:00 2001 From: actuallylemoncurd Date: Tue, 5 Nov 2024 20:00:57 -0500 Subject: [PATCH] VW PQ: re-add ACC signals --- selfdrive/car/volkswagen/carcontroller.py | 12 ++++++------ selfdrive/car/volkswagen/pqcan.py | 4 ++-- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index c6343551c9c539..abe0e605070639 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -41,7 +41,7 @@ def __init__(self, dbc_name, CP, VM): self.ratelimitV = [4., 2.02, 1.02, .5] # jerk-limits (m/s squared) # SMA to EMA conversion: alpha = 2 / (n + 1) n = SMA-sample self.longSignalSmooth = 0.00995 # closer to 0 = more smoothing, 1 = no smoothing (eq = 200 SMA-sample) - self.accel_diff_smoothed = 0 + self.accel_diff = 0 self.sm = messaging.SubMaster(['longitudinalPlanSP']) self.param_s = Params() @@ -156,7 +156,6 @@ def update(self, CC, CS, now_nanos): stopping = actuators.longControlState == LongCtrlState.stopping starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < self.CP.vEgoStopping) if self.CCS == pqcan and CC.longActive and actuators.accel <= 0 and CS.out.vEgoRaw <= 5: - accel = 0 self.EPB_brake = clip(actuators.accel, self.CCP.ACCEL_MIN, 0) if self.EPB_enable else 0 if not self.EPB_enable: self.EPB_counter = 0 # Reset frame counter when EPB_enable is first activated @@ -166,9 +165,13 @@ def update(self, CC, CS, now_nanos): acc_control = 0 if acc_control != 6 and self.EPB_enable else acc_control # Pulse ACC status to 0 for one frame self.EPB_enable = 0 self.EPB_brake = 0 + 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, .1, .2], [.13, .1, 0]) # floating comfort band calculation + self.long_ratelimit = (0.007 * (clip(abs(accel), 0.7, 3))) + (1 - 0.007) * self.long_ratelimit # set jerk/rate limit based on accel + self.accel_last = accel # Increment the EPB counter when EPB is enabled - # Keep ACC status 0 for first 8 frames of EPB + # Keep ACC status 0 for first 9 frames of EPB if self.EPB_enable: self.EPB_counter = min(self.EPB_counter + 1, 10) if self.EPB_counter <= 9: @@ -176,9 +179,6 @@ def update(self, CC, CS, now_nanos): else: self.EPB_counter = 0 - self.long_deviation = 0 # TODO: make dynamic again - self.long_ratelimit = 4 # TODO: make dynamic again - if self.CCS == pqcan: can_sends.append(self.CCS.create_epb_control(self.packer_pt, CANBUS.br, self.EPB_brake, self.EPB_enable)) can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, CS.acc_type, accel, diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index d9b413c4935458..91dca97ec44287 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -91,8 +91,8 @@ def create_acc_accel_control(packer, bus, acc_type, accel, acc_control, stopping "ACS_Anhaltewunsch": acc_type == 1 and stopping, "ACS_FreigSollB": acc_enabled, "ACS_Sollbeschl": accel if acc_enabled else 3.01, - "ACS_zul_Regelabw": 0 if acc_enabled else 1.27, # set to 0 for PIF tuning / behavior analysis - "ACS_max_AendGrad": 4 if acc_enabled else 5.08, # set to 4 for PIF tuning + "ACS_zul_Regelabw": comfortBand if acc_enabled else 1.27, + "ACS_max_AendGrad": jerkLimit if acc_enabled else 5.08, } commands.append(packer.make_can_msg("ACC_System", bus, values))