diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index fff5548671..f9ce2dc326 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -2,6 +2,7 @@ from cereal import car from opendbc.can.packer import CANPacker from common.numpy_fast import clip from common.conversions import Conversions as CV +from common.realtime import DT_CTRL from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.volkswagen import mqbcan, pqcan from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams @@ -88,10 +89,13 @@ class CarController: CS.out.steeringPressed, hud_alert, hud_control)) if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl: + 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) set_speed = hud_control.setSpeed * CV.MS_TO_KPH # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem? can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed, - hud_control.leadVisible)) + lead_distance)) # **** Stock ACC Button Controls **************************************** # diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 263edbfc9d..7ea9aa871b 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -13,6 +13,7 @@ class CarState(CarStateBase): self.CCP = CarControllerParams(CP) self.button_states = {button.event_type: False for button in self.CCP.BUTTONS} self.esp_hold_confirmation = False + self.upscale_lead_car_signal = False def create_button_events(self, pt_cp, buttons): button_events = [] @@ -141,6 +142,9 @@ class CarState(CarStateBase): # Additional safety checks performed in CarInterface. ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0 + # Digital instrument clusters expect the ACC HUD lead car distance to be scaled differently + self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) + return ret def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type): @@ -281,6 +285,7 @@ class CarState(CarStateBase): ("ESP_Tastung_passiv", "ESP_21"), # Stability control disabled ("ESP_Haltebestaetigung", "ESP_21"), # ESP hold confirmation ("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 @@ -312,6 +317,7 @@ class CarState(CarStateBase): ("Airbag_02", 5), # From J234 Airbag control module ("Kombi_01", 2), # From J285 Instrument cluster ("Blinkmodi_02", 1), # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active) + ("Kombi_03", 0), # From J285 instrument cluster (not present on older cars, 1Hz when present) ] if CP.transmissionType == TransmissionType.automatic: diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 25a710dbb8..30a51f6fe6 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -96,13 +96,13 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_visible): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance): values = { "ACC_Status_Anzeige": acc_hud_status, "ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36, "ACC_Gesetzte_Zeitluecke": 3, "ACC_Display_Prio": 3, - # TODO: ACC_Abstandsindex for lead car distance, must determine analog vs digital cluster for scaling + "ACC_Abstandsindex": lead_distance, } return packer.make_can_msg("ACC_02", bus, values) diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py index 6beb90c092..130f107950 100644 --- a/selfdrive/car/volkswagen/pqcan.py +++ b/selfdrive/car/volkswagen/pqcan.py @@ -77,12 +77,12 @@ def create_acc_accel_control(packer, bus, acc_type, enabled, accel, acc_control, return commands -def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_visible): +def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance): values = { "ACA_StaACC": acc_hud_status, "ACA_Zeitluecke": 2, "ACA_V_Wunsch": set_speed, - "ACA_gemZeitl": 8 if lead_visible else 0, + "ACA_gemZeitl": lead_distance, # TODO: ACA_ID_StaACC, ACA_AnzDisplay, ACA_kmh_mph, ACA_PrioDisp, ACA_Aend_Zeitluecke # display/display-prio handling probably needed to stop confusing the instrument cluster # kmh_mph handling probably needed to resolve rounding errors in displayed setpoint