From 5d91fb2b90b35ef9baf1a9d0c4e74eb9db8fb9a5 Mon Sep 17 00:00:00 2001 From: actuallylemoncurd Date: Sun, 1 Sep 2024 15:49:36 -0400 Subject: [PATCH] VW PQ: add OEM like OP long gasOverride --- selfdrive/car/volkswagen/carcontroller.py | 4 ++-- selfdrive/car/volkswagen/pqcan.py | 13 +++++++------ 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 28c48283a782bb..930564de5a5db0 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -139,7 +139,7 @@ def update(self, CC, CS, now_nanos): # **** Acceleration Controls ******************************************** # if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl: - acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) + acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive, CC.cruiseControl.override) accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0 stopping = actuators.longControlState == LongCtrlState.stopping starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < self.CP.vEgoStopping) @@ -159,7 +159,7 @@ def update(self, CC, CS, now_nanos): lead_distance = 0 if hud_control.leadVisible and self.frame * DT_CTRL > 1.0: # Don't display lead until we know the scaling factor lead_distance = 512 if CS.upscale_lead_car_signal else 8 - acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) + acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, acc_control, CC.cruiseControl.override) # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem? set_speed = hud_control.setSpeed * CV.MS_TO_KPH can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed, diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 52f22745473bf9..ff4487bd780e02 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -56,8 +56,8 @@ def create_acc_buttons_control(packer, bus, gra_stock_values, frame=0, buttons=0 return packer.make_can_msg("GRA_Neu", bus, values) -def acc_control_value(main_switch_on, acc_faulted, long_active): - if long_active: +def acc_control_value(main_switch_on, acc_faulted, long_active, cruiseOverride): + if long_active or cruiseOverride: acc_control = 1 elif main_switch_on: acc_control = 2 @@ -67,11 +67,11 @@ def acc_control_value(main_switch_on, acc_faulted, long_active): return acc_control -def acc_hud_status_value(main_switch_on, acc_faulted, long_active): +def acc_hud_status_value(main_switch_on, acc_faulted, acc_control, cruiseOverride): if acc_faulted: hud_status = 6 - elif long_active: - hud_status = 3 + elif acc_control == 1: + hud_status = 4 if cruiseOverride else 3 elif main_switch_on: hud_status = 2 else: @@ -80,8 +80,9 @@ def acc_hud_status_value(main_switch_on, acc_faulted, long_active): return hud_status -def create_acc_accel_control(packer, bus, acc_type, acc_enabled, accel, acc_control, stopping, starting, esp_hold): +def create_acc_accel_control(packer, bus, acc_type, accel, acc_control, stopping, starting, esp_hold): commands = [] + acc_enabled = 1 if acc_control == 1 else 0 values = { "ACS_Sta_ADR": acc_control,