diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 5bb5dcc10..ff63de014 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, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams +from selfdrive.car.volkswagen.values import DBC, CANBUS, NWL, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams from opendbc.can.packer import CANPacker @@ -10,6 +10,7 @@ class CarController(): self.apply_steer_last = 0 self.packer_pt = CANPacker(DBC[CP.carFingerprint]['pt']) + self.acc_bus = CANBUS.pt if CP.networkLocation == NWL.fwdCamera else CANBUS.cam self.hcaSameTorqueCount = 0 self.hcaEnabledFrameCount = 0 @@ -119,7 +120,9 @@ class CarController(): can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, hcaEnabled, CS.out.steeringPressed, hud_alert, leftLaneVisible, - rightLaneVisible)) + rightLaneVisible, CS.ldw_lane_warning_left, + CS.ldw_lane_warning_right, CS.ldw_side_dlc_tlc, + CS.ldw_dlc, CS.ldw_tlc)) #-------------------------------------------------------------------------- # # @@ -176,7 +179,7 @@ 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, CANBUS.pt, self.graButtonStatesToSend, CS, idx)) + can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, self.acc_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 e346ac4a9..4a317f929 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -4,16 +4,19 @@ from selfdrive.config 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, CANBUS, BUTTON_STATES, CarControllerParams +from selfdrive.car.volkswagen.values import DBC, CANBUS, NWL, TRANS, GEAR, BUTTON_STATES, CarControllerParams class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]['pt']) - self.shifter_values = can_define.dv["Getriebe_11"]['GE_Fahrstufe'] + if CP.transmissionType == TRANS.automatic: + self.shifter_values = can_define.dv["Getriebe_11"]['GE_Fahrstufe'] + elif CP.transmissionType == TRANS.direct: + self.shifter_values = can_define.dv["EV_Gearshift"]['GearPosition'] self.buttonStates = BUTTON_STATES.copy() - def update(self, pt_cp): + def update(self, pt_cp, cam_cp, acc_cp, trans_type): ret = car.CarState.new_message() # Update vehicle speed and acceleration from ABS wheel speeds. ret.wheelSpeeds.fl = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS @@ -212,22 +215,67 @@ class CarState(CarStateBase): ("Einheiten_01", 1), # From J??? not known if gateway, cluster, or BCM ] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, CANBUS.pt) + if CP.transmissionType == TRANS.automatic: + signals += [("GE_Fahrstufe", "Getriebe_11", 0)] # Auto trans gear selector position + checks += [("Getriebe_11", 20)] # From J743 Auto transmission control module + elif CP.transmissionType == TRANS.direct: + signals += [("GearPosition", "EV_Gearshift", 0)] # EV gear selector position + checks += [("EV_Gearshift", 10)] # From J??? unknown EV control module + elif CP.transmissionType == TRANS.manual: + signals += [("MO_Kuppl_schalter", "Motor_14", 0), # Clutch switch + ("BCM1_Rueckfahrlicht_Schalter", "Gateway_72", 0)] # Reverse light from BCM + checks += [("Motor_14", 10)] # From J623 Engine control module + + if CP.networkLocation == NWL.fwdCamera: + # Extended CAN devices other than the camera are here on CANBUS.pt + # FIXME: gate SWA_01 checks on module being detected, and reduce duplicate network location code + signals += [("AWV2_Priowarnung", "ACC_10", 0), # FCW related + ("AWV2_Freigabe", "ACC_10", 0), # FCW related + ("ANB_Teilbremsung_Freigabe", "ACC_10", 0), # AEB related + ("ANB_Zielbremsung_Freigabe", "ACC_10", 0), # AEB related + ("SWA_Infostufe_SWA_li", "SWA_01", 0), # Blindspot object info, left + ("SWA_Warnung_SWA_li", "SWA_01", 0), # Blindspot object warning, left + ("SWA_Infostufe_SWA_re", "SWA_01", 0), # Blindspot object info, right + ("SWA_Warnung_SWA_re", "SWA_01", 0), # Blindspot object warning, right + ("ACC_Wunschgeschw", "ACC_02", 0)] # ACC set speed + checks += [("ACC_10", 50), # From J428 ACC radar control module + ("SWA_01", 20), # From J1086 Lane Change Assist module + ("ACC_02", 17)] # From J428 ACC radar control module - # A single signal is monitored from the camera CAN bus, and then ignored, - # so the presence of CAN traffic can be verified with cam_cp.valid. + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, CANBUS.pt) @staticmethod def get_cam_can_parser(CP): + # FIXME: gate LDW_02 checks on module being detected signals = [ # sig_name, sig_address, default - ("Kombi_Lamp_Green", "LDW_02", 0), # Lane Assist status LED + ("LDW_SW_Warnung_links", "LDW_02", 0), # Blind spot in warning mode on left side due to lane departure + ("LDW_SW_Warnung_rechts", "LDW_02", 0), # Blind spot in warning mode on right side due to lane departure + ("LDW_Seite_DLCTLC", "LDW_02", 0), # Direction of most likely lane departure (left or right) + ("LDW_DLC", "LDW_02", 0), # Lane departure, distance to line crossing + ("LDW_TLC", "LDW_02", 0), # Lane departure, time to line crossing ] checks = [ # sig_address, frequency - ("LDW_02", 10) # From R242 Driver assistance camera + ("LDW_02", 10), # From R242 Driver assistance camera ] + if CP.networkLocation == NWL.gateway: + # All Extended CAN devices are here on CANBUS.cam + # FIXME: gate SWA_01 checks on module being detected, and reduce duplicate network location code + signals += [("AWV2_Priowarnung", "ACC_10", 0), # FCW related + ("AWV2_Freigabe", "ACC_10", 0), # FCW related + ("ANB_Teilbremsung_Freigabe", "ACC_10", 0), # AEB related + ("ANB_Zielbremsung_Freigabe", "ACC_10", 0), # AEB related + ("SWA_Infostufe_SWA_li", "SWA_01", 0), # Blindspot object info, left + ("SWA_Warnung_SWA_li", "SWA_01", 0), # Blindspot object warning, left + ("SWA_Infostufe_SWA_re", "SWA_01", 0), # Blindspot object info, right + ("SWA_Warnung_SWA_re", "SWA_01", 0), # Blindspot object warning, right + ("ACC_Wunschgeschw", "ACC_02", 0)] # ACC set speed + checks += [("ACC_10", 50), # From J428 ACC radar control module + ("SWA_01", 20), # From J1086 Lane Change Assist module + ("ACC_02", 17)] # From J428 ACC radar control module + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, CANBUS.cam) diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 2d4b889fe..3d3ad08da 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,5 +1,7 @@ from cereal import car -from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES +from selfdrive.swaglog import cloudlog +from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES, NWL, TRANS, GEAR +#from common.params import put_nonblocking from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase @@ -12,6 +14,9 @@ class CarInterface(CarInterfaceBase): self.displayMetricUnitsPrev = None self.buttonStatesPrev = BUTTON_STATES.copy() + # Set up an alias to PT/CAM parser for ACC depending on its detected network location + self.cp_acc = self.cp if CP.networkLocation == NWL.fwdCamera else self.cp_cam + @staticmethod def compute_gb(accel, speed): return float(accel) / 4.0 @@ -20,23 +25,24 @@ class CarInterface(CarInterfaceBase): def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None): ret = CarInterfaceBase.get_std_params(candidate, fingerprint) - # VW port is a community feature, since we don't own one to test - ret.communityFeature = True + ret.enableCamera = True # Stock camera detection doesn't apply to VW + ret.carName = "volkswagen" + ret.radarOffCan = True - if candidate == CAR.GOLF: + if candidate == CAR.GENERICMQB: # Set common MQB parameters that will apply globally - ret.carName = "volkswagen" - ret.radarOffCan = True ret.safetyModel = car.CarParams.SafetyModel.volkswagen # Additional common MQB parameters that may be overridden per-vehicle ret.steerRateCost = 1.0 - ret.steerActuatorDelay = 0.05 # Hopefully all MQB racks are similar here + ret.steerActuatorDelay = 0.1 # Hopefully all MQB racks are similar here ret.steerLimitTimer = 0.4 ret.lateralTuning.pid.kpBP = [0.] ret.lateralTuning.pid.kiBP = [0.] + # FIXME: Per-vehicle parameters need to be reintegrated. + # Until that time, defaulting to VW Golf Mk7 as a baseline. ret.mass = 1500 + STD_CARGO_KG ret.wheelbase = 2.64 ret.centerToFront = ret.wheelbase * 0.45 @@ -46,8 +52,26 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kiV = [0.2] tire_stiffness_factor = 1.0 - ret.enableCamera = True # Stock camera detection doesn't apply to VW - ret.transmissionType = car.CarParams.TransmissionType.automatic + # Determine installed network location by checking for radar-camera + # sensor fusion messages on bus 1. + if 0x238 in fingerprint[1]: + ret.networkLocation = NWL.fwdCamera + else: + ret.networkLocation = NWL.gateway + + # Determine transmission type by CAN message(s) present on the bus + if 0xAD in fingerprint[0]: + # Getribe_11 message detected: traditional automatic or DSG gearbox + ret.transmissionType = TRANS.automatic + elif 0x187 in fingerprint[0]: + # EV_Gearshift message detected: e-Golf or similar direct-drive electric + ret.transmissionType = TRANS.direct + else: + # No trans message at all, must be a true stick-shift manual + ret.transmissionType = TRANS.manual + + cloudlog.warning("Detected network location: %s", ret.networkLocation) + cloudlog.warning("Detected transmission type: %s", ret.transmissionType) # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase @@ -70,8 +94,8 @@ class CarInterface(CarInterfaceBase): self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) - ret = self.CS.update(self.cp) - ret.canValid = self.cp.can_valid and self.cp_cam.can_valid + ret = self.CS.update(self.cp, self.cp_cam, self.cp_acc, self.CP.transmissionType) + ret.canValid = self.cp.can_valid # FIXME: Restore cp_cam valid check after proper LKAS camera detect ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False # TODO: add a field for this to carState, car interface code shouldn't write params diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index d99d78102..bf357f93f 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -1,5 +1,4 @@ -# flake8: noqa - +from cereal import car from selfdrive.car import dbc_dict class CarControllerParams: @@ -24,6 +23,10 @@ class CANBUS: pt = 0 cam = 2 +NWL = car.CarParams.NetworkLocation +TRANS = car.CarParams.TransmissionType +GEAR = car.CarState.GearShifter + BUTTON_STATES = { "accelCruise": False, "decelCruise": False, @@ -46,15 +49,28 @@ MQB_LDW_MESSAGES = { } class CAR: - GOLF = "VOLKSWAGEN GOLF" + GENERICMQB = "Generic Volkswagen MQB Platform Vehicle" +# Mega-fingerprint used to identify any and all MQB platform vehicles. Specific +# make and model characteristics are looked up from the VIN later. +# Note: 1471:8 observed as 1471:4 on a 2019 Jetta, and we can't carry both in one FP, effect TBD FINGERPRINTS = { - CAR.GOLF: [ - # 76b83eb0245de90e|2019-10-21--17-40-42 - jyoung8607 car - {64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 264: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 385: 8, 418: 8, 427: 8, 668: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1120: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8 - }], + CAR.GENERICMQB: [ + {178: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 695: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1122: 8, 1123: 8, + 1124: 8, 1646: 8, 1648: 8, 1153: 8, 134: 8, 1162: 8, 1175: 8, 159: 8, 795: 8, 679: 8, 681: 8, 173: 8, 1712: 6, + 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8, 1312: 8, 806: 8, 253: 8, 1792: 8, 257: 8, 260: 8, 262: 8, + 897: 8, 264: 8, 779: 8, 780: 8, 783: 8, 278: 8, 279: 8, 792: 8, 283: 8, 285: 8, 286: 8, 901: 8, 288: 8, 289: 8, + 290: 8, 804: 8, 294: 8, 807: 8, 808: 8, 809: 8, 299: 8, 302: 8, 1351: 8, 346: 8, 870: 8, 1385: 8, 896: 8, 64: 8, + 898: 8, 1413: 8, 917: 8, 919: 8, 927: 8, 1440: 5, 929: 8, 930: 8, 427: 8, 949: 8, 958: 8, 960: 4, 418: 8, 981: 8, + 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1514: 8, 1515: 8, 1520: 8, 1019: 8, 385: 8, 668: 8, 1120: 8, + 1438: 8, 1461: 8, 391: 8, 1511: 8, 1516: 8, 568: 8, 569: 8, 826: 8, 827: 8, 1156: 8, 1157: 8, 1158: 8, 1471: 8, + 1635: 8, 376: 8, 295: 8, 791: 8, 799: 8, 838: 8, 389: 8, 840: 8, 841: 8, 842: 8, 843: 8, 844: 8, 845: 8, + 314: 8, 787: 8, 788: 8, 789: 8, 802: 8, 839: 8, 1332: 8, 1872: 8, 1976: 8, 1977: 8, 1985: 8, 2015: 8, 592: 8, + 593: 8, 594: 8, 595: 8, 596: 8, 684: 8 + }, + ], } DBC = { - CAR.GOLF: dbc_dict('vw_mqb_2010', None), + CAR.GENERICMQB: dbc_dict('vw_mqb_2010', None), } diff --git a/selfdrive/car/volkswagen/volkswagencan.py b/selfdrive/car/volkswagen/volkswagencan.py index 3cd47a095..ef93aadec 100644 --- a/selfdrive/car/volkswagen/volkswagencan.py +++ b/selfdrive/car/volkswagen/volkswagencan.py @@ -15,22 +15,26 @@ def create_mqb_steering_control(packer, bus, apply_steer, idx, lkas_enabled): } return packer.make_can_msg("HCA_01", bus, values, idx) -def create_mqb_hud_control(packer, bus, hca_enabled, steering_pressed, hud_alert, leftLaneVisible, rightLaneVisible): - +def create_mqb_hud_control(packer, bus, hca_enabled, steering_pressed, hud_alert, left_lane_visible, right_lane_visible, + ldw_lane_warning_left, ldw_lane_warning_right, ldw_side_dlc_tlc, ldw_dlc, ldw_tlc): if hca_enabled: - leftlanehud = 3 if leftLaneVisible else 1 - rightlanehud = 3 if rightLaneVisible else 1 + left_lane_hud = 3 if left_lane_visible else 1 + right_lane_hud = 3 if right_lane_visible else 1 else: - leftlanehud = 2 if leftLaneVisible else 1 - rightlanehud = 2 if rightLaneVisible else 1 + left_lane_hud = 2 if left_lane_visible else 1 + right_lane_hud = 2 if right_lane_visible else 1 values = { - "LDW_Unknown": 2, # FIXME: possible speed or attention relationship - "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, - "Left_Lane_Status": leftlanehud, - "Right_Lane_Status": rightlanehud, - "Alert_Message": hud_alert, + "LDW_Status_LED_gelb": 1 if hca_enabled and steering_pressed else 0, + "LDW_Status_LED_gruen": 1 if hca_enabled and not steering_pressed else 0, + "LDW_SW_Info_links": left_lane_hud, + "LDW_SW_Info_rechts": right_lane_hud, + "LDW_Texte": hud_alert, + "LDW_SW_Warnung_links": ldw_lane_warning_left, + "LDW_SW_Warnung_rechts": ldw_lane_warning_right, + "LDW_Seite_DLCTLC": ldw_side_dlc_tlc, + "LDW_DLC": ldw_dlc, + "LDW_TLC": ldw_tlc } return packer.make_can_msg("LDW_02", bus, values) @@ -46,6 +50,8 @@ def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx): "GRA_Typ_Hauptschalter": CS.graTypHauptschalter, "GRA_Codierung": 2, "GRA_Tip_Stufe_2": CS.graTipStufe2, + "GRA_Typ468": CS.graTyp468, "GRA_ButtonTypeInfo": CS.graButtonTypeInfo } + return packer.make_can_msg("GRA_ACC_01", bus, values, idx)