""" Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos """ import sys import os import math import time from iqdbc.can import CANParser from iqdbc.car import DT_CTRL, Bus, structs from iqdbc.car.interfaces import CarStateBase from iqdbc.car.common.conversions import Conversions as CV from iqdbc.car.common.filter_simple import FirstOrderFilter from iqdbc.car.volkswagen.values import CAR, DBC, CanBus, NetworkLocation, RADAR_DISABLE_STATE, TransmissionType, GearShifter, \ CarControllerParams, VolkswagenFlags, VolkswagenFlagsIQ from iqdbc.car.volkswagen.speed_limit_manager import SpeedLimitManager iqpilot_path = os.path.join(os.path.dirname(__file__), '..', '..', '..') sys.path.insert(0, iqpilot_path) try: from iqpilot.common.params import Params except ImportError: pass ButtonType = structs.CarState.ButtonEvent.Type class CarState(CarStateBase): CRUISE_FAULT_LATERAL_ENABLE_FRAMES = 5 CRUISE_FAULT_LATERAL_DISABLE_FRAMES = 20 MEB_TEMP_CRUISE_FAULT = 6 MEB_TOLERANCE_MAX = 100 DRIVER_TORQUE_TAU = 0.10 def __init__(self, CP, CP_IQ): from iqpilot.system.proprietary_runtime._verified_import import import_verified_module self._iq_lvbs_alc = import_verified_module("iqpilot_alc_private", "iqpilot_private.konn3kt.iqlvbs.alc") super().__init__(CP, CP_IQ) self._params = Params() self.frame = 0 self.eps_init_complete = False self.CCP = CarControllerParams(CP) self.driver_torque_filter = FirstOrderFilter(0.0, self.DRIVER_TORQUE_TAU, DT_CTRL) self.button_states = {button.event_type: False for button in self.CCP.BUTTONS} self.esp_hold_confirmation = False self.upscale_lead_car_signal = False self.eps_stock_values = False self.curvature = 0. self.klr_stock_values = {} self.ea_hud_stock_values = {} self.ea_control_stock_values = {} self.acc_type = 0 self.acc_radar_sollbeschl = 0.0 self.acc_radar_regelabw = 0.0 self.acc_radar_aendgrad = 0.0 self.acc_radar_sta_adr = 0 self.acc_radar_fehler = False self.acc_radar_v_wunsch = 0.0 self.acc_radar_sta_acc = 0 self.epb_freigabe_ver = False self.acc_stock_counters: dict[str, int] = {} self.esp_stopping = False self.tsk_brake_torque = 0.0 self.last_cruiseActive = False self.cruise_faulted_frames = 0 self.cruise_fault_clear_frames = 0 self.cruise_fault_lateral_active = False self.cruise_faulted = False self.grade = 0.0 self.rolling_backward = False self.rolling_forward = False self.sum_wegimpulse = 0 self.speed_limit_mgr = SpeedLimitManager(CP) self.enable_predicative_speed_limit = False self.enable_speed_limit_predicative = False self.enable_pred_react_to_speed_limits = False self.enable_pred_react_to_curves = False self.speed_limit_predicative_type = 0 self.force_rhd_for_bsm = False self.grade = 0.0 self.rolling_backward = False self.rolling_forward = False self.sum_wegimpulse = 0 self.travel_assist_available = False self.left_blinker_active = False self.right_blinker_active = False self._param_update_time = 0.0 self.cruise_recovery_timer = 0 self.tolerance_counter = self.MEB_TOLERANCE_MAX self.lkas_button = 0 self.prev_lkas_button = 0 self.vw_iq_lvbs_alc_available = False self.vw_iq_lvbs_alc_entering = False self.vw_iq_lvbs_alc_active = False self.vw_iq_lvbs_alc_key_slot = 0 self.EPS_ALC_Status = 0 self.EPS_ALC_Abbruch = 0 self.PQ_ALC_Status_raw = 0 self.alcOverrideAlert = False self.bremse8_stock = None self.br8_acc_anf = False self.tsk_verzoeg_anf = False self.cruise_main_switch = False self.motor3_stock = {} self.motor1_stock = {} self.motor3_frame = 0 self.motor1_frame = 0 self._odometer_store = self._iq_lvbs_alc.create_vehicle_odometer_store(CP, self._params) def _update_odometer(self, ret: structs.CarState, raw_km: float) -> None: """Publish the cluster value while proprietary Konn3kt code owns persistence.""" odometer_km = self._odometer_store.record(raw_km) if odometer_km is not None: ret.odometer = odometer_km def _apply_iq_private_flags(self, ret_iq: structs.IQCarState) -> None: ret_iq.alcOverrideAlert = bool(self.alcOverrideAlert) def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]): if not self.CP.pcmCruise: if self.cruise_fault_lateral_active and self.cruise_faulted: return False for b in buttonEvents: # Enable OP long on falling edge of enable buttons if b.type in (ButtonType.setCruise, ButtonType.resumeCruise) and not b.pressed: return True return False def create_button_events(self, pt_cp, buttons): button_events = [] for button in buttons: state = pt_cp.vl[button.can_addr][button.can_msg] in button.values if self.button_states[button.event_type] != state: event = structs.CarState.ButtonEvent() event.type = button.event_type event.pressed = state button_events.append(event) self.button_states[button.event_type] = state return button_events def _update_mqb_iq_alc_state(self, pt_cp): self._iq_lvbs_alc.update_mqb_carstate_alc_state(self, pt_cp) def _update_mlb_iq_alc_state(self, pt_cp): self._iq_lvbs_alc.update_mlb_carstate_alc_state(self, pt_cp) def _update_pq_iq_alc_state(self, pt_cp): self._iq_lvbs_alc.update_pq_carstate_alc_state(self, pt_cp) def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]: pt_cp = can_parsers[Bus.pt] cam_cp = can_parsers[Bus.cam] ext_cp = pt_cp if self.CP.networkLocation == NetworkLocation.fwdCamera else cam_cp if self.CP.flags & VolkswagenFlags.PQ: aux_cp = can_parsers.get(Bus.aux) return self.update_pq(pt_cp, cam_cp, ext_cp, aux_cp) elif self.CP.flags & VolkswagenFlags.MLB: br_cp = can_parsers[Bus.aux] return self.update_mlb(pt_cp, br_cp, cam_cp, ext_cp) elif self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO): main_cp = can_parsers[Bus.main] return self.update_meb(pt_cp, main_cp, cam_cp, ext_cp) ret = structs.CarState() ret_iq = structs.IQCarState() if self.CP.transmissionType == TransmissionType.direct: ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Motor_EV_01"]["MO_Waehlpos"], None)) elif self.CP.transmissionType == TransmissionType.manual: if bool(pt_cp.vl["Gateway_72"]["BCM1_Rueckfahrlicht_Schalter"]): ret.gearShifter = GearShifter.reverse else: ret.gearShifter = GearShifter.drive else: ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Gateway_73"]["GE_Fahrstufe"], None)) if True: # MQB-specific if self.CP.flags & VolkswagenFlags.KOMBI_PRESENT: self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) # Analog v digital instrument cluster self.parse_wheel_speeds(ret, pt_cp.vl["ESP_19"]["ESP_VL_Radgeschw_02"], pt_cp.vl["ESP_19"]["ESP_VR_Radgeschw_02"], pt_cp.vl["ESP_19"]["ESP_HL_Radgeschw_02"], pt_cp.vl["ESP_19"]["ESP_HR_Radgeschw_02"], ) self.rolling_backward = ( pt_cp.vl["ESP_10"]["ESP_HR_Fahrtrichtung"] == 1 or pt_cp.vl["ESP_10"]["ESP_HL_Fahrtrichtung"] == 1 or pt_cp.vl["ESP_10"]["ESP_VR_Fahrtrichtung"] == 1 or pt_cp.vl["ESP_10"]["ESP_VL_Fahrtrichtung"] == 1 ) self.rolling_forward = ( pt_cp.vl["ESP_10"]["ESP_HR_Fahrtrichtung"] == 0 or pt_cp.vl["ESP_10"]["ESP_HL_Fahrtrichtung"] == 0 or pt_cp.vl["ESP_10"]["ESP_VR_Fahrtrichtung"] == 0 or pt_cp.vl["ESP_10"]["ESP_VL_Fahrtrichtung"] == 0 ) self.sum_wegimpulse = int( pt_cp.vl["ESP_10"]["ESP_Wegimpuls_VL"] + pt_cp.vl["ESP_10"]["ESP_Wegimpuls_VR"] + pt_cp.vl["ESP_10"]["ESP_Wegimpuls_HL"] + pt_cp.vl["ESP_10"]["ESP_Wegimpuls_HR"] ) if self.CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT: ret.carFaultedNonCritical = bool(cam_cp.vl["HCA_01"]["EA_Ruckfreigabe"]) or cam_cp.vl["HCA_01"]["EA_ACC_Sollstatus"] > 0 # EA ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects brake_pedal_pressed = bool(pt_cp.vl["Motor_14"]["MO_Fahrer_bremst"]) brake_pressure_detected = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"]) ret.brakePressed = brake_pedal_pressed or brake_pressure_detected ret.parkingBrake = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"]) # FIXME: need to include an EPB check as well ret.doorOpen = any([pt_cp.vl["Gateway_72"]["ZV_FT_offen"], pt_cp.vl["Gateway_72"]["ZV_BT_offen"], pt_cp.vl["Gateway_72"]["ZV_HFS_offen"], pt_cp.vl["Gateway_72"]["ZV_HBFS_offen"], pt_cp.vl["Gateway_72"]["ZV_HD_offen"]]) if self.CP.enableBsm: # Infostufe: BSM LED on, Warnung: BSM LED flashing 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"]) if not (self.CP.flags & VolkswagenFlagsIQ.IQ_CC_ONLY_NO_RADAR): 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"]) else: ret.stockFcw = False ret.stockAeb = False cc_only = self.CP.flags & VolkswagenFlagsIQ.IQ_CC_ONLY or self.CP.flags & VolkswagenFlagsIQ.IQ_CC_ONLY_NO_RADAR self.acc_type = 0 if cc_only else ext_cp.vl["ACC_06"]["ACC_Typ"] if not cc_only: self.acc_stock_counters["ACC_02"] = int(ext_cp.vl["ACC_02"]["COUNTER"]) self.acc_stock_counters["ACC_06"] = int(ext_cp.vl["ACC_06"]["COUNTER"]) self.acc_stock_counters["ACC_07"] = int(ext_cp.vl["ACC_07"]["COUNTER"]) self.acc_stock_counters["ACC_10"] = int(ext_cp.vl["ACC_10"]["COUNTER"]) self.esp_stopping = bool(pt_cp.vl["ESP_21"]["ESP_Anhaltevorgang_ACC_aktiv"]) self.esp_hold_confirmation = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]) self.tsk_brake_torque = pt_cp.vl["TSK_06"]["TSK_Radbremsmom"] if pt_cp.vl["TSK_06"]["TSK_Freig_Verzoeg_Anf"] else 0.0 self.grade = pt_cp.vl["Motor_16"]["TSK_Steigung"] acc_limiter_mode = False if cc_only else ext_cp.vl["ACC_02"]["ACC_Gesetzte_Zeitluecke"] == 0 speed_limiter_mode = bool(pt_cp.vl["TSK_06"]["TSK_Limiter_ausgewaehlt"]) self.tsk_verzoeg_anf = bool(pt_cp.vl["TSK_06"]["TSK_Freig_Verzoeg_Anf"]) self._update_mqb_iq_alc_state(pt_cp) ret.cruiseState.available = pt_cp.vl["TSK_06"]["TSK_Status"] in (2, 3, 4, 5) ret.cruiseState.enabled = pt_cp.vl["TSK_06"]["TSK_Status"] in (3, 4, 5) ret.cruiseState.speed = 0 if cc_only else ext_cp.vl["ACC_02"]["ACC_Wunschgeschw_02"] * CV.KPH_TO_MS if self.CP.pcmCruise else 0 ret.accFaulted = pt_cp.vl["TSK_06"]["TSK_Status"] in (6, 7) ret.leftBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Left"]) ret.rightBlinker = bool(pt_cp.vl["Blinkmodi_02"]["Comfort_Signal_Right"]) # Shared logic ret.vEgoCluster = pt_cp.vl["Kombi_01"]["KBI_angez_Geschw"] * CV.KPH_TO_MS if ret.cruiseState.speed > 0 and ret.vEgo > 1.0: cluster_ratio = ret.vEgoCluster / ret.vEgo ret.cruiseState.speedCluster = ret.cruiseState.speed * cluster_ratio self.parse_mlb_mqb_steering_state(ret, pt_cp) ret.gasPressed = pt_cp.vl["Motor_20"]["MO_Fahrpedalrohwert_01"] > 0 ret.espActive = bool(pt_cp.vl["ESP_21"]["ESP_Eingriff"]) ret.espDisabled = pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"] != 0 ret.seatbeltUnlatched = pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] != 3 ret.standstill = ret.vEgoRaw == 0 ret.cruiseState.standstill = self.CP.pcmCruise and self.esp_hold_confirmation ret.cruiseState.nonAdaptive = acc_limiter_mode or speed_limiter_mode if ret.cruiseState.speed > 90: ret.cruiseState.speed = 0 self.eps_stock_values = pt_cp.vl["LH_EPS_03"] self.ldw_stock_values = cam_cp.vl["LDW_02"] if self.CP.networkLocation == NetworkLocation.fwdCamera else {} self.gra_stock_values = pt_cp.vl["GRA_ACC_01"] ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS) ret.lowSpeedAlert = self.update_low_speed_alert(ret.vEgo) allow_lat_only = self._params.get_bool("AllowLateralWhenLongUnavailable") and self._params.get_bool("AolEnabled") cruise_main_switch = bool(self.gra_stock_values["GRA_Hauptschalter"]) ret.cruiseFaultLateralMode = allow_lat_only and ret.accFaulted and cruise_main_switch ret.lateralAvailable = ret.cruiseState.available or ret.cruiseFaultLateralMode ret.blockPcmEnable = ret.cruiseFaultLateralMode ret.fuelGauge = pt_cp.vl["Kombi_02"]["KBI_Inhalt_Tank"] / 55.0 ret.fuelTankLevelL = pt_cp.vl["Kombi_02"]["KBI_Inhalt_Tank"] # raw liters for konn3kt self._update_odometer(ret, pt_cp.vl["Kombi_02"]["KBI_Kilometerstand"]) self.cruise_faulted = ret.accFaulted self._apply_iq_private_flags(ret_iq) self.frame += 1 return ret, ret_iq def update_meb(self, pt_cp, main_cp, cam_cp, ext_cp) -> tuple[structs.CarState, structs.IQCarState]: ret = structs.CarState() ret_iq = structs.IQCarState() if time.monotonic() - self._param_update_time > 2.0: self.enable_speed_limit_predicative = self._params.get_bool("EnableSpeedLimitPredicative") self.enable_pred_react_to_speed_limits = self._params.get_bool("EnableSLPredReactToSL") self.enable_pred_react_to_curves = self._params.get_bool("EnableSLPredReactToCurves") self._param_update_time = time.monotonic() self.parse_wheel_speeds(ret, pt_cp.vl["ESC_51"]["VL_Radgeschw"], pt_cp.vl["ESC_51"]["VR_Radgeschw"], pt_cp.vl["ESC_51"]["HL_Radgeschw"], pt_cp.vl["ESC_51"]["HR_Radgeschw"], ) if self.CP.flags & VolkswagenFlags.KOMBI_PRESENT: ret.vEgoCluster = pt_cp.vl["Kombi_01"]["KBI_angez_Geschw"] * CV.KPH_TO_MS ret.standstill = ret.vEgoRaw == 0 ret.steeringAngleDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradwinkel"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradwinkel"])] ret.steeringRateDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradw_Geschw"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradw_Geschw"])] ret.steeringTorque = pt_cp.vl["LH_EPS_03"]["EPS_Lenkmoment"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_Lenkmoment"])] driver_override_threshold = self._iq_lvbs_alc.vw_driver_override_threshold_cnm(self, "mqb", self.CCP.STEER_DRIVER_ALLOWANCE) ret.steeringPressed = abs(self.driver_torque_filter.update(ret.steeringTorque)) > driver_override_threshold self.curvature = -pt_cp.vl["QFK_01"]["Curvature"] * (1, -1)[int(pt_cp.vl["QFK_01"]["Curvature_VZ"])] ret.steeringCurvature = self.curvature ret.yawRate = -pt_cp.vl["ESC_50"]["Yaw_Rate"] * (1, -1)[int(pt_cp.vl["ESC_50"]["Yaw_Rate_Sign"])] * CV.DEG_TO_RAD if self.CP.flags & VolkswagenFlags.ALT_GEAR: gear_raw = pt_cp.vl["Gateway_73"]["GE_Fahrstufe"] else: gear_raw = pt_cp.vl["Getriebe_11"]["GE_Fahrstufe"] ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(gear_raw, None)) if (self.CP.flags & VolkswagenFlags.MQB_EVO) and ret.gearShifter == GearShifter.unknown: reversing = bool(pt_cp.vl["Gateway_72"]["BCM1_Rueckfahrlicht_Schalter"]) ret.gearShifter = GearShifter.reverse if reversing else GearShifter.drive drive_mode = ret.gearShifter == GearShifter.drive hca_status = self.CCP.hca_status_values.get(pt_cp.vl["QFK_01"]["LatCon_HCA_Status"]) ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status, drive_mode=drive_mode) self.eps_stock_values = pt_cp.vl["LH_EPS_03"] self.klr_stock_values = pt_cp.vl["KLR_01"] if self.CP.flags & VolkswagenFlags.STOCK_KLR_PRESENT else {} self.ea_hud_stock_values = cam_cp.vl["EA_02"] self.ea_control_stock_values = cam_cp.vl["EA_01"] ret.carFaultedNonCritical = cam_cp.vl["EA_01"]["EA_Funktionsstatus"] in (3, 4, 5, 6) ret.gasPressed = pt_cp.vl["Motor_51"]["Accel_Pedal_Pressure"] > 0 # Motor_54 has unreliable offset on MQBevo ret.brakePressed = bool(pt_cp.vl["Motor_14"]["MO_Fahrer_bremst"]) ret.brake = pt_cp.vl["ESC_51"]["Brake_Pressure"] ret.parkingBrake = pt_cp.vl["ESC_50"]["EPB_Status"] in (1, 4) doors = pt_cp.vl["ZV_02"] if bool(pt_cp.vl["Gateway_72"]["ZV_02_alt"]) else pt_cp.vl["Gateway_72"] ret.doorOpen = any([doors["ZV_FT_offen"], doors["ZV_BT_offen"], doors["ZV_HFS_offen"], doors["ZV_HBFS_offen"], doors["ZV_HD_offen"]]) ret.seatbeltUnlatched = pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] != 3 if self.CP.enableBsm: bsm_bus = pt_cp if self.CP.flags & (VolkswagenFlags.MEB_GEN2 | VolkswagenFlags.MQB_EVO) else ext_cp blindspot_driver = bool(bsm_bus.vl["MEB_Side_Assist_01"]["Blind_Spot_Info_Driver"]) or bool(bsm_bus.vl["MEB_Side_Assist_01"]["Blind_Spot_Warn_Driver"]) blindspot_passenger = bool(bsm_bus.vl["MEB_Side_Assist_01"]["Blind_Spot_Info_Passenger"]) or bool(bsm_bus.vl["MEB_Side_Assist_01"]["Blind_Spot_Warn_Passenger"]) force_rhd = self.force_rhd_for_bsm or self._params.get_bool("ForceRHDForBSM") car_is_lhd = not force_rhd ret.leftBlindspot = blindspot_driver if car_is_lhd else blindspot_passenger ret.rightBlindspot = blindspot_passenger if car_is_lhd else blindspot_driver self.ldw_stock_values = cam_cp.vl["LDW_02"] awv_values = ext_cp.vl.get("AWV_03", ext_cp.vl.get("ACC_10", {})) ret.stockFcw = bool(awv_values.get("FCW_Active", 0)) or bool(awv_values.get("AWV2_Freigabe", 0)) ret.stockAeb = False self.acc_type = ext_cp.vl["ACC_18"]["ACC_Typ"] self.travel_assist_available = bool(pt_cp.vl.get("TA_01", {}).get("Travel_Assist_Available", 0)) ret.cruiseState.available = pt_cp.vl["Motor_51"]["TSK_Status"] in (2, 3, 4, 5) ret.cruiseState.enabled = pt_cp.vl["Motor_51"]["TSK_Status"] in (3, 4, 5) # TSK winds its braking down through brake_only after a driver brake. Requesting drive-off in this # state can fault TSK, and stock refuses to engage here as well, so block entry until it clears. ret.carNotReady = pt_cp.vl["Motor_51"]["TSK_Status"] == 5 # brake_only acc_values = ext_cp.vl.get("MEB_ACC_01", ext_cp.vl.get("ACC_19", {})) ret.cruiseState.nonAdaptive = bool(acc_values.get("ACC_Limiter_Mode", 0)) if self.CP.pcmCruise else bool(pt_cp.vl["Motor_51"]["TSK_Limiter_ausgewaehlt"]) acc_faulted = pt_cp.vl["Motor_51"]["TSK_Status"] in (6, 7) ret.accFaulted = self.update_acc_fault(acc_faulted, parking_brake=ret.parkingBrake, drive_mode=drive_mode) if self.CP.flags & VolkswagenFlags.MQB_EVO: self.esp_hold_confirmation = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"]) else: self.esp_hold_confirmation = pt_cp.vl["ESC_50"]["Motion_State"] == 3 ret.cruiseState.standstill = self.CP.pcmCruise and self.esp_hold_confirmation if self.CP.pcmCruise: ret.cruiseState.speed = float(int(round(acc_values.get("ACC_Wunschgeschw_02", 0)))) * CV.KPH_TO_MS if ret.cruiseState.speed > 90: ret.cruiseState.speed = 0 if ret.cruiseState.speed > 0 and ret.vEgo > 1.0 and ret.vEgoCluster > 0: cluster_ratio = ret.vEgoCluster / ret.vEgo ret.cruiseState.speedCluster = ret.cruiseState.speed * cluster_ratio raining = pt_cp.vl["RLS_01"]["RS_Regenmenge"] > 0 vze_01_values = cam_cp.vl.get("MEB_VZE_01", cam_cp.vl.get("VZE_04", {})) psd_04_values = main_cp.vl["PSD_04"] if self.CP.flags & VolkswagenFlags.STOCK_PSD_PRESENT else {} psd_05_values = main_cp.vl["PSD_05"] if self.CP.flags & VolkswagenFlags.STOCK_PSD_PRESENT else {} psd_06_values = main_cp.vl["PSD_06"] if self.CP.flags & VolkswagenFlags.STOCK_PSD_PRESENT else {} psd_06_values = pt_cp.vl["PSD_06"] if not psd_06_values and self.CP.flags & VolkswagenFlags.STOCK_PSD_06_PRESENT else psd_06_values diagnose_01_values = pt_cp.vl["Diagnose_01"] if self.CP.flags & VolkswagenFlags.STOCK_DIAGNOSE_01_PRESENT else {} self._update_odometer(ret, pt_cp.vl["Diagnose_01"]["KBI_Kilometerstand"]) if self.enable_speed_limit_predicative and not self.enable_predicative_speed_limit: self.enable_predicative_speed_limit = True self.speed_limit_mgr.enable_predicative_speed_limit(self.enable_predicative_speed_limit, self.enable_pred_react_to_speed_limits, self.enable_pred_react_to_curves) self.speed_limit_mgr.update(ret.vEgo, psd_04_values, psd_05_values, psd_06_values, vze_01_values, raining, diagnose_01_values) ret.cruiseState.speedLimit = self.speed_limit_mgr.get_speed_limit() ret.cruiseState.speedLimitPredicative = self.speed_limit_mgr.get_speed_limit_predicative() self.speed_limit_predicative_type = self.speed_limit_mgr.get_speed_limit_predicative_type() ret_iq.speedLimit = ret.cruiseState.speedLimit self.left_blinker_active = bool(pt_cp.vl["Blinkmodi_02"]["BM_links"]) self.right_blinker_active = bool(pt_cp.vl["Blinkmodi_02"]["BM_rechts"]) stalk_values = pt_cp.vl.get("SMLS_01", {}) ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(240, stalk_values.get("BH_Blinker_li", 0), stalk_values.get("BH_Blinker_re", 0)) main_cruise_latching = not bool(pt_cp.vl["GRA_ACC_01"]["GRA_Typ_Hauptschalter"]) buttons = getattr(self.CCP, "BUTTONS_ALT", self.CCP.BUTTONS) if main_cruise_latching else self.CCP.BUTTONS ret.buttonEvents = self.create_button_events(pt_cp, buttons) self.gra_stock_values = pt_cp.vl["GRA_ACC_01"] ret.espDisabled = bool(pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"]) ret.espActive = bool(pt_cp.vl["ESP_21"]["ESP_Eingriff"]) allow_lat_only = self._params.get_bool("AllowLateralWhenLongUnavailable") and self._params.get_bool("AolEnabled") cruise_fault_candidate = allow_lat_only and ret.accFaulted and bool(pt_cp.vl["GRA_ACC_01"]["GRA_Hauptschalter"]) if cruise_fault_candidate: self.cruise_faulted_frames += 1 self.cruise_fault_clear_frames = 0 if self.cruise_faulted_frames >= self.CRUISE_FAULT_LATERAL_ENABLE_FRAMES: self.cruise_fault_lateral_active = True else: self.cruise_faulted_frames = 0 if self.cruise_fault_lateral_active: self.cruise_fault_clear_frames += 1 if self.cruise_fault_clear_frames >= self.CRUISE_FAULT_LATERAL_DISABLE_FRAMES: self.cruise_fault_lateral_active = False else: self.cruise_fault_clear_frames = 0 if not allow_lat_only: self.cruise_fault_lateral_active = False self.cruise_faulted_frames = 0 self.cruise_fault_clear_frames = 0 ret.cruiseFaultLateralMode = self.cruise_fault_lateral_active ret.lateralAvailable = ret.cruiseState.available or ret.cruiseFaultLateralMode ret.blockPcmEnable = ret.cruiseFaultLateralMode if self.CP.flags & VolkswagenFlags.MEB: ret.batteryDetails.charge = pt_cp.vl["Motor_16"]["MO_Energieinhalt_BMS"] if self.CP.networkLocation == NetworkLocation.gateway: ret.batteryDetails.heaterActive = bool(main_cp.vl["MEB_HVEM_03"]["PTC_ON"]) ret.batteryDetails.voltage = main_cp.vl["MEB_HVEM_01"]["Battery_Voltage"] ret.batteryDetails.capacity = main_cp.vl["BMS_04"]["BMS_Kapazitaet_02"] * ret.batteryDetails.voltage ret.batteryDetails.soc = ret.batteryDetails.charge / ret.batteryDetails.capacity * 100.0 if ret.batteryDetails.capacity > 0 else 0.0 ret.batteryDetails.power = main_cp.vl["MEB_HVEM_01"]["Engine_Power"] ret.batteryDetails.temperature = main_cp.vl["DCDC_03"]["DC_Temperatur"] ret.batteryDetails.chargingMode = int(main_cp.vl["BMS_04"]["BMS_IstModus"]) ret.fuelGauge = ret.batteryDetails.soc / 100.0 self.update_meb_virtual_lkas(ret, pt_cp, hca_status) ret.lowSpeedAlert = self.update_low_speed_alert(ret.vEgo) # Propagate radar disable failure state so carcontroller can skip ACC sends if self.CP.flags & VolkswagenFlags.DISABLE_RADAR: ret.radarDisableFailed = RADAR_DISABLE_STATE["error"] self.cruise_faulted = ret.accFaulted self.frame += 1 self._apply_iq_private_flags(ret_iq) return ret, ret_iq def update_pq(self, pt_cp, cam_cp, ext_cp, aux_cp=None) -> tuple[structs.CarState, structs.IQCarState]: ret = structs.CarState() ret_iq = structs.IQCarState() sng_ecd_enabled = bool(self.CP.flags & VolkswagenFlagsIQ.IQ_PQ_SNG_ECD) # 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"]["BR1_Rad_kmh"] * CV.KPH_TO_MS ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.standstill = ret.vEgoRaw == 0 # Update EPS position and state info. For signed values, VW sends the sign in a separate signal. 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"]["LW1_Lenk_Gesch"] * (1, -1)[int(pt_cp.vl["Lenkwinkel_1"]["LW1_Gesch_Sign"])] ret.steeringTorque = pt_cp.vl["Lenkhilfe_3"]["LH3_LM"] * (1, -1)[int(pt_cp.vl["Lenkhilfe_3"]["LH3_LMSign"])] hca_status = self.CCP.hca_status_values.get(pt_cp.vl["Lenkhilfe_2"]["LH2_Sta_HCA"]) ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status, ready_confirms_init=False) # Update gas, brakes, and gearshift. ret.gasPressed = pt_cp.vl["Motor_3"]["MO3_Pedalwert"] > 0 ret.brake = pt_cp.vl["Bremse_5"]["BR5_Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects ret.brakePressed = bool(pt_cp.vl["Motor_2"]["MO2_BLS"]) ret.parkingBrake = bool(pt_cp.vl["Kombi_1"]["Bremsinfo"]) # Update gear and/or clutch position data. if self.CP.transmissionType == TransmissionType.automatic: ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Getriebe_1"]["GE1_Wahl_Pos"], None)) elif self.CP.transmissionType == TransmissionType.manual: 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"]) self.LH_3_Sign = pt_cp.vl["Lenkhilfe_3"]["LH3_BLWSign"] self.LH2_steeringState = pt_cp.vl["Lenkhilfe_2"]["LH2_aktLenkeingriff"] self._update_pq_iq_alc_state(pt_cp) driver_override_threshold = self._iq_lvbs_alc.vw_driver_override_threshold_cnm(self, "pq", self.CCP.STEER_DRIVER_ALLOWANCE) ret.steeringPressed = abs(ret.steeringTorque) > driver_override_threshold # Consume blind-spot monitoring info/warning LED states, if available. # Infostufe: BSM LED on, Warnung: BSM LED flashing if self.CP.enableBsm: blindspot_li = bool(ext_cp.vl["SWA_1"]["SWA_Infostufe_SWA_li"]) or bool(ext_cp.vl["SWA_1"]["SWA_Warnung_SWA_li"]) blindspot_re = bool(ext_cp.vl["SWA_1"]["SWA_Infostufe_SWA_re"]) or bool(ext_cp.vl["SWA_1"]["SWA_Warnung_SWA_re"]) force_rhd = self.force_rhd_for_bsm or self._params.get_bool("ForceRHDForBSM") ret.leftBlindspot = blindspot_re if force_rhd else blindspot_li ret.rightBlindspot = blindspot_li if force_rhd else blindspot_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 {} cc_only = self.CP.flags & VolkswagenFlagsIQ.IQ_CC_ONLY or self.CP.flags & VolkswagenFlagsIQ.IQ_CC_ONLY_NO_RADAR if not (self.CP.flags & VolkswagenFlagsIQ.IQ_CC_ONLY_NO_RADAR): ret.stockFcw = bool(ext_cp.vl["AWV"]["AWV_2_Freigabe"]) ret.stockAeb = bool(ext_cp.vl["AWV"]["ANB_Teilbremsung_Freigabe"]) or bool(ext_cp.vl["AWV"]["ANB_Zielbremsung_Freigabe"]) else: ret.stockFcw = False ret.stockAeb = False ret.espActive = bool(pt_cp.vl["Bremse_1"]["BR1_Lampe_ASR"]) # Update ACC radar status. self.acc_type = 0 if cc_only else ext_cp.vl["ACC_System"]["ACS_Typ_ACC"] cruise_main_switch = bool(pt_cp.vl["Motor_5"]["MO5_GRA_Hauptsch"]) self.cruise_main_switch = cruise_main_switch MO2_StaGRA = pt_cp.vl["Motor_2"]["MO2_Sta_GRA"] in (1, 2) ACS_StaADR = False if cc_only else ext_cp.vl["ACC_System"]["ACS_Sta_ADR"] == 1 cruiseActive = MO2_StaGRA or ACS_StaADR self.epb_freigabe_ver = bool(aux_cp.vl["EPB_1"]["EP1_Freigabe_Ver"]) if sng_ecd_enabled and not cc_only else False sng_holding = sng_ecd_enabled and self.epb_freigabe_ver if cruiseActive or sng_holding: self.last_cruiseActive = True elif not MO2_StaGRA and not ACS_StaADR and not sng_holding: self.last_cruiseActive = False ret.cruiseState.enabled = self.last_cruiseActive self.br8_acc_anf = bool(pt_cp.vl["Bremse_8"]["BR8_Sta_ACC_Anf"]) if not cc_only else False if self.CP.pcmCruise: if cc_only: cruise_faulted = pt_cp.vl["Motor_2"]["MO2_Sta_GRA"] == 3 else: cruise_faulted = ext_cp.vl["ACC_GRA_Anzeige"]["ACA_StaACC"] in (6, 7) or ext_cp.vl["ACC_System"]["ACS_Sta_ADR"] == 3 or pt_cp.vl["Motor_2"]["MO2_Sta_GRA"] == 3 else: cruise_faulted = pt_cp.vl["Motor_2"]["MO2_Sta_GRA"] == 3 ret.accFaulted = cruise_faulted ret.cruiseState.available = cruise_main_switch and not cruise_faulted allow_lat_only = self._params.get_bool("AllowLateralWhenLongUnavailable") and self._params.get_bool("AolEnabled") cruise_main_available = cruise_main_switch ret.cruiseFaultLateralMode = allow_lat_only and cruise_faulted and cruise_main_available ret.lateralAvailable = ret.cruiseState.available or ret.cruiseFaultLateralMode ret.blockPcmEnable = ret.cruiseFaultLateralMode ret.carNotReady = bool(pt_cp.vl["Bremse_8"]["BR8_Sta_VerzReg"]) # Update ACC setpoint. When the setpoint reads as 255, the driver has not # yet established an ACC setpoint, so treat it as zero. if cc_only: ret.cruiseState.speed = pt_cp.vl["Motor_2"]["MO2_GRA_Soll"] * CV.KPH_TO_MS elif self.CP.pcmCruise: ret.cruiseState.speed = ext_cp.vl["ACC_GRA_Anzeige"]["ACA_V_Wunsch"] * CV.KPH_TO_MS else: ret.cruiseState.speed = 0 if ret.cruiseState.speed > 70: # 255 kph in m/s == no current setpoint ret.cruiseState.speed = 0 self.motor2_stock = pt_cp.vl["Motor_2"] self.motor5_stock = pt_cp.vl["Motor_5"] if self.CP.flags & VolkswagenFlagsIQ.IQ_PQ_ACC_FTS_EPB: self.motor3_stock = aux_cp.vl["Motor_3"] self.motor1_stock = aux_cp.vl["Motor_1"] self.motor3_frame += 1 self.motor1_frame += 1 if cc_only: self.acc_radar_sollbeschl = 0.0 self.acc_radar_regelabw = 0.0 self.acc_radar_aendgrad = 0.0 self.acc_radar_sta_adr = 0 self.acc_radar_fehler = False self.acc_radar_v_wunsch = 0.0 self.acc_radar_sta_acc = 0 else: self.acc_radar_sollbeschl = ext_cp.vl["ACC_System"]["ACS_Sollbeschl"] self.acc_radar_regelabw = ext_cp.vl["ACC_System"]["ACS_zul_Regelabw"] self.acc_radar_aendgrad = ext_cp.vl["ACC_System"]["ACS_max_AendGrad"] self.acc_radar_sta_adr = int(ext_cp.vl["ACC_System"]["ACS_Sta_ADR"]) self.acc_radar_fehler = bool(ext_cp.vl["ACC_System"]["ACS_Fehler"]) self.acc_radar_v_wunsch = ext_cp.vl["ACC_GRA_Anzeige"]["ACA_V_Wunsch"] self.acc_radar_sta_acc = int(ext_cp.vl["ACC_GRA_Anzeige"]["ACA_StaACC"]) ret_iq.accRadarStaAdr = self.acc_radar_sta_adr ret_iq.accRadarFehler = self.acc_radar_fehler # 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"]) self.leftBlinkerUpdate = pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"] self.rightBlinkerUpdate = 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"]["BR1_ESPASR_passive"]) ret.lowSpeedAlert = self.update_low_speed_alert(ret.vEgo) ret.fuelGauge = pt_cp.vl["Kombi_1"]["Tankinhalt"] / 55.0 ret.fuelTankLevelL = pt_cp.vl["Kombi_1"]["Tankinhalt"] # raw liters for konn3kt if aux_cp is not None: self._update_odometer(ret, aux_cp.vl["Kombi_3"]["Kilometerstand"]) if self.CP.flags & VolkswagenFlagsIQ.IQ_PQ_ACC_FTS_EPB: ret.cruiseState.standstill = self.CP.pcmCruise and bool(pt_cp.vl["Bremse_5"]["BR5_Stillstand"]) and ret.cruiseState.enabled elif sng_ecd_enabled: ret.cruiseState.standstill = sng_holding and ret.standstill self.cruise_faulted = ret.accFaulted self.frame += 1 self._apply_iq_private_flags(ret_iq) return ret, ret_iq def update_mlb(self, pt_cp, br_cp, cam_cp, ext_cp) -> structs.CarState: ret = structs.CarState() ret_iq = structs.IQCarState() self.parse_wheel_speeds(ret, pt_cp.vl["ESP_03"]["ESP_VL_Radgeschw"], pt_cp.vl["ESP_03"]["ESP_VR_Radgeschw"], pt_cp.vl["ESP_03"]["ESP_HL_Radgeschw"], pt_cp.vl["ESP_03"]["ESP_HR_Radgeschw"], ) ret.gasPressed = pt_cp.vl["Motor_03"]["MO_Fahrpedalrohwert_01"] > 0 if self.CP.carFingerprint == CAR.PORSCHE_MACAN_MK1: ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Getriebe_03"]["GE_Waehlhebel"], None)) elif self.CP.transmissionType == TransmissionType.manual: reverse = bool(br_cp.vl["Gateway_05"]["BCM1_Rueckfahrlicht_Schalter"]) ret.gearShifter = GearShifter.reverse if reverse else GearShifter.drive else: ret.gearShifter = GearShifter.drive cc_only = bool(self.CP.flags & (VolkswagenFlagsIQ.IQ_CC_ONLY | VolkswagenFlagsIQ.IQ_CC_ONLY_NO_RADAR)) cruise_main_switch = bool(pt_cp.vl["LS_01"]["LS_Hauptschalter"]) if not self.CP.pcmCruise: ret.cruiseState.available = cruise_main_switch ret.cruiseState.enabled = False ret.accFaulted = False elif self.CP.carFingerprint == CAR.PORSCHE_MACAN_MK1: ret.cruiseState.available = ext_cp.vl["ACC_05"]["ACC_Status_ACC"] in (2, 3, 4, 5) ret.cruiseState.enabled = ext_cp.vl["ACC_05"]["ACC_Status_ACC"] in (3, 4, 5) ret.accFaulted = ext_cp.vl["ACC_05"]["ACC_Status_ACC"] in (6, 7) else: ret.cruiseState.available = pt_cp.vl["TSK_02"]["TSK_Status"] in (0, 1, 2) ret.cruiseState.enabled = pt_cp.vl["TSK_02"]["TSK_Status"] in (1, 2) if not cc_only: ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw_02"] * CV.KPH_TO_MS ret.accFaulted = pt_cp.vl["TSK_02"]["TSK_Status"] in (3,) ret.cruiseState.nonAdaptive = bool(pt_cp.vl["LS_01"]["LS_Limiter"]) if not self.CP.pcmCruise: self.acc_stock_counters["ACC_01"] = int(ext_cp.vl["ACC_01"]["COUNTER"]) self.acc_stock_counters["ACC_02"] = int(ext_cp.vl["ACC_02"]["COUNTER"]) self.esp_hold_confirmation = bool(pt_cp.vl["ESP_02"]["ESP_Stillstandsflag"]) self.parse_mlb_mqb_steering_state(ret, pt_cp) self._update_mlb_iq_alc_state(pt_cp) ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0 brake_pedal_pressed = bool(pt_cp.vl["Motor_03"]["MO_BLS"]) # MO_Fahrer_bremst sticks on real MLB hardware brake_pressure_detected = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"]) ret.brakePressed = brake_pedal_pressed or brake_pressure_detected ret.parkingBrake = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"]) ret.espDisabled = pt_cp.vl["ESP_01"]["ESP_Tastung_passiv"] != 0 if self.CP.carFingerprint == CAR.PORSCHE_MACAN_MK1: ret.leftBlinker = bool(pt_cp.vl["Gateway_11"]["BH_Blinker_li"]) ret.rightBlinker = bool(pt_cp.vl["Gateway_11"]["BH_Blinker_re"]) ret.seatbeltUnlatched = pt_cp.vl["Gateway_06"]["AB_Gurtschloss_FA"] != 3 ret.doorOpen = any([pt_cp.vl["Gateway_05"]["FT_Tuer_geoeffnet"], pt_cp.vl["Gateway_05"]["BT_Tuer_geoeffnet"], pt_cp.vl["Gateway_05"]["HL_Tuer_geoeffnet"], pt_cp.vl["Gateway_05"]["HR_Tuer_geoeffnet"]]) else: ret.leftBlinker = bool(pt_cp.vl["Blinkmodi_01"]["BM_links"]) ret.rightBlinker = bool(pt_cp.vl["Blinkmodi_01"]["BM_rechts"]) ret.seatbeltUnlatched = pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] != 3 # 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_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"]) self.ldw_stock_values = cam_cp.vl["LDW_02"] if self.CP.networkLocation == NetworkLocation.fwdCamera else {} self.gra_stock_values = pt_cp.vl["LS_01"] ret.fuelGauge = br_cp.vl["Kombi_02"]["KBI_Inhalt_Tank"] / 55.0 ret.fuelTankLevelL = br_cp.vl["Kombi_02"]["KBI_Inhalt_Tank"] # raw liters for konn3kt self._update_odometer(ret, br_cp.vl["Kombi_02"]["KBI_Kilometerstand"]) ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS) ret.cruiseState.standstill = self.CP.pcmCruise and self.esp_hold_confirmation ret.standstill = ret.vEgoRaw == 0 allow_lat_only = self._params.get_bool("AllowLateralWhenLongUnavailable") and self._params.get_bool("AolEnabled") ret.cruiseFaultLateralMode = allow_lat_only and ret.accFaulted and cruise_main_switch ret.lateralAvailable = ret.cruiseState.available or ret.cruiseFaultLateralMode ret.blockPcmEnable = ret.cruiseFaultLateralMode self.cruise_faulted = ret.accFaulted self.frame += 1 self._apply_iq_private_flags(ret_iq) return ret, ret_iq def update_low_speed_alert(self, v_ego: float) -> bool: # Low speed steer alert hysteresis logic if (self.CP.minSteerSpeed - 1e-3) > CarControllerParams.DEFAULT_MIN_STEER_SPEED and v_ego < (self.CP.minSteerSpeed + 1.): self.low_speed_alert = True elif v_ego > (self.CP.minSteerSpeed + 2.): self.low_speed_alert = False return self.low_speed_alert def parse_mlb_mqb_steering_state(self, ret, pt_cp, drive_mode=True): ret.steeringAngleDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradwinkel"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradwinkel"])] ret.steeringRateDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradw_Geschw"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradw_Geschw"])] if self.CP.flags & VolkswagenFlagsIQ.IQ_MLB_NO_HCA_EPS: ret.steeringTorque = 0.0 ret.steeringPressed = False ret.steerFaultTemporary, ret.steerFaultPermanent = False, True return if self.CP.flags & VolkswagenFlags.MLB: # MLB LWS zero is vehicle-specific (measured 2.5 deg off centre on an 8R); the EPS angle is what the rack closes its own loop on ret.steeringAngleDeg = pt_cp.vl["LH_EPS_03"]["EPS_Berechneter_LW"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_BLW"])] ret.steeringTorque = pt_cp.vl["LH_EPS_03"]["EPS_Lenkmoment"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_Lenkmoment"])] ret.steeringPressed = abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE hca_status = self.CCP.hca_status_values.get(pt_cp.vl["LH_EPS_03"]["EPS_HCA_Status"]) ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status, drive_mode) return def update_hca_state(self, hca_status, drive_mode=True, ready_confirms_init=True): # Treat FAULT as temporary for worst likely EPS recovery time, for cars without factory Lane Assist # DISABLED means the EPS hasn't been configured to support Lane Assist init_statuses = ("DISABLED", "READY", "ACTIVE") if ready_confirms_init else ("DISABLED", "ACTIVE") self.eps_init_complete = self.eps_init_complete or hca_status in init_statuses or self.frame > 1000 perm_fault = drive_mode and hca_status == "DISABLED" or (self.eps_init_complete and hca_status == "FAULT") temp_fault = drive_mode and hca_status in ("REJECTED", "PREEMPTED") or not self.eps_init_complete return temp_fault, perm_fault def update_acc_fault(self, acc_fault, parking_brake=False, drive_mode=True, recovery_frames_max=100): fault = acc_fault if parking_brake and not drive_mode: fault = False self.cruise_recovery_timer = self.frame elif self.frame - self.cruise_recovery_timer < recovery_frames_max: fault = False return fault def update_meb_virtual_lkas(self, ret, pt_cp, hca_status): temp_cruise_fault = pt_cp.vl["Motor_51"]["TSK_Status"] == self.MEB_TEMP_CRUISE_FAULT drive_mode = ret.gearShifter == GearShifter.drive if temp_cruise_fault and ret.parkingBrake and not drive_mode: ret.cruiseState.available = True self.tolerance_counter = 0 elif self.tolerance_counter < self.MEB_TOLERANCE_MAX: ret.cruiseState.available = True self.tolerance_counter = min(self.tolerance_counter + 1, self.MEB_TOLERANCE_MAX) self.prev_lkas_button = self.lkas_button user_disable = any(b.type == ButtonType.cancel and b.pressed for b in ret.buttonEvents) steering_enabled = hca_status == "ACTIVE" cruise_standby = not ret.cruiseState.enabled self.lkas_button = steering_enabled and user_disable and cruise_standby if self.prev_lkas_button != self.lkas_button: event = structs.CarState.ButtonEvent() event.type = ButtonType.lkas event.pressed = self.lkas_button ret.buttonEvents = list(ret.buttonEvents) + [event] @staticmethod def get_can_parsers(CP, CP_IQ): if CP.flags & VolkswagenFlags.PQ: return CarState.get_can_parsers_pq(CP) if CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO): return CarState.get_can_parsers_meb(CP) # manually configure some optional and variable-rate/edge-triggered messages pt_messages, cam_messages = [], [] if not CP.flags & VolkswagenFlags.MLB: pt_messages += [ ("Blinkmodi_02", 1) # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active) ] if CP.flags & VolkswagenFlags.MLB: pt_messages += [ ("Blinkmodi_01", math.nan), # From J519 BCM (is inactive when no lights active, 50Hz when active) ("Kombi_02", math.nan), # Auxiliary-bus cluster odometer ] else: pt_messages += [("Kombi_02", math.nan)] # Auxiliary-bus cluster odometer if CP.flags & VolkswagenFlagsIQ.IQ_MLB_NO_HCA_EPS: pt_messages += [("LH_EPS_03", math.nan)] if CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT: cam_messages += [ ("HCA_01", 1), # From R242 Driver assistance camera, 50Hz if steering/1Hz if not ] pt_bus = CanBus(CP).aux if CP.flags & VolkswagenFlagsIQ.IQ_MLB_NO_ECAN else CanBus(CP).pt return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, pt_bus), Bus.aux: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, CanBus(CP).aux), Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CanBus(CP).cam), } @staticmethod def get_can_parsers_pq(CP): aux_messages = [("Kombi_3", math.nan)] # Bus 1 cluster odometer if CP.flags & VolkswagenFlagsIQ.IQ_PQ_ACC_FTS_EPB: aux_messages.append(("Motor_3", 0)) return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).powertrain), Bus.aux: CANParser(DBC[CP.carFingerprint][Bus.pt], aux_messages, CanBus(CP).aux), Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).cam), } @staticmethod def get_can_parsers_meb(CP): pt_messages = [ ("Blinkmodi_02", 1), ("SMLS_01", 1), # TA_01 lives on bus 0 (car ECU / OP-generated when long is active). # math.nan → ignore_alive=True so it never contributes to can_valid. ("TA_01", math.nan), ("Diagnose_01", math.nan), # Bus 0 cluster odometer ] if CP.networkLocation == NetworkLocation.fwdCamera: pt_messages.append(("AWV_03", 1)) cam_messages = [] if CP.networkLocation == NetworkLocation.gateway: cam_messages.append(("AWV_03", 1)) return { Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, CanBus(CP).pt), Bus.main: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).main), Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CanBus(CP).cam), }