diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index d816ba3974..970ba87825 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -21,7 +21,7 @@ class CarController(): self.steer_rate_limited = False - def update(self, enabled, CS, frame, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): + def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): """ Controls thread """ can_sends = [] @@ -106,7 +106,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, 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 ddc193766f..f74484bd20 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -4,7 +4,7 @@ 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_FILES, CANBUS, TransmissionType, GearShifter, BUTTON_STATES, CarControllerParams +from selfdrive.car.volkswagen.values import DBC_FILES, CANBUS, NetworkLocation, TransmissionType, GearShifter, BUTTON_STATES, CarControllerParams class CarState(CarStateBase): def __init__(self, CP): @@ -17,7 +17,7 @@ class CarState(CarStateBase): self.hca_status_values = can_define.dv["LH_EPS_03"]["EPS_HCA_Status"] self.buttonStates = BUTTON_STATES.copy() - def update(self, pt_cp, cam_cp, trans_type): + def update(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.fl = pt_cp.vl["ESP_19"]["ESP_VL_Radgeschw_02"] * CV.KPH_TO_MS @@ -79,8 +79,8 @@ class CarState(CarStateBase): # 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(pt_cp.vl["SWA_01"]["SWA_Infostufe_SWA_li"]) or bool(pt_cp.vl["SWA_01"]["SWA_Warnung_SWA_li"]) - ret.rightBlindspot = bool(pt_cp.vl["SWA_01"]["SWA_Infostufe_SWA_re"]) or bool(pt_cp.vl["SWA_01"]["SWA_Warnung_SWA_re"]) + ret.leftBlindspot = bool(ext_cp.vl["SWA_01"]["SWA_Infostufe_SWA_li"]) or bool(ext_cp.vl["SWA_01"]["SWA_Warnung_SWA_li"]) + ret.rightBlindspot = bool(ext_cp.vl["SWA_01"]["SWA_Infostufe_SWA_re"]) or bool(ext_cp.vl["SWA_01"]["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 @@ -95,8 +95,8 @@ class CarState(CarStateBase): # 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 - ret.stockFcw = bool(pt_cp.vl["ACC_10"]["AWV2_Freigabe"]) - ret.stockAeb = bool(pt_cp.vl["ACC_10"]["ANB_Teilbremsung_Freigabe"]) or bool(pt_cp.vl["ACC_10"]["ANB_Zielbremsung_Freigabe"]) + ret.stockFcw = bool(ext_cp.vl["ACC_10"]["AWV2_Freigabe"]) + ret.stockAeb = bool(ext_cp.vl["ACC_10"]["ANB_Teilbremsung_Freigabe"]) or bool(ext_cp.vl["ACC_10"]["ANB_Zielbremsung_Freigabe"]) # Update ACC radar status. accStatus = pt_cp.vl["TSK_06"]["TSK_Status"] @@ -115,7 +115,7 @@ class CarState(CarStateBase): # Update ACC setpoint. When the setpoint is zero or there's an error, the # radar sends a set-speed of ~90.69 m/s / 203mph. - ret.cruiseState.speed = pt_cp.vl["ACC_02"]["ACC_Wunschgeschw"] * CV.KPH_TO_MS + ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw"] * CV.KPH_TO_MS if ret.cruiseState.speed > 90: ret.cruiseState.speed = 0 @@ -222,13 +222,13 @@ class CarState(CarStateBase): ("BCM1_Rueckfahrlicht_Schalter", "Gateway_72", 0)] # Reverse light from BCM checks += [("Motor_14", 10)] # From J623 Engine control module - # TODO: Detect ACC radar bus location - signals += MqbExtraSignals.fwd_radar_signals - checks += MqbExtraSignals.fwd_radar_checks - # TODO: Detect BSM radar bus location - if CP.enableBsm: - signals += MqbExtraSignals.bsm_radar_signals - checks += MqbExtraSignals.bsm_radar_checks + if CP.networkLocation == NetworkLocation.fwdCamera: + # Radars are here on CANBUS.pt + signals += MqbExtraSignals.fwd_radar_signals + checks += MqbExtraSignals.fwd_radar_checks + if CP.enableBsm: + signals += MqbExtraSignals.bsm_radar_signals + checks += MqbExtraSignals.bsm_radar_checks return CANParser(DBC_FILES.mqb, signals, checks, CANBUS.pt) @@ -249,6 +249,14 @@ class CarState(CarStateBase): ("LDW_02", 10) # From R242 Driver assistance camera ] + if CP.networkLocation == NetworkLocation.gateway: + # Radars are here on CANBUS.cam + signals += MqbExtraSignals.fwd_radar_signals + checks += MqbExtraSignals.fwd_radar_checks + if CP.enableBsm: + signals += MqbExtraSignals.bsm_radar_signals + checks += MqbExtraSignals.bsm_radar_checks + return CANParser(DBC_FILES.mqb, signals, checks, CANBUS.cam) class MqbExtraSignals: diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 6282ea139e..77d3be4b9f 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,5 +1,5 @@ from cereal import car -from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES, TransmissionType, GearShifter +from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES, CANBUS, NetworkLocation, TransmissionType, GearShifter from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase @@ -13,6 +13,13 @@ class CarInterface(CarInterfaceBase): self.displayMetricUnitsPrev = None self.buttonStatesPrev = BUTTON_STATES.copy() + if CP.networkLocation == NetworkLocation.fwdCamera: + self.ext_bus = CANBUS.pt + self.cp_ext = self.cp + else: + self.ext_bus = CANBUS.cam + self.cp_ext = self.cp_cam + @staticmethod def compute_gb(accel, speed): return float(accel) / 4.0 @@ -36,6 +43,11 @@ class CarInterface(CarInterfaceBase): else: # No trans message at all, must be a true stick-shift manual ret.transmissionType = TransmissionType.manual + if 0x86 in fingerprint[1]: # LWI_01 seen on bus 1, we're wired to the CAN gateway + ret.networkLocation = NetworkLocation.gateway + else: # We're wired to the LKAS camera + ret.networkLocation = NetworkLocation.fwdCamera + # Global tuning defaults, can be overridden per-vehicle ret.steerActuatorDelay = 0.05 @@ -129,7 +141,7 @@ class CarInterface(CarInterfaceBase): self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) - ret = self.CS.update(self.cp, self.cp_cam, self.CP.transmissionType) + ret = self.CS.update(self.cp, self.cp_cam, self.cp_ext, self.CP.transmissionType) ret.canValid = self.cp.can_valid and self.cp_cam.can_valid ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False @@ -165,7 +177,7 @@ class CarInterface(CarInterfaceBase): return self.CS.out def apply(self, c): - can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, + can_sends = self.CC.update(c.enabled, self.CS, self.frame, self.ext_bus, c.actuators, c.hudControl.visualAlert, c.hudControl.leftLaneVisible, c.hudControl.rightLaneVisible, diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index a98374a3a4..8d1a88e930 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -7,6 +7,7 @@ from cereal import car from selfdrive.car import dbc_dict Ecu = car.CarParams.Ecu +NetworkLocation = car.CarParams.NetworkLocation TransmissionType = car.CarParams.TransmissionType GearShifter = car.CarState.GearShifter