From 9ff76829e33ec13ea64c26df786686d02bb3aed9 Mon Sep 17 00:00:00 2001 From: Jason Young Date: Tue, 7 Jun 2022 00:58:39 -0400 Subject: [PATCH] VW PQ: Volkswagen Passat NMS --- opendbc | 2 +- release/files_common | 1 + selfdrive/car/volkswagen/carcontroller.py | 39 +++- selfdrive/car/volkswagen/carstate.py | 271 +++++++++++++++++++++- selfdrive/car/volkswagen/interface.py | 25 +- selfdrive/car/volkswagen/pqcan.py | 42 ++++ selfdrive/car/volkswagen/values.py | 37 ++- selfdrive/car/volkswagen/volkswagencan.py | 3 - 8 files changed, 390 insertions(+), 30 deletions(-) create mode 100644 selfdrive/car/volkswagen/pqcan.py diff --git a/opendbc b/opendbc index 7701277d26..f107e767bd 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 7701277d2666119bc7fcaca9f8cfefd50cd5b071 +Subproject commit f107e767bda05b2101c88275646fffe33ad6dec0 diff --git a/release/files_common b/release/files_common index 974ea27711..68dccb3e8f 100644 --- a/release/files_common +++ b/release/files_common @@ -511,6 +511,7 @@ opendbc/toyota_nodsu_pt_generated.dbc opendbc/toyota_adas.dbc opendbc/toyota_tss2_adas.dbc +opendbc/vw_golf_mk4.dbc opendbc/vw_mqb_2010.dbc opendbc/tesla_can.dbc diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index aeb56c5d67..efe74344ac 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -1,7 +1,7 @@ from cereal import car from selfdrive.car import apply_std_steer_torque_limits -from selfdrive.car.volkswagen import volkswagencan -from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams as P +from selfdrive.car.volkswagen import volkswagencan, pqcan +from selfdrive.car.volkswagen.values import PQ_CARS, DBC_FILES, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams as P from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -11,7 +11,12 @@ class CarController(): self.apply_steer_last = 0 self.CP = CP - self.packer_pt = CANPacker(DBC_FILES.mqb) + if CP.carFingerprint in PQ_CARS: + self.packer_pt = CANPacker(DBC_FILES.pq) + self.ldw_step = P.PQ_LDW_STEP + else: + self.packer_pt = CANPacker(DBC_FILES.mqb) + self.ldw_step = P.MQB_LDW_STEP self.hcaSameTorqueCount = 0 self.hcaEnabledFrameCount = 0 @@ -67,21 +72,30 @@ class CarController(): self.apply_steer_last = apply_steer idx = (frame / P.HCA_STEP) % 16 - can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_pt, CANBUS.pt, apply_steer, - idx, hcaEnabled)) + if self.CP.carFingerprint in PQ_CARS: + can_sends.append(pqcan.create_pq_steering_control(self.packer_pt, CANBUS.pt, apply_steer, idx, hcaEnabled)) + else: + can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_pt, CANBUS.pt, apply_steer, idx, hcaEnabled)) # **** HUD Controls ***************************************************** # - if frame % P.LDW_STEP == 0: + if frame % self.ldw_step == 0: + # FIXME: Need MQB vs PQ handling of displayed messages here if visual_alert in (VisualAlert.steerRequired, VisualAlert.ldw): hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"] else: hud_alert = MQB_LDW_MESSAGES["none"] - can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, c.enabled, - CS.out.steeringPressed, hud_alert, left_lane_visible, - right_lane_visible, CS.ldw_stock_values, - left_lane_depart, right_lane_depart)) + if self.CP.carFingerprint in PQ_CARS: + can_sends.append(pqcan.create_pq_hud_control(self.packer_pt, CANBUS.pt, c.enabled, + CS.out.steeringPressed, hud_alert, left_lane_visible, + right_lane_visible, CS.ldw_stock_values, + left_lane_depart, right_lane_depart)) + else: + can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, c.enabled, + CS.out.steeringPressed, hud_alert, left_lane_visible, + right_lane_visible, CS.ldw_stock_values, + left_lane_depart, right_lane_depart)) # **** ACC Button Controls ********************************************** # @@ -105,7 +119,10 @@ class CarController(): if self.graMsgSentCount == 0: self.graMsgStartFramePrev = frame idx = (CS.graMsgBusCounter + 1) % 16 - can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx)) + if self.CP.carFingerprint in PQ_CARS: + can_sends.append(pqcan.create_pq_acc_buttons_control(self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx)) + else: + can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, ext_bus, self.graButtonStatesToSend, CS, idx)) self.graMsgSentCount += 1 if self.graMsgSentCount >= P.GRA_VBP_COUNT: self.graButtonStatesToSend = None diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 4193030d87..136492d76a 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -4,20 +4,30 @@ from common.conversions import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine -from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, NetworkLocation, TransmissionType, GearShifter, BUTTON_STATES, CarControllerParams +from selfdrive.car.volkswagen.values import PQ_CARS, DBC_FILES, CANBUS, NetworkLocation, TransmissionType, GearShifter, BUTTON_STATES, CarControllerParams class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) - can_define = CANDefine(DBC_FILES.mqb) - if CP.transmissionType == TransmissionType.automatic: - self.shifter_values = can_define.dv["Getriebe_11"]["GE_Fahrstufe"] - elif CP.transmissionType == TransmissionType.direct: - 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() + if CP.carFingerprint in PQ_CARS: + can_define = CANDefine(DBC_FILES.pq) + self.hca_status_values = can_define.dv["Lenkhilfe_2"]["LH2_Sta_HCA"] + if CP.transmissionType == TransmissionType.automatic: + self.shifter_values = can_define.dv["Getriebe_1"]["Waehlhebelposition__Getriebe_1_"] + else: + can_define = CANDefine(DBC_FILES.mqb) + self.hca_status_values = can_define.dv["LH_EPS_03"]["EPS_HCA_Status"] + if CP.transmissionType == TransmissionType.automatic: + self.shifter_values = can_define.dv["Getriebe_11"]["GE_Fahrstufe"] + elif CP.transmissionType == TransmissionType.direct: + self.shifter_values = can_define.dv["EV_Gearshift"]["GearPosition"] + def update(self, pt_cp, cam_cp, ext_cp, trans_type): + if self.CP.carFingerprint in PQ_CARS: + return self.update_pq(pt_cp, cam_cp, ext_cp, trans_type) + ret = car.CarState.new_message() # Update vehicle speed and acceleration from ABS wheel speeds. ret.wheelSpeeds = self.get_wheel_speeds( @@ -140,8 +150,138 @@ class CarState(CarStateBase): return ret + def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type): + ret = car.CarState.new_message() + # Update vehicle speed and acceleration from ABS wheel speeds. + ret.wheelSpeeds = self.get_wheel_speeds( + pt_cp.vl["Bremse_3"]["Radgeschw__VL_4_1"], + pt_cp.vl["Bremse_3"]["Radgeschw__VR_4_1"], + pt_cp.vl["Bremse_3"]["Radgeschw__HL_4_1"], + pt_cp.vl["Bremse_3"]["Radgeschw__HR_4_1"], + ) + + # FIXME: Match current Panda/support NSF by getting vEgoRaw from ego speed signal + ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgo < 0.1 + + # Update steering angle, rate, yaw rate, and driver input torque. VW send + # the sign/direction in a separate signal so they must be recombined. + ret.steeringAngleDeg = pt_cp.vl["Lenkhilfe_3"]["LH3_BLW"] * (1, -1)[int(pt_cp.vl["Lenkhilfe_3"]["LH3_BLWSign"])] + ret.steeringRateDeg = pt_cp.vl["Lenkwinkel_1"]["Lenkradwinkel_Geschwindigkeit"] * (1, -1)[int(pt_cp.vl["Lenkwinkel_1"]["Lenkradwinkel_Geschwindigkeit_S"])] + ret.steeringTorque = pt_cp.vl["Lenkhilfe_3"]["LH3_LM"] * (1, -1)[int(pt_cp.vl["Lenkhilfe_3"]["LH3_LMSign"])] + ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE + ret.yawRate = pt_cp.vl["Bremse_5"]["Giergeschwindigkeit"] * (1, -1)[int(pt_cp.vl["Bremse_5"]["Vorzeichen_der_Giergeschwindigk"])] * CV.DEG_TO_RAD + + # Verify EPS readiness to accept steering commands + hca_status = self.hca_status_values.get(pt_cp.vl["Lenkhilfe_2"]["LH2_Sta_HCA"]) + ret.steerFaultPermanent = hca_status in ["DISABLED", "FAULT"] + ret.steerFaultTemporary = hca_status in ["INITIALIZING", "REJECTED"] + + # Update gas, brakes, and gearshift. + ret.gas = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] / 100.0 + ret.gasPressed = ret.gas > 0 + ret.brake = pt_cp.vl["Bremse_5"]["Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects + ret.brakePressed = bool(pt_cp.vl["Motor_2"]["Bremstestschalter"]) + self.parkingBrakeSet = bool(pt_cp.vl["Kombi_1"]["Bremsinfo"]) + + # Update gear and/or clutch position data. + if trans_type == TransmissionType.automatic: + ret.gearShifter = self.parse_gear_shifter( + self.shifter_values.get(pt_cp.vl["Getriebe_1"]["Waehlhebelposition__Getriebe_1_"], None)) + elif trans_type == TransmissionType.manual: + ret.clutchPressed = not pt_cp.vl["Motor_1"]["Kupplungsschalter"] + reverse_light = bool(pt_cp.vl["Gate_Komf_1"]["GK1_Rueckfahr"]) + if reverse_light: + ret.gearShifter = GearShifter.reverse + else: + ret.gearShifter = GearShifter.drive + + # Update door and trunk/hatch lid open status. + # FIXME: need to locate signals for other three doors and trunk if possible + ret.doorOpen = bool(pt_cp.vl["Gate_Komf_1"]["GK1_Fa_Tuerkont"]) + + # Update seatbelt fastened status. + ret.seatbeltUnlatched = not bool(pt_cp.vl["Airbag_1"]["Gurtschalter_Fahrer"]) + + # Consume blind-spot monitoring info/warning LED states, if available. + # Infostufe: BSM LED on, Warnung: BSM LED flashing + if self.CP.enableBsm: + ret.leftBlindspot = bool(ext_cp.vl["SWA_1"]["SWA_Infostufe_SWA_li"]) or bool(ext_cp.vl["SWA_1"]["SWA_Warnung_SWA_li"]) + ret.rightBlindspot = bool(ext_cp.vl["SWA_1"]["SWA_Infostufe_SWA_re"]) or bool(ext_cp.vl["SWA_1"]["SWA_Warnung_SWA_re"]) + + # Consume factory LDW data relevant for factory SWA (Lane Change Assist) + # and capture it for forwarding to the blind spot radar controller + # TODO: populate this + self.ldw_stock_values = None + + # Stock FCW is considered active if the release bit for brake-jerk warning + # is set. Stock AEB considered active if the partial braking or target + # braking release bits are set. + # Refer to VW Self Study Program 890253: Volkswagen Driver Assistance + # Systems, chapter on Front Assist with Braking: Golf Family for all MQB + # TODO: fix document ref and populate this + ret.stockFcw = False + ret.stockAeb = False + + # Update ACC radar status. + accStatus = ext_cp.vl["ACC_GRA_Anziege"]["ACA_StaACC"] + if accStatus == 2: + # ACC okay and enabled, but not currently engaged + ret.cruiseState.available = True + ret.cruiseState.enabled = False + elif accStatus in [3, 4, 5]: + # ACC okay and enabled, currently engaged and regulating speed (3) or engaged with driver accelerating (4) or overrun (5) + # Verify against Motor_2 to keep in lockstep with Panda safety + ret.cruiseState.available = True + if pt_cp.vl["Motor_2"]["GRA_Status"] in [1, 2]: + ret.cruiseState.enabled = True + else: + ret.cruiseState.enabled = False + else: + # ACC okay but disabled (1), or a radar visibility or other fault/disruption (6 or 7) + ret.cruiseState.available = False + ret.cruiseState.enabled = False + + # Update ACC setpoint. When the setpoint reads as 255, the driver has not + # yet established an ACC setpoint, so treat it as zero. + ret.cruiseState.speed = ext_cp.vl["ACC_GRA_Anziege"]["ACA_V_Wunsch"] * CV.KPH_TO_MS + if ret.cruiseState.speed > 70: # 255 kph in m/s == no current setpoint + ret.cruiseState.speed = 0 + + # Update control button states for turn signals and ACC controls. + self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Up_kurz"]) or bool(pt_cp.vl["GRA_Neu"]["GRA_Up_lang"]) + self.buttonStates["decelCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Down_kurz"]) or bool(pt_cp.vl["GRA_Neu"]["GRA_Down_lang"]) + self.buttonStates["cancel"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Abbrechen"]) + self.buttonStates["setCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Neu_Setzen"]) + self.buttonStates["resumeCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Recall"]) + self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_Neu"]["GRA_Zeitluecke"]) + ret.leftBlinker = bool(pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"]) + ret.rightBlinker = bool(pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"]) + + # Read ACC hardware button type configuration info that has to pass thru + # to the radar. Ends up being different for steering wheel buttons vs + # third stalk type controls. + self.graHauptschalter = pt_cp.vl["GRA_Neu"]["GRA_Hauptschalt"] + self.graSenderCoding = pt_cp.vl["GRA_Neu"]["GRA_Sender"] + self.graTypHauptschalter = False + self.graButtonTypeInfo = False + self.graTipStufe2 = False + + # Pick up the GRA_ACC_01 CAN message counter so we can sync to it for + # later cruise-control button spamming. + self.graMsgBusCounter = pt_cp.vl["GRA_Neu"]["GRA_Neu_Zaehler"] + + # Additional safety checks performed in CarInterface. + ret.espDisabled = bool(pt_cp.vl["Bremse_1"]["ESP_Passiv_getastet"]) + + return ret + @staticmethod def get_can_parser(CP): + if CP.carFingerprint in PQ_CARS: + return CarState.get_can_parser_pq(CP) + signals = [ # sig_name, sig_address ("LWI_Lenkradwinkel", "LWI_01"), # Absolute steering angle @@ -226,6 +366,9 @@ class CarState(CarStateBase): @staticmethod def get_cam_can_parser(CP): + if CP.carFingerprint in PQ_CARS: + return CarState.get_cam_can_parser_pq(CP) + signals = [] checks = [] @@ -252,6 +395,101 @@ class CarState(CarStateBase): return CANParser(DBC_FILES.mqb, signals, checks, CANBUS.cam) + @staticmethod + def get_can_parser_pq(CP): + signals = [ + # sig_name, sig_address, default + ("LH3_BLW", "Lenkhilfe_3"), # Absolute steering angle + ("LH3_BLWSign", "Lenkhilfe_3"), # Steering angle sign + ("LH3_LM", "Lenkhilfe_3"), # Absolute driver torque input + ("LH3_LMSign", "Lenkhilfe_3"), # Driver torque input sign + ("LH2_Sta_HCA", "Lenkhilfe_2"), # Steering rack HCA status + ("Lenkradwinkel_Geschwindigkeit", "Lenkwinkel_1"), # Absolute steering rate + ("Lenkradwinkel_Geschwindigkeit_S", "Lenkwinkel_1"), # Steering rate sign + ("Radgeschw__VL_4_1", "Bremse_3"), # ABS wheel speed, front left + ("Radgeschw__VR_4_1", "Bremse_3"), # ABS wheel speed, front right + ("Radgeschw__HL_4_1", "Bremse_3"), # ABS wheel speed, rear left + ("Radgeschw__HR_4_1", "Bremse_3"), # ABS wheel speed, rear right + ("Giergeschwindigkeit", "Bremse_5"), # Absolute yaw rate + ("Vorzeichen_der_Giergeschwindigk", "Bremse_5"), # Yaw rate sign + ("Gurtschalter_Fahrer", "Airbag_1"), # Seatbelt status, driver + ("Gurtschalter_Beifahrer", "Airbag_1"), # Seatbelt status, passenger + ("Bremstestschalter", "Motor_2"), # Brake pedal pressed (brake light test switch) + ("Bremslichtschalter", "Motor_2"), # Brakes applied (brake light switch) + ("Bremsdruck", "Bremse_5"), # Brake pressure applied + ("Vorzeichen_Bremsdruck", "Bremse_5"), # Brake pressure applied sign (???) + ("Fahrpedal_Rohsignal", "Motor_3"), # Accelerator pedal value + ("ESP_Passiv_getastet", "Bremse_1"), # Stability control disabled + ("GRA_Status", "Motor_2"), # ACC engagement status + ("GK1_Fa_Tuerkont", "Gate_Komf_1"), # Door open, driver + ("GK1_Blinker_li", "Gate_Komf_1"), # Left turn signal on + ("GK1_Blinker_re", "Gate_Komf_1"), # Right turn signal on + ("Bremsinfo", "Kombi_1"), # Manual handbrake applied + ("GRA_Hauptschalt", "GRA_Neu"), # ACC button, on/off + ("GRA_Abbrechen", "GRA_Neu"), # ACC button, cancel + ("GRA_Neu_Setzen", "GRA_Neu"), # ACC button, set + ("GRA_Up_lang", "GRA_Neu"), # ACC button, increase or accel, long press + ("GRA_Down_lang", "GRA_Neu"), # ACC button, decrease or decel, long press + ("GRA_Up_kurz", "GRA_Neu"), # ACC button, increase or accel, short press + ("GRA_Down_kurz", "GRA_Neu"), # ACC button, decrease or decel, short press + ("GRA_Recall", "GRA_Neu"), # ACC button, resume + ("GRA_Zeitluecke", "GRA_Neu"), # ACC button, time gap adj + ("GRA_Neu_Zaehler", "GRA_Neu"), # ACC button, time gap adj + ("GRA_Sender", "GRA_Neu"), # GRA Sender Coding + ] + + checks = [ + # sig_address, frequency + ("Bremse_1", 100), # From J104 ABS/ESP controller + ("Bremse_3", 100), # From J104 ABS/ESP controller + ("Lenkhilfe_3", 100), # From J500 Steering Assist with integrated sensors + ("Lenkwinkel_1", 100), # From J500 Steering Assist with integrated sensors + ("Motor_3", 100), # From J623 Engine control module + ("Airbag_1", 50), # From J234 Airbag control module + ("Bremse_5", 50), # From J104 ABS/ESP controller + ("GRA_Neu", 50), # From J??? steering wheel control buttons + ("Kombi_1", 50), # From J285 Instrument cluster + ("Motor_2", 50), # From J623 Engine control module + ("Lenkhilfe_2", 20), # From J500 Steering Assist with integrated sensors + ("Gate_Komf_1", 10), # From J533 CAN gateway + ] + + if CP.transmissionType == TransmissionType.automatic: + signals += [("Waehlhebelposition__Getriebe_1_", "Getriebe_1", 0)] # Auto trans gear selector position + checks += [("Getriebe_1", 100)] # From J743 Auto transmission control module + elif CP.transmissionType == TransmissionType.manual: + signals += [("Kupplungsschalter", "Motor_1", 0), # Clutch switch + ("GK1_Rueckfahr", "Gate_Komf_1", 0)] # Reverse light from BCM + checks += [("Motor_1", 100)] # From J623 Engine control module + + if CP.networkLocation == NetworkLocation.fwdCamera: + # Extended CAN devices other than the camera are here on CANBUS.pt + signals += PqExtraSignals.fwd_radar_signals + checks += PqExtraSignals.fwd_radar_checks + if CP.enableBsm: + signals += PqExtraSignals.bsm_radar_signals + checks += PqExtraSignals.bsm_radar_checks + + return CANParser(DBC_FILES.pq, signals, checks, CANBUS.pt) + + @staticmethod + def get_cam_can_parser_pq(CP): + + # TODO: Populate this for NetworkLocation.fwdCamera + signals = [] + checks = [] + + if CP.networkLocation == NetworkLocation.gateway: + # Radars are here on CANBUS.cam + signals += PqExtraSignals.fwd_radar_signals + checks += PqExtraSignals.fwd_radar_checks + if CP.enableBsm: + signals += PqExtraSignals.bsm_radar_signals + checks += PqExtraSignals.bsm_radar_checks + + return CANParser(DBC_FILES.pq, signals, checks, CANBUS.cam) + + class MqbExtraSignals: # Additional signal and message lists for optional or bus-portable controllers fwd_radar_signals = [ @@ -273,3 +511,22 @@ class MqbExtraSignals: bsm_radar_checks = [ ("SWA_01", 20), # From J1086 Lane Change Assist ] + +class PqExtraSignals: + # Additional signal and message lists for optional or bus-portable controllers + fwd_radar_signals = [ + ("ACA_StaACC", "ACC_GRA_Anziege", 0), # ACC drivetrain coordinator status + ("ACA_V_Wunsch", "ACC_GRA_Anziege", 0), # ACC set speed + ] + fwd_radar_checks = [ + ("ACC_GRA_Anziege", 25), # From J428 ACC radar control module + ] + bsm_radar_signals = [ + ("SWA_Infostufe_SWA_li", "SWA_1", 0), # Blind spot object info, left + ("SWA_Warnung_SWA_li", "SWA_1", 0), # Blind spot object warning, left + ("SWA_Infostufe_SWA_re", "SWA_1", 0), # Blind spot object info, right + ("SWA_Warnung_SWA_re", "SWA_1", 0), # Blind spot object warning, right + ] + bsm_radar_checks = [ + ("SWA_1", 20), # From J1086 Lane Change Assist + ] diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 3a0d7c8ce8..d5446ef408 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,5 +1,6 @@ from cereal import car -from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES, CANBUS, NetworkLocation, TransmissionType, GearShifter +from common.conversions import Conversions as CV +from selfdrive.car.volkswagen.values import CAR, PQ_CARS, BUTTON_STATES, CANBUS, NetworkLocation, TransmissionType, GearShifter from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase @@ -25,7 +26,22 @@ class CarInterface(CarInterfaceBase): ret.carName = "volkswagen" ret.radarOffCan = True - if True: # pylint: disable=using-constant-test + if candidate in PQ_CARS: + # Set global PQ35/PQ46/NMS parameters + ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagenPq)] + ret.enableBsm = 0x3BA in fingerprint[0] # SWA_1 + + if 0x440 in fingerprint[0]: # Getriebe_1 + ret.transmissionType = TransmissionType.automatic + else: + ret.transmissionType = TransmissionType.manual + + if any(msg in fingerprint[1] for msg in (0x1A0, 0xAE)): # FIXME: what are these messages + ret.networkLocation = NetworkLocation.gateway + else: + ret.networkLocation = NetworkLocation.fwdCamera + + else: # Set global MQB parameters ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagen)] ret.enableBsm = 0x30F in fingerprint[0] # SWA_01 @@ -77,6 +93,11 @@ class CarInterface(CarInterfaceBase): ret.mass = 1551 + STD_CARGO_KG ret.wheelbase = 2.79 + elif candidate == CAR.PASSAT_NMS: + ret.mass = 1503 + STD_CARGO_KG + ret.wheelbase = 2.80 + ret.minSteerSpeed = 50 * CV.KPH_TO_MS # FIXME: may be lower depending on model-year/EPS FW + elif candidate == CAR.POLO_MK6: ret.mass = 1230 + STD_CARGO_KG ret.wheelbase = 2.55 diff --git a/selfdrive/car/volkswagen/pqcan.py b/selfdrive/car/volkswagen/pqcan.py new file mode 100644 index 0000000000..fee08b6c59 --- /dev/null +++ b/selfdrive/car/volkswagen/pqcan.py @@ -0,0 +1,42 @@ +def create_pq_steering_control(packer, bus, apply_steer, idx, lkas_enabled): + values = { + "HCA_Zaehler": idx, + "LM_Offset": abs(apply_steer), + "LM_OffSign": 1 if apply_steer < 0 else 0, + "HCA_Status": 5 if (lkas_enabled and apply_steer != 0) else 3, + "Vib_Freq": 16, + } + + dat = packer.make_can_msg("HCA_1", bus, values)[2] + values["HCA_Checksumme"] = dat[1] ^ dat[2] ^ dat[3] ^ dat[4] + return packer.make_can_msg("HCA_1", bus, values) + +def create_pq_hud_control(packer, bus, hca_enabled, steering_pressed, hud_alert, left_lane_visible, right_lane_visible, + ldw_stock_values, left_lane_depart, right_lane_depart): + if hca_enabled: + left_lane_hud = 3 if left_lane_depart else 1 + left_lane_visible + right_lane_hud = 3 if right_lane_depart else 1 + right_lane_visible + else: + left_lane_hud = 0 + right_lane_hud = 0 + + values = { + "Right_Lane_Status": right_lane_hud, + "Left_Lane_Status": left_lane_hud, + "SET_ME_X1": 1, + "Kombi_Lamp_Orange": 1 if hca_enabled and steering_pressed else 0, + "Kombi_Lamp_Green": 1 if hca_enabled and not steering_pressed else 0, + } + return packer.make_can_msg("LDW_1", bus, values) + +def create_pq_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx): + values = { + "GRA_Neu_Zaehler": idx, + "GRA_Sender": CS.graSenderCoding, + "GRA_Abbrechen": 1 if (buttonStatesToSend["cancel"] or CS.buttonStates["cancel"]) else 0, + "GRA_Hauptschalt": CS.graHauptschalter, + } + + dat = packer.make_can_msg("GRA_Neu", bus, values)[2] + values["GRA_Checksum"] = dat[1] ^ dat[2] ^ dat[3] + return packer.make_can_msg("GRA_Neu", bus, values) diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 6367dd8969..c111c70cf0 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -14,9 +14,10 @@ GearShifter = car.CarState.GearShifter class CarControllerParams: - HCA_STEP = 2 # HCA_01 message frequency 50Hz - LDW_STEP = 10 # LDW_02 message frequency 10Hz - GRA_ACC_STEP = 3 # GRA_ACC_01 message frequency 33Hz + HCA_STEP = 2 # HCA_01/HCA_1 message frequency 50Hz + MQB_LDW_STEP = 10 # LDW_02 message frequency 10Hz on MQB + PQ_LDW_STEP = 5 # LDW_1 message frequency 20Hz on PQ35/PQ46/NMS + GRA_ACC_STEP = 3 # GRA_ACC_01/GRA_Neu message frequency 33Hz GRA_VBP_STEP = 100 # Send ACC virtual button presses once a second GRA_VBP_COUNT = 16 # Send VBP messages for ~0.5s (GRA_ACC_STEP * 16) @@ -36,11 +37,11 @@ class CANBUS: pt = 0 cam = 2 - class DBC_FILES: mqb = "vw_mqb_2010" # Used for all cars with MQB-style CAN messaging + pq = "vw_golf_mk4" # Used for all cars with PQ-style (legacy) CAN messaging - +# FIXME: PlotJuggler will assume wrong DBC for PQ, redo this and replace current handling of CANPacker/CANDefine DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict(DBC_FILES.mqb, None)) BUTTON_STATES = { @@ -76,6 +77,7 @@ class CAR: GOLF_MK7 = "VOLKSWAGEN GOLF 7TH GEN" # Chassis 5G/AU/BA/BE, Mk7 VW Golf and variants JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN" # Chassis BU, Mk7 VW Jetta PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN" # Chassis 3G, Mk8 VW Passat and variants + PASSAT_NMS = "VOLKSWAGEN PASSAT NMS" # Chassis A3, North America/China/Mideast NMS Passat, incl. facelift POLO_MK6 = "VOLKSWAGEN POLO 6TH GEN" # Chassis AW, Mk6 VW Polo TAOS_MK1 = "VOLKSWAGEN TAOS 1ST GEN" # Chassis B2, Mk1 VW Taos and Tharu TCROSS_MK1 = "VOLKSWAGEN T-CROSS 1ST GEN" # Chassis C1, Mk1 VW T-Cross SWB and LWB variants @@ -95,13 +97,17 @@ class CAR: SKODA_SUPERB_MK3 = "SKODA SUPERB 3RD GEN" # Chassis 3V/NP, Mk3 Skoda Superb and variants SKODA_OCTAVIA_MK3 = "SKODA OCTAVIA 3RD GEN" # Chassis NE, Mk3 Skoda Octavia and variants +PQ_CARS = {CAR.PASSAT_NMS} class Footnote(Enum): KAMIQ = CarFootnote( "Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.", Column.MODEL) PASSAT = CarFootnote( - "Not including the USA/China market Passat, which is based on the (currently) unsupported PQ35/NMS platform.", + "European B8 Passat on MQB platform, not including USA/China/Mideast.", + Column.MODEL) + PASSAT_NMS = CarFootnote( + "USA/China/Mideast NMS Passat made on the PQ46 platform, not including Europe.", Column.MODEL) VW_HARNESS = CarFootnote( "Model-years 2021 and beyond may have a new camera harness design, which isn't yet available from the comma " + @@ -136,6 +142,7 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { VWCarInfo("Volkswagen Jetta GLI 2021"), ], CAR.PASSAT_MK8: VWCarInfo("Volkswagen Passat 2015-18", footnotes=[Footnote.PASSAT]), + CAR.PASSAT_NMS: VWCarInfo("Volkswagen Passat 2017, 2021", footnotes=[Footnote.PASSAT_NMS]), CAR.POLO_MK6: VWCarInfo("Volkswagen Polo 2020"), CAR.TAOS_MK1: VWCarInfo("Volkswagen Taos 2022", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), CAR.TCROSS_MK1: VWCarInfo("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), @@ -472,6 +479,24 @@ FW_VERSIONS = { b'\xf1\x875Q0907572R \xf1\x890771', ], }, + CAR.PASSAT_NMS: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8706K906016G \xf1\x891124', + b'\xf1\x8706K906071BJ\xf1\x894891', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x8709G927158BD\xf1\x893121', + b'\xf1\x8709G927158FQ\xf1\x893745', + ], + (Ecu.srs, 0x715, None): [ + b'\xf1\x87561959655 \xf1\x890210\xf1\x82\02212121111113000102011--121012--101312', + b'\xf1\x87561959655C \xf1\x890508\xf1\x82\02215141111121100314919--153015--304831', + ], + (Ecu.fwdRadar, 0x757, None): [ + b'\xf1\x87561907567A \xf1\x890132', + b'\xf1\x877N0907572C \xf1\x890211\xf1\x82\00152', + ], + }, CAR.POLO_MK6: { (Ecu.engine, 0x7e0, None): [ b'\xf1\x8704C906025H \xf1\x895177', diff --git a/selfdrive/car/volkswagen/volkswagencan.py b/selfdrive/car/volkswagen/volkswagencan.py index 10e0054c79..0d8d04766b 100644 --- a/selfdrive/car/volkswagen/volkswagencan.py +++ b/selfdrive/car/volkswagen/volkswagencan.py @@ -1,6 +1,3 @@ -# CAN controls for MQB platform Volkswagen, Audi, Skoda and SEAT. -# PQ35/PQ46/NMS, and any future MLB, to come later. - def create_mqb_steering_control(packer, bus, apply_steer, idx, lkas_enabled): values = { "SET_ME_0X3": 0x3,