diff --git a/panda b/panda index 5e0dde7b48..199bc10db3 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 5e0dde7b480a930d3c3a164331c930541e905d71 +Subproject commit 199bc10db3a937fab0c50d9b708f6c76234f5c3a diff --git a/release/files_common b/release/files_common index 8bf99f0228..01386b5858 100644 --- a/release/files_common +++ b/release/files_common @@ -552,6 +552,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/tests/routes.py b/selfdrive/car/tests/routes.py index 0b1726b010..44672bf9dd 100644 --- a/selfdrive/car/tests/routes.py +++ b/selfdrive/car/tests/routes.py @@ -168,6 +168,7 @@ routes = [ TestRoute("cae14e88932eb364|2021-03-26--14-43-28", VOLKSWAGEN.GOLF_MK7), TestRoute("58a7d3b707987d65|2021-03-25--17-26-37", VOLKSWAGEN.JETTA_MK7), TestRoute("4d134e099430fba2|2021-03-26--00-26-06", VOLKSWAGEN.PASSAT_MK8), + TestRoute("3cfdec54aa035f3f|2022-07-19--23-45-10", VOLKSWAGEN.PASSAT_NMS), TestRoute("0cd0b7f7e31a3853|2021-11-03--19-30-22", VOLKSWAGEN.POLO_MK6), TestRoute("7d82b2f3a9115f1f|2021-10-21--15-39-42", VOLKSWAGEN.TAOS_MK1), TestRoute("2744c89a8dda9a51|2021-07-24--21-28-06", VOLKSWAGEN.TCROSS_MK1), diff --git a/selfdrive/car/torque_data/override.yaml b/selfdrive/car/torque_data/override.yaml index 0a958a9d03..8dd8c43220 100644 --- a/selfdrive/car/torque_data/override.yaml +++ b/selfdrive/car/torque_data/override.yaml @@ -25,6 +25,7 @@ RAM 1500 5TH GEN: [2.0, 2.0, 0.0] RAM HD 5TH GEN: [1.4, 1.4, 0.0] SUBARU OUTBACK 6TH GEN: [2.3, 2.3, 0.11] CHEVROLET BOLT EUV 2022: [2.0, 2.0, 0.05] +VOLKSWAGEN PASSAT NMS: [2.5, 2.5, 0.1] # Dashcam or fallback configured as ideal car mock: [10.0, 10, 0.0] diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 2c5830f868..f14c119892 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -1,8 +1,10 @@ from cereal import car from opendbc.can.packer import CANPacker +from common.numpy_fast import clip +from common.conversions import Conversions as CV from selfdrive.car import apply_std_steer_torque_limits -from selfdrive.car.volkswagen import mqbcan -from selfdrive.car.volkswagen.values import CANBUS, CarControllerParams +from selfdrive.car.volkswagen import mqbcan, pqcan +from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -11,7 +13,7 @@ class CarController: def __init__(self, dbc_name, CP, VM): self.CP = CP self.CCP = CarControllerParams(CP) - self.CCS = mqbcan + self.CCS = pqcan if CP.carFingerprint in PQ_CARS else mqbcan self.packer_pt = CANPacker(dbc_name) self.apply_steer_last = 0 @@ -65,6 +67,13 @@ class CarController: self.apply_steer_last = apply_steer can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer, hcaEnabled)) + # **** Acceleration Controls ******************************************** # + + if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl: + tsk_status = self.CCS.tsk_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive) + accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0 + can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, tsk_status, accel)) + # **** HUD Controls ***************************************************** # if self.frame % self.CCP.LDW_STEP == 0: @@ -74,6 +83,12 @@ class CarController: can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.enabled, CS.out.steeringPressed, hud_alert, hud_control)) + if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl: + 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)) + # **** Stock ACC Button Controls **************************************** # if self.CP.pcmCruise and self.frame % self.CCP.GRA_ACC_STEP == 0: diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 50be6bf9ad..facc740a15 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -3,7 +3,7 @@ from cereal import car from common.conversions import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser -from selfdrive.car.volkswagen.values import DBC, CANBUS, NetworkLocation, TransmissionType, GearShifter, \ +from selfdrive.car.volkswagen.values import DBC, CANBUS, PQ_CARS, NetworkLocation, TransmissionType, GearShifter, \ CarControllerParams @@ -28,6 +28,9 @@ class CarState(CarStateBase): return button_events 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( @@ -135,8 +138,111 @@ 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"], + ) + + # vEgo obtained from Bremse_1 vehicle speed rather than Bremse_3 wheel speeds because Bremse_3 isn't present on NSF + ret.vEgoRaw = pt_cp.vl["Bremse_1"]["Geschwindigkeit_neu__Bremse_1_"] * CV.KPH_TO_MS + 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) > self.CCP.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.CCP.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"]) + ret.parkingBrake = 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.CCP.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. + ret.doorOpen = any([pt_cp.vl["Gate_Komf_1"]["GK1_Fa_Tuerkont"], + pt_cp.vl["Gate_Komf_1"]["BSK_BT_geoeffnet"], + pt_cp.vl["Gate_Komf_1"]["BSK_HL_geoeffnet"], + pt_cp.vl["Gate_Komf_1"]["BSK_HR_geoeffnet"], + pt_cp.vl["Gate_Komf_1"]["BSK_HD_Hauptraste"]]) + + # 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 + self.ldw_stock_values = cam_cp.vl["LDW_Status"] if self.CP.networkLocation == NetworkLocation.fwdCamera else {} + + # 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, chapters on Front Assist with Braking and City Emergency + # Braking for the 2016 Passat NMS + # TODO: deferred until we can collect data on pre-MY2016 behavior, AWV message may be shorter with fewer signals + ret.stockFcw = False + ret.stockAeb = False + + # Update ACC radar status. + ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["GRA_Hauptschalter"]) + ret.cruiseState.enabled = bool(pt_cp.vl["Motor_2"]["GRA_Status"]) + if self.CP.pcmCruise: + ret.accFaulted = ext_cp.vl["ACC_GRA_Anziege"]["ACA_StaACC"] in (6, 7) + # TODO: update opendbc with PQ TSK state for OP long accFaulted + + # 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 button states for turn signals and ACC controls, capture all ACC button state/config for passthrough + ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(300, pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"], + pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"]) + ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS) + self.gra_stock_values = pt_cp.vl["GRA_Neu"] + + # 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 @@ -222,6 +328,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 = [] @@ -248,6 +357,123 @@ class CarState(CarStateBase): return CANParser(DBC[CP.carFingerprint]["pt"], 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 + ("Geschwindigkeit_neu__Bremse_1_", "Bremse_1"), # Vehicle speed from ABS + ("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_Hauptschalter", "Motor_5"), # ACC main switch + ("GRA_Status", "Motor_2"), # ACC engagement status + ("GK1_Fa_Tuerkont", "Gate_Komf_1"), # Door open, driver + ("BSK_BT_geoeffnet", "Gate_Komf_1"), # Door open, passenger + ("BSK_HL_geoeffnet", "Gate_Komf_1"), # Door open, rear left + ("BSK_HR_geoeffnet", "Gate_Komf_1"), # Door open, rear right + ("BSK_HD_Hauptraste", "Gate_Komf_1"), # Trunk or hatch open + ("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_Typ_Hauptschalt", "GRA_Neu"), # ACC button, momentary vs latching + ("GRA_Kodierinfo", "GRA_Neu"), # ACC button, configuration + ("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 + ("COUNTER", "GRA_Neu"), # ACC button, message counter + ("GRA_Sender", "GRA_Neu"), # ACC button, CAN message originator + ] + + 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 + ("Motor_5", 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[CP.carFingerprint]["pt"], signals, checks, CANBUS.pt) + + @staticmethod + def get_cam_can_parser_pq(CP): + + signals = [] + checks = [] + + if CP.networkLocation == NetworkLocation.fwdCamera: + signals += [ + # sig_name, sig_address + ("LDW_SW_Warnung_links", "LDW_Status"), # Blind spot in warning mode on left side due to lane departure + ("LDW_SW_Warnung_rechts", "LDW_Status"), # Blind spot in warning mode on right side due to lane departure + ("LDW_Seite_DLCTLC", "LDW_Status"), # Direction of most likely lane departure (left or right) + ("LDW_DLC", "LDW_Status"), # Lane departure, distance to line crossing + ("LDW_TLC", "LDW_Status"), # Lane departure, time to line crossing + ] + checks += [ + # sig_address, frequency + ("LDW_Status", 10) # From R242 Driver assistance camera + ] + + 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[CP.carFingerprint]["pt"], signals, checks, CANBUS.cam) + + class MqbExtraSignals: # Additional signal and message lists for optional or bus-portable controllers fwd_radar_signals = [ @@ -269,3 +495,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 dfa0a555e3..22206635ac 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,8 +1,10 @@ from cereal import car +from panda import Panda +from common.conversions import Conversions as CV from selfdrive.car import STD_CARGO_KG, create_button_enable_events, scale_rot_inertia, scale_tire_stiffness, \ gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase -from selfdrive.car.volkswagen.values import CAR, CANBUS, NetworkLocation, TransmissionType, GearShifter +from selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter EventName = car.CarEvent.EventName @@ -25,7 +27,36 @@ 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, 0xC2)): # Bremse_1, Lenkwinkel_1 + ret.networkLocation = NetworkLocation.gateway + else: + ret.networkLocation = NetworkLocation.fwdCamera + + # The PQ port is in dashcam-only mode due to a fixed six-minute maximum timer on HCA steering. An unsupported + # EPS flash update to work around this timer, and enable steering down to zero, is available from: + # https://github.com/pd0wm/pq-flasher + # It is documented in a four-part blog series: + # https://blog.willemmelching.nl/carhacking/2022/01/02/vw-part1/ + # Panda ALLOW_DEBUG firmware required. + ret.dashcamOnly = True + + if disable_radar and ret.networkLocation == NetworkLocation.gateway: + # Proof-of-concept, prep for E2E only. No radar points available. Follow-to-stop not yet supported, but should + # be simple to add when a suitable test car becomes available. Panda ALLOW_DEBUG firmware required. + ret.openpilotLongitudinalControl = True + ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL + + else: # Set global MQB parameters ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagen)] ret.enableBsm = 0x30F in fingerprint[0] # SWA_01 @@ -83,6 +114,14 @@ 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.minEnableSpeed = 20 * CV.KPH_TO_MS # ACC "basic", no FtS + ret.minSteerSpeed = 50 * CV.KPH_TO_MS + ret.steerActuatorDelay = 0.2 + CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning) + 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..1e7ed21342 --- /dev/null +++ b/selfdrive/car/volkswagen/pqcan.py @@ -0,0 +1,84 @@ +def create_steering_control(packer, bus, apply_steer, lkas_enabled): + values = { + "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, + } + + return packer.make_can_msg("HCA_1", bus, values) + + +def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control): + values = ldw_stock_values.copy() + + values.update({ + "LDW_Lampe_gelb": 1 if enabled and steering_pressed else 0, + "LDW_Lampe_gruen": 1 if enabled and not steering_pressed else 0, + "LDW_Lernmodus_links": 3 if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible, + "LDW_Lernmodus_rechts": 3 if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible, + "LDW_Textbits": hud_alert, + }) + + return packer.make_can_msg("LDW_Status", bus, values) + + +def create_acc_buttons_control(packer, bus, gra_stock_values, idx, cancel=False, resume=False): + values = gra_stock_values.copy() + + values.update({ + "COUNTER": idx, + "GRA_Abbrechen": cancel, + "GRA_Recall": resume, + }) + + return packer.make_can_msg("GRA_Neu", bus, values) + + +def tsk_status_value(main_switch_on, acc_faulted, long_active): + if long_active: + tsk_status = 1 + elif main_switch_on: + tsk_status = 2 + else: + tsk_status = 0 + + return tsk_status + + +def acc_hud_status_value(main_switch_on, acc_faulted, long_active): + if acc_faulted: + hud_status = 6 + elif long_active: + hud_status = 3 + elif main_switch_on: + hud_status = 2 + else: + hud_status = 0 + + return hud_status + + +def create_acc_accel_control(packer, bus, adr_status, accel): + values = { + "ACS_Sta_ADR": adr_status, + "ACS_StSt_Info": adr_status != 1, + "ACS_Typ_ACC": 0, # TODO: this is ACC "basic", find a way to detect FtS support (1) + "ACS_Sollbeschl": accel if adr_status == 1 else 3.01, + "ACS_zul_Regelabw": 0.2 if adr_status == 1 else 1.27, + "ACS_max_AendGrad": 3.0 if adr_status == 1 else 5.08, + } + + return packer.make_can_msg("ACC_System", bus, values) + + +def create_acc_hud_control(packer, bus, acc_status, set_speed, lead_visible): + values = { + "ACA_StaACC": acc_status, + "ACA_Zeitluecke": 2, + "ACA_V_Wunsch": set_speed, + "ACA_gemZeitl": 8 if lead_visible else 0, + } + # TODO: ACA_ID_StaACC, ACA_AnzDisplay, ACA_kmh_mph, ACA_PrioDisp, ACA_Aend_Zeitluecke + + return packer.make_can_msg("ACC_GRA_Anziege", bus, values) diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 785a70d2da..58f071901b 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -18,6 +18,11 @@ Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values']) class CarControllerParams: HCA_STEP = 2 # HCA_01/HCA_1 message frequency 50Hz GRA_ACC_STEP = 3 # GRA_ACC_01/GRA_Neu message frequency 33Hz + ACC_CONTROL_STEP = 2 # ACC_06/ACC_07/ACC_System frequency 50Hz + ACC_HUD_STEP = 4 # ACC_GRA_Anziege frequency 25Hz + + ACCEL_MAX = 2.0 # 2.0 m/s max acceleration + ACCEL_MIN = -3.5 # 3.5 m/s max deceleration def __init__(self, CP): # Documented lateral limits: 3.00 Nm max, rate of change 5.00 Nm/sec. @@ -29,7 +34,35 @@ class CarControllerParams: can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) - if True: # pylint: disable=using-constant-test + if CP.carFingerprint in PQ_CARS: + self.LDW_STEP = 5 # LDW_1 message frequency 20Hz + self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm + self.STEER_DELTA_UP = 6 # Max HCA reached in 1.00s (STEER_MAX / (50Hz * 1.00)) + self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60)) + + if CP.transmissionType == TransmissionType.automatic: + self.shifter_values = can_define.dv["Getriebe_1"]["Waehlhebelposition__Getriebe_1_"] + self.hca_status_values = can_define.dv["Lenkhilfe_2"]["LH2_Sta_HCA"] + + self.BUTTONS = [ + Button(car.CarState.ButtonEvent.Type.setCruise, "GRA_Neu", "GRA_Neu_Setzen", [1]), + Button(car.CarState.ButtonEvent.Type.resumeCruise, "GRA_Neu", "GRA_Recall", [1]), + Button(car.CarState.ButtonEvent.Type.accelCruise, "GRA_Neu", "GRA_Up_kurz", [1]), + Button(car.CarState.ButtonEvent.Type.decelCruise, "GRA_Neu", "GRA_Down_kurz", [1]), + Button(car.CarState.ButtonEvent.Type.cancel, "GRA_Neu", "GRA_Abbrechen", [1]), + Button(car.CarState.ButtonEvent.Type.gapAdjustCruise, "GRA_Neu", "GRA_Zeitluecke", [1]), + ] + + self.LDW_MESSAGES = { + "none": 0, # Nothing to display + "laneAssistUnavail": 1, # "Lane Assist currently not available." + "laneAssistUnavailSysError": 2, # "Lane Assist system error" + "laneAssistUnavailNoSensorView": 3, # "Lane Assist not available. No sensor view." + "laneAssistTakeOver": 4, # "Lane Assist: Please Take Over Steering" + "laneAssistDeactivTrailer": 5, # "Lane Assist: no function with trailer" + } + + else: self.LDW_STEP = 10 # LDW_02 message frequency 10Hz self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm self.STEER_DELTA_UP = 4 # Max HCA reached in 1.50s (STEER_MAX / (50Hz * 1.50)) @@ -79,6 +112,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 @@ -99,7 +133,12 @@ class CAR: SKODA_OCTAVIA_MK3 = "SKODA OCTAVIA 3RD GEN" # Chassis NE, Mk3 Skoda Octavia and variants +PQ_CARS = {CAR.PASSAT_NMS} + + DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("vw_mqb_2010", None)) +for car_type in PQ_CARS: + DBC[car_type] = dbc_dict("vw_golf_mk4", None) class Footnote(Enum): @@ -160,6 +199,7 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = { VWCarInfo("Volkswagen Passat Alltrack 2015-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), VWCarInfo("Volkswagen Passat GTE 2015-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533), ], + CAR.PASSAT_NMS: VWCarInfo("Volkswagen Passat NMS 2017-22", harness=Harness.j533), CAR.POLO_MK6: [ VWCarInfo("Volkswagen Polo 2020-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), VWCarInfo("Volkswagen Polo GTI 2020-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533), @@ -514,6 +554,26 @@ FW_VERSIONS = { b'\xf1\x875Q0907572R \xf1\x890771', ], }, + CAR.PASSAT_NMS: { + (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8706K906016C \xf1\x899609', + b'\xf1\x8706K906016G \xf1\x891124', + b'\xf1\x8706K906071BJ\xf1\x894891', + ], + (Ecu.transmission, 0x7e1, None): [ + b'\xf1\x8709G927158AB\xf1\x893318', + 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/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 68c1666b59..f5aaec3622 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -32,8 +32,9 @@ original_segments = [ ("VOLKSWAGEN", "de9592456ad7d144|2021-06-29--11-00-15--6"), # VOLKSWAGEN.GOLF ("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--2"), # MAZDA.CX9_2021 - # Enable when port is tested and dascamOnly is no longer set + # Enable when port is tested and dashcamOnly is no longer set #("TESLA", "bb50caf5f0945ab1|2021-06-19--17-20-18--3"), # TESLA.AP2_MODELS + #("VOLKSWAGEN2", "3cfdec54aa035f3f|2022-07-19--23-45-10--2"), # VOLKSWAGEN.PASSAT_NMS ] segments = [