From bfd4fb12eb255f4c9f32cb54a68aa185f3c293e0 Mon Sep 17 00:00:00 2001 From: Jason Young Date: Wed, 20 Apr 2022 15:45:00 -0400 Subject: [PATCH] support both analog and digital clusters --- selfdrive/car/volkswagen/carcontroller.py | 7 ++++++- selfdrive/car/volkswagen/carstate.py | 6 ++++++ selfdrive/car/volkswagen/volkswagencan.py | 4 ++-- 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 6eda1fc08..badf748eb 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -77,9 +77,14 @@ class CarController(): acc_hold_type, stopping_distance, idx)) if frame % P.ACC_HUD_STEP == 0: + if lead_visible: + lead_distance = 512 if CS.digital_cluster_installed else 8 # TODO: look up actual distance to lead + else: + lead_distance = 0 + idx = (frame / P.ACC_HUD_STEP) % 16 can_sends.append(volkswagencan.create_mqb_acc_02_control(self.packer_pt, CANBUS.pt, CS.tsk_status, - set_speed * CV.MS_TO_KPH, speed_visible, lead_visible, + set_speed * CV.MS_TO_KPH, speed_visible, lead_distance, idx)) can_sends.append(volkswagencan.create_mqb_acc_04_control(self.packer_pt, CANBUS.pt, CS.acc_04_stock_values, idx)) diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 27eace20a..98e35d832 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -16,6 +16,7 @@ class CarState(CarStateBase): self.shifter_values = can_define.dv["EV_Gearshift"]["GearPosition"] self.hca_status_values = can_define.dv["LH_EPS_03"]["EPS_HCA_Status"] self.buttonStates = BUTTON_STATES.copy() + self.digital_cluster_installed = False def update(self, pt_cp, cam_cp, ext_cp, trans_type): ret = car.CarState.new_message() @@ -145,6 +146,9 @@ class CarState(CarStateBase): # Additional safety checks performed in CarInterface. ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0 + # Update misc car configuration/equipment info + self.digital_cluster_installed = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) + return ret @staticmethod @@ -180,6 +184,7 @@ class CarState(CarStateBase): ("ESP_Haltebestaetigung", "ESP_21"), # ESP hold confirmation ("KBI_MFA_v_Einheit_02", "Einheiten_01"), # MPH vs KMH speed display ("KBI_Handbremse", "Kombi_01"), # Manual handbrake applied + ("KBI_Variante", "Kombi_03"), # Digital/full-screen instrument cluster installed ("TSK_Status", "TSK_06"), # ACC engagement status from drivetrain coordinator ("GRA_Hauptschalter", "GRA_ACC_01"), # ACC button, on/off ("GRA_Abbrechen", "GRA_ACC_01"), # ACC button, cancel @@ -210,6 +215,7 @@ class CarState(CarStateBase): ("Kombi_01", 2), # From J285 Instrument cluster ("Blinkmodi_02", 1), # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active) ("Einheiten_01", 1), # From J??? not known if gateway, cluster, or BCM + ("Kombi_03", 1), # From J285 instrument cluster ] if CP.transmissionType == TransmissionType.automatic: diff --git a/selfdrive/car/volkswagen/volkswagencan.py b/selfdrive/car/volkswagen/volkswagencan.py index ea7847488..2f29e258d 100644 --- a/selfdrive/car/volkswagen/volkswagencan.py +++ b/selfdrive/car/volkswagen/volkswagencan.py @@ -48,13 +48,13 @@ def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx): } return packer.make_can_msg("GRA_ACC_01", bus, values, idx) -def create_mqb_acc_02_control(packer, bus, acc_status, set_speed, speed_visible, lead_visible, idx): +def create_mqb_acc_02_control(packer, bus, acc_status, set_speed, speed_visible, lead_distance, idx): values = { "ACC_Status_Anzeige": 3 if acc_status == 5 else acc_status, "ACC_Wunschgeschw": 327.36 if not speed_visible else set_speed, "ACC_Gesetzte_Zeitluecke": 3, "ACC_Display_Prio": 3, - "ACC_Abstandsindex": 637 if lead_visible else 0, + "ACC_Abstandsindex": lead_distance, } return packer.make_can_msg("ACC_02", bus, values, idx)