IQ.Lvbs/IQ.Pilot
8
7
Fork 2
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

IQ.Pilot Prebuilt Release @ 27f668a

Browse Source
This commit is contained in:
IQ.Lvbs CI [bot]
2026-09-03 18:23:24 -05:00
commit b073c5182b
2554 changed files with 679696 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

0
artifacts/package_runtime/iqdbc/car/volkswagen/__init__.py Normal file
View File

712
artifacts/package_runtime/iqdbc/car/volkswagen/carcontroller.py Normal file
View File

@@ -0,0 +1,712 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
import sys
import os
import math
import numpy as np
import random
from iqdbc.can import CANPacker
from iqdbc.car import Bus, DT_CTRL, make_tester_present_msg, structs
from iqdbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_simple
from iqdbc.car.lateral import apply_std_curvature_limits
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.common.numpy_fast import clip, interp
from iqdbc.car.interfaces import CarControllerBase
from iqdbc.car.volkswagen import mlbcan, mqbcan, pqcan, mebcan
from iqdbc.car.volkswagen.pq_radar_handler import PQRadarHandler
from iqdbc.car.volkswagen.values import (
CanBus, CarControllerParams, MQB_A0_CARS, VolkswagenFlags, VolkswagenFlagsIQ, apply_pq_stopping_accel,
)
from iqdbc.car.volkswagen.mebutils import LongControlJerk, LongControlLimit, LatControlCurvature
from iqdbc.car.vehicle_model import VehicleModel
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
VisualAlert = structs.CarControl.HUDControl.VisualAlert
AudibleAlert = structs.CarControl.HUDControl.AudibleAlert
LongCtrlState = structs.CarControl.Actuators.LongControlState
def dVisual(CCS, CS):
if CCS == mqbcan:
decelV = CS.tsk_verzoeg_anf
elif CCS == pqcan:
decelV = CS.br8_acc_anf
else:
decelV = False
return decelV
class MQBStandstillManager:
BRAKE_TORQUE_RAMP_RATE = 2000.0 # Nm/s
ASSUMED_WHEEL_RADIUS = 0.328 # m, typical tire rolling radius
GRAVITY = 9.81 # m/s^2
WEGIMPULSE_STILLNESS_FRAMES = 5 # frames of no wheel tick change before assuming standstill
ESP_OVERRIDE_SPEED = 9.5 * CV.KPH_TO_MS
MAX_SAFE_STOPPING_SPEED = 10.0 * CV.KPH_TO_MS
def __init__(self, vehicle_mass: float = 1540.0, accel_min: float = -3.5):
self.vehicle_mass = vehicle_mass
self.accel_min = accel_min
self.can_stop_forever = False
self.rollback_detected = False
self.start_commit_active = False
self.frames_since_last_wheel_pulse = 0
self.prev_sum_wegimpulse: int | None = None
self.prev_accel = 0
self.hold_recovery_active = False
def get_hill_hold_decel_deficit(self, pitch: float, brake_torque: float) -> float:
if self.vehicle_mass <= 0:
return 0.0
uphill_pitch = max(pitch, 0.0)
hill_hold_decel = self.GRAVITY * math.sin(uphill_pitch)
brake_decel = max(brake_torque, 0.0) / (self.vehicle_mass * self.ASSUMED_WHEEL_RADIUS)
return max(hill_hold_decel - brake_decel, 0.0)
def get_safe_speed_for_brake_torque(self, pitch: float, brake_torque: float) -> float:
missing_brake_decel = self.get_hill_hold_decel_deficit(pitch, brake_torque)
if missing_brake_decel <= 0 or self.vehicle_mass <= 0:
return 0.0
brake_decel_build_rate = self.BRAKE_TORQUE_RAMP_RATE / (self.vehicle_mass * self.ASSUMED_WHEEL_RADIUS)
forward_speed_needed_while_brake_builds = 1.5 * missing_brake_decel ** 2 / brake_decel_build_rate
return min(forward_speed_needed_while_brake_builds, self.MAX_SAFE_STOPPING_SPEED)
def get_blended_brake_accel(self, raw_accel: float, v_ego: float, pitch: float, brake_torque: float) -> float:
zero_brake_decel_deficit = self.get_hill_hold_decel_deficit(pitch, 0.0)
current_brake_decel_deficit = self.get_hill_hold_decel_deficit(pitch, brake_torque)
zero_brake_safe_speed = self.get_safe_speed_for_brake_torque(pitch, 0.0)
if zero_brake_decel_deficit <= 0 or zero_brake_safe_speed <= 0:
return raw_accel
brake_deficit_risk = current_brake_decel_deficit / zero_brake_decel_deficit
speed_risk = max(zero_brake_safe_speed - v_ego, 0.0) / zero_brake_safe_speed
rollback_risk = float(np.clip(speed_risk * brake_deficit_risk, 0.0, 1.0))
blended_accel = raw_accel + rollback_risk * (self.accel_min - raw_accel)
return min(raw_accel, blended_accel)
def update(self, CS, long_active: bool, accel: float, stopping: bool, starting: bool,
max_planned_speed: float, pitch: float = 0.0,
tsk_brake_torque: float = 0.0) -> tuple[bool, float, bool, bool, bool | None, bool | None]:
safe_stopping_speed = self.get_safe_speed_for_brake_torque(pitch, 0.0)
below_safe_stop_speed = CS.out.vEgo < safe_stopping_speed
can_accelerate = max_planned_speed > safe_stopping_speed
uphill_grade_pct = max(math.tan(pitch) * 100.0, 0.0)
takeoff_acceleration = max(0.2, 0.1 * uphill_grade_pct)
if CS.out.vEgo < self.ESP_OVERRIDE_SPEED:
esp_starting_override: bool | None = True
esp_stopping_override: bool | None = False
else:
esp_starting_override = None
esp_stopping_override = None
if CS.rolling_backward:
self.rollback_detected = True
elif CS.rolling_forward:
self.rollback_detected = False
wheel_did_pulse = CS.sum_wegimpulse != self.prev_sum_wegimpulse
self.prev_sum_wegimpulse = CS.sum_wegimpulse
if wheel_did_pulse:
self.frames_since_last_wheel_pulse = 0
else:
self.frames_since_last_wheel_pulse += 1
near_standstill = self.frames_since_last_wheel_pulse >= self.WEGIMPULSE_STILLNESS_FRAMES
# acc type 1 is sensitive to control signals when brake is pressed (when preEnabled)
if CS.out.brakePressed:
long_active = False
if long_active and not CS.out.gasPressed:
if CS.esp_hold_confirmation:
self.start_commit_active = True
if can_accelerate and below_safe_stop_speed and accel > 0:
self.start_commit_active = True
elif self.start_commit_active:
if CS.out.vEgo > safe_stopping_speed:
self.start_commit_active = False
else:
self.start_commit_active = False
if long_active:
raw_accel = accel
if self.start_commit_active:
accel = max(accel, takeoff_acceleration)
stopping = False
starting = True
elif self.rollback_detected:
accel = self.accel_min
stopping = True
starting = False
elif below_safe_stop_speed:
accel = self.get_blended_brake_accel(accel, CS.out.vEgo, pitch, tsk_brake_torque)
if accel < raw_accel:
stopping = True
starting = False
if near_standstill and accel < 0 and tsk_brake_torque == 0:
accel = self.accel_min
stopping = True
starting = False
if CS.out.standstill and accel < 0:
accel = min(accel, self.prev_accel)
if long_active:
if CS.out.vEgo > self.ESP_OVERRIDE_SPEED:
self.can_stop_forever = False
if CS.esp_hold_confirmation:
self.can_stop_forever = False
self.hold_recovery_active = True
if self.start_commit_active:
esp_starting_override = True
esp_stopping_override = False
elif CS.esp_stopping:
self.can_stop_forever = True
self.hold_recovery_active = False
esp_starting_override = True
esp_stopping_override = False
elif self.can_stop_forever:
esp_starting_override = True
esp_stopping_override = False
elif near_standstill:
esp_starting_override = False
esp_stopping_override = True
# recover from hold confirmations while moving to prevent reconfirming them
elif self.hold_recovery_active and not CS.out.standstill:
esp_starting_override = False
esp_stopping_override = True
else:
self.can_stop_forever = False
self.hold_recovery_active = False
self.prev_accel = accel
return long_active, accel, stopping, starting, esp_starting_override, esp_stopping_override
def accel_during_driver_override(accel: float, gas_pressed: bool, keep_long_active: bool) -> float:
return 0.0 if gas_pressed and keep_long_active else accel
def ea_send_ready(stock_values, last_counter):
return bool(stock_values) and stock_values["COUNTER"] != last_counter
EA_BLINKER_STEP = 2
def next_ea_counter(tx_counter, stock_counter):
return ((stock_counter if tx_counter is None else tx_counter) + 1) % 16
def ea_blinker_command(left_request, right_request, left_active, right_active):
blinker_active = left_active or right_active
return left_request and not blinker_active, right_request and not blinker_active
class CarController(CarControllerBase):
def __init__(self, dbc_names, 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__(dbc_names, CP, CP_IQ)
self._params = Params()
self.CCP = CarControllerParams(CP)
self.CAN = CanBus(CP)
self.packer_pt = CANPacker(dbc_names[Bus.pt])
self._pt_tx_bus = self.CAN.pt
if CP.flags & VolkswagenFlags.PQ:
self.CCS = pqcan
if CP.flags & VolkswagenFlagsIQ.IQ_PQ_LOWLINE:
self._pt_tx_bus = self.CAN.aux
elif CP.flags & VolkswagenFlags.MLB:
self.CCS = mlbcan
if CP.flags & VolkswagenFlagsIQ.IQ_MLB_NO_ECAN:
self._pt_tx_bus = self.CAN.aux
elif CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
self.CCS = mebcan
else:
self.CCS = mqbcan
self.accel = 0
self.apply_torque_last = 0
self.apply_curvature_last = 0.
self.apply_angle_last = 0
self.ALC_entryCounter = 0
self.ALC_driverExit = False
self.ALC_reentry_blocked = False
self.ALC_override_last = False
self.ALC_override_counter = 0
self.entering = False
self.active = False
self.CSLH3_SignLast = 0
self.CSsteeringAngleDegLast = 0
self.steering_power_last = 0
self.long_jerk_control = LongControlJerk(dt=(DT_CTRL * self.CCP.ACC_CONTROL_STEP)) if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO) else None
self.long_limit_control = LongControlLimit(dt=(DT_CTRL * self.CCP.ACC_CONTROL_STEP)) if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO) else None
self.gra_acc_counter_last = None
self.gra_cancel_ticks = 0
self.motor3_frame_last = None
self.motor3_was_stopping = False
self.motor3_resuming = False
self.sng_handoff_active = False
self.acc_counter_seeded = False
self.klr_counter_last = None
self.ea_counter_last = None
self.ea_tx_counter = None
self.eps_timer_soft_disable_alert = False
self.hca_frame_timer_running = 0
self.hca_frame_same_torque = 0
self.accel_last = 0
self.long_deviation = 0
self.long_jerklimit = 0
self.HCA_Status = 3
self.leadDistanceBars = 0
self.lead_distance_bars_last = None
self.distance_bar_frame = 0
self.mlb_hud_text = 0
self.mlb_hud_text_frame = 0
self.mlb_set_speed_last = 0
self.mlb_lead_distance_bars_last = None
self.speed_limit_last = 0
self.speed_limit_changed_timer = 0
self.blinkerActive = None
self.hide_ea_error = False
self.radar_disabled_warning_timer = 0
# Check once at init whether the DBC includes MEB_AWV_01 (AEB HUD for radar-disabled camera harness cars)
self._has_aeb_hud_msg = "MEB_AWV_01" in self.packer_pt.dbc.name_to_msg
self.eps_timer_workaround = bool(CP.flags & (VolkswagenFlags.MLB | VolkswagenFlagsIQ.IQ_PQ_TIMEBOMB))
self._pq_patch_checked = not bool(CP.flags & VolkswagenFlags.PQ) or self.eps_timer_workaround
self.hca_frame_timer_resetting = 0
self.hca_frame_low_torque = 0
self.acc_hold_type_last = mebcan.ACC_HMS_NO_REQUEST
self.acc_hold_ramp_counter = 0
self.standstill_manager = MQBStandstillManager(CP.mass, self.CCP.ACCEL_MIN) if self.CCS == mqbcan else None
self.radar_handler = PQRadarHandler(self.CAN) if self.CCS is pqcan else None
self.blend_stock_radar = False
self.unavailable = False
self.unavailable_hold = 0
self.VM = VehicleModel(CP)
self.is_mqb_a0 = self._is_mqb_a0_car(CP.carFingerprint)
self.LateralController = (
LatControlCurvature(self.CCP.CURVATURE_PID, self.CCP.CURVATURE_LIMITS.CURVATURE_MAX, 1 / (DT_CTRL * self.CCP.STEER_STEP))
if (CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO))
else None
)
@staticmethod
def _is_mqb_a0_car(candidate) -> bool:
return candidate in MQB_A0_CARS
def _get_mqb_steering_torque_scale(self, v_ego: float, enabled: bool) -> float:
if enabled and self.CCS == mqbcan:
return float(np.interp(v_ego, [0.4, 3.5, 4.0], [0.8, 0.95, 1.0]))
return 1.0
def _mlb_acc_hud_text(self, hud_control, set_speed: float) -> int:
# ACC_02 primary display text, briefly surfaced on a follow distance or set speed change
if hud_control.leadDistanceBars != self.mlb_lead_distance_bars_last:
self.mlb_hud_text_frame = self.frame
self.mlb_hud_text = self.CCP.ACC_HUD_TEXT_DISTANCE.get(hud_control.leadDistanceBars, self.CCP.ACC_HUD_TEXTS["none"])
elif set_speed != self.mlb_set_speed_last and hud_control.speedVisible:
self.mlb_hud_text_frame = self.frame
self.mlb_hud_text = self.CCP.ACC_HUD_TEXTS["setSpeed"]
elif self.frame - self.mlb_hud_text_frame >= self.CCP.ACC_HUD_TEXT_STEP:
self.mlb_hud_text = self.CCP.ACC_HUD_TEXTS["none"]
self.mlb_lead_distance_bars_last = hud_control.leadDistanceBars
self.mlb_set_speed_last = set_speed
return self.mlb_hud_text
def _tap_gra_cancel(self, cancel_req: bool, gra_send_ready: bool) -> bool:
if not cancel_req:
self.gra_cancel_ticks = 0
return False
period = self.CCP.GRA_CANCEL_TAP_ON + self.CCP.GRA_CANCEL_TAP_OFF
if self.gra_cancel_ticks >= period * self.CCP.GRA_CANCEL_MAX_TAPS:
return False
pressed = self.gra_cancel_ticks % period < self.CCP.GRA_CANCEL_TAP_ON
if gra_send_ready:
self.gra_cancel_ticks += 1
return pressed
def _should_spam_mqb_a0_resume(self, CS, enabled: bool) -> bool:
return bool(
enabled and
self.is_mqb_a0 and
self.CCS == mqbcan and
CS.out.standstill and
self.frame % 50 < 15
)
def update(self, CC, CC_IQ, CS, now_nanos):
actuators = CC.actuators
hud_control = CC.hudControl
can_sends = []
output_torque = 0
apply_torque = 0
pqhca5or7Toggle = self._params.get_bool("pqhca5or7Toggle")
eBrakeActive = self._params.get_bool("eBrakeActive")
iq_mqb_steering_lockout = self._params.get_bool("iqMqbSteeringLockout")
iq_mqb_acc_resume = self._params.get_bool("iqMqbAccResume")
self.blend_stock_radar = self._params.get_bool("IQDynamicBlendStockRadar")
if not self._pq_patch_checked:
self._pq_patch_checked = True
self.eps_timer_workaround = not self._params.get_bool("VwPqEpsPatched")
blend_active = bool(self.blend_stock_radar and self.radar_handler is not None and self.CP.openpilotLongitudinalControl)
AngleLateralControl = self._iq_lvbs_alc.angle_lateral_control_enabled(self, CS)
self.entering = CS.vw_iq_lvbs_alc_entering
self.active = CS.vw_iq_lvbs_alc_active
if hud_control.audibleAlert == AudibleAlert.refuse:
self.unavailable_hold = self.CCP.IQ_PQ_UNAVAILABLE_HUD_FRAMES
else:
self.unavailable_hold = max(0, self.unavailable_hold - 1)
self.unavailable = self.unavailable_hold > 0
CS.force_rhd_for_bsm = getattr(CC, "forceRHDForBSM", False)
CS.enable_predicative_speed_limit = getattr(CC.cruiseControl, "speedLimitPredicative", False)
CS.enable_pred_react_to_speed_limits = getattr(CC.cruiseControl, "speedLimitPredReactToSL", False)
CS.enable_pred_react_to_curves = getattr(CC.cruiseControl, "speedLimitPredReactToCurves", False)
if self.frame % self.CCP.STEER_STEP == 0:
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
if CC.latActive:
hca_enabled = True
if CC.curvatureControllerActive:
apply_curvature = self.LateralController.update(CS.out, CC, actuators.curvature)
apply_curvature = apply_curvature + (CS.out.steeringCurvature - (CC.currentCurvature - CC.rollCompensation))
else:
apply_curvature = actuators.curvature + (CS.out.steeringCurvature - CC.currentCurvature)
apply_curvature = apply_std_curvature_limits(apply_curvature, self.apply_curvature_last, CS.out.vEgoRaw, CS.out.steeringCurvature,
CS.out.steeringPressed, self.CCP.STEER_STEP, CC.latActive, self.CCP.CURVATURE_LIMITS)
min_power = max(self.steering_power_last - self.CCP.STEERING_POWER_STEP, self.CCP.STEERING_POWER_MIN)
max_power = min(self.steering_power_last + self.CCP.STEERING_POWER_STEP, self.CCP.STEERING_POWER_MAX)
target_power_driver = int(np.interp(abs(CS.out.steeringTorque), [self.CCP.STEER_DRIVER_ALLOWANCE, self.CCP.STEER_DRIVER_MAX],
[self.CCP.STEERING_POWER_MAX, self.CCP.STEERING_POWER_MIN]))
target_power = int(np.interp(CS.out.vEgo, [0., 0.5], [self.CCP.STEERING_POWER_MIN, target_power_driver]))
steering_power = min(max(target_power, min_power), max_power)
else:
if self.LateralController is not None:
self.LateralController.reset()
if self.steering_power_last > 0:
hca_enabled = True
handoff_curvature = self.apply_curvature_last + (CS.out.steeringCurvature - self.apply_curvature_last) * self.CCP.CURVATURE_HANDOFF_RATE
apply_curvature = apply_std_curvature_limits(handoff_curvature, self.apply_curvature_last, CS.out.vEgoRaw, CS.out.steeringCurvature,
CS.out.steeringPressed, self.CCP.STEER_STEP, True, self.CCP.CURVATURE_LIMITS)
steering_power = max(self.steering_power_last - self.CCP.STEERING_POWER_STEP, 0)
else:
hca_enabled = False
apply_curvature = 0.
steering_power = 0
can_sends.append(self.CCS.create_steering_control(self.packer_pt, self.CAN.pt, apply_curvature, hca_enabled, steering_power))
self.apply_curvature_last = apply_curvature
self.steering_power_last = steering_power
else:
if CC.latActive and not AngleLateralControl:
torque_scale = self._get_mqb_steering_torque_scale(CS.out.vEgo, iq_mqb_steering_lockout)
new_torque = int(round(actuators.torque * self.CCP.STEER_MAX * torque_scale))
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.CCP)
self.hca_frame_timer_running += self.CCP.STEER_STEP
if self.apply_torque_last == apply_torque:
self.hca_frame_same_torque += self.CCP.STEER_STEP
if self.hca_frame_same_torque > self.CCP.STEER_TIME_STUCK_TORQUE / DT_CTRL:
apply_torque -= (1, -1)[apply_torque < 0]
self.hca_frame_same_torque = 0
else:
self.hca_frame_same_torque = 0
hca_enabled = abs(apply_torque) > 0
if self.eps_timer_workaround and self.hca_frame_timer_running >= self.CCP.STEER_TIME_BM / DT_CTRL:
if abs(apply_torque) <= self.CCP.STEER_LOW_TORQUE:
self.hca_frame_low_torque += self.CCP.STEER_STEP
if self.hca_frame_low_torque >= self.CCP.STEER_TIME_LOW_TORQUE / DT_CTRL:
hca_enabled = False
else:
self.hca_frame_low_torque = 0
if self.hca_frame_timer_resetting > 0:
apply_torque = 0
else:
self.hca_frame_low_torque = 0
hca_enabled = False
apply_torque = 0
if hca_enabled:
output_torque = apply_torque
self.hca_frame_timer_resetting = 0
else:
output_torque = 0
self.hca_frame_timer_resetting += self.CCP.STEER_STEP
if self.hca_frame_timer_resetting >= self.CCP.STEER_TIME_RESET / DT_CTRL or not self.eps_timer_workaround:
self.hca_frame_timer_running = 0
apply_torque = 0
if hca_enabled and abs(apply_torque) > 0:
if pqhca5or7Toggle and (self.CP.flags & (VolkswagenFlags.PQ | VolkswagenFlags.MLB)):
self.HCA_Status = 7
else:
self.HCA_Status = 5
else:
self.HCA_Status = 3
self.eps_timer_soft_disable_alert = self.hca_frame_timer_running > self.CCP.STEER_TIME_ALERT / DT_CTRL
self.apply_torque_last = apply_torque
if not (AngleLateralControl and self.CCS in (mqbcan, pqcan, mlbcan)):
can_sends.append(self.CCS.create_hca_steering_control(self.packer_pt, self._pt_tx_bus, output_torque, self.HCA_Status))
if self.CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT and self.CCS == mqbcan:
ea_simulated_torque = float(np.clip(apply_torque * 2, -self.CCP.STEER_MAX, self.CCP.STEER_MAX))
if abs(CS.out.steeringTorque) > abs(ea_simulated_torque):
ea_simulated_torque = CS.out.steeringTorque
can_sends.append(self.CCS.create_eps_update(self.packer_pt, self.CAN.cam, CS.eps_stock_values, ea_simulated_torque))
self._iq_lvbs_alc.update_vw_alc(self, CC, CS, actuators, can_sends, apply_torque)
if self.frame % self.CCP.STEER_STEP == 0:
self._iq_lvbs_alc.append_private_apd(self, CC_IQ, can_sends)
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO) and self.CP.flags & VolkswagenFlags.STOCK_KLR_PRESENT:
if CS.klr_stock_values:
klr_send_ready = CS.klr_stock_values["COUNTER"] != self.klr_counter_last
if klr_send_ready:
can_sends.append(mebcan.create_capacitive_wheel_touch(self.packer_pt, self.CAN.cam, CC.latActive, CS.klr_stock_values))
can_sends.append(mebcan.create_capacitive_wheel_touch(self.packer_pt, self.CAN.pt, CC.latActive, CS.klr_stock_values))
self.klr_counter_last = CS.klr_stock_values["COUNTER"]
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
if CS.ea_hud_stock_values and self.frame % EA_BLINKER_STEP == 0:
self.ea_tx_counter = next_ea_counter(self.ea_tx_counter, CS.ea_hud_stock_values["COUNTER"])
left_blinker, right_blinker = ea_blinker_command(
CC.leftBlinker, CC.rightBlinker, CS.left_blinker_active, CS.right_blinker_active,
)
can_sends.append(self.CCS.create_blinker_control(self.packer_pt, self.CAN.pt, CS.ea_hud_stock_values, CS.ea_control_stock_values,
left_blinker, right_blinker, self.hide_ea_error, self.ea_tx_counter))
self.ea_counter_last = CS.ea_hud_stock_values["COUNTER"]
if self.CP.openpilotLongitudinalControl and self.CCS in (mqbcan, mlbcan) and not self.acc_counter_seeded and CS.acc_stock_counters:
seed_msgs = ("ACC_01", "ACC_02") if self.CCS is mlbcan else ("ACC_02", "ACC_06", "ACC_07", "ACC_10")
for name in seed_msgs:
addr = self.packer_pt.dbc.name_to_msg[name].address
self.packer_pt.counters[addr] = (CS.acc_stock_counters[name] + 1) % 16
self.acc_counter_seeded = True
if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl and not CS.out.radarDisableFailed:
stopping = actuators.longControlState == LongCtrlState.stopping
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < 0.25)
accel = float(np.clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.enabled else 0)
long_override = CC.cruiseControl.override or CS.out.gasPressed
critical_state = hud_control.visualAlert == VisualAlert.fcw
if CC.longComfortMode and self.long_jerk_control is not None and self.long_limit_control is not None:
self.long_jerk_control.update(CC.enabled, long_override, hud_control.leadDistance, hud_control.leadVisible, accel, critical_state)
self.long_limit_control.update(CC.enabled, CS.out.vEgoRaw, hud_control.setSpeed, hud_control.leadDistance, hud_control.leadVisible, critical_state)
acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.enabled, long_override)
acc_hold_type, self.acc_hold_ramp_counter = self.CCS.acc_hold_type(
CS.out.cruiseState.available, CS.out.accFaulted, CC.enabled, starting, stopping,
CS.esp_hold_confirmation, CS.out.vEgo, self.acc_hold_type_last, self.acc_hold_ramp_counter)
self.acc_hold_type_last = acc_hold_type
can_sends.extend(self.CCS.create_acc_accel_control(
self.packer_pt, self.CAN.pt, self.CP, CS.acc_type, CC.enabled,
self.long_jerk_control.get_jerk_up() if CC.longComfortMode and self.long_jerk_control is not None else 4.0,
self.long_jerk_control.get_jerk_down() if CC.longComfortMode and self.long_jerk_control is not None else 4.0,
self.long_limit_control.get_upper_limit() if CC.longComfortMode and self.long_limit_control is not None else 0.,
self.long_limit_control.get_lower_limit() if CC.longComfortMode and self.long_limit_control is not None else 0.,
accel, acc_control, acc_hold_type, stopping, starting, CS.esp_hold_confirmation,
CS.out.vEgoRaw * CV.MS_TO_KPH, long_override, CS.travel_assist_available,
))
self.accel_last = accel
else:
stopping = actuators.longControlState == LongCtrlState.stopping
starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < 0.25)
long_active = CC.longActive
accel = accel_during_driver_override(actuators.accel, CS.out.gasPressed, self.CP_IQ.longActiveWithGasOverride)
esp_starting_override = None
esp_stopping_override = None
if self.CCS == mqbcan and CS.acc_type == 1 and self.standstill_manager is not None:
pitch = CC.orientationNED[1] if len(CC.orientationNED) == 3 else 0.0
long_active, accel, stopping, starting, esp_starting_override, esp_stopping_override = self.standstill_manager.update(
CS, long_active, accel, stopping, starting, float(getattr(actuators, "speed", 0.0)),
pitch, CS.tsk_brake_torque,
)
acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, long_active, CC.cruiseControl.override, CS.out.accFaulted)
accel = float(np.clip(accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if (long_active or CC.cruiseControl.override) else 0)
self.long_jerklimit = float(interp(accel, [-1.5, -0.3, 0.05], [2.0, 0.52, 0.30]))
self.long_deviation = float(interp(accel, [-0.3, 0.1], [0.0, 0.20]))
self.accel_last = accel
if blend_active and getattr(CC_IQ, "useRadarAccel", False) and CS.acc_radar_sta_adr == 1 and not self.radar_handler.failed:
accel = float(np.clip(CS.acc_radar_sollbeschl, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX))
self.long_jerklimit = CS.acc_radar_aendgrad
self.long_deviation = CS.acc_radar_regelabw
self.accel_last = accel
if self.CCS == mqbcan:
can_sends.extend(self.CCS.create_acc_accel_control(
self.packer_pt, self.CAN.pt, CS.acc_type, accel, acc_control, stopping, starting, CS.esp_hold_confirmation,
self.long_deviation, self.long_jerklimit, eBrakeActive,
esp_starting_override=esp_starting_override, esp_stopping_override=esp_stopping_override,
))
elif self.CCS == mlbcan:
can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, self.CAN.pt, accel, acc_control, stopping))
else:
accel = apply_pq_stopping_accel(self.CP.carFingerprint, accel, stopping)
sng_ecd_enabled = bool(self.CP.flags & VolkswagenFlagsIQ.IQ_PQ_SNG_ECD)
low_speed = CS.out.vEgo <= self.CCP.SNG_HANDOFF_SPEED
if sng_ecd_enabled and long_active and low_speed and accel <= 0.05 and not starting:
self.sng_handoff_active = True
else:
self.sng_handoff_active = False
sng_decel_req = float(np.clip(accel, self.CCP.ACCEL_MIN, self.CCP.SNG_HOLD_DECEL_MAX)) if self.sng_handoff_active else 0.0
can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, self.CAN.pt, CS.acc_type, accel, acc_control, stopping and not self.motor3_resuming, starting, CS.esp_hold_confirmation, self.long_deviation, self.long_jerklimit, eBrakeActive, sng_active=self.sng_handoff_active))
if sng_ecd_enabled:
can_sends.append(self.CCS.create_sng_handoff_control(self.packer_pt, self.CAN.aux, self.sng_handoff_active, sng_decel_req))
if (self.CP.flags & VolkswagenFlags.DISABLE_RADAR) and self.CP.openpilotLongitudinalControl and not CS.out.radarDisableFailed:
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
if self.radar_disabled_warning_timer < 600:
self.radar_disabled_warning_timer += 1
else:
self.hide_ea_error = True
if self.frame % self.CCP.AEB_CONTROL_STEP == 0:
can_sends.append(make_tester_present_msg(0x700, self.CAN.pt, suppress_response=True))
can_sends.append(self.CCS.create_aeb_control(self.packer_pt, self.CAN.pt, self.CP))
if self.frame % self.CCP.AEB_HUD_STEP == 0 and self._has_aeb_hud_msg:
can_sends.append(self.CCS.create_aeb_hud(self.packer_pt, self.CAN.pt, self.radar_disabled_warning_timer < 600))
if self.frame % 4 == 0:
can_sends.append(self.CCS.create_radar_objects(self.packer_pt, self.CAN.pt))
if self.frame % self.CCP.LDW_STEP == 0:
hud_alert = 0
if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) or CS.out.steerFaultTemporary:
hud_alert = self.CCP.LDW_MESSAGES["laneAssistTakeOver"]
steering_pressed_hud = (self.frame // 2) % 2 == 0 if self.entering else CS.out.steeringPressed
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
disable_alerts = getattr(CC, "disableCarSteerAlerts", False)
sound_alert = self.CCP.LDW_SOUNDS["Chime"] if hud_alert != 0 and not disable_alerts else self.CCP.LDW_SOUNDS["None"]
can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, self.CAN.pt, CS.ldw_stock_values, CC.latActive, CS.out.steeringPressed,
hud_alert, hud_control, sound_alert))
else:
can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, self._pt_tx_bus, CS.ldw_stock_values, CC.latActive, steering_pressed_hud,
hud_alert, hud_control, self.entering, self.CCS is pqcan and AngleLateralControl, self.active))
if hud_control.leadDistanceBars != self.lead_distance_bars_last:
self.distance_bar_frame = self.frame
if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl:
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
d_unresponsive = hud_control.driverUnresponsive
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
show_distance_bars = self.frame - self.distance_bar_frame < 400
gap = max(8, CS.out.vEgo * hud_control.leadFollowTime)
distance = max(8, hud_control.leadDistance) if hud_control.leadDistance != 0 else 0
acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.enabled,
CC.cruiseControl.override or CS.out.gasPressed)
sl_predicative_active = CC.cruiseControl.speedLimitPredicative and CS.out.cruiseState.speedLimitPredicative != 0
if CC.cruiseControl.speedLimit and CS.out.cruiseState.speedLimit != 0 and self.speed_limit_last != CS.out.cruiseState.speedLimit:
self.speed_limit_changed_timer = self.frame
self.speed_limit_last = CS.out.cruiseState.speedLimit
sl_active = self.frame - self.speed_limit_changed_timer < 400
speed_limit = CS.out.cruiseState.speedLimitPredicative if sl_predicative_active else (CS.out.cruiseState.speedLimit if sl_active else 0)
acc_hud_event = self.CCS.acc_hud_event(acc_hud_status, CS.esp_hold_confirmation, sl_predicative_active,
CS.speed_limit_predicative_type, sl_active)
can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, self.CAN.pt, acc_hud_status, hud_control.setSpeed * CV.MS_TO_KPH,
hud_control.leadVisible, hud_control.leadDistanceBars + 1, show_distance_bars,
CS.esp_hold_confirmation, distance, gap, fcw_alert, acc_hud_event, speed_limit))
else:
# MLB scales the raw lead distance against the set follow gap in the packer, the others clamp to a bar count
leadDistance = hud_control.leadDistance if self.CCS is mlbcan else \
(min(8, hud_control.leadDistance) if hud_control.leadDistance != 0 else 0)
self.leadDistanceBars = min(3, hud_control.leadDistanceBars)
acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive,
CC.cruiseControl.override or CS.out.gasPressed)
set_speed = hud_control.setSpeed * CV.MS_TO_KPH
decel = dVisual(self.CCS, CS)
hud_kwargs = {"hud_text": self._mlb_acc_hud_text(hud_control, set_speed),
"desired_distance": max(8.0, CS.out.vEgo * hud_control.leadFollowTime)} if self.CCS is mlbcan else {}
can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, self.CAN.pt, acc_hud_status, set_speed, leadDistance,
self.leadDistanceBars, fcw_alert, hud_control.leadVisible, self.unavailable,
decel, d_unresponsive, **hud_kwargs))
if self.CP.flags & VolkswagenFlags.PQ:
self._iq_lvbs_alc.update_turn_signals(self, CC, CS, can_sends)
if self.CP.openpilotLongitudinalControl and (self.CP.flags & VolkswagenFlags.PQ):
if blend_active:
can_sends.extend(self.radar_handler.update(
self.packer_pt, self.frame, CS,
blend_active=True,
engage_req=getattr(CC_IQ, "radarEngageReq", False),
cancel_req=getattr(CC_IQ, "radarCancelReq", False),
set_speed_kph=getattr(CC_IQ, "radarSetSpeedKph", 0.0),
gap_bars=getattr(CC_IQ, "radarGapBars", 0),
v_ego=CS.out.vEgo,
))
elif self.frame % 2 == 0:
can_sends.append(self.CCS.filter_motor2(self.packer_pt, self.CAN.ext, CS.motor2_stock))
can_sends.append(self.CCS.filter_motor5(self.packer_pt, self.CAN.ext, CS.motor5_stock))
gra_send_ready = self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last
if self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
main_cruise_latching = not bool(CS.gra_stock_values["GRA_Typ_Hauptschalter"])
stock_cancel_pressed = bool(CS.gra_stock_values["GRA_Abbrechen"] if main_cruise_latching else CS.gra_stock_values["GRA_Hauptschalter"])
elif self.CP.flags & VolkswagenFlags.MLB:
stock_cancel_pressed = bool(CS.gra_stock_values["LS_Abbrechen"])
else:
stock_cancel_pressed = bool(CS.gra_stock_values["GRA_Abbrechen"])
cancel_cmd = stock_cancel_pressed or self._tap_gra_cancel(CC.cruiseControl.cancel, gra_send_ready)
resume_cmd = CC.cruiseControl.resume or self._should_spam_mqb_a0_resume(CS, iq_mqb_acc_resume)
if gra_send_ready and (cancel_cmd or resume_cmd):
stalk_on_powertrain = self.CP.flags & VolkswagenFlags.PQ or self.CP.flags & VolkswagenFlagsIQ.IQ_MLB_NO_ECAN
bus_send = self.CAN.aux if stalk_on_powertrain else self.CAN.ext
can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, bus_send, CS.gra_stock_values,
cancel=cancel_cmd, resume=resume_cmd))
if self.CP.openpilotLongitudinalControl and self.CCS == pqcan and not blend_active:
if self.frame % 3:
can_sends.append(self.CCS.create_gra_neu(self.packer_pt, self.CAN.ext, CS.gra_stock_values, CC.longActive))
if self.CP.flags & VolkswagenFlagsIQ.IQ_PQ_ACC_FTS_EPB:
is_stopping = actuators.longControlState == LongCtrlState.stopping
if CS.out.vEgo > 0.5 or not CC.longActive:
self.motor3_resuming = False
elif (self.motor3_was_stopping and not is_stopping and self.CP.openpilotLongitudinalControl and CC.longActive):
self.motor3_resuming = True
if self.motor3_resuming and CS.motor3_stock:
can_sends.append(self.CCS.create_motor3_resume(self.packer_pt, self.CAN.aux, CS.motor1_stock, CS.motor3_stock, resume=True))
self.motor3_was_stopping = is_stopping
new_actuators = actuators.as_builder()
new_actuators.torque = self.apply_torque_last / self.CCP.STEER_MAX
new_actuators.torqueOutputCan = self.apply_torque_last
if self.CP.steerControlType == structs.CarParams.SteerControlType.angle:
new_actuators.steeringAngleDeg = float(self.apply_angle_last)
new_actuators.curvature = float(self.apply_curvature_last)
new_actuators.accel = self.accel_last
new_actuators.speed = float(getattr(actuators, "speed", 0.0))
self.lead_distance_bars_last = hud_control.leadDistanceBars
self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"]
self.motor3_frame_last = CS.motor3_frame
self.frame += 1
return new_actuators, can_sends

898
artifacts/package_runtime/iqdbc/car/volkswagen/carstate.py Normal file
View File

@@ -0,0 +1,898 @@
"""
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),
}

1590
artifacts/package_runtime/iqdbc/car/volkswagen/fingerprints.py Normal file
View File

File diff suppressed because it is too large Load Diff

385
artifacts/package_runtime/iqdbc/car/volkswagen/interface.py Normal file
View File

@@ -0,0 +1,385 @@
import time
from iqdbc.car import get_safety_config, structs, uds
from iqdbc.car.carlog import carlog
from iqdbc.car.interfaces import CarInterfaceBase
from iqdbc.car.isotp_parallel_query import IsoTpParallelQuery
from iqdbc.car.volkswagen.carcontroller import CarController
from iqdbc.car.volkswagen.carstate import CarState
from iqdbc.car.volkswagen.values import (
CAR, CanBus, DashcamOnlyReason, MLB_ACC_COORDINATOR_MSGS, MLB_GEARBOX_MSGS, MLB_MSG_ACC_10,
MLB_MSG_GATEWAY_05, MLB_MSG_LH_EPS_03, NetworkLocation, RADAR_DISABLE_STATE, TransmissionType,
VolkswagenFlags, VolkswagenSafetyFlags, VolkswagenFlagsIQ, get_longitudinal_stopping_speed_override,
)
from iqdbc.car.volkswagen.radar_interface import RadarInterface
import sys
import os
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
try:
from iqpilot.system.proprietary_runtime._verified_import import import_verified_module
except Exception:
import_verified_module = None
class CarInterface(CarInterfaceBase):
CarState = CarState
CarController = CarController
RadarInterface = RadarInterface
DRIVABLE_GEARS = (structs.CarState.GearShifter.eco, structs.CarState.GearShifter.sport,
structs.CarState.GearShifter.manumatic, structs.CarState.GearShifter.neutral)
@staticmethod
def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
ret.brand = "volkswagen"
ret.radarUnavailable = True
_params = Params()
angle_lat_enabled = _params.get_bool("AngleLateralControl")
joystick_mode = _params.get_bool("JoystickDebugMode")
if ret.flags & VolkswagenFlags.PQ:
# Set global PQ35/PQ46/NMS parameters
safety_configs = [get_safety_config(structs.CarParams.SafetyModel.volkswagenPq)]
if candidate == CAR.SEAT_ALHAMBRA_MK1:
ret.flags |= VolkswagenFlagsIQ.IQ_PQ_TIMEBOMB.value
if not (ret.flags & VolkswagenFlagsIQ.IQ_PQ_TIMEBOMB) and _params.get_bool("VwPqEpsPatched"):
ret.minSteerSpeed = 0
if angle_lat_enabled:
ret.flags |= VolkswagenFlagsIQ.IQ_LVBS_ALC_MODULE.value
safety_configs[0].safetyParam |= VolkswagenSafetyFlags.PQ_ALC_MODULE.value
if alpha_long:
ret.flags |= VolkswagenFlagsIQ.IQ_PQ_SNG_ECD.value
safety_configs[0].safetyParam |= VolkswagenSafetyFlags.PQ_SNG_ECD.value
ret.enableBsm = 0x3BA in fingerprint[0] # SWA_1
if 0x440 in fingerprint[0] or docs: # Getriebe_1
ret.transmissionType = TransmissionType.automatic
else:
ret.transmissionType = TransmissionType.manual
# Auto-detect CC only mode by checking for ACC / AWV presence
# ACC_System = 0x368, ACC_GRA_Anzeige = 0x56A, AWV = 0x366
has_acc = 0x368 in fingerprint[0] or 0x56A in fingerprint[0]
if not has_acc:
has_radar = 0x366 in fingerprint[0] # AWV for FCW/AEB
if has_radar:
ret.flags |= VolkswagenFlagsIQ.IQ_CC_ONLY.value
else:
ret.flags |= VolkswagenFlagsIQ.IQ_CC_ONLY_NO_RADAR.value
cc_only_flags = VolkswagenFlagsIQ.IQ_CC_ONLY | VolkswagenFlagsIQ.IQ_CC_ONLY_NO_RADAR
if ret.flags & cc_only_flags:
safety_configs[0].safetyParam |= VolkswagenSafetyFlags.PQ_NO_CAM_BUS.value
if (ret.flags & cc_only_flags) and not fingerprint[0]:
ret.flags |= VolkswagenFlagsIQ.IQ_PQ_LOWLINE.value
safety_configs[0].safetyParam |= VolkswagenSafetyFlags.PQ_LOWLINE.value
if any(msg in fingerprint[1] for msg in (0x1A0, 0xC2)): # Bremse_1, Lenkwinkel_1
ret.networkLocation = NetworkLocation.gateway
else:
ret.networkLocation = NetworkLocation.fwdCamera
ret.dashcamOnly = False
elif ret.flags & VolkswagenFlags.MLB:
# Set global MLB parameters
safety_configs = [get_safety_config(structs.CarParams.SafetyModel.volkswagenMlb)]
ret.enableBsm = 0x30F in fingerprint[0] # SWA_01
ret.networkLocation = NetworkLocation.gateway
ret.transmissionType = TransmissionType.automatic
ret.dashcamOnly = False
ecan_msgs = fingerprint[0] | fingerprint[2]
fingerprinted = bool(ecan_msgs or fingerprint[1])
if fingerprinted and not ecan_msgs:
ret.flags |= VolkswagenFlagsIQ.IQ_MLB_NO_ECAN.value
safety_configs[0].safetyParam |= VolkswagenSafetyFlags.MLB_NO_ECAN.value
ret.dashcamOnly = True
has_acc = any(msg in ecan_msgs for msg in MLB_ACC_COORDINATOR_MSGS)
if fingerprinted and not has_acc:
if MLB_MSG_ACC_10 in ecan_msgs:
ret.flags |= VolkswagenFlagsIQ.IQ_CC_ONLY.value
else:
ret.flags |= VolkswagenFlagsIQ.IQ_CC_ONLY_NO_RADAR.value
pt_msgs = fingerprint[1] if ret.flags & VolkswagenFlagsIQ.IQ_MLB_NO_ECAN else ecan_msgs
if fingerprinted and MLB_MSG_LH_EPS_03 not in pt_msgs:
ret.flags |= VolkswagenFlagsIQ.IQ_MLB_NO_HCA_EPS.value
all_msgs = ecan_msgs | fingerprint[1]
gearbox_seen = any(msg in all_msgs for msg in MLB_GEARBOX_MSGS)
transmission_ecu = any(fw.ecu == structs.CarParams.Ecu.transmission for fw in car_fw)
reverse_switch_seen = MLB_MSG_GATEWAY_05 in fingerprint[1]
if fingerprinted and not gearbox_seen and not transmission_ecu and reverse_switch_seen:
ret.transmissionType = TransmissionType.manual
elif ret.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
if ret.flags & VolkswagenFlags.MEB:
safety_configs = [get_safety_config(structs.CarParams.SafetyModel.volkswagenMeb)]
else:
safety_configs = [get_safety_config(structs.CarParams.SafetyModel.volkswagenMqbEvo)]
if ret.flags & VolkswagenFlags.MEB_GEN2:
safety_configs[0].safetyParam |= VolkswagenSafetyFlags.ALT_CRC_VARIANT_1.value
if ret.flags & VolkswagenFlags.MQB_EVO:
safety_configs[0].safetyParam |= VolkswagenSafetyFlags.NO_GAS_OFFSET.value
ret.enableBsm = 0x24C in fingerprint[0] # MEB_Side_Assist_01
ret.transmissionType = TransmissionType.direct
ret.steerControlType = structs.CarParams.SteerControlType.curvatureDEPRECATED
ret.steerAtStandstill = True
if any(msg in fingerprint[1] for msg in (0x520, 0x86, 0xFD, 0x13D)): # Airbag_02, LWI_01, ESP_21, QFK_01
ret.networkLocation = NetworkLocation.gateway
else:
ret.networkLocation = NetworkLocation.fwdCamera
if ret.networkLocation == NetworkLocation.gateway:
ret.radarUnavailable = False
if ret.networkLocation == NetworkLocation.fwdCamera:
ret.flags |= VolkswagenFlags.DISABLE_RADAR.value
safety_configs[0].safetyParam |= VolkswagenSafetyFlags.DISABLE_RADAR.value
if ret.flags & VolkswagenFlags.MQB_EVO and 0x30B in fingerprint[0]:
ret.flags |= VolkswagenFlags.KOMBI_PRESENT.value
if 0x25D in fingerprint[0]: # KLR_01
ret.flags |= VolkswagenFlags.STOCK_KLR_PRESENT.value
if all(msg in fingerprint[1] for msg in (0x462, 0x463, 0x464)): # PSD_04, PSD_05, PSD_06
ret.flags |= VolkswagenFlags.STOCK_PSD_PRESENT.value
if 0x464 in fingerprint[0]: # PSD_06
ret.flags |= VolkswagenFlags.STOCK_PSD_06_PRESENT.value
if 0x6B2 in fingerprint[0]: # Diagnose_01
ret.flags |= VolkswagenFlags.STOCK_DIAGNOSE_01_PRESENT.value
if 0x3DC in fingerprint[0]: # Gateway_73
ret.flags |= VolkswagenFlags.ALT_GEAR.value
else:
# Set global MQB parameters
safety_configs = [get_safety_config(structs.CarParams.SafetyModel.volkswagen)]
ret.enableBsm = 0x30F in fingerprint[0] # SWA_01
if 0xAD in fingerprint[0] or docs: # Getriebe_11
ret.transmissionType = TransmissionType.automatic
elif 0x187 in fingerprint[0]: # Motor_EV_01
ret.transmissionType = TransmissionType.direct
else:
ret.transmissionType = TransmissionType.manual
if any(msg in fingerprint[1] for msg in (0x40, 0x86, 0xB2, 0xFD)): # Airbag_01, LWI_01, ESP_19, ESP_21
ret.networkLocation = NetworkLocation.gateway
else:
ret.networkLocation = NetworkLocation.fwdCamera
if 0x126 in fingerprint[2]: # HCA_01
ret.flags |= VolkswagenFlags.STOCK_HCA_PRESENT.value
if 0x6B8 in fingerprint[0]: # Kombi_03
ret.flags |= VolkswagenFlags.KOMBI_PRESENT.value
# Auto-detect CC only mode by checking for ACC_06/ACC_07 presence
# ACC_06 = 0x122, ACC_07 = 0x12E, ACC_10 = 0x117
has_acc = 0x122 in fingerprint[0] or 0x12E in fingerprint[0]
if not has_acc:
has_radar = 0x117 in fingerprint[0] # ACC_10 for FCW/AEB
if has_radar:
ret.flags |= VolkswagenFlagsIQ.IQ_CC_ONLY.value
else:
ret.flags |= VolkswagenFlagsIQ.IQ_CC_ONLY_NO_RADAR.value
# Global lateral tuning defaults, can be overridden per-vehicle
ret.steerLimitTimer = 0.4
if ret.flags & VolkswagenFlags.PQ:
ret.steerActuatorDelay = 0.2
ret.longitudinalTuning.kf = 1.2
ret.longitudinalTuning.kpBP = [0.]
ret.longitudinalTuning.kpV = [.45]
ret.longitudinalTuning.kiBP = [0.]
ret.longitudinalTuning.kiV = [.69]
ret.longitudinalActuatorDelay = 0.6
if angle_lat_enabled:
ret.steerControlType = structs.CarParams.SteerControlType.angle
ret.steerAtStandstill = bool(joystick_mode)
else:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif ret.flags & VolkswagenFlags.MLB:
ret.steerActuatorDelay = 0.2
if angle_lat_enabled:
ret.steerControlType = structs.CarParams.SteerControlType.angle
ret.steerAtStandstill = bool(joystick_mode)
else:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif ret.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
ret.steerActuatorDelay = 0.3
else:
ret.steerActuatorDelay = 0.1
if angle_lat_enabled:
ret.steerControlType = structs.CarParams.SteerControlType.angle
ret.steerAtStandstill = bool(joystick_mode)
else:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
# Global longitudinal tuning defaults, can be overridden per-vehicle
if ret.flags & VolkswagenFlags.MEB:
ret.longitudinalActuatorDelay = 0.5
ret.radarDelay = 0.8
ret.longitudinalTuning.kiBP = [0., 30.]
ret.longitudinalTuning.kiV = [0.4, 0.]
ret.alphaLongitudinalAvailable = ret.networkLocation == NetworkLocation.gateway or docs or bool(ret.flags & VolkswagenFlags.DISABLE_RADAR)
if ret.flags & VolkswagenFlags.MLB and ret.flags & (VolkswagenFlagsIQ.IQ_CC_ONLY | VolkswagenFlagsIQ.IQ_CC_ONLY_NO_RADAR):
ret.alphaLongitudinalAvailable = False
alpha_long = False
if alpha_long:
# Proof-of-concept, prep for E2E only. No radar points available. Panda ALLOW_DEBUG firmware required.
ret.openpilotLongitudinalControl = True
safety_configs[0].safetyParam |= VolkswagenSafetyFlags.LONG_CONTROL.value
safety_configs[0].safetyParam |= VolkswagenSafetyFlags.ALLOW_LONG_ACCEL_WITH_GAS_PRESSED.value
if ret.transmissionType == TransmissionType.manual:
ret.minEnableSpeed = 4.5
# Per-vehicle overrides
if candidate == CAR.PORSCHE_MACAN_MK1:
ret.steerActuatorDelay = 0.07
if candidate in (CAR.VOLKSWAGEN_PASSAT_B7, CAR.SEAT_ALHAMBRA_MK1):
ret.flags |= VolkswagenFlagsIQ.IQ_PQ_ACC_FTS_EPB.value
safety_configs[0].safetyParam |= VolkswagenSafetyFlags.PQ_ACC_FTS_EPB.value
ret.pcmCruise = not ret.openpilotLongitudinalControl
ret.stopAccel = -0.55
ret.autoResumeSng = ret.minEnableSpeed == -1
CAN = CanBus(fingerprint=fingerprint)
if CAN.pt >= 4:
safety_configs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput))
ret.safetyConfigs = safety_configs
return ret
@staticmethod
def pre_init(CP: structs.CarParams, CP_IQ: structs.IQCarParams, can_recv, can_send):
if not (CP.flags & VolkswagenFlags.PQ) or (CP.flags & VolkswagenFlagsIQ.IQ_PQ_TIMEBOMB) or import_verified_module is None:
return
try:
params = Params()
if params.get_bool("VwPqEpsPatched"):
return
flasher = import_verified_module("iqpilot_hephaestusd_private", "iqpilot_private.konn3kt.hephaestus.vw_pq_flasher")
status = flasher.check_eps_patch_status(1, can_recv, can_send)
except Exception:
return
if status == "patched":
params.put_bool("VwPqEpsPatched", True)
@staticmethod
def init(CP: structs.CarParams, CP_IQ: structs.IQCarParams, can_recv, can_send):
# Disable radar via UDS programming session so openpilot can take over longitudinal.
# The radar stops transmitting AWV_03/Strukturen_01 and carcontroller replaces those messages.
if CP.openpilotLongitudinalControl and (CP.flags & VolkswagenFlags.DISABLE_RADAR):
RADAR_DISABLE_STATE["error"] = False
if CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
if CarInterface._is_engine_state_allowed_meb(can_recv):
carlog.warning("VW MEB/MQBevo: disabling radar for longitudinal control")
if not CarInterface._radar_communication_control(CP, can_recv, can_send):
RADAR_DISABLE_STATE["error"] = True
else:
RADAR_DISABLE_STATE["error"] = True
carlog.warning("VW MEB/MQBevo: radar cannot be disabled — engine is on")
@staticmethod
def deinit(CP: structs.CarParams, can_recv, can_send):
# Re-enable radar TX on exit (currently never called by openpilot, car recovers after ignition cycle)
if CP.openpilotLongitudinalControl and (CP.flags & VolkswagenFlags.DISABLE_RADAR):
if CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
CarInterface._radar_communication_control(CP, can_recv, can_send, disable=False)
@staticmethod
def _radar_communication_control(CP, can_recv, can_send, disable=True) -> bool:
# Send UDS commands to put the radar (addr 0x757) into programming session,
# which silences its CAN TX so openpilot can send replacement messages.
bus = CanBus(CP).pt
addr_radar = 0x757
addr_diag = 0x700 # Functional address for TesterPresent broadcast
vw_rx_offset = 0x6A
tp_req = bytes([uds.SERVICE_TYPE.TESTER_PRESENT, 0x00])
tp_resp = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40, 0x00])
ext_diag_req = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC])
ext_diag_resp = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40, uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC])
flash_req = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL, uds.SESSION_TYPE.PROGRAMMING])
empty_resp = b''
txt = "disable" if disable else "enable"
for attempt in range(3):
try:
if disable:
# Step 1: TesterPresent — wake up the radar
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr_radar, None)],
[tp_req], [tp_resp], vw_rx_offset, functional_addrs=[addr_diag])
if not query.get_data(0.5):
carlog.warning(f"VW radar {txt}: TesterPresent no response on attempt {attempt + 1}")
continue
# Step 2: Extended diagnostic session
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr_radar, None)],
[ext_diag_req], [ext_diag_resp], vw_rx_offset)
if not query.get_data(0.5):
carlog.warning(f"VW radar {txt}: ExtendedDiagSession no response on attempt {attempt + 1}")
continue
# Step 3: Programming session — radar stops transmitting
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr_radar, None)],
[flash_req], [empty_resp], vw_rx_offset)
query.get_data(0) # fire-and-forget, no wait needed
carlog.warning(f"VW radar {txt}: programming session sent on attempt {attempt + 1}")
return True
except Exception as e:
carlog.error(f"VW radar {txt}: exception on attempt {attempt + 1}: {repr(e)}")
continue
carlog.error(f"VW radar {txt}: all attempts failed")
return False
@staticmethod
def _is_engine_state_allowed_meb(can_recv, timeout: float = 0.5) -> bool:
# Read Motor_54 (0x14C) to check Engine_On bit before attempting radar disable.
# Programming session is rejected by radar when engine is running.
end_time = time.monotonic() + timeout
while time.monotonic() < end_time:
packets = can_recv(wait_for_one=True) or []
for packet in packets:
for msg in packet:
if msg.address != 0x14C:
continue
engine_on = bool((msg.dat[9] >> 5) & 0x01)
if engine_on:
carlog.warning(f"VW radar disable: engine is on, skipping")
return False
else:
carlog.warning(f"VW radar disable: engine is off, proceeding")
return True
carlog.warning("VW radar disable: Motor_54 not seen within timeout, assuming allowed")
return True
@staticmethod
def _get_params_iq(stock_cp: structs.CarParams, ret: structs.IQCarParams, candidate, fingerprint: dict[int, dict[int, int]], car_fw: list[structs.CarParams.CarFw], alpha_long: bool, is_release_iq: bool, docs: bool) -> structs.IQCarParams:
ret.longitudinalStoppingSpeedOverride = get_longitudinal_stopping_speed_override(candidate, stock_cp.flags)
override_excluded_platforms = VolkswagenFlags.MLB | VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO
ret.longActiveWithGasOverride = alpha_long and not bool(stock_cp.flags & override_excluded_platforms)
return ret

401
artifacts/package_runtime/iqdbc/car/volkswagen/mebcan.py Normal file
View File

@@ -0,0 +1,401 @@
from iqdbc.car.volkswagen.mebutils import map_speed_to_acc_tempolimit
from iqdbc.car.volkswagen.values import VolkswagenFlags
from iqdbc.car.volkswagen.speed_limit_manager import PSD_TYPE_CURV_SPEED
from iqdbc.car.common.conversions import Conversions as CV
ACCEL_INACTIVE = 3.01
ACCEL_OVERRIDE = 0.00
ACC_CTRL_ERROR = 6
ACC_CTRL_OVERRIDE = 4
ACC_CTRL_ACTIVE = 3
ACC_CTRL_ENABLED = 2
ACC_CTRL_DISABLED = 0
ACC_HMS_RAMP_RELEASE = 5
ACC_HMS_RELEASE = 4
ACC_HMS_HOLD = 1
ACC_HMS_NO_REQUEST = 0
ACC_HMS_RAMP_FRAMES = 5
ACC_HMS_RELEASE_SPEED = 5 * CV.KPH_TO_MS
ACC_HUD_ERROR = 6
ACC_HUD_OVERRIDE = 4
ACC_HUD_ACTIVE = 3
ACC_HUD_ENABLED = 2
ACC_HUD_DISABLED = 0
def create_steering_control(packer, bus, apply_curvature, lkas_enabled, power):
values = {
"Curvature": abs(apply_curvature), # in rad/m
"Curvature_VZ": 1 if apply_curvature > 0 and lkas_enabled else 0,
"Power": power if lkas_enabled else 0,
"RequestStatus": 4 if lkas_enabled else 2,
"HighSendRate": lkas_enabled,
}
return packer.make_can_msg("HCA_03", bus, values)
def create_eps_update(packer, bus, eps_stock_values, ea_simulated_torque):
values = {s: eps_stock_values[s] for s in [
"COUNTER", # Sync counter value to EPS output
"EPS_Lenkungstyp", # EPS rack type
"EPS_Berechneter_LW", # Absolute raw steering angle
"EPS_VZ_BLW", # Raw steering angle sign
"EPS_HCA_Status", # EPS HCA control status
]}
values.update({
# Absolute driver torque input and sign, with EA inactivity mitigation
"EPS_Lenkmoment": abs(ea_simulated_torque),
"EPS_VZ_Lenkmoment": 1 if ea_simulated_torque < 0 else 0,
})
return packer.make_can_msg("LH_EPS_03", bus, values)
def create_blinker_control(packer, bus, ea_hud_stock_values, ea_control_stock_values, left_blinker, right_blinker, hide_error, counter=None):
values = {s: ea_hud_stock_values[s] for s in [
"COUNTER",
"EA_Texte",
"ACF_Lampe_Hands_Off",
"EA_Infotainment_Anf",
"EA_Tueren_Anf",
"EA_Innenraumlicht_Anf",
"zFAS_Warnblinken",
"STP_Primaeranz",
"EA_Bremslichtblinken",
"EA_Blinken",
"EA_Unknown",
]}
if counter is not None:
values["COUNTER"] = counter
if ea_hud_stock_values["EA_Blinken"] == 0:
values.update({
"EA_Blinken": 1 if left_blinker else (2 if right_blinker else ea_hud_stock_values["EA_Blinken"]),
})
if hide_error and ea_control_stock_values["EA_Funktionsstatus"] in (0, 1, 7, 8):
values.update({
"EA_Texte": 0,
"EA_Unknown": 1,
})
return packer.make_can_msg("EA_02", bus, values)
def create_lka_hud_control(packer, bus, ldw_stock_values, lat_active, steering_pressed, hud_alert, hud_control, sound_alert):
display_mode = 1 if lat_active else 0 # travel assist style showing yellow lanes when op is active
values = {}
if len(ldw_stock_values):
values = {s: ldw_stock_values[s] for s in [
"LDW_SW_Warnung_links", # Blind spot in warning mode on left side due to lane departure
"LDW_SW_Warnung_rechts", # Blind spot in warning mode on right side due to lane departure
"LDW_Seite_DLCTLC", # Direction of most likely lane departure (left or right)
"LDW_DLC", # Lane departure, distance to line crossing
"LDW_TLC", # Lane departure, time to line crossing
]}
values.update({
"LDW_Gong": sound_alert,
"LDW_Status_LED_gelb": 1 if lat_active and steering_pressed else 0,
"LDW_Status_LED_gruen": 1 if lat_active and not steering_pressed else 0,
"LDW_Lernmodus_links": 3 + display_mode if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible + display_mode,
"LDW_Lernmodus_rechts": 3 + display_mode if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible + display_mode,
"LDW_Texte": hud_alert,
})
return packer.make_can_msg("LDW_02", bus, values)
def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resume=False, up=False, down=False, set_button=False):
values = {s: gra_stock_values[s] for s in [
"GRA_Hauptschalter", # ACC button, on/off
"GRA_Typ_Hauptschalter", # ACC main button type
"GRA_Codierung", # ACC button configuration/coding
"GRA_Tip_Stufe_2", # unknown related to stalk type
"GRA_ButtonTypeInfo", # unknown related to stalk type
]}
values.update({
"COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
"GRA_Abbrechen": cancel,
"GRA_Tip_Wiederaufnahme": resume or up,
"GRA_Tip_Setzen": down,
})
return packer.make_can_msg("GRA_ACC_01", bus, values)
def create_capacitive_wheel_touch(packer, bus, lat_active, klr_stock_values):
values = {s: klr_stock_values[s] for s in [
"COUNTER",
"KLR_Touchintensitaet_1",
"KLR_Touchintensitaet_2",
"KLR_Touchintensitaet_3",
"KLR_Touchauswertung",
]}
if lat_active:
values.update({
"COUNTER": (klr_stock_values["COUNTER"] + 1) % 16,
"KLR_Touchintensitaet_1": 80,
"KLR_Touchintensitaet_2": 200,
"KLR_Touchintensitaet_3": 10,
"KLR_Touchauswertung": 10,
})
return packer.make_can_msg("KLR_01", bus, values)
def acc_control_value(main_switch_on, acc_faulted, long_active, override):
if acc_faulted:
acc_control = ACC_CTRL_ERROR # error state
elif long_active:
if override:
acc_control = ACC_CTRL_OVERRIDE # overriding
else:
acc_control = ACC_CTRL_ACTIVE # active long control state
elif main_switch_on:
acc_control = ACC_CTRL_ENABLED # long control ready
else:
acc_control = ACC_CTRL_DISABLED # long control deactivated state
return acc_control
def acc_hold_type(main_switch_on, acc_faulted, long_active, starting, stopping, esp_hold, v_ego,
prev_acc_hold_type, ramp_counter):
# warning: car is reacting to hold mechanic even with long control off
# HALTEN or ANFAHREN straight to KEINE_ANFORDERUNG faults the car into park, so a ramp always sits
# in between: going inactive ramps for a fixed time, and a release while engaged ramps until the
# car is actually rolling
active = long_active and not acc_faulted
if not active:
if ramp_counter > 0:
acc_hold_type = ACC_HMS_RAMP_RELEASE
ramp_counter -= 1
else:
acc_hold_type = ACC_HMS_NO_REQUEST
else:
was_active = ramp_counter == ACC_HMS_RAMP_FRAMES
ramp_counter = ACC_HMS_RAMP_FRAMES
if stopping:
acc_hold_type = ACC_HMS_HOLD
elif starting:
acc_hold_type = ACC_HMS_RELEASE
else:
releasing = was_active and prev_acc_hold_type in (ACC_HMS_HOLD, ACC_HMS_RELEASE, ACC_HMS_RAMP_RELEASE)
if releasing and v_ego < ACC_HMS_RELEASE_SPEED:
acc_hold_type = ACC_HMS_RAMP_RELEASE
else:
acc_hold_type = ACC_HMS_NO_REQUEST
return acc_hold_type, ramp_counter
def create_acc_accel_control(packer, bus, CP, acc_type, acc_enabled, upper_jerk, lower_jerk, upper_control_limit, lower_control_limit,
accel, acc_control, acc_hold_type, stopping, starting, esp_hold, speed, override, travel_assist_available):
# active longitudinal control disables one pedal driving (regen mode) while using overriding mechnism
# error mitigation when stopping or stopped: (newer gen cars can be very sensitive)
# - send 0 m stopping distance for cars in kind of parameterized stopping mode (stopping accel -0.2 seen for those cars)
# -> this mode is seen for different cars with same firmware radars so could be a coded operational mode
# - jerk and control limits values set to 0 when fully stopped
# - set accel to 0 / no stop accel for full stop (seems to be compatible with old (non 0 stop accel) and new gen, because HMS state holds the car anyways)
# - stopping command sent as long as actually stopping
commands = []
terminal_rollout = 0.5 if CP.flags & VolkswagenFlags.MQB_EVO else 0
full_stop = stopping and esp_hold
full_stop_no_start = esp_hold and not starting
actually_stopping = stopping and not esp_hold
if acc_enabled:
if override: # the car expects a non inactive accel while overriding
acceleration = ACCEL_OVERRIDE # original ACC still sends active accel in this case (seamless experience)
elif full_stop:
acceleration = ACCEL_INACTIVE # inactive accel, newer gen >2024 error of not neutral value
else:
acceleration = accel
else:
acceleration = ACCEL_INACTIVE # inactive accel
values = {
"ACC_Typ": acc_type,
"ACC_Status_ACC": acc_control,
"ACC_StartStopp_Info": acc_enabled,
"ACC_Sollbeschleunigung_02": acceleration,
"ACC_zul_Regelabw_unten": lower_control_limit if acc_control in (ACC_CTRL_ACTIVE, ACC_CTRL_OVERRIDE) and not full_stop_no_start else 0,
"ACC_zul_Regelabw_oben": upper_control_limit if acc_control in (ACC_CTRL_ACTIVE, ACC_CTRL_OVERRIDE) and not full_stop_no_start else 0,
"ACC_neg_Sollbeschl_Grad_02": lower_jerk if acc_control in (ACC_CTRL_ACTIVE, ACC_CTRL_OVERRIDE) and not full_stop_no_start else 0,
"ACC_pos_Sollbeschl_Grad_02": upper_jerk if acc_control in (ACC_CTRL_ACTIVE, ACC_CTRL_OVERRIDE) and not full_stop_no_start else 0,
"ACC_Anfahren": starting,
"ACC_Anhalten": 1 if actually_stopping else 0,
"ACC_Anhalteweg": terminal_rollout if actually_stopping else 20.46, # if used the car can execute a hard brake probably if target is too close
"ACC_Anforderung_HMS": acc_hold_type,
"ACC_AKTIV_regelt": 1 if acc_control == ACC_CTRL_ACTIVE else 0,
"Speed": speed,
"SET_ME_0XFE": 0xFE,
"SET_ME_0X1": 0x1,
"SET_ME_0X9": 0x9,
}
if CP.flags & VolkswagenFlags.MEB_GEN2:
values.update({
"SET_ME_0x2FE": 0x2FE, # unclear if neccessary
})
commands.append(packer.make_can_msg("ACC_18", bus, values))
if travel_assist_available:
# satisfy car to prevent errors when pressing Travel Assist Button
values_ta = {
"Travel_Assist_Status": 4 if acc_enabled else 2,
"Travel_Assist_Request": 0,
"Travel_Assist_Available": 1,
}
commands.append(packer.make_can_msg("TA_01", bus, values_ta))
return commands
def acc_hud_status_value(main_switch_on, acc_faulted, long_active, override):
if acc_faulted:
acc_hud_control = ACC_HUD_ERROR # error state
elif long_active:
if override:
acc_hud_control = ACC_HUD_OVERRIDE # overriding
else:
acc_hud_control = ACC_HUD_ACTIVE # active
elif main_switch_on:
acc_hud_control = ACC_HUD_ENABLED # inactive
else:
acc_hud_control = ACC_HUD_DISABLED # deactivated
return acc_hud_control
def acc_hud_event(acc_hud_control, esp_hold, speed_limit_predicative, speed_limit_predicative_type, speed_limit):
acc_event = 0
if esp_hold and acc_hud_control == ACC_HUD_ACTIVE:
acc_event = 3 # acc ready message at standstill
elif acc_hud_control in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) and speed_limit_predicative:
if speed_limit_predicative_type == PSD_TYPE_CURV_SPEED:
acc_event = 6 # acc limited by curve (predicative)
else:
acc_event = 4 # acc limited by speed limit by nav (predicative)
elif acc_hud_control in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) and speed_limit:
acc_event = 5 # acc limited by speed limit by camera (recently detected)
return acc_event
def get_desired_gap(distance_bars, desired_gap, current_gap_signal):
# mapping desired gap to correct signal of corresponding distance bar
gap = 0
if distance_bars == current_gap_signal:
gap = desired_gap
return gap
def create_acc_hud_control(packer, bus, acc_control, set_speed, lead_visible, distance_bars, show_distance_bars, esp_hold, distance, desired_gap, fcw_alert, acc_event, speed_limit):
values = {
"ACC_Status_ACC": acc_control,
"ACC_Tempolimit": map_speed_to_acc_tempolimit(speed_limit) if acc_control in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0, # display speed limits (message type defined by ACC_Events)
"ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36,
"ACC_Gesetzte_Zeitluecke": distance_bars, # 5 distance bars available (3 are used by OP)
"ACC_Display_Prio": 0 if fcw_alert and acc_control in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 1, # probably keeping warning in front
"ACC_Optischer_Fahrerhinweis": 1 if fcw_alert and acc_control in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0, # enables optical warning
"ACC_Akustischer_Fahrerhinweis": 3 if fcw_alert and acc_control in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0, # enables sound warning
"ACC_Texte_Zusatzanz_02": 11 if fcw_alert and acc_control in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0, # type of warning: Break!
"ACC_Abstandsindex_02": 569, # seems to be default for MEB but is not static in every case
"ACC_EGO_Fahrzeug": 2 if fcw_alert and acc_control in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else (1 if acc_control == ACC_HUD_ACTIVE else 0), # red car warn symbol for fcw
"Lead_Type_Detected": 1 if lead_visible else 0, # object should be displayed
"Lead_Type": 3 if lead_visible else 0, # displaying a car
"Lead_Distance": distance if lead_visible else 0, # hud distance of object
"ACC_Enabled": 1 if acc_control in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0,
"ACC_Standby_Override": 1 if acc_control != ACC_HUD_ACTIVE else 0,
"Street_Color": 1 if acc_control in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0, # light grey (1) or dark (0) street
"Lead_Brightness": 3 if acc_control == ACC_HUD_ACTIVE else 0, # object shows in colour
"ACC_Events": acc_event, # e.g. pACC Events
"ACC_Event_Wunschgeschw": speed_limit * CV.MS_TO_KPH, # this speed is shown for curve event speeds, not for speed signs (speed signs in "ACC_Tempolimit")
"Zeitluecke_1": get_desired_gap(distance_bars, desired_gap, 1), # desired distance to lead object for distance bar 1
"Zeitluecke_2": get_desired_gap(distance_bars, desired_gap, 2), # desired distance to lead object for distance bar 2
"Zeitluecke_3": get_desired_gap(distance_bars, desired_gap, 3), # desired distance to lead object for distance bar 3
"Zeitluecke_4": get_desired_gap(distance_bars, desired_gap, 4), # desired distance to lead object for distance bar 4
"Zeitluecke_5": get_desired_gap(distance_bars, desired_gap, 5), # desired distance to lead object for distance bar 5
"Zeitluecke_Farbe": 1 if acc_control in (ACC_HUD_ENABLED, ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0, # yellow (1) or white (0) time gap
"ACC_Anzeige_Zeitluecke": show_distance_bars if acc_control != ACC_HUD_DISABLED else 0, # show distance bar selection
"SET_ME_0X1": 0x1, # unknown
"SET_ME_0X6A": 0x6A, # unknown
"SET_ME_0XFFFF": 0xFFFF, # unknown
"SET_ME_0X7FFF": 0x7FFF, # unknown
}
return packer.make_can_msg("MEB_ACC_01", bus, values)
def create_ea_control(packer, bus):
values = {
"EA_Funktionsstatus": 1, # Configured but disabled
"EA_Sollbeschleunigung": 2046, # Inactive value
}
return packer.make_can_msg("EA_01", bus, values)
def create_ea_hud(packer, bus):
values = {
"EA_Unknown": 1, # Undocumented, value when inactive
}
return packer.make_can_msg("EA_02", bus, values)
def create_aeb_control(packer, bus, CP):
# Replacement for radar-generated AWV_03 when radar is disabled via UDS programming session.
# Sends inactive/safe default values so downstream ECUs don't fault.
values = {
"SET_ME_63": 63,
"SET_ME_30": 30,
"SET_ME_127": 127,
"SET_ME_127_2": 127,
"SET_ME_63_2": 63,
"SET_ME_15_1": 15,
"SET_ME_255": 255,
"SET_ME_1023": 1023,
"SET_ME_1": 1,
}
if CP.flags & VolkswagenFlags.MQB_EVO:
values.update({"SET_ME_1_2": 1})
return packer.make_can_msg("AWV_03", bus, values)
def create_aeb_hud(packer, bus, disabled):
# Replacement for radar-generated MEB_AWV_01 HUD message.
# When disabled=True (shortly after radar disable), shows AEB-unavailable state.
# When disabled=False (steady state), shows AEB active to prevent dash warnings.
values = {
"AWV_Enabled": not disabled,
"AWV_Init": 1,
"SET_ME_1": 1,
"SET_ME_511": 511,
}
return packer.make_can_msg("MEB_AWV_01", bus, values)
def create_radar_objects(packer, bus):
# Send empty MEB_Distance_01 (radar object list at 0x24F) to prevent ECU faults when radar is silent.
# Note: infinitecable2 calls this "Strukturen_01"; IQPilot DBC names it "MEB_Distance_01".
return packer.make_can_msg("MEB_Distance_01", bus, {})

274
artifacts/package_runtime/iqdbc/car/volkswagen/mebutils.py Normal file
View File

@@ -0,0 +1,274 @@
import numpy as np
from iqdbc.car.common.filter_simple import FirstOrderFilter
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.common.pid import PIDController
from iqdbc.car import DT_CTRL
class LongControlJerk():
JERK_LIMIT_MIN = 0.5
JERK_LIMIT_MIN_NO_LEAD = 0.7
JERK_LIMIT_MAX = 5.0
FILTER_GAIN_DISTANCE = [10, 50]
FILTER_GAIN_DISTANCE_CHANGE = [0, 20]
FILTER_GAIN_MAX = 0.95
FILTER_GAIN_MIN = 0.75
FILTER_GAIN_NO_LEAD = 0.95
def __init__(self, dt=DT_CTRL):
self.dy_up = 0.
self.dy_down = 0.
self.jerk_up = 0.
self.jerk_down = 0.
self.dt = dt
self.accel_last = 0.
self.distance_last = 0.
self.jerk_limit_min = self.JERK_LIMIT_MIN_NO_LEAD
def update(self, enabled, override, distance, has_lead, accel, critical_state):
# jerk limits by accel change and distance are used to improve comfort while ensuring a fast enough car reaction
# override mechanics reminder:
# (1) sending accel = 0 and directly setting jerk to zero results in round about steady accel until harder accel pedal press -> lack of control
# (2) sending accel = 0 and allowing a high jerk results in a abrupt accel cut -> lack of comfort
if not enabled:
self.jerk_up = 0.
self.jerk_down = 0.
self.dy_up = 0.
self.dy_down = 0.
elif override:
self.jerk_up = self.JERK_LIMIT_MIN
self.jerk_down = self.JERK_LIMIT_MIN
self.dy_up = 0.
self.dy_down = 0.
elif critical_state: # force best car reaction
self.jerk_up = self.JERK_LIMIT_MAX
self.jerk_down = self.JERK_LIMIT_MAX
self.dy_up = 0.
self.dy_down = 0.
else:
jerk_limit_min_target = self.JERK_LIMIT_MIN if has_lead else self.JERK_LIMIT_MIN_NO_LEAD # jerk limit min base line
jerk_limit_min_delta = abs(self.JERK_LIMIT_MIN_NO_LEAD - self.JERK_LIMIT_MIN) * self.dt
if self.jerk_limit_min < jerk_limit_min_target:
self.jerk_limit_min = min(self.jerk_limit_min + jerk_limit_min_delta, jerk_limit_min_target)
elif self.jerk_limit_min > jerk_limit_min_target:
self.jerk_limit_min = max(self.jerk_limit_min - jerk_limit_min_delta, jerk_limit_min_target)
if has_lead:
distance_change = (self.distance_last - distance) / self.dt if 0 not in (self.distance_last, distance) else 0
filter_gain_dist = np.interp(distance, self.FILTER_GAIN_DISTANCE, [self.FILTER_GAIN_MAX, self.jerk_limit_min]) # gain by distance
filter_gain_dist_change = np.interp(abs(distance_change), self.FILTER_GAIN_DISTANCE_CHANGE, [self.jerk_limit_min, self.FILTER_GAIN_MAX]) # gain by distance change
filter_gain = max(filter_gain_dist, filter_gain_dist_change) # use highest gain
else:
filter_gain = self.FILTER_GAIN_NO_LEAD
j = (accel - self.accel_last) / self.dt
tgt_up = abs(j) if j > 0 else 0.
tgt_down = abs(j) if j < 0 else 0.
# how fast does the car react to acceleration
self.dy_up += filter_gain * (tgt_up - self.jerk_up - self.dy_up)
self.jerk_up += self.dt * self.dy_up
self.jerk_up = np.clip(self.jerk_up, self.jerk_limit_min, self.JERK_LIMIT_MAX)
# how fast does the car react to braking
self.dy_down += filter_gain * (tgt_down - self.jerk_down - self.dy_down)
self.jerk_down += self.dt * self.dy_down
self.jerk_down = np.clip(self.jerk_down, self.jerk_limit_min, self.JERK_LIMIT_MAX)
self.accel_last = accel
self.distance_last = distance
def get_jerk_up(self):
return self.jerk_up
def get_jerk_down(self):
return self.jerk_down
class LongControlLimit():
LOWER_LIMIT_FACTOR = 0.024
LOWER_LIMIT_MAX = LOWER_LIMIT_FACTOR * 8
LOWER_LIMIT_MIN = LOWER_LIMIT_FACTOR * 2
UPPER_LIMIT_FACTOR = 0.0625
UPPER_LIMIT_MAX = UPPER_LIMIT_FACTOR * 3
LIMIT_MIN = 0.
LIMIT_DISTANCE = [10, 100] # limit range
LIMIT_DISTANCE_CHANGE_DOWN = [0, 20] # high precision for worst case high speed approaching a stopped lead
LIMIT_DISTANCE_CHANGE_UP = [0, 5] # precisely follow an accelerating lead especially from stop
LIMIT_DISTANCE_CHANGE_UP_ACT = [0, 60]
DISTANCE_FILTER_RC = [0.15, 0.6] # smooth noisy distance signal for distant leads
DISTANCE_TIMEOUT = 1. # seconds
def __init__(self, dt=DT_CTRL):
self.upper_limit = self.LIMIT_MIN
self.lower_limit = self.LIMIT_MIN
self.dt = dt
self.distance_last = 0.
self.distance_filter = FirstOrderFilter(0.0, rc=self.DISTANCE_FILTER_RC[0], dt=dt, initialized=False)
self.distance_valid_timer = 0
def update(self, enabled: bool, speed: float, set_speed: float, distance: float, has_lead: bool, critical_state: bool):
# control limits by distance are used to improve comfort while ensuring precise car reaction if neccessary
# also used to reduce an effect of decel overshoot when target is breaking
# limits are controlled mainly by distance of lead car
if not enabled or critical_state: # force most precise accel command execution
self.upper_limit = self.LIMIT_MIN
self.lower_limit = self.LIMIT_MIN
self.distance_valid_timer = 0
elif not has_lead:
if self.distance_valid_timer < self.DISTANCE_TIMEOUT: # fluctuation block: keep alive
self.distance_valid_timer += self.dt
else: # force most precise
self.upper_limit = self.LIMIT_MIN
self.lower_limit = self.LIMIT_MIN
else:
distance_change_raw = (self.distance_last - distance) / self.dt if 0 not in (self.distance_last, distance) else 0
distance_filter_rc = np.interp(distance, self.LIMIT_DISTANCE, self.DISTANCE_FILTER_RC)
if (self.distance_last == 0 or self.distance_valid_timer != 0) and distance != 0: # for new lead detection reset filter and correctly force current state upon next iteration
self.distance_filter = FirstOrderFilter(0.0, rc=distance_filter_rc, dt=self.dt, initialized=False)
distance_change = distance_change_raw
else:
self.distance_filter.update_alpha(distance_filter_rc)
distance_change = self.distance_filter.update(distance_change_raw)
self.distance_valid_timer = 0
# how far can the true accel vary downwards from requested accel
upper_limit_dist = np.interp(distance, self.LIMIT_DISTANCE, [self.LIMIT_MIN, self.UPPER_LIMIT_MAX]) # base line based on distance
upper_limit_dist_change = np.interp(-min(0, distance_change), self.LIMIT_DISTANCE_CHANGE_UP, [self.UPPER_LIMIT_MAX, self.LIMIT_MIN]) # limit by distance change up
upper_limit_dist_change = np.interp(distance, self.LIMIT_DISTANCE_CHANGE_UP_ACT, [upper_limit_dist_change, upper_limit_dist]) # distance change activation
self.upper_limit = min(upper_limit_dist, upper_limit_dist_change) # use lowest limit
# how far can the true accel vary upwards from requested accel
set_speed_diff_up = max(0, abs(speed) - abs(set_speed)) # set speed difference down requested by user or speed overshoot (includes hud - real speed difference!)
set_speed_diff_up_factor = np.interp(set_speed_diff_up, [1, 1.75], [1., 0.]) # faster requested speed decrease and less speed overshoot downhill
lower_limit_dist = np.interp(distance, self.LIMIT_DISTANCE, [self.LOWER_LIMIT_MIN, self.LOWER_LIMIT_MAX]) # base line based on distance
lower_limit_dist_speed = lower_limit_dist * set_speed_diff_up_factor
lower_limit_dist_change = np.interp(max(0, distance_change), self.LIMIT_DISTANCE_CHANGE_DOWN, [self.LOWER_LIMIT_MAX, self.LIMIT_MIN]) # limit by distance change down
self.lower_limit = min(lower_limit_dist_speed, lower_limit_dist_change) # use lowest limit
self.distance_last = distance
def get_upper_limit(self):
return self.upper_limit
def get_lower_limit(self):
return self.lower_limit
def sigmoid_curvature_boost_meb(kappa: float, v_ego: float, kappa_thresh: float = 0.0) -> float:
# compensate non linear behaviour: boost low curvatures
# this is either a model issue (nerfing low curvatures) or a specific steering rack behaviour
v_points = np.array([20.0, 40.0])
boost_values = np.array([1.5, 2.1]) # increase boost amplitude with speed
boost = float(np.interp(v_ego, v_points, boost_values))
steepness_values = np.array([5000.0, 3200.0]) # increase boost area with speed
steepness = float(np.interp(v_ego, v_points, steepness_values))
abs_kappa = abs(kappa)
boost_factor = 1.0 + (boost - 1.0) / (1 + np.exp(steepness * (abs_kappa - kappa_thresh)))
return np.sign(kappa) * abs_kappa * boost_factor
def map_speed_to_acc_tempolimit(v_ms):
acc_tempolimit_kph = { # DBC Mapping
1: 5, 2: 7, 3: 10, 4: 15, 5: 20, 6: 25, 7: 30, 8: 35,
9: 40, 10: 45, 11: 50, 12: 55, 13: 60, 14: 65, 15: 70,
16: 75, 17: 80, 18: 85, 19: 90, 20: 95, 21: 100, 22: 110,
23: 120, 24: 130, 25: 140, 26: 150, 27: 160, 28: 200,
30: 250
}
v_kph = int(round(v_ms * CV.MS_TO_KPH))
acc_value = 0
for val, limit in sorted(acc_tempolimit_kph.items()):
if v_kph >= limit:
acc_value = val
else:
break
return acc_value
def get_acc_warning_meb(self, acc_hud):
# this works as long our radar does not fault while using OP
if (acc_hud["ACC_Status_ACC"] in (3, 4) # ACC active or in override mode
and acc_hud["ACC_EGO_Fahrzeug"] == 2 # a warning for the lead car is active
and acc_hud["ACC_Optischer_Fahrerhinweis"] != 0 # there is an optical warning
and acc_hud["ACC_Akustischer_Fahrerhinweis"] != 0 # there is a sound warning
and acc_hud["ACC_Display_Prio"] == 0): # this warning has highest priority
return True
return False
class MultiplicativeUnwindPID(PIDController):
def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100, min_cmd=1e-10, ki_red_time=1.0):
super().__init__(k_p, k_i, k_f=k_f, k_d=k_d, pos_limit=pos_limit, neg_limit=neg_limit, rate=rate)
self.min_cmd = abs(min_cmd)
self.ki_red_time = float(ki_red_time)
self.rate = rate
self.override_prev = False
self.i_unwind_factor = 1.0
def _calc_unwind_factor(self, override):
if not override or self.override_prev:
return
if self.ki_red_time <= 0.0:
self.i_unwind_factor = 1.0
return
if abs(self.i) <= self.min_cmd:
self.i_unwind_factor = 0.0
return
steps = max(int(self.ki_red_time * self.rate), 1)
factor = (self.min_cmd / abs(self.i)) ** (1.0 / steps)
self.i_unwind_factor = min(factor, 1.0)
def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False):
self.speed = speed
self.p = float(error) * self.k_p
self.f = feedforward * self.k_f
self.d = error_rate * self.k_d
if override:
self._calc_unwind_factor(override)
self.i *= self.i_unwind_factor
if abs(self.i) < self.min_cmd:
self.i = 0.0
else:
if not freeze_integrator:
self.i = self.i + error * self.k_i * self.i_rate
# Clip i to prevent exceeding control limits
control_no_i = self.p + self.d + self.f
control_no_i = np.clip(control_no_i, self.neg_limit, self.pos_limit)
self.i = np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
control = self.p + self.i + self.d + self.f
self.control = np.clip(control, self.neg_limit, self.pos_limit)
self.override_prev = override
return self.control
class LatControlCurvature():
def __init__(self, pid_params, limit, rate):
self.pid = MultiplicativeUnwindPID((pid_params.kpBP, pid_params.kpV),
(pid_params.kiBP, pid_params.kiV),
k_f=pid_params.kf, pos_limit=limit, neg_limit=-limit,
rate=rate, min_cmd=6.7e-6, ki_red_time=2.0)
def reset(self):
self.pid.reset()
def update(self, CS, CC, desired_curvature):
actual_curvature_vm = CC.currentCurvature # includes roll
speed_floor = max(CS.vEgo, 0.1)
yaw_rate_pose = CC.angularVelocity[2] if len(CC.angularVelocity) > 2 else actual_curvature_vm * speed_floor
actual_curvature_pose = yaw_rate_pose / speed_floor
actual_curvature = np.interp(CS.vEgo, [2.0, 5.0], [actual_curvature_vm, actual_curvature_pose])
desired_curvature_corr = desired_curvature - CC.rollCompensation
error = desired_curvature - actual_curvature
freeze_integrator = CC.steerLimited or CS.vEgo < 5
output_curvature = self.pid.update(error, feedforward=desired_curvature_corr, speed=CS.vEgo,
freeze_integrator=freeze_integrator, override=CS.steeringPressed)
return output_curvature

126
artifacts/package_runtime/iqdbc/car/volkswagen/mlbcan.py Normal file
View File

@@ -0,0 +1,126 @@
from iqdbc.car.volkswagen.mqbcan import (volkswagen_mqb_meb_checksum, xor_checksum,
acc_hud_status_value as mqb_acc_hud_status_value,
create_lka_hud_control as mqb_create_lka_hud_control)
def create_hca_steering_control(packer, bus, apply_steer, HCA_Status):
values = {
"HCA_01_Status_HCA": HCA_Status,
"HCA_01_LM_Offset": abs(apply_steer),
"HCA_01_LM_OffSign": 1 if apply_steer < 0 else 0,
"HCA_01_Vib_Freq": 18,
"HCA_01_Sendestatus": 1 if HCA_Status in (5, 7) else 0,
}
return packer.make_can_msg("HCA_01", bus, values)
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control,
entering=False, special_mode=False, special_active=False):
return mqb_create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control,
entering, special_mode, special_active)
def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resume=False, set_button=False):
values = {s: gra_stock_values[s] for s in [
"LS_Hauptschalter",
"LS_Typ_Hauptschalter",
"LS_Codierung",
"LS_Tip_Stufe_2",
]}
values.update({
"COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
"LS_Abbrechen": cancel,
"LS_Tip_Wiederaufnahme": resume,
})
return packer.make_can_msg("LS_01", bus, values)
def acc_control_value(main_switch_on, long_active, cruiseOverride, accFaulted):
# ACC_01.ACC_Status_ACC shares the MQB ACC_06 enum, but a fault outranks active regulation on MLB
if cruiseOverride:
acc_control = 4
elif accFaulted:
acc_control = 6
elif long_active:
acc_control = 3
elif main_switch_on:
acc_control = 2
else:
acc_control = 0
return acc_control
def acc_hud_status_value(main_switch_on, acc_faulted, longActive, longOverride):
return mqb_acc_hud_status_value(main_switch_on, acc_faulted, longActive, longOverride)
def create_acc_accel_control(packer, bus, accel, acc_control, stopping):
acc_enabled = acc_control in (3, 4)
acc_01_values = {
"ACC_Status_ACC": acc_control,
"ACC_Sollbeschleunigung": accel if acc_enabled else 0,
"ACC_zul_Regelabw_unten": 0.2 if acc_enabled else 0,
"ACC_zul_Regelabw_oben": 0.2 if acc_enabled else 0,
"ACC_neg_Sollbeschl_Grad": 4.0 if acc_enabled else 0,
"ACC_pos_Sollbeschl_Grad": 4.0 if acc_enabled else 0,
"ACC_Dynamik": 3,
"ACC_Anhalten": stopping if acc_enabled else False,
"ACC_Minimale_Bremsung": 0,
}
return [packer.make_can_msg("ACC_01", bus, acc_01_values)]
def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, leadDistance, distanceBars, fcw_alert, leadVisible,
unavailable, decel, d_unresponsive, hud_text=0, desired_distance=8.0):
engaged = acc_hud_status in (3, 4)
priodisp = 0 if fcw_alert else 1 if (acc_hud_status == 4 or decel or leadVisible) else 2 if (acc_hud_status in (3, 2)) else 0
# The cluster renders the lead as a position on a fixed scale rather than a raw distance, with the
# set follow gap sitting at mid scale. The scale spans out to 1.5x the gap before it saturates.
if not engaged:
acc_distance_index = 1022
elif not leadVisible:
acc_distance_index = 1023
else:
distance_ratio = leadDistance / max(desired_distance, 1.0)
acc_distance_index = int(max(1, min(1021, round(490 * (3 - 2 * distance_ratio)))))
values = {
"ACC_Status_Anzeige": acc_hud_status, # 0 off, 1 init, 2 standby, 3 active, 4 overridden, 5 shutdown reaction, 6/7 fault
"ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36, # 327.36 (raw 1023) = "no display"
"ACC_Gesetzte_Zeitluecke": distanceBars, # 1 aggressive, 2 standard, 3 relaxed
"ACC_Anzeige_Zeitluecke": 1 if engaged else 0,
"ACC_Tachokranz": 1 if engaged else 0,
"ACC_Display_Prio": priodisp, # 0 highest prio, 1 medium, 2 low, 3 none
"ACC_Abstandsindex": acc_distance_index, # 1-1020 lead distance, 1021 emergency brake, 1022 ACC off, 1023 ACC on without lead
"ACC_Relevantes_Objekt": 2 if fcw_alert else (1 if leadVisible else 0), # lead car: 1 green, 2 red, 0 off
"ACC_Status_Prim_Anz": 2 if fcw_alert else (1 if engaged else 0), # ACC symbol: 1 green, 2 red, 3 yellow, 0 off
"ACC_Optischer_Fahrerhinweis": 1 if fcw_alert else 0,
"ACC_Akustik": 1 if (fcw_alert or d_unresponsive) else 0, # 0 none, 1 high prio, 2 low prio, 3 high prio continuous
"ACC_Texte_Primaeranz": hud_text,
}
return packer.make_can_msg("ACC_02", bus, values)
def volkswagen_mlb_checksum(address: int, sig, d: bytearray) -> int:
xor_starting_value = {
0x109: 0x08, # ACC_01
0x111: 0x10, # TSK_05
0x30C: 0x0F, # ACC_02
0x324: 0x27, # ACC_04
0x10B: 0xA, # LS_01
0x10D: 0x0C, # ACC_05
0x10F: 0x0E, # ACC_0x10F
0x311: 0x12, # ACC_0x311
0x397: 0x94, # LDW_02
0x10C: 0x0D, # TSK_02
}
if address in xor_starting_value:
return xor_checksum(address, sig, d, xor_starting_value[address])
else:
return volkswagen_mqb_meb_checksum(address, sig, d)

326
artifacts/package_runtime/iqdbc/car/volkswagen/mqbcan.py Normal file
View File

@@ -0,0 +1,326 @@
from iqdbc.car.crc import CRC8H2F
def create_hca_steering_control(packer, bus, apply_torque, HCA_Status):
values = {
"HCA_01_Status_HCA": HCA_Status,
"HCA_01_LM_Offset": abs(apply_torque),
"HCA_01_LM_OffSign": 1 if apply_torque < 0 else 0,
"HCA_01_Vib_Freq": 18,
"HCA_01_Sendestatus": 1 if HCA_Status == 5 else 0,
"EA_ACC_Wunschgeschwindigkeit": 327.36,
}
return packer.make_can_msg("HCA_01", bus, values)
def create_eps_update(packer, bus, eps_stock_values, ea_simulated_torque):
values = {s: eps_stock_values[s] for s in [
"COUNTER", # Sync counter value to EPS output
"EPS_Lenkungstyp", # EPS rack type
"EPS_Berechneter_LW", # Absolute raw steering angle
"EPS_VZ_BLW", # Raw steering angle sign
"EPS_HCA_Status", # EPS HCA control status
]}
values.update({
# Absolute driver torque input and sign, with EA inactivity mitigation
"EPS_Lenkmoment": abs(ea_simulated_torque),
"EPS_VZ_Lenkmoment": 1 if ea_simulated_torque < 0 else 0,
})
return packer.make_can_msg("LH_EPS_03", bus, values)
def create_lka_hud_control(packer, bus, ldw_stock_values, lat_active, steering_pressed, hud_alert, hud_control, entering, special_mode=False, special_active=False):
values = {}
if len(ldw_stock_values):
values = {s: ldw_stock_values[s] for s in [
"LDW_SW_Warnung_links", # Blind spot in warning mode on left side due to lane departure
"LDW_SW_Warnung_rechts", # Blind spot in warning mode on right side due to lane departure
"LDW_Seite_DLCTLC", # Direction of most likely lane departure (left or right)
"LDW_DLC", # Lane departure, distance to line crossing
"LDW_TLC", # Lane departure, time to line crossing
]}
if entering:
yellow_led = int(steering_pressed)
green_led = int(not steering_pressed)
else:
yellow_led = 1 if lat_active and steering_pressed else 0
green_led = 1 if lat_active and not steering_pressed else 0
values.update({
"LDW_Status_LED_gelb": yellow_led,
"LDW_Status_LED_gruen": green_led,
"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_Texte": hud_alert,
})
return packer.make_can_msg("LDW_02", bus, values)
def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resume=False, set_button=False):
values = {s: gra_stock_values[s] for s in [
"GRA_Hauptschalter", # ACC button, on/off
"GRA_Typ_Hauptschalter", # ACC main button type
"GRA_Codierung", # ACC button configuration/coding
"GRA_Tip_Stufe_2", # unknown related to stalk type
"GRA_ButtonTypeInfo", # unknown related to stalk type
]}
values.update({
"COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
"GRA_Abbrechen": cancel,
"GRA_Tip_Wiederaufnahme": resume,
})
return packer.make_can_msg("GRA_ACC_01", bus, values)
def acc_control_value(main_switch_on, long_active, cruiseOverride, accFaulted):
if cruiseOverride:
acc_control = 4
elif long_active:
acc_control = 3
elif accFaulted:
acc_control = 6
elif main_switch_on:
acc_control = 2
else:
acc_control = 0
return acc_control
def acc_hud_status_value(main_switch_on, acc_faulted, longActive, longOverride):
if longOverride:
hud_status = 4
elif longActive:
hud_status = 3
elif acc_faulted:
hud_status = 6
elif main_switch_on:
hud_status = 2
else:
hud_status = 0
return hud_status
def create_acc_accel_control(packer, bus, acc_type, accel, acc_control, stopping, starting, esp_hold, comfortBand, jerkLimit, eBrakeActive, esp_starting_override=None, esp_stopping_override=None):
commands = []
acc_enabled = acc_control in (3, 4)
acc_06_values = {
"ACC_Typ": acc_type,
"ACC_Status_ACC": acc_control,
"ACC_StartStopp_Info": acc_enabled,
"ACC_Sollbeschleunigung_02": accel if acc_enabled else 3.01,
"ACC_zul_Regelabw_unten": 0.2,
"ACC_zul_Regelabw_oben": 0.2,
"ACC_neg_Sollbeschl_Grad_02": 3.0 if acc_enabled else 0,
"ACC_pos_Sollbeschl_Grad_02": 3.0 if acc_enabled else 0,
"ACC_Anfahren": starting if acc_enabled else False,
"ACC_Anhalten": stopping if acc_enabled else False,
}
commands.append(packer.make_can_msg("ACC_06", bus, acc_06_values))
acc_07_starting = starting if esp_starting_override is None else esp_starting_override
acc_07_stopping = stopping if esp_stopping_override is None else esp_stopping_override
if acc_07_starting:
acc_hold_type = 4 # hold release / startup
elif esp_hold:
acc_hold_type = 3 # hold standby
elif acc_07_stopping:
acc_hold_type = 1 # hold request
else:
acc_hold_type = 0
acc_07_values = {
"ACC_Anhalteweg": 0.3 if acc_07_stopping and acc_enabled else 20.46, # Distance to stop (stopping coordinator handles terminal roll-out)
"ACC_Freilauf_Info": 2 if acc_enabled else 0,
"ACC_Folgebeschl": 3.02, # Not using secondary controller accel unless and until we understand its impact
"ACC_Sollbeschleunigung_02": accel if acc_enabled else 3.01,
"ACC_Anforderung_HMS": acc_hold_type,
"ACC_Anfahren": acc_07_starting if acc_enabled else False,
"ACC_Anhalten": acc_07_stopping if acc_enabled else False,
}
commands.append(packer.make_can_msg("ACC_07", bus, acc_07_values))
return commands
def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, leadDistance, distanceBars, fcw_alert, leadVisible, unavailable, decel, d_unresponsive):
priodisp = 0 if fcw_alert else 1 if (acc_hud_status == 4 or decel or leadVisible) else 2 if (acc_hud_status in (3, 2)) else 0
leadDistanceBars = distanceBars + 1 if distanceBars in (1, 2, 3) else 2
values = {
"ACC_Status_Anzeige": acc_hud_status,
"ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36,
"ACC_Gesetzte_Zeitluecke": leadDistanceBars,
"ACC_Optischer_Fahrerhinweis": 1 if fcw_alert else 0,
"ACC_Display_Prio": priodisp,
"ACC_Relevantes_Objekt": leadDistanceBars,
"ACC_Abstandsindex": leadDistance if leadVisible else 0,
"ACC_Akustik_02": fcw_alert,
}
return packer.make_can_msg("ACC_02", bus, values)
# AWV = Stopping Distance Reduction
# Refer to Self Study Program 890253: Volkswagen Driver Assistance Systems, Design and Function
def create_aeb_control(packer, fcw_active, aeb_active, accel):
values = {
"AWV_Vorstufe": 0, # Preliminary stage
"AWV1_Anf_Prefill": 0, # Brake pre-fill request
"AWV1_HBA_Param": 0, # Brake pre-fill level
"AWV2_Freigabe": 0, # Stage 2 braking release
"AWV2_Ruckprofil": 0, # Brake jerk level
"AWV2_Priowarnung": 0, # Suppress lane departure warning in favor of FCW
"ANB_Notfallblinken": 0, # Hazard flashers request
"ANB_Teilbremsung_Freigabe": 0, # Target braking release
"ANB_Zielbremsung_Freigabe": 0, # Partial braking release
"ANB_Zielbrems_Teilbrems_Verz_Anf": 0.0, # Acceleration requirement for target/partial braking, m/s/s
"AWV_Halten": 0, # Vehicle standstill request
"PCF_Time_to_collision": 0xFF, # Pre Crash Front, populated only with a target, might be used on Audi only
}
return packer.make_can_msg("ACC_10", 0, values)
def create_aeb_hud(packer, aeb_supported, fcw_active):
values = {
"AWV_Texte": 5 if aeb_supported else 7, # FCW/AEB system status, display text (from menu in VAL)
"AWV_Status_Anzeige": 1 if aeb_supported else 2, # FCW/AEB system status, available or disabled
}
return packer.make_can_msg("ACC_15", 0, values)
def volkswagen_mqb_meb_checksum(address: int, sig, d: bytearray, const: list[int] | None = None) -> int:
crc = 0xFF
for i in range(1, len(d)):
crc ^= d[i]
crc = CRC8H2F[crc]
counter = d[1] & 0x0F
if const is None:
const = VOLKSWAGEN_MQB_MEB_CONSTANTS.get(address)
if const:
crc ^= const[counter]
crc = CRC8H2F[crc]
return crc ^ 0xFF
def volkswagen_mqb_meb_dyn_len_checksum(address: int, sig, d: bytearray, entry: dict | None = None) -> int:
const = None
if entry:
d = d[:entry["length"]]
const = entry["magic"]
return volkswagen_mqb_meb_checksum(address, sig, d, const)
def volkswagen_mqb_meb_gen2_checksum(address: int, sig, d: bytearray) -> int:
entry = VOLKSWAGEN_MQB_MEB_GEN2_CONSTANTS.get(address)
if entry:
checksum = volkswagen_mqb_meb_dyn_len_checksum(address, sig, d, entry)
if checksum == d[0]:
return checksum
return volkswagen_mqb_meb_checksum(address, sig, d)
def xor_checksum(address: int, sig, d: bytearray, initial_value: int = 0) -> int:
checksum = initial_value
checksum_byte = sig.start_bit // 8
for i in range(len(d)):
if i != checksum_byte:
checksum ^= d[i]
return checksum
VOLKSWAGEN_MQB_MEB_CONSTANTS: dict[int, list[int]] = {
0x40: [0x40] * 16, # Airbag_01
0x86: [0x86] * 16, # LWI_01
0x9F: [0xF5] * 16, # LH_EPS_03
0xAD: [0x3F, 0x69, 0x39, 0xDC, 0x94, 0xF9, 0x14, 0x64,
0xD8, 0x6A, 0x34, 0xCE, 0xA2, 0x55, 0xB5, 0x2C], # Getriebe_11
0x0DB: [0x09, 0xFA, 0xCA, 0x8E, 0x62, 0xD5, 0xD1, 0xF0,
0x31, 0xA0, 0xAF, 0xDA, 0x4D, 0x1A, 0x0A, 0x97], # AWV_03
0xFC: [0x77, 0x5C, 0xA0, 0x89, 0x4B, 0x7C, 0xBB, 0xD6,
0x1F, 0x6C, 0x4F, 0xF6, 0x20, 0x2B, 0x43, 0xDD], # ESC_51
0xFD: [0xB4, 0xEF, 0xF8, 0x49, 0x1E, 0xE5, 0xC2, 0xC0,
0x97, 0x19, 0x3C, 0xC9, 0xF1, 0x98, 0xD6, 0x61], # ESP_21
0x101: [0xAA] * 16, # ESP_02
0x102: [0xD7, 0x12, 0x85, 0x7E, 0x0B, 0x34, 0xFA, 0x16,
0x7A, 0x25, 0x2D, 0x8F, 0x04, 0x8E, 0x5D, 0x35], # ESC_50
0x106: [0x07] * 16, # ESP_05
0x10B: [0x77, 0x5C, 0xA0, 0x89, 0x4B, 0x7C, 0xBB, 0xD6,
0x1F, 0x6C, 0x4F, 0xF6, 0x20, 0x2B, 0x43, 0xDD], # Motor_51
0x116: [0xAC] * 16, # ESP_10
0x117: [0x16] * 16, # ACC_10
0x120: [0xC4, 0xE2, 0x4F, 0xE4, 0xF8, 0x2F, 0x56, 0x81,
0x9F, 0xE5, 0x83, 0x44, 0x05, 0x3F, 0x97, 0xDF], # TSK_06
0x121: [0xE9, 0x65, 0xAE, 0x6B, 0x7B, 0x35, 0xE5, 0x5F,
0x4E, 0xC7, 0x86, 0xA2, 0xBB, 0xDD, 0xEB, 0xB4], # Motor_20
0x122: [0x37, 0x7D, 0xF3, 0xA9, 0x18, 0x46, 0x6D, 0x4D,
0x3D, 0x71, 0x92, 0x9C, 0xE5, 0x32, 0x10, 0xB9], # ACC_06
0x126: [0xDA] * 16, # HCA_01
0x12B: [0x6A, 0x38, 0xB4, 0x27, 0x22, 0xEF, 0xE1, 0xBB,
0xF8, 0x80, 0x84, 0x49, 0xC7, 0x9E, 0x1E, 0x2B], # GRA_ACC_01
0x12E: [0xF8, 0xE5, 0x97, 0xC9, 0xD6, 0x07, 0x47, 0x21,
0x66, 0xDD, 0xCF, 0x6F, 0xA1, 0x94, 0x74, 0x63], # ACC_07
0x139: [0xED, 0x03, 0x1C, 0x13, 0xC6, 0x23, 0x78, 0x7A,
0x8B, 0x40, 0x14, 0x51, 0xBF, 0x68, 0x32, 0xBA], # VMM_02
0x13D: [0x20, 0xCA, 0x68, 0xD5, 0x1B, 0x31, 0xE2, 0xDA,
0x08, 0x0A, 0xD4, 0xDE, 0x9C, 0xE4, 0x35, 0x5B], # QFK_01
0x14C: [0x16, 0x35, 0x59, 0x15, 0x9A, 0x2A, 0x97, 0xB8,
0x0E, 0x4E, 0x30, 0xCC, 0xB3, 0x07, 0x01, 0xAD], # Motor_54
0x14D: [0x1A, 0x65, 0x81, 0x96, 0xC0, 0xDF, 0x11, 0x92,
0xD3, 0x61, 0xC6, 0x95, 0x8C, 0x29, 0x21, 0xB5], # ACC_18
0x187: [0x7F, 0xED, 0x17, 0xC2, 0x7C, 0xEB, 0x44, 0x21,
0x01, 0xFA, 0xDB, 0x15, 0x4A, 0x6B, 0x23, 0x05], # Motor_EV_01
0x1A4: [0x69, 0xBB, 0x54, 0xE6, 0x4E, 0x46, 0x8D, 0x7B,
0xEA, 0x87, 0xE9, 0xB3, 0x63, 0xCE, 0xF8, 0xBF], # EA_01
0x1AB: [0x13, 0x21, 0x9B, 0x6A, 0x9A, 0x62, 0xD4, 0x65,
0x18, 0xF1, 0xAB, 0x16, 0x32, 0x89, 0xE7, 0x26], # ESP_33
0x1F0: [0x2F, 0x3C, 0x22, 0x60, 0x18, 0xEB, 0x63, 0x76,
0xC5, 0x91, 0x0F, 0x27, 0x34, 0x04, 0x7F, 0x02], # EA_02
0x20A: [0x9D, 0xE8, 0x36, 0xA1, 0xCA, 0x3B, 0x1D, 0x33,
0xE0, 0xD5, 0xBB, 0x5F, 0xAE, 0x3C, 0x31, 0x9F], # EML_06
0x25D: [0xDA, 0x6B, 0x0E, 0xB2, 0x78, 0xBD, 0x5A, 0x81,
0x7B, 0xD6, 0x41, 0x39, 0x76, 0xB6, 0xD7, 0x35], # KLR_01
0x26B: [0xCE, 0xCC, 0xBD, 0x69, 0xA1, 0x3C, 0x18, 0x76,
0x0F, 0x04, 0xF2, 0x3A, 0x93, 0x24, 0x19, 0x51], # TA_01
0x30C: [0x0F] * 16, # ACC_02
0x30F: [0x0C] * 16, # SWA_01
0x324: [0x27] * 16, # ACC_04
0x3BE: [0x1F, 0x28, 0xC6, 0x85, 0xE6, 0xF8, 0xB0, 0x19,
0x5B, 0x64, 0x35, 0x21, 0xE4, 0xF7, 0x9C, 0x24], # Motor_14
0x3C0: [0xC3] * 16, # Klemmen_Status_01
0x3D5: [0xC5, 0x39, 0xC7, 0xF9, 0x92, 0xD8, 0x24, 0xCE,
0xF1, 0xB5, 0x7A, 0xC4, 0xBC, 0x60, 0xE3, 0xD1], # Licht_Anf_01
0x65D: [0xAC, 0xB3, 0xAB, 0xEB, 0x7A, 0xE1, 0x3B, 0xF7,
0x73, 0xBA, 0x7C, 0x9E, 0x06, 0x5F, 0x02, 0xD9], # ESP_20
}
VOLKSWAGEN_MQB_MEB_GEN2_CONSTANTS: dict[int, dict] = {
0x0DB: {"length": 42,
"magic": [0x09, 0xFA, 0xCA, 0x8E, 0x62, 0xD5, 0xD1, 0xF0,
0x31, 0xA0, 0xAF, 0xDA, 0x4D, 0x1A, 0x0A, 0x97]},
0xFC: {"length": 60,
"magic": [0x69, 0xDC, 0xF9, 0x64, 0x6A, 0xCE, 0x55, 0x2C,
0xC4, 0x38, 0x8F, 0xD1, 0xC6, 0x43, 0xB4, 0xB1]},
0x102: {"length": 44,
"magic": [0xD7, 0x12, 0x85, 0x7E, 0x0B, 0x34, 0xFA, 0x16,
0x7A, 0x25, 0x2D, 0x8F, 0x04, 0x8E, 0x5D, 0x35]},
0x10B: {"length": 44,
"magic": [0x2C, 0xB1, 0x1A, 0x75, 0xBB, 0x65, 0x79, 0x47,
0x81, 0x2B, 0xCC, 0x96, 0x17, 0xDB, 0xC0, 0x94]},
0x13D: {"length": 28,
"magic": [0x18, 0x71, 0x10, 0x8D, 0xD7, 0xAA, 0xB0, 0x78,
0xAC, 0x12, 0xAE, 0x0C, 0xDD, 0xF1, 0x85, 0x68]},
0x139: {"length": 28,
"magic": [0x96, 0x92, 0x95, 0xB5, 0x6E, 0xE3, 0xBD, 0xB4,
0xFA, 0xAE, 0xBE, 0xCB, 0xCF, 0xA5, 0x77, 0xEF]},
}

106
artifacts/package_runtime/iqdbc/car/volkswagen/pq_radar_handler.py Normal file
View File

@@ -0,0 +1,106 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.volkswagen import pqcan
class PQRadarHandler:
GRA_STEP = 3 # GRA_Neu cadence (~33 Hz), matches stock stalk module
SPOOF_STEP = 2 # Motor_2 / Motor_5 spoof cadence (50 Hz)
ENGAGE_FLOOR = 1.0 * CV.KPH_TO_MS # never SET at/below 1 kph
REENGAGE_FLOOR = 2.0 * CV.KPH_TO_MS # hysteresis: only (re)engage above 2 kph
SETSPEED_TOL_KPH = 1.0 # don't chase set-speed within this band
SHORT_STEP_KPH = 1.0 * CV.MPH_TO_KPH # GRA_*_kurz ~= 1 mph
LONG_STEP_KPH = 5.0 * CV.MPH_TO_KPH # GRA_*_lang ~= 5 mph
TAP_RELEASE_CYCLES = 2 # GRA cycles to release between set-speed taps
def __init__(self, CAN):
self.bus = CAN.ext # radar lives on bus 2 (ext/cam)
self.counter = 0
self.failed = False # ACS_Fehler / irrev_Fehler -> radar dead for the drive
self.want_engaged = False # our belief the radar cruise should be on
self._press_phase = 0 # alternate press/release for SET/cancel discrete edges
self._tap_cooldown = 0 # set-speed tap rate limiter
def reset(self):
self.want_engaged = False
self._press_phase = 0
self._tap_cooldown = 0
@staticmethod
def _map_gap_bars(gap_bars):
if not gap_bars:
return None
return int(min(3, max(1, gap_bars)))
def update(self, packer, frame, CS, *, blend_active, engage_req, cancel_req,
set_speed_kph, gap_bars, v_ego):
can_sends = []
if not blend_active:
self.reset()
return can_sends
if CS.acc_radar_fehler or CS.acc_radar_sta_adr == 3:
self.failed = True
radar_active = (CS.acc_radar_sta_adr == 1) and not self.failed
if self.failed:
self.want_engaged = False
elif cancel_req:
self.want_engaged = False
elif engage_req and v_ego > self.REENGAGE_FLOOR:
self.want_engaged = True
if (frame % self.SPOOF_STEP) == 0:
# Mirror the stock engage handshake ORDER: the radar leads (SET -> ADR active) and only THEN
# does the engine report GRA regulating. Asserting MO2_Sta_GRA=1 before the radar's ADR is
# active presents an impossible state ("engine regulating but ADR didn't initiate it") and the
# radar latches an irreversible fault. So only assert cruise-active once ADR is actually active;
# relay Motor_5 (main switch) as verbatim OEM passthrough throughout.
hold_engaged = self.want_engaged and radar_active
can_sends.append(pqcan.filter_motor2(packer, self.bus, CS.motor2_stock, gra_active=hold_engaged))
can_sends.append(pqcan.filter_motor5(packer, self.bus, CS.motor5_stock, gra_active=False))
if (frame % self.GRA_STEP) == 0:
set_btn = resume_btn = cancel = up_s = down_s = up_l = down_l = False
self._press_phase ^= 1
pressing = self._press_phase == 0
if self.failed:
pass
elif cancel_req:
# Only actually press cancel when the radar is engaged (or overdriven) -- otherwise it's a
# no-op against a passive/off radar and just spams GRA_Abbrechen at ~16 Hz. Fires while
# active and stops once the radar leaves the active state.
cancel = pressing and radar_active
elif self.want_engaged and not radar_active and v_ego > self.ENGAGE_FLOOR:
# Engage with RESUME (GRA_Recall), not SET. Ground-truth from an OEM ACC engage (radar leads:
# RESUME -> ACS_Sta_ADR 2->1 in ~90ms -> engine MO2_Sta_GRA 0->1 ~80ms later) shows this radar
# engages from passive on GRA_Recall; it ignores GRA_Neu_Setzen from that state.
resume_btn = pressing
elif self.want_engaged and radar_active:
if self._tap_cooldown > 0:
self._tap_cooldown -= 1
elif set_speed_kph > 0:
delta = set_speed_kph - CS.acc_radar_v_wunsch
if abs(delta) >= self.SETSPEED_TOL_KPH:
big = abs(delta) >= self.LONG_STEP_KPH
if delta > 0:
up_l, up_s = big, not big
else:
down_l, down_s = big, not big
self._tap_cooldown = self.TAP_RELEASE_CYCLES
self.counter = (self.counter + 1) % 16
can_sends.append(pqcan.create_radar_gra(
packer, self.bus, CS.gra_stock_values, self.counter,
set_btn=set_btn, resume=resume_btn, cancel=cancel, up_short=up_s, down_short=down_s,
up_long=up_l, down_long=down_l, zeitluecke=self._map_gap_bars(gap_bars),
))
return can_sends

207
artifacts/package_runtime/iqdbc/car/volkswagen/pqcan.py Normal file
View File

@@ -0,0 +1,207 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
def create_hca_steering_control(packer, bus, apply_torque, HCA_Status):
values = {
"LM_Offset": abs(apply_torque),
"LM_OffSign": 1 if apply_torque < 0 else 0,
"HCA_Status": HCA_Status,
"Vib_Freq": 16,
}
return packer.make_can_msg("HCA_1", bus, values)
def create_lka_hud_control(packer, bus, ldw_stock_values, lat_active, steering_pressed, hud_alert, hud_control, entering, special_mode, special_active):
values = {}
if len(ldw_stock_values):
values = {s: ldw_stock_values[s] for s in [
"LDW_SW_Warnung_links", # Blind spot in warning mode on left side due to lane departure
"LDW_SW_Warnung_rechts", # Blind spot in warning mode on right side due to lane departure
"LDW_Seite_DLCTLC", # Direction of most likely lane departure (left or right)
"LDW_DLC", # Lane departure, distance to line crossing
"LDW_TLC", # Lane departure, time to line crossing
]}
if entering:
yellow_led = int(steering_pressed)
green_led = int(not steering_pressed)
elif special_mode:
yellow_led = 1 if (lat_active and (steering_pressed or not special_active)) or not lat_active else 0
green_led = 1 if lat_active and special_active and not steering_pressed else 0
else:
yellow_led = 1 if (lat_active and steering_pressed) or not lat_active else 0
green_led = 1 if lat_active and not steering_pressed else 0
values.update({
"LDW_Kameratyp": 1,
"LDW_Lampe_gelb": yellow_led,
"LDW_Lampe_gruen": green_led,
"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, cancel=False, resume=False, set_button=False):
values = {s: gra_stock_values[s] for s in [
"GRA_Hauptschalt", # ACC button, on/off
"GRA_Typ_Hauptschalt", # ACC button, momentary vs latching
"GRA_Kodierinfo", # ACC button, configuration
"GRA_Sender", # ACC button, CAN message originator
]}
values.update({
"COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
"GRA_Abbrechen": cancel,
"GRA_Recall": resume,
"GRA_Neu_Setzen": set_button,
})
return packer.make_can_msg("GRA_Neu", bus, values)
def create_gra_neu(packer, bus, gra_stock, longActive):
values = gra_stock.copy()
if longActive:
values.update({
"GRA_Neu_Setzen": 0,
"GRA_Recall": 0,
"GRA_Hauptschalt": 0,
})
return packer.make_can_msg("GRA_Neu", bus, values)
def acc_control_value(main_switch_on, long_active, cruiseOverride, accFaulted):
if long_active or cruiseOverride:
acc_control = 1
elif accFaulted:
acc_control = 3
elif main_switch_on:
acc_control = 2
else:
acc_control = 0
return acc_control
def acc_hud_status_value(main_switch_on, acc_faulted, longActive, longOverride):
if longOverride:
hud_status = 4
elif longActive:
hud_status = 3
elif acc_faulted:
hud_status = 6
elif main_switch_on:
hud_status = 2
else:
hud_status = 0
return hud_status
def create_acc_accel_control(packer, bus, acc_type, accel, acc_control, stopping, starting, esp_hold, comfortBand, jerkLimit, eBrakeActive, sng_active=False):
commands = []
acc_enabled = acc_control == 1 and not sng_active
values = {
"ACS_Sta_ADR": 0 if sng_active else acc_control,
"ACS_StSt_Info": acc_enabled,
"ACS_Typ_ACC": acc_type,
"ACS_Anhaltewunsch": (acc_type == 1 and stopping or eBrakeActive) or sng_active,
"ACS_FreigSollB": acc_enabled,
"ACS_Sollbeschl": accel if acc_enabled else 3.01,
"ACS_zul_Regelabw": comfortBand if acc_enabled else 1.27,
"ACS_max_AendGrad": jerkLimit if acc_enabled else 5.08,
"ACS_Schubabsch": 1 if acc_enabled and accel > 0.05 else 0,
"ACS_MomEingriff": 0,
"ACS_ADR_Schub": 0,
}
commands.append(packer.make_can_msg("ACC_System", bus, values))
return commands
def create_sng_handoff_control(packer, bus, handoff_active, decel_req):
values = {
"SNG_HandoffActive": handoff_active,
"SNG_DecelReq": decel_req if handoff_active else 0.0,
}
return packer.make_can_msg("SNG_1", bus, values)
def create_blinker_control(packer, bus, leftBlinker, rightBlinker):
values = {
"BM_rechts": rightBlinker,
"BM_links": leftBlinker,
}
return packer.make_can_msg("Blinkmodi_02", bus, values)
def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, leadDistance, distanceBars, fcw_alert, leadVisible, unavailable, decel, d_unresponsive):
priodisp = 0 if fcw_alert else 0 if (acc_hud_status == 4 or decel) else 2 if (acc_hud_status in (3, 2) or leadVisible) else 0
leadDistanceBars = distanceBars + 1 if distanceBars in (1, 2, 3) else 2
values = {
"ACA_StaACC": acc_hud_status,
"ACA_AnzDisplay": 1 if acc_hud_status in (3, 4) else 0,
"ACA_Zeitluecke": leadDistanceBars,
"ACA_V_Wunsch": set_speed,
"ACA_gemZeitl": min(15, max(1, int(round(leadDistance)))) if leadVisible else 0,
"ACA_PrioDisp": priodisp,
"ACA_Akustik1": d_unresponsive,
"ACA_Akustik2": fcw_alert,
"ACA_ACC_Verz": decel,
}
return packer.make_can_msg("ACC_GRA_Anzeige", bus, values)
def filter_motor2(packer, bus, motor2_stock, gra_active=False):
values = dict(motor2_stock)
if gra_active:
values.update({
"MO2_Sta_GRA": 1,
"MO2_Status_TSK": 1,
})
else:
values.update({
"MO2_Sta_GRA": 0,
})
return packer.make_can_msg("Motor_2", bus, values)
def filter_motor5(packer, bus, motor5_stock, gra_active=False):
values = dict(motor5_stock)
if gra_active:
values["MO5_GRA_Hauptsch"] = 1
return packer.make_can_msg("Motor_5", bus, values)
def create_motor3_resume(packer, bus, motor1_stock, motor3_stock, resume=False):
values = dict(motor3_stock)
values_motor1 = dict(motor1_stock)
if resume:
values["MO3_Pedalwert"] = values_motor1["MO1_Pedalwert"]
return packer.make_can_msg("Motor_3", bus, values)
def create_radar_gra(packer, bus, gra_stock, counter, set_btn=False, cancel=False, resume=False,
up_short=False, down_short=False, up_long=False, down_long=False, zeitluecke=None):
values = {s: gra_stock[s] for s in [
"GRA_Hauptschalt", # ACC main switch passthrough
"GRA_Typ_Hauptschalt", # momentary vs latching
"GRA_Kodierinfo", # configuration
"GRA_Sender", # CAN originator
]}
values.update({
"COUNTER": counter % 16,
"GRA_Neu_Setzen": 1 if set_btn else 0,
"GRA_Abbrechen": 1 if cancel else 0,
"GRA_Recall": 1 if resume else 0,
"GRA_Up_kurz": 1 if up_short else 0,
"GRA_Down_kurz": 1 if down_short else 0,
"GRA_Up_lang": 1 if up_long else 0,
"GRA_Down_lang": 1 if down_long else 0,
})
if zeitluecke is not None:
values["GRA_Zeitluecke"] = zeitluecke
return packer.make_can_msg("GRA_Neu", bus, values)

120
artifacts/package_runtime/iqdbc/car/volkswagen/radar_interface.py Normal file
View File

@@ -0,0 +1,120 @@
import math
from iqdbc.can import CANParser
from iqdbc.can.dbc import DBC as DBCLoader
from iqdbc.car import Bus, structs
from iqdbc.car.interfaces import RadarInterfaceBase
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.volkswagen.values import DBC, VolkswagenFlags
RADAR_ADDR = 0x24F
NO_OBJECT = 0
LANE_TYPES = ("Same_Lane", "Left_Lane", "Right_Lane")
SIGNAL_SETS = tuple(
(
f"{prefix}_ObjectID",
f"{prefix}_Long_Distance",
f"{prefix}_Lat_Distance",
f"{prefix}_Rel_Velo",
)
for lane in LANE_TYPES
for idx in (1, 2)
for prefix in (f"{lane}_0{idx}",)
)
def get_radar_can_parser(CP):
if CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
dbc_name = DBC[CP.carFingerprint][Bus.radar]
dbc = DBCLoader(dbc_name)
if "Strukturen_01" in dbc.name_to_msg:
messages = [("Strukturen_01", 25)]
elif "MEB_Distance_01" in dbc.name_to_msg:
messages = [("MEB_Distance_01", 25)]
else:
return None
else:
return None
return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, 2)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP, CP_IQ):
super().__init__(CP, CP_IQ)
self.updated_messages: set[int] = set()
self.trigger_msg: int = RADAR_ADDR
self._track_id_counter: int = 0
self.radar_off_can: bool = CP.radarUnavailable
self.rcp: CANParser | None = get_radar_can_parser(CP)
self._pts = self.pts
def update(self, can_strings):
"""Entry-point called by the vehicle loop every CAN tick."""
if self.radar_off_can or self.rcp is None:
return super().update(None)
vls = self.rcp.update(can_strings)
self.updated_messages.update(vls)
if self.trigger_msg not in self.updated_messages:
return None
radar_data = self._process_radar_frame()
self.updated_messages.clear()
return radar_data
def _process_radar_frame(self):
ret = structs.RadarData()
if self.rcp is None:
return ret
if not self.rcp.can_valid:
ret.errors.canError = True
return ret
msg = self.rcp.vl["Strukturen_01"] if "Strukturen_01" in self.rcp.vl else self.rcp.vl["MEB_Distance_01"]
get = msg.__getitem__
active_objects: dict[int, tuple[float, float, float]] = {}
for obj_id_sig, long_sig, lat_sig, vel_sig in SIGNAL_SETS:
obj_id = get(obj_id_sig)
if obj_id == NO_OBJECT:
continue
if obj_id in active_objects:
ret.errors.canError = True
return ret
active_objects[obj_id] = (
get(long_sig), # dRel
get(lat_sig), # yRel
get(vel_sig), # vRel
)
for obj_id, (d_rel, y_rel, v_rel) in active_objects.items():
if obj_id not in self._pts:
pt = structs.RadarData.RadarPoint()
pt.trackId = self._track_id_counter
self._track_id_counter += 1
self._pts[obj_id] = pt
else:
pt = self._pts[obj_id]
pt.measured = True
pt.dRel = d_rel
pt.yRel = y_rel
pt.vRel = v_rel
pt.aRel = math.nan
pt.yvRel = math.nan
inactive_ids = self._pts.keys() - active_objects.keys()
for obj_id in inactive_ids:
self._pts.pop(obj_id, None)
ret.points = list(self._pts.values())
return ret

563
artifacts/package_runtime/iqdbc/car/volkswagen/speed_limit_manager.py Normal file
View File

@@ -0,0 +1,563 @@
import time
import math
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.volkswagen.values import VolkswagenFlags
from iqdbc.car.lateral import ISO_LATERAL_ACCEL
NOT_SET = 0
SPEED_SUGGESTED_MAX_HIGHWAY_GER_KPH = 130 # 130 kph in germany
STREET_TYPE_URBAN = 1
STREET_TYPE_NONURBAN = 2
STREET_TYPE_HIGHWAY = 3
SANITY_CHECK_DIFF_PERCENT_LOWER = 30
SPEED_LIMIT_UNLIMITED_VZE_KPH = int(round(144 * CV.MS_TO_KPH))
DECELERATION_PREDICATIVE = 1.0
SEGMENT_DECAY = 10
PSD_TYPE_SPEED_LIMIT = 1
PSD_TYPE_CURV_SPEED = 2
PSD_CURV_SPEED_DECAY = 4
PSD_UNIT_KPH = 0
PSD_UNIT_MPH = 1
class SpeedLimitManager:
def __init__(self, car_params, speed_limit_max_kph=SPEED_SUGGESTED_MAX_HIGHWAY_GER_KPH, predicative=False, predicative_speed_limit=False, predicative_curve=False):
self.CP = car_params
self.v_limit_psd = NOT_SET
self.v_limit_psd_next = NOT_SET
self.v_limit_psd_legal = NOT_SET
self.v_limit_psd_next_type = NOT_SET
self.v_limit_vze = NOT_SET
self.v_limit_speed_unit_psd = PSD_UNIT_KPH
self.v_limit_vze_sanity_error = False
self.v_limit_output_last = NOT_SET
self.v_limit_max = speed_limit_max_kph
self.predicative = predicative
self.predicative_speed_limit = predicative_speed_limit
self.predicative_curve = predicative_curve
self.predicative_segments = {}
self.current_predicative_segment = {"ID": NOT_SET, "Length": NOT_SET, "Speed": NOT_SET, "StreetType": NOT_SET, "OnRampExit": NOT_SET}
self.v_limit_psd_next_last_timestamp = 0
self.v_limit_psd_next_last = NOT_SET
self.v_limit_psd_next_decay_time = NOT_SET
self.v_limit_changed = False
def _reset_predicative(self):
self.v_limit_psd_next = NOT_SET
self.v_limit_psd_next_type = NOT_SET
self.v_limit_psd_next_last_timestamp = 0
self.v_limit_psd_next_last = NOT_SET
self.v_limit_psd_next_decay_time = NOT_SET
def enable_predicative_speed_limit(self, predicative=False, reaction_to_speed_limits=False, reaction_to_curves=False):
if self.predicative == predicative and self.predicative_speed_limit == reaction_to_speed_limits and self.predicative_curve == reaction_to_curves:
return # perf
if not predicative or (not reaction_to_speed_limits and not reaction_to_curves):
self._reset_predicative()
self.predicative = False # fully disable execution
else:
self.predicative = predicative
if (not reaction_to_speed_limits and self.predicative_speed_limit) or (not reaction_to_curves and self.predicative_curve):
self._reset_predicative() # force reset when disabling
self.predicative_speed_limit = reaction_to_speed_limits
self.predicative_curve = reaction_to_curves
def update(self, current_speed_ms, psd_04, psd_05, psd_06, vze, raining, time_car):
# try reading speed form traffic sign recognition
if vze and self.CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
self._receive_speed_limit_vze_meb(vze)
# read speed unit from PSD_06 and also use it for traffic sign recognition if present (for now always seen on bus 0)
# the vze location/unit flag is not stable and no corresponding flag has been found yet
if psd_06:
self._receive_speed_unit_psd(psd_06)
# try reading speed from predicative street data
if psd_04 and psd_05 and psd_06:
self._receive_current_segment_psd(psd_05)
self._refresh_current_segment()
self._build_predicative_segments(psd_04, psd_06, raining, time_car)
self._receive_speed_limit_psd_legal(psd_06)
self._get_speed_limit_psd()
if self.predicative:
self._get_speed_limit_psd_next(current_speed_ms) # this is very cpu heavy
def get_speed_limit_predicative(self):
v_limit_output = self.v_limit_psd_next if self.predicative and self.v_limit_psd_next != NOT_SET and self.v_limit_psd_next < self.v_limit_output_last else NOT_SET
return v_limit_output * CV.KPH_TO_MS
def get_speed_limit_predicative_type(self):
return self.v_limit_psd_next_type
def get_speed_limit(self):
candidates = {
"vze": self.v_limit_vze if self.v_limit_vze != NOT_SET and not self.v_limit_vze_sanity_error else NOT_SET,
"psd": self.v_limit_psd if self.v_limit_psd != NOT_SET else NOT_SET,
"legal": self.v_limit_psd_legal
}
v_limit_output = NOT_SET
for source in ["vze", "psd", "legal"]:
v = candidates[source]
if v != NOT_SET:
v_limit_output = v
break
if v_limit_output > self.v_limit_max:
v_limit_output = self.v_limit_max
self.v_limit_changed = True if self.v_limit_output_last != v_limit_output else False
self.v_limit_output_last = v_limit_output
return v_limit_output * CV.KPH_TO_MS
def _speed_limit_vze_sanitiy_check(self, speed_limit_vze_new):
if self.v_limit_output_last == NOT_SET:
self.v_limit_vze_sanity_error = False
return
diff_p = 100 * speed_limit_vze_new / self.v_limit_output_last
self.v_limit_vze_sanity_error = True if diff_p < SANITY_CHECK_DIFF_PERCENT_LOWER else False
if speed_limit_vze_new > SPEED_LIMIT_UNLIMITED_VZE_KPH: # unlimited sign detected: use psd logic for setting maximum speed
self.v_limit_vze_sanity_error = True
def _receive_speed_unit_psd(self, psd_06):
# keep it simple for now, the unit is supplied shortly before the corresponding speed limits are supplied for given segment ID
if psd_06["PSD_06_Mux"] == 0 and psd_06["PSD_Sys_Segment_ID"] > 1:
self.v_limit_speed_unit_psd = psd_06["PSD_Sys_Geschwindigkeit_Einheit"]
def _convert_raw_speed_psd(self, raw_speed, street_type):
speed = NOT_SET
if self.v_limit_speed_unit_psd == PSD_UNIT_KPH:
if 0 < raw_speed < 11: # 0 - 45 kph
speed = (raw_speed - 1) * 5
elif 11 <= raw_speed < 23: # 50 - 160 kph
speed = 50 + (raw_speed - 11) * 10
elif raw_speed == 23: # explicitly no legal speed limit
if street_type == STREET_TYPE_HIGHWAY:
speed = self.v_limit_max
elif self.v_limit_speed_unit_psd == PSD_UNIT_MPH:
if 3 < raw_speed < 18: # 0 - 70 mph
speed = (5 * (raw_speed - 3)) * CV.MPH_TO_KPH
elif 18 <= raw_speed < 23: # 75 - 105 mph
speed = ((5 * (raw_speed - 3)) + 10) * CV.MPH_TO_KPH
elif raw_speed == 23: # explicitly no legal speed limit
if street_type == STREET_TYPE_HIGHWAY:
speed = self.v_limit_max
return speed
def _receive_speed_limit_vze_meb(self, vze):
v_limit_vze = vze["VZE_Verkehrszeichen_1"] # main traffic sign
# "VZE_Anzeigemodus" has been seen not being stable for different drives in same USA mph car, no other flag has been found yet
# for now additionally assume a car with mph speed unit nav data sgements is also supplied with mph converted vze data ("PSD_06" is available on bus 0)
# mph speed unit set in signal "Einheiten_01" does not mean the speed data being supplied in mph unit!
v_limit_vze = v_limit_vze * CV.MPH_TO_KPH if vze["VZE_Anzeigemodus"] == 1 or self.v_limit_speed_unit_psd == PSD_UNIT_MPH else v_limit_vze
self._speed_limit_vze_sanitiy_check(v_limit_vze)
self.v_limit_vze = v_limit_vze
def _receive_current_segment_psd(self, psd_05):
if psd_05["PSD_Pos_Standort_Eindeutig"] == 1 and psd_05["PSD_Pos_Segment_ID"] != NOT_SET:
self.current_predicative_segment["Length"] = psd_05["PSD_Pos_Segmentlaenge"]
if self.current_predicative_segment["ID"] != psd_05["PSD_Pos_Segment_ID"]:
self.current_predicative_segment["ID"] = psd_05["PSD_Pos_Segment_ID"]
self.current_predicative_segment["Speed"] = NOT_SET
self.current_predicative_segment["StreetType"] = NOT_SET
self.current_predicative_segment["OnRampExit"] = False
def _get_segment_curvature_psd(self, psd_curvature, psd_sign, scale=2e-5):
if psd_curvature in (0, 255):
return NOT_SET
curvature = (255 - psd_curvature) * scale
if psd_sign == 1:
curvature *= -1
return curvature
def _calculate_curve_speed(self, curvature):
if curvature == NOT_SET:
return NOT_SET
curv_speed_ms = math.sqrt(ISO_LATERAL_ACCEL / abs(curvature))
if self.v_limit_speed_unit_psd == PSD_UNIT_MPH:
curv_speed = int((curv_speed_ms * CV.MS_TO_MPH) // 5 * 5) * CV.MPH_TO_KPH
else:
curv_speed = int((curv_speed_ms * CV.MS_TO_KPH) // 5 * 5)
return curv_speed
def _refresh_current_segment(self):
current_segment = self.current_predicative_segment["ID"]
if current_segment != NOT_SET:
seg = self.predicative_segments.get(current_segment)
if seg:
self.current_predicative_segment["Speed"] = self.predicative_segments[current_segment]["Speed"]
self.current_predicative_segment["StreetType"] = self.predicative_segments[current_segment]["StreetType"]
self.current_predicative_segment["OnRampExit"] = self.predicative_segments[current_segment]["OnRampExit"]
def _build_predicative_segments(self, psd_04, psd_06, raining, time_car):
now = time.time()
# Segment erfassen/aktualisieren
if (psd_04["PSD_ADAS_Qualitaet"] == 1 and
psd_04["PSD_wahrscheinlichster_Pfad"] == 1 and
psd_04["PSD_Segment_ID"] != NOT_SET):
segment_id = psd_04["PSD_Segment_ID"]
seg = self.predicative_segments.get(segment_id)
if seg:
seg["Length"] = psd_04["PSD_Segmentlaenge"]
seg["Curvature_Begin"] = self._get_segment_curvature_psd(psd_04["PSD_Anfangskruemmung"], psd_04["PSD_Anfangskruemmung_Vorz"])
seg["Curvature_End"] = self._get_segment_curvature_psd(psd_04["PSD_Endkruemmung"], psd_04["PSD_Endkruemmung_Vorz"])
seg["StreetType"] = self._get_street_type(psd_04["PSD_Strassenkategorie"], psd_04["PSD_Bebauung"])
seg["OnRampExit"] = psd_04["PSD_Rampe"] in (1, 2)
seg["ID_Prev"] = psd_04["PSD_Vorgaenger_Segment_ID"]
seg["Curve_Speed_Begin"] = self._calculate_curve_speed(seg["Curvature_Begin"])
seg["Curve_Speed_End"] = self._calculate_curve_speed(seg["Curvature_End"])
seg["Timestamp"] = now
else:
self.predicative_segments[segment_id] = {
"ID": segment_id,
"Length": psd_04["PSD_Segmentlaenge"],
"Curvature_Begin": self._get_segment_curvature_psd(psd_04["PSD_Anfangskruemmung"], psd_04["PSD_Anfangskruemmung_Vorz"]),
"Curvature_End": self._get_segment_curvature_psd(psd_04["PSD_Endkruemmung"], psd_04["PSD_Endkruemmung_Vorz"]),
"StreetType": self._get_street_type(psd_04["PSD_Strassenkategorie"], psd_04["PSD_Bebauung"]),
"OnRampExit": psd_04["PSD_Rampe"] in (1, 2),
"ID_Prev": psd_04["PSD_Vorgaenger_Segment_ID"],
"Speed": NOT_SET,
"Curve_Speed_Begin": NOT_SET,
"Curve_Speed_End": NOT_SET,
"QualityFlag": False,
"Timestamp": now
}
seg = self.predicative_segments.get(segment_id)
seg["Curve_Speed_Begin"] = self._calculate_curve_speed(seg["Curvature_Begin"])
seg["Curve_Speed_End"] = self._calculate_curve_speed(seg["Curvature_End"])
# Schritt 2: Alte Segmente bereinigen
current_id = self.current_predicative_segment["ID"]
if current_id != NOT_SET:
self.predicative_segments = {
sid: seg for sid, seg in self.predicative_segments.items()
if now - seg.get("Timestamp", 0) <= SEGMENT_DECAY
}
# Geschwindigkeit setzen (speed limits seen for changed limits only)
if (psd_06["PSD_06_Mux"] == 2 and
psd_06["PSD_Ges_Typ"] == 1 and
psd_06["PSD_Ges_Gesetzlich_Kategorie"] == 0 and
psd_06["PSD_Ges_Segment_ID"] != NOT_SET):
raw_speed = psd_06["PSD_Ges_Geschwindigkeit"] if self._speed_limit_is_valid_now_psd(psd_06, raining, time_car) else NOT_SET
segment_id = psd_06["PSD_Ges_Segment_ID"]
if segment_id in self.predicative_segments:
speed = self._convert_raw_speed_psd(raw_speed, self.predicative_segments[segment_id]["StreetType"])
self.predicative_segments[segment_id]["Speed"] = speed
self.predicative_segments[segment_id]["Speed_Type"] = PSD_TYPE_SPEED_LIMIT
self.predicative_segments[segment_id]["QualityFlag"] = True
def _get_time_from_vw_datetime(self, time_car):
if time_car:
try:
t = (time_car["UH_Jahr"], time_car["UH_Monat"], time_car["UH_Tag"], time_car["UH_Stunde"], time_car["UH_Minute"], time_car["UH_Sekunde"], 0, 0, -1)
local_time = time.mktime(t)
return time.localtime(local_time)
except Exception:
return time.localtime()
else:
return time.localtime()
def _speed_limit_is_valid_now_psd(self, psd_06, raining, time_car):
local_time = self._get_time_from_vw_datetime(time_car)
# by day
day_start = psd_06["PSD_Ges_Geschwindigkeit_Tag_Anf"]
day_end = psd_06["PSD_Ges_Geschwindigkeit_Tag_Ende"]
now_weekday = (local_time.tm_wday + 1) # Python: 0=Mon -> PSD: 1=Mon
if 1 <= day_start <= 7 and 1 <= day_end <= 7:
if day_start <= day_end:
is_valid_by_day = day_start <= now_weekday <= day_end
else:
is_valid_by_day = now_weekday >= day_start or now_weekday <= day_end
else:
is_valid_by_day = True
# by time
hour_start = psd_06["PSD_Ges_Geschwindigkeit_Std_Anf"]
hour_end = psd_06["PSD_Ges_Geschwindigkeit_Std_Ende"]
now_hour = local_time.tm_hour
if (hour_start != 25 and hour_end != 25):
if hour_start <= hour_end:
is_valid_by_time = hour_start <= now_hour < hour_end
else:
is_valid_by_time = now_hour >= hour_start or now_hour < hour_end
else:
is_valid_by_time = True
# by weather condition
weather_condition = psd_06["PSD_Ges_Geschwindigkeit_Witter"]
is_valid_by_weather_conditions = weather_condition == 0 or ( raining and weather_condition == 1 )
checks = [
is_valid_by_time,
is_valid_by_day,
is_valid_by_weather_conditions,
]
return all(checks)
def _speed_limit_curve_allowed(self, seg):
currently_on_ramp = self.current_predicative_segment.get("OnRampExit", False)
seg_on_ramp = seg.get("OnRampExit", False)
ramp_allowed_on_ramp = currently_on_ramp and seg_on_ramp
ramp_allowed = ramp_allowed_on_ramp or not seg_on_ramp
# on ramp is probably type 2 TODO
# for now only allow non urban, there are problems with highway curvature data
street_type = seg.get("StreetType", NOT_SET)
street_type_allowed = True if street_type == STREET_TYPE_NONURBAN or (street_type == STREET_TYPE_HIGHWAY and ramp_allowed_on_ramp) else False
speed_curve = seg.get("Curve_Speed", NOT_SET)
if NOT_SET not in (self.v_limit_output_last, speed_curve):
diff_p = 100 * speed_curve / self.v_limit_output_last
sanity_error = True if diff_p < SANITY_CHECK_DIFF_PERCENT_LOWER else False
else:
sanity_error = False
checks = [
ramp_allowed,
street_type_allowed,
not sanity_error,
]
return all(checks)
def _hit_linear_profile(self, v0_ms, a, total_dist_m, L_m, v_b_kmh, v_e_kmh):
if L_m is None or L_m <= 0:
return None, None
v_b = v_b_kmh * CV.KPH_TO_MS
v_e = v_e_kmh * CV.KPH_TO_MS
r = (v_e - v_b) / L_m # m/s pro m
# Flaches Profil => Standard-Bremsweg gegen v_b am Beginn
if abs(r) < 1e-9:
if v0_ms <= v_b:
return None, None
brake_dist = (v0_ms**2 - v_b**2) / (2*a)
if brake_dist >= total_dist_m:
# Aktivierung am Segmentbeginn
return 0.0, v_b_kmh
return None, None
A = r*r
B = 2*v_b*r + 2*a
C = v_b*v_b + 2*a*total_dist_m - v0_ms*v0_ms
D = B*B - 4*A*C
if D < 0:
return None, None
sqrtD = math.sqrt(D)
# Smallest non-negative solution
s1 = (-B - sqrtD) / (2*A)
s2 = (-B + sqrtD) / (2*A)
s_candidates = [s for s in (s1, s2) if s >= 0.0]
if not s_candidates:
return None, None
s_hit = min(s_candidates)
if s_hit > L_m:
return None, None
v_req_ms = v_b + r * s_hit
return float(s_hit), (v_req_ms * CV.MS_TO_KPH)
def _dfs(self, seg_id, total_dist, visited, current_speed_ms, best_result, path):
if seg_id in visited or seg_id not in path:
return
visited.add(seg_id)
seg = self.predicative_segments.get(seg_id)
if not seg:
return
candidates = []
length = seg.get("Length", NOT_SET)
# Speed-Limit
if self.predicative_speed_limit:
sl = seg.get("Speed", NOT_SET)
if sl != NOT_SET:
qual_ok = seg.get("QualityFlag", False)
candidates.append((sl, PSD_TYPE_SPEED_LIMIT, 0.0, qual_ok))
# Curve-Speed
if self.predicative_curve and self._speed_limit_curve_allowed(seg):
cs_begin = seg.get("Curve_Speed_Begin", NOT_SET)
cs_end = seg.get("Curve_Speed_End", NOT_SET)
if cs_begin != NOT_SET:
candidates.append((cs_begin, PSD_TYPE_CURV_SPEED, 0.0, True))
if cs_end != NOT_SET and length > 0:
candidates.append((cs_end, PSD_TYPE_CURV_SPEED, length, True))
# Curve-Speed Profile
if cs_begin != NOT_SET and cs_end != NOT_SET and length > 0:
s_hit, v_req_kmh = self._hit_linear_profile(v0_ms=current_speed_ms, a=DECELERATION_PREDICATIVE,
total_dist_m=total_dist, L_m=length, v_b_kmh=cs_begin, v_e_kmh=cs_end)
if s_hit is not None and v_req_kmh is not None:
candidates.append((v_req_kmh, PSD_TYPE_CURV_SPEED, s_hit, True))
# check candidates
for cand_speed_kmh, cand_type, activation_offset, qual_ok in candidates:
if cand_speed_kmh == NOT_SET:
continue
if not qual_ok:
continue
v_target_ms = cand_speed_kmh * CV.KPH_TO_MS
if not (v_target_ms < current_speed_ms and (cand_speed_kmh < self.v_limit_output_last or self.v_limit_output_last == NOT_SET)):
continue
brake_dist = (current_speed_ms**2 - v_target_ms**2) / (2 * DECELERATION_PREDICATIVE)
if brake_dist <= 0:
continue
dist_to_activation = total_dist + activation_offset
if dist_to_activation <= brake_dist:
# choose lowest limit or shorter length when equal
better = False
if cand_speed_kmh < best_result["limit"]:
better = True
elif cand_speed_kmh == best_result["limit"] and dist_to_activation < best_result["dist"]:
better = True
if better:
best_result["limit"] = cand_speed_kmh
best_result["type"] = cand_type
best_result["dist"] = dist_to_activation # represents distance to limit
best_result["length"] = length - activation_offset # represents remaining distance with limit for curves
children = [sid for sid, s in self.predicative_segments.items() if s.get("ID_Prev") == seg_id]
if len(children) > 1:
return # Split detected, can not decide unique limit on current path
for next_id in children:
if seg_id == self.current_predicative_segment.get("ID"):
next_length = self.current_predicative_segment.get("Length", 0)
else:
next_length = seg.get("Length", 0)
self._dfs(next_id, total_dist + next_length, visited.copy(), current_speed_ms, best_result, path)
def _build_path_psd(self, start_seg_id):
path = []
current_id = start_seg_id
visited = set()
segment_children_map = {}
for sid, seg in self.predicative_segments.items():
parent = seg.get("ID_Prev")
if parent not in (None, NOT_SET):
segment_children_map.setdefault(parent, []).append(sid)
while current_id != NOT_SET:
if current_id in visited:
break
visited.add(current_id)
path.append(current_id)
children = segment_children_map.get(current_id, [])
if len(children) != 1:
break
current_id = children[0]
return path
def _get_speed_limit_psd_next(self, current_speed_ms):
current_id = self.current_predicative_segment.get("ID")
self.v_limit_psd_next = NOT_SET
if current_id == NOT_SET:
return
path = self._build_path_psd(current_id)
if len(path) <= 1:
return
best_result = {"limit": float('inf'), "type": NOT_SET, "dist": float('inf'), "length": float('inf')}
self._dfs(current_id, 0, set(), current_speed_ms, best_result, path)
now = time.time()
if best_result["limit"] != float('inf'):
self.v_limit_psd_next = best_result["limit"]
self.v_limit_psd_next_type = best_result["type"]
self.v_limit_psd_next_last = best_result["limit"]
self.v_limit_psd_next_last_timestamp = now
self.v_limit_psd_next_decay_time = math.sqrt(2 * best_result["dist"] / DECELERATION_PREDICATIVE)
if self.v_limit_psd_next_type == PSD_TYPE_CURV_SPEED:
self.v_limit_psd_next_decay_time += max((best_result["length"] / (self.v_limit_psd_next * CV.KPH_TO_MS)), PSD_CURV_SPEED_DECAY)
else:
if now - self.v_limit_psd_next_last_timestamp <= self.v_limit_psd_next_decay_time and self.v_limit_output_last > self.v_limit_psd_next_last and not self.v_limit_changed:
self.v_limit_psd_next = self.v_limit_psd_next_last
else:
self.v_limit_psd_next_last = NOT_SET
self.v_limit_psd_next_decay_time = NOT_SET
self.v_limit_psd_next_type = NOT_SET
def _get_speed_limit_psd(self):
seg_id = self.current_predicative_segment.get("ID")
if seg_id == NOT_SET:
self.v_limit_psd = NOT_SET
return
seg = self.predicative_segments.get(seg_id)
if seg and seg.get("Speed") != NOT_SET:
self.v_limit_psd = seg.get("Speed")
def _get_street_type(self, strassenkategorie, bebauung):
street_type = NOT_SET
if strassenkategorie == 1: # base type: urban
street_type = STREET_TYPE_URBAN
elif strassenkategorie in (2, 3, 4): # base type: non urban
if bebauung == 1:
street_type = STREET_TYPE_URBAN
else:
street_type = STREET_TYPE_NONURBAN
elif strassenkategorie == 5: # base type: highway
street_type = STREET_TYPE_HIGHWAY
return street_type
def _receive_speed_limit_psd_legal(self, psd_06):
if psd_06["PSD_06_Mux"] == 2:
if psd_06["PSD_Ges_Typ"] == 2:
street_type = self.current_predicative_segment["StreetType"]
if ((psd_06["PSD_Ges_Gesetzlich_Kategorie"] == STREET_TYPE_URBAN and street_type == STREET_TYPE_URBAN) or
(psd_06["PSD_Ges_Gesetzlich_Kategorie"] == STREET_TYPE_NONURBAN and street_type == STREET_TYPE_NONURBAN) or
(psd_06["PSD_Ges_Gesetzlich_Kategorie"] == STREET_TYPE_HIGHWAY and street_type == STREET_TYPE_HIGHWAY)):
raw_speed = psd_06["PSD_Ges_Geschwindigkeit"]
self.v_limit_psd_legal = self._convert_raw_speed_psd(raw_speed, street_type)

0
artifacts/package_runtime/iqdbc/car/volkswagen/tests/__init__.py Normal file
View File

17
artifacts/package_runtime/iqdbc/car/volkswagen/tests/test_driver_override.py Normal file
View File

@@ -0,0 +1,17 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
import pytest
from iqdbc.car.volkswagen.carcontroller import accel_during_driver_override
@pytest.mark.parametrize("accel", [-3.5, -0.5, 0.0, 0.5, 2.0])
def test_opted_in_driver_override_sends_neutral_accel(accel):
assert accel_during_driver_override(accel, True, True) == 0.0
@pytest.mark.parametrize("gas_pressed,keep_long_active", [(False, False), (False, True), (True, False)])
def test_other_longitudinal_paths_preserve_accel(gas_pressed, keep_long_active):
assert accel_during_driver_override(-0.7, gas_pressed, keep_long_active) == -0.7

42
artifacts/package_runtime/iqdbc/car/volkswagen/tests/test_gra_cancel_tap.py Normal file
View File

@@ -0,0 +1,42 @@
from iqdbc.car.volkswagen.carcontroller import CarController
from iqdbc.car.volkswagen.values import CarControllerParams
class _Tapper:
CCP = CarControllerParams
def __init__(self):
self.gra_cancel_ticks = 0
def tap(self, cancel_req, gra_send_ready=True):
return CarController._tap_gra_cancel(self, cancel_req, gra_send_ready)
def _pattern(tapper, ticks, cancel_req=True):
return "".join("1" if tapper.tap(cancel_req) else "0" for _ in range(ticks))
def test_held_cancel_is_bounded():
held = _pattern(_Tapper(), 2000).count("1")
assert held == CarControllerParams.GRA_CANCEL_TAP_ON * CarControllerParams.GRA_CANCEL_MAX_TAPS
assert held < 50
def test_cancel_is_tapped_not_held():
on, off = CarControllerParams.GRA_CANCEL_TAP_ON, CarControllerParams.GRA_CANCEL_TAP_OFF
expected = ("1" * on + "0" * off) * CarControllerParams.GRA_CANCEL_MAX_TAPS
assert _pattern(_Tapper(), len(expected)) == expected
def test_taps_rearm_after_request_clears():
tapper = _Tapper()
_pattern(tapper, 2000)
assert not tapper.tap(False)
assert _pattern(tapper, CarControllerParams.GRA_CANCEL_TAP_ON) == "1" * CarControllerParams.GRA_CANCEL_TAP_ON
def test_ticks_only_advance_on_stock_counter_change():
tapper = _Tapper()
for _ in range(500):
assert tapper.tap(True, gra_send_ready=False)
assert tapper.gra_cancel_ticks == 0

49
artifacts/package_runtime/iqdbc/car/volkswagen/tests/test_iqpilot_vw_toggles.py Normal file
View File

@@ -0,0 +1,49 @@
from types import SimpleNamespace
from iqdbc.car.volkswagen import mqbcan, pqcan
from iqdbc.car.volkswagen.carcontroller import CarController
from iqdbc.car.volkswagen.values import CAR, MQB_A0_CARS
def test_is_mqb_a0_car_matches_expected_platforms():
assert CAR.VOLKSWAGEN_POLO_MK6 in MQB_A0_CARS
assert CAR.VOLKSWAGEN_TCROSS_MK1 in MQB_A0_CARS
assert CAR.SKODA_FABIA_MK4 in MQB_A0_CARS
assert CAR.SKODA_KAMIQ_MK1 in MQB_A0_CARS
assert CarController._is_mqb_a0_car(CAR.VOLKSWAGEN_POLO_MK6)
assert CarController._is_mqb_a0_car(CAR.VOLKSWAGEN_TCROSS_MK1)
assert CarController._is_mqb_a0_car(CAR.SKODA_FABIA_MK4)
assert CarController._is_mqb_a0_car(CAR.SKODA_KAMIQ_MK1)
assert not CarController._is_mqb_a0_car(CAR.VOLKSWAGEN_GOLF_MK7)
def test_mqb_steering_torque_scale_only_changes_when_toggle_enabled():
controller = object.__new__(CarController)
controller.CCS = mqbcan
assert controller._get_mqb_steering_torque_scale(0.4, False) == 1.0
assert controller._get_mqb_steering_torque_scale(0.4, True) == 0.8
assert controller._get_mqb_steering_torque_scale(4.0, True) == 1.0
controller.CCS = pqcan
assert controller._get_mqb_steering_torque_scale(0.4, True) == 1.0
def test_mqb_a0_resume_spam_requires_toggle_platform_and_window():
controller = object.__new__(CarController)
controller.CCS = mqbcan
controller.is_mqb_a0 = True
controller.frame = 10
cs = SimpleNamespace(out=SimpleNamespace(standstill=True))
assert controller._should_spam_mqb_a0_resume(cs, True)
controller.frame = 20
assert not controller._should_spam_mqb_a0_resume(cs, True)
controller.frame = 10
controller.is_mqb_a0 = False
assert not controller._should_spam_mqb_a0_resume(cs, True)
controller.is_mqb_a0 = True
assert not controller._should_spam_mqb_a0_resume(cs, False)

98
artifacts/package_runtime/iqdbc/car/volkswagen/tests/test_meb_nav_blinker.py Normal file
View File

@@ -0,0 +1,98 @@
"""Copyright (c) IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved."""
from iqdbc.can import CANPacker, CANParser
from iqdbc.car.volkswagen import mebcan
from iqdbc.car.volkswagen.carcontroller import ea_blinker_command, ea_send_ready, next_ea_counter
EA_HUD_VALUES = {
"COUNTER": 0,
"EA_Texte": 0,
"ACF_Lampe_Hands_Off": 0,
"EA_Infotainment_Anf": 0,
"EA_Tueren_Anf": 0,
"EA_Innenraumlicht_Anf": 0,
"zFAS_Warnblinken": 0,
"STP_Primaeranz": 0,
"EA_Bremslichtblinken": 0,
"EA_Blinken": 0,
"EA_Unknown": 0,
}
EA_CONTROL_VALUES = {"EA_Funktionsstatus": 2}
def blinker_values(dbc, requests):
packer = CANPacker(dbc)
parser = CANParser(dbc, [("EA_02", 50)], 0)
values = []
for counter, stock_blinker, left, right in requests:
ea_hud_values = {**EA_HUD_VALUES, "COUNTER": counter, "EA_Blinken": stock_blinker}
msg = mebcan.create_blinker_control(
packer, 0, ea_hud_values, EA_CONTROL_VALUES, left, right, False,
)
parser.update([0, [msg]])
values.append((int(parser.vl["EA_02"]["COUNTER"]), int(parser.vl["EA_02"]["EA_Blinken"])))
return values
def blinker_value(dbc, counter, stock_blinker, left, right, override_counter=None):
packer = CANPacker(dbc)
parser = CANParser(dbc, [("EA_02", 50)], 0)
ea_hud_values = {**EA_HUD_VALUES, "COUNTER": counter, "EA_Blinken": stock_blinker}
msg = mebcan.create_blinker_control(
packer, 0, ea_hud_values, EA_CONTROL_VALUES, left, right, False, override_counter,
)
parser.update([0, [msg]])
return int(parser.vl["EA_02"]["COUNTER"]), int(parser.vl["EA_02"]["EA_Blinken"])
def test_nav_blinker_request_remains_asserted_until_released():
requests = [(counter % 16, 0, True, False) for counter in range(100)] + [(4, 0, False, False)]
assert blinker_values("vw_meb", requests) == [(counter % 16, 1) for counter in range(100)] + [(4, 0)]
def test_nav_blinker_directions_on_meb_variants():
requests = [(7, 0, True, False), (8, 0, False, True), (9, 0, False, False)]
for dbc in ("vw_meb", "vw_meb_2024", "vw_mqbevo"):
assert blinker_values(dbc, requests) == [(7, 1), (8, 2), (9, 0)]
def test_stock_blinker_has_priority_over_nav_request():
requests = [(10, 1, False, True), (11, 2, True, False), (12, 3, True, False)]
assert blinker_values("vw_meb", requests) == [(10, 1), (11, 2), (12, 3)]
def test_ea_frame_is_sent_once_per_stock_counter():
last_counter = None
sends = []
for counter in (5, 5, 5, 6, 6, 7):
stock_values = {"COUNTER": counter}
if ea_send_ready(stock_values, last_counter):
sends.append(counter)
last_counter = counter
assert sends == [5, 6, 7]
def test_blinker_counter_can_continue_between_stock_frames():
assert blinker_value("vw_meb", 8, 0, True, False, 9) == (9, 1)
assert blinker_value("vw_meb", 8, 0, True, False, 10) == (10, 1)
def test_stock_blinker_priority_survives_counter_override():
assert blinker_value("vw_meb", 8, 2, True, False, 9) == (9, 2)
def test_nav_blinker_retriggers_only_between_lamp_cycles():
assert ea_blinker_command(True, False, False, False) == (True, False)
assert ea_blinker_command(True, False, True, False) == (False, False)
assert ea_blinker_command(True, False, False, False) == (True, False)
assert ea_blinker_command(False, True, False, True) == (False, False)
def test_blinker_counter_runs_at_20_ms():
tx_counter = None
counters = []
for _ in range(10):
tx_counter = next_ea_counter(tx_counter, 14)
counters.append(tx_counter)
assert counters == [15, 0, 1, 2, 3, 4, 5, 6, 7, 8]

363
artifacts/package_runtime/iqdbc/car/volkswagen/tests/test_mqb_extended_standstill.py Normal file
View File

@@ -0,0 +1,363 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
"""
import math
from types import SimpleNamespace
from iqdbc.car.volkswagen.carcontroller import MQBStandstillManager
def _pitch(grade_pct):
return math.atan(grade_pct / 100.0)
def _mgr():
return MQBStandstillManager(vehicle_mass=1540.0, accel_min=-3.5)
def _cs(*, esp_hold_confirmation=False, esp_stopping=False, rolling_backward=False,
rolling_forward=False, brake_pressed=False, gas_pressed=False, standstill=True, v_ego=0.0,
sum_wegimpulse=0):
out = SimpleNamespace(brakePressed=brake_pressed, gasPressed=gas_pressed, standstill=standstill, vEgo=v_ego)
return SimpleNamespace(out=out, esp_hold_confirmation=esp_hold_confirmation,
esp_stopping=esp_stopping, rolling_backward=rolling_backward,
rolling_forward=rolling_forward, sum_wegimpulse=sum_wegimpulse)
def _run(mgr, cs, *, long_active=True, accel=0.0, stopping=False, starting=False,
max_planned_speed=0.0, grade_pct=0.0, tsk_brake_torque=0.0):
return mgr.update(cs, long_active, accel, stopping, starting, max_planned_speed,
_pitch(grade_pct), tsk_brake_torque)
def _safe_speed(mgr, grade_pct, brake_torque=0.0):
return mgr.get_safe_speed_for_brake_torque(_pitch(grade_pct), brake_torque)
# ── brake press ──────────────────────────────────────────────────────────────
def test_brake_pressed_disables_long_active():
mgr = _mgr()
long_active, *_ = _run(mgr, _cs(brake_pressed=True), long_active=True)
assert not long_active
# ── rollback detection ───────────────────────────────────────────────────────
def test_rollback_detected_on_rolling_backward():
mgr = _mgr()
_run(mgr, _cs(rolling_backward=True))
assert mgr.rollback_detected
def test_rollback_clears_on_rolling_forward():
mgr = _mgr()
_run(mgr, _cs(rolling_backward=True))
assert mgr.rollback_detected
_run(mgr, _cs(rolling_forward=True))
assert not mgr.rollback_detected
def test_rollback_forces_brake():
mgr = _mgr()
_run(mgr, _cs(rolling_backward=True))
_, accel, *_ = _run(mgr, _cs(), accel=-0.5)
assert accel == mgr.accel_min
def test_rollback_cleared_when_rolling_forward_passes_through_accel():
mgr = _mgr()
_run(mgr, _cs(rolling_backward=True))
_, accel, *_ = _run(mgr, _cs(rolling_forward=True, standstill=False), accel=-0.5)
assert accel == -0.5
def test_rollback_forces_brake_with_positive_accel():
mgr = _mgr()
_run(mgr, _cs(rolling_backward=True))
_, accel, stopping, starting, *_ = _run(mgr, _cs(), accel=0.5)
assert accel == mgr.accel_min
assert stopping
assert not starting
# ── safe speed braking ───────────────────────────────────────────────────────
def test_flat_ground_passes_raw_accel():
mgr = _mgr()
_, accel, stopping, starting, *_ = _run(mgr, _cs(v_ego=0.0), accel=-0.55,
stopping=True, starting=False, grade_pct=0.0)
assert accel == -0.55
assert stopping
assert not starting
def test_current_brake_torque_reduces_safe_speed():
mgr = _mgr()
zero_brake_safe_speed = _safe_speed(mgr, 20.0)
current_brake_safe_speed = _safe_speed(mgr, 20.0, 500.0)
assert 0.0 < current_brake_safe_speed < zero_brake_safe_speed
def test_current_brake_torque_can_eliminate_safe_speed():
mgr = _mgr()
assert _safe_speed(mgr, 20.0, 10000.0) == 0.0
def test_safe_speed_is_capped_at_10_kph():
mgr = _mgr()
assert _safe_speed(mgr, 100.0) == MQBStandstillManager.MAX_SAFE_STOPPING_SPEED
def test_esp_stopping_passes_raw_accel_on_flat():
mgr = _mgr()
_, accel, stopping, starting, esp_starting_override, esp_stopping_override = _run(
mgr, _cs(esp_stopping=True, v_ego=0.2), accel=-0.55, stopping=True, starting=False,
)
assert accel == -0.55
assert stopping
assert not starting
assert esp_starting_override is True
assert esp_stopping_override is False
def test_below_safe_speed_blends_brake():
mgr = _mgr()
grade = 20.0
safe_speed = _safe_speed(mgr, grade)
required_torque = mgr.get_hill_hold_decel_deficit(_pitch(grade), 0.0) * mgr.vehicle_mass * mgr.ASSUMED_WHEEL_RADIUS
_, accel, stopping, starting, *_ = _run(
mgr, _cs(v_ego=safe_speed * 0.5), accel=-0.55, grade_pct=grade,
tsk_brake_torque=required_torque * 0.5,
)
assert mgr.accel_min < accel < -0.55
assert stopping
assert not starting
def test_sufficient_brake_torque_passes_raw_accel():
mgr = _mgr()
grade = 20.0
safe_speed = _safe_speed(mgr, grade)
_, accel, stopping, starting, *_ = _run(
mgr, _cs(v_ego=safe_speed * 0.5), accel=-0.2, stopping=True, starting=False,
grade_pct=grade, tsk_brake_torque=10000.0,
)
assert accel == -0.2
assert stopping
assert not starting
def test_below_safe_speed_with_low_planned_speed_uses_blended_braking():
mgr = _mgr()
grade = 20.0
safe_speed = _safe_speed(mgr, grade)
_, accel, stopping, starting, *_ = _run(
mgr, _cs(v_ego=safe_speed * 0.5), accel=-0.55,
max_planned_speed=safe_speed * 0.5, grade_pct=grade,
)
assert mgr.accel_min <= accel < -0.55
assert stopping
assert not starting
def test_below_safe_speed_with_high_planned_speed_and_negative_accel_uses_blended_braking():
mgr = _mgr()
grade = 20.0
safe_speed = _safe_speed(mgr, grade)
_, accel, stopping, starting, *_ = _run(
mgr, _cs(v_ego=safe_speed * 0.5), accel=-0.55,
max_planned_speed=safe_speed * 2.0, grade_pct=grade,
)
assert mgr.accel_min <= accel < -0.55
assert stopping
assert not starting
def test_below_safe_speed_with_high_planned_speed_and_positive_accel_uses_hill_takeoff():
mgr = _mgr()
grade = 20.0
safe_speed = _safe_speed(mgr, grade)
_, accel, stopping, starting, *_ = _run(
mgr, _cs(v_ego=safe_speed * 0.5), accel=0.1,
max_planned_speed=safe_speed * 2.0, grade_pct=grade,
)
assert accel == max(0.1, 0.1 * grade, 0.2)
assert starting
assert not stopping
# ── start commit ─────────────────────────────────────────────────────────────
def test_start_commit_on_pre_enable():
mgr = _mgr()
_run(mgr, _cs(esp_hold_confirmation=True))
assert mgr.start_commit_active
def test_start_commit_forces_accel():
mgr = _mgr()
grade = 10.0
expected_min = max(0.1 * grade, 0.2)
_run(mgr, _cs(esp_hold_confirmation=True), grade_pct=grade)
_, accel, stopping, starting, *_ = _run(
mgr, _cs(esp_hold_confirmation=True), accel=0.0, grade_pct=grade,
)
assert accel >= expected_min
assert starting
assert not stopping
def test_start_commit_clears_above_safe_speed_while_moving():
mgr = _mgr()
grade = 20.0
safe_speed = _safe_speed(mgr, grade)
_run(mgr, _cs(esp_hold_confirmation=True), grade_pct=grade)
assert mgr.start_commit_active
_run(mgr, _cs(v_ego=safe_speed * 2.0, standstill=True), grade_pct=grade)
assert not mgr.start_commit_active
def test_start_commit_persists_below_safe_speed():
mgr = _mgr()
grade = 20.0
safe_speed = _safe_speed(mgr, grade)
_run(mgr, _cs(esp_hold_confirmation=True), grade_pct=grade)
assert mgr.start_commit_active
_, accel, stopping, starting, *_ = _run(
mgr, _cs(v_ego=safe_speed * 0.5), accel=-0.55, grade_pct=grade,
)
assert mgr.start_commit_active
assert accel == max(-0.55, 0.1 * grade, 0.2)
assert starting
assert not stopping
# ── can_stop_forever / ESP override ──────────────────────────────────────────
def test_can_stop_forever_latches_on_esp_stopping():
mgr = _mgr()
*_, esp_start, esp_stop = _run(mgr, _cs(esp_stopping=True), accel=-1.0, stopping=True)
assert mgr.can_stop_forever
assert esp_start is True
assert esp_stop is False
def test_can_stop_forever_persists():
mgr = _mgr()
_run(mgr, _cs(esp_stopping=True), accel=-1.0, stopping=True)
*_, esp_start, esp_stop = _run(mgr, _cs(), accel=-1.0, stopping=True)
assert mgr.can_stop_forever
assert esp_start is True
assert esp_stop is False
def test_can_stop_forever_cleared_by_hold_confirmation():
mgr = _mgr()
_run(mgr, _cs(esp_stopping=True), accel=-1.0, stopping=True)
assert mgr.can_stop_forever
_run(mgr, _cs(esp_hold_confirmation=True), accel=-1.0, stopping=True)
assert not mgr.can_stop_forever
def test_hold_confirmation_recovers_stop_forever_after_launch_commit():
mgr = _mgr()
grade = 20.0
safe_speed = _safe_speed(mgr, grade)
*_, esp_start, esp_stop = _run(mgr, _cs(esp_hold_confirmation=True, sum_wegimpulse=0),
accel=0.0, grade_pct=grade)
assert mgr.start_commit_active
assert mgr.hold_recovery_active
assert esp_start is True
assert esp_stop is False
*_, esp_start, esp_stop = _run(mgr, _cs(v_ego=safe_speed * 2.0, standstill=False, sum_wegimpulse=1),
accel=0.0, grade_pct=grade)
assert not mgr.start_commit_active
assert mgr.hold_recovery_active
assert esp_start is False
assert esp_stop is True
*_, esp_start, esp_stop = _run(mgr, _cs(esp_stopping=True, v_ego=safe_speed * 2.0, standstill=False,
sum_wegimpulse=2), accel=0.0, grade_pct=grade)
assert mgr.can_stop_forever
assert not mgr.hold_recovery_active
assert esp_start is True
assert esp_stop is False
def test_can_stop_forever_cleared_when_moving():
mgr = _mgr()
_run(mgr, _cs(esp_stopping=True), accel=-1.0, stopping=True)
assert mgr.can_stop_forever
_run(mgr, _cs(v_ego=MQBStandstillManager.ESP_OVERRIDE_SPEED + 0.01), accel=0.5)
assert not mgr.can_stop_forever
def test_can_stop_forever_cleared_when_long_inactive():
mgr = _mgr()
_run(mgr, _cs(esp_stopping=True), accel=-1.0, stopping=True)
assert mgr.can_stop_forever
_run(mgr, _cs(), long_active=False)
assert not mgr.can_stop_forever
def test_esp_override_stop_at_standstill():
mgr = _mgr()
esp_start = esp_stop = None
for _ in range(MQBStandstillManager.WEGIMPULSE_STILLNESS_FRAMES + 1):
*_, esp_start, esp_stop = _run(mgr, _cs(sum_wegimpulse=0), accel=-1.0, stopping=True)
assert esp_start is False
assert esp_stop is True
def test_esp_override_stop_persists_at_standstill():
mgr = _mgr()
for _ in range(MQBStandstillManager.WEGIMPULSE_STILLNESS_FRAMES + 1):
_run(mgr, _cs(sum_wegimpulse=0), accel=-1.0, stopping=True)
*_, esp_start, esp_stop = _run(mgr, _cs(sum_wegimpulse=0), accel=-1.0, stopping=True)
assert esp_start is False
assert esp_stop is True
def test_esp_override_stop_not_sent_while_esp_stopping():
mgr = _mgr()
*_, esp_start, esp_stop = _run(mgr, _cs(esp_stopping=True, sum_wegimpulse=0),
accel=-1.0, stopping=True)
assert mgr.can_stop_forever
assert esp_start is True
assert esp_stop is False
def test_esp_override_stop_not_requested_while_moving():
mgr = _mgr()
esp_start = esp_stop = None
for sum_wegimpulse in range(MQBStandstillManager.WEGIMPULSE_STILLNESS_FRAMES + 1):
*_, esp_start, esp_stop = _run(mgr, _cs(sum_wegimpulse=sum_wegimpulse), accel=-1.0, stopping=True)
assert esp_start is True
assert esp_stop is False
def test_esp_override_default_below_grant_speed_when_inactive():
mgr = _mgr()
*_, esp_start, esp_stop = _run(mgr, _cs(), long_active=False)
assert esp_start is True
assert esp_stop is False
def test_wegimpulse_at_standstill_after_stillness_frames():
mgr = _mgr()
for _ in range(MQBStandstillManager.WEGIMPULSE_STILLNESS_FRAMES + 1):
_run(mgr, _cs(sum_wegimpulse=0))
assert mgr.frames_since_last_wheel_pulse >= MQBStandstillManager.WEGIMPULSE_STILLNESS_FRAMES
def test_wegimpulse_resets_on_change():
mgr = _mgr()
for _ in range(MQBStandstillManager.WEGIMPULSE_STILLNESS_FRAMES):
_run(mgr, _cs(sum_wegimpulse=0))
_run(mgr, _cs(sum_wegimpulse=1))
assert mgr.frames_since_last_wheel_pulse == 0

26
artifacts/package_runtime/iqdbc/car/volkswagen/tests/test_pq_accel_control.py Normal file
View File

@@ -0,0 +1,26 @@
"""
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos/
"""
import pytest
from iqdbc.can import CANPacker
from iqdbc.car.volkswagen import pqcan
@pytest.mark.parametrize("accel, acc_control, expected", [
(-1.0, 1, False),
(0.0, 1, False),
(0.05, 1, False),
(0.051, 1, True),
(1.0, 1, True),
(1.0, 0, False),
])
def test_positive_acceleration_prevents_fuel_cutoff(accel, acc_control, expected):
packer = CANPacker("vw_pq")
messages = pqcan.create_acc_accel_control(
packer, 0, 1, accel, acc_control, False, False, False, 0.2, 0.3, False,
)
assert len(messages) == 1
_, data, _ = messages[0]
assert bool(data[1] & 0x80) is expected

33
artifacts/package_runtime/iqdbc/car/volkswagen/tests/test_stopping.py Normal file
View File

@@ -0,0 +1,33 @@
import pytest
from iqdbc.car.volkswagen.interface import CarInterface
from iqdbc.car.volkswagen.values import (
CAR, PASSAT_B7_STOP_ACCEL, PASSAT_B7_STOPPING_SPEED, PQ_STOPPING_SPEED,
VolkswagenFlags, apply_pq_stopping_accel, get_longitudinal_stopping_speed_override,
)
@pytest.mark.parametrize("candidate, flags, expected", [
(CAR.VOLKSWAGEN_PASSAT_B7, VolkswagenFlags.PQ, PASSAT_B7_STOPPING_SPEED),
(CAR.VOLKSWAGEN_JETTA_MK6, VolkswagenFlags.PQ, PQ_STOPPING_SPEED),
(CAR.VOLKSWAGEN_GOLF_MK7, 0, 0.0),
(CAR.VOLKSWAGEN_ID4_MK1, VolkswagenFlags.MEB, 0.0),
])
def test_stopping_speed_override(candidate, flags, expected):
assert get_longitudinal_stopping_speed_override(candidate, flags) == expected
def test_passat_b7_stop_accel_is_exact():
assert apply_pq_stopping_accel(CAR.VOLKSWAGEN_PASSAT_B7, -0.2, True) == PASSAT_B7_STOP_ACCEL == -0.55
assert apply_pq_stopping_accel(CAR.VOLKSWAGEN_PASSAT_B7, -0.2, False) == -0.2
assert apply_pq_stopping_accel(CAR.VOLKSWAGEN_JETTA_MK6, -0.2, True) == -0.2
@pytest.mark.parametrize("candidate", [
CAR.VOLKSWAGEN_JETTA_MK6,
CAR.VOLKSWAGEN_PASSAT_NMS,
])
def test_pq_longitudinal_feedforward_uses_active_schema_field(candidate):
fingerprint = {bus: {} for bus in range(8)}
cp = CarInterface.get_params(candidate, fingerprint, [], alpha_long=True, is_release=False, docs=False)
assert cp.longitudinalTuning.kf == pytest.approx(1.2)

552
artifacts/package_runtime/iqdbc/car/volkswagen/tests/test_volkswagen.py Normal file
View File

@@ -0,0 +1,552 @@
import random
import re
from types import SimpleNamespace
import pytest
from iqdbc.can import CANPacker
from iqdbc.car import Bus, DT_CTRL, structs
from iqdbc.car.volkswagen.carstate import CarState
from iqdbc.car.structs import CarParams
from iqdbc.car.volkswagen.interface import CarInterface
from iqdbc.car.volkswagen.values import (CAR, FW_QUERY_CONFIG, MLB_ACC_COORDINATOR_MSGS, MLB_GEARBOX_MSGS, MLB_MSG_ACC_10,
MLB_MSG_GATEWAY_05, MLB_MSG_GETRIEBE_01, MLB_MSG_LH_EPS_03, WMI, VolkswagenFlags,
VolkswagenFlagsIQ, VolkswagenSafetyFlags)
from iqdbc.car.volkswagen.fingerprints import FW_VERSIONS
Ecu = CarParams.Ecu
CHASSIS_CODE_PATTERN = re.compile('[A-Z0-9]{2}')
# TODO: determine the unknown groups
SPARE_PART_FW_PATTERN = re.compile(b'\xf1\x87(?P<gateway>[0-9][0-9A-Z]{2})(?P<unknown>[0-9][0-9A-Z][0-9])(?P<unknown2>[0-9A-Z]{2}[0-9])([A-Z0-9]| )')
class TestVolkswagenPlatformConfigs:
def test_spare_part_fw_pattern(self, subtests):
# Relied on for determining if a FW is likely VW
for platform, ecus in FW_VERSIONS.items():
with subtests.test(platform=platform.value):
for fws in ecus.values():
for fw in fws:
assert SPARE_PART_FW_PATTERN.match(fw) is not None, f"Bad FW: {fw}"
def test_chassis_codes(self, subtests):
for platform in CAR:
with subtests.test(platform=platform.value):
assert len(platform.config.wmis) > 0, "WMIs not set"
assert len(platform.config.chassis_codes) > 0, "Chassis codes not set"
assert all(CHASSIS_CODE_PATTERN.match(cc) for cc in
platform.config.chassis_codes), "Bad chassis codes"
# No two platforms should share chassis codes
for comp in CAR:
if platform == comp:
continue
shared_chassis_codes = platform.config.chassis_codes & comp.config.chassis_codes
if len(shared_chassis_codes) == 0:
continue
# A shared chassis code is unambiguous when the VIN WMI separates the candidates.
if platform.config.wmis.isdisjoint(comp.config.wmis):
continue
platform_model_years = getattr(platform.config, "model_years", set())
comp_model_years = getattr(comp.config, "model_years", set())
if platform_model_years and comp_model_years and platform_model_years.isdisjoint(comp_model_years):
continue
radar_ecu = (Ecu.fwdRadar, 0x757, None)
platform_radar_fw = set(FW_VERSIONS.get(platform, {}).get(radar_ecu, []))
comp_radar_fw = set(FW_VERSIONS.get(comp, {}).get(radar_ecu, []))
if not platform_radar_fw or not comp_radar_fw:
continue
assert platform_radar_fw.isdisjoint(comp_radar_fw), f"Ambiguous VIN and radar firmware: {comp}"
def test_custom_fuzzy_fingerprinting(self, subtests):
all_radar_fw = list({fw for ecus in FW_VERSIONS.values() for fw in ecus.get((Ecu.fwdRadar, 0x757, None), [])})
for platform in CAR:
with subtests.test(platform=platform.name):
for wmi in WMI:
for chassis_code in platform.config.chassis_codes | {"00"}:
platform_model_years = getattr(platform.config, "model_years", set())
model_years = platform_model_years if platform_model_years else {"0"}
for model_year in model_years | {"0"}:
vin = ["0"] * 17
vin[0:3] = wmi
vin[6:8] = chassis_code
vin[9] = model_year
vin = "".join(vin)
for radar_fw in random.sample(all_radar_fw, 5) + [b'\xf1\x875Q0907572G \xf1\x890571', b'\xf1\x877H9907572AA\xf1\x890396']:
live_fws = {(0x757, None): [radar_fw]}
matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fws, vin, FW_VERSIONS)
expected_matches = set()
for candidate in CAR:
candidate_model_years = getattr(candidate.config, "model_years", set())
candidate_has_radar = (Ecu.fwdRadar, 0x757, None) in FW_VERSIONS.get(candidate, {})
model_year_match = not candidate_model_years or model_year in candidate_model_years
if (wmi in candidate.config.wmis and chassis_code in candidate.config.chassis_codes and model_year_match and
radar_fw in all_radar_fw and candidate_has_radar):
expected_matches.add(candidate)
assert expected_matches == matches, "Bad match"
@pytest.mark.parametrize("candidate, expected", (
(CAR.VOLKSWAGEN_PASSAT_B7, True),
(CAR.SEAT_ALHAMBRA_MK1, True),
(CAR.VOLKSWAGEN_GOLF_MK7, False),
))
def test_pq_acc_fts_epb_flags(self, candidate, expected):
params = CarInterface.get_params(candidate, {bus: {} for bus in range(7)}, [], alpha_long=False, is_release=False, docs=False)
assert bool(params.flags & VolkswagenFlagsIQ.IQ_PQ_ACC_FTS_EPB) is expected
assert bool(params.safetyConfigs[-1].safetyParam & VolkswagenSafetyFlags.PQ_ACC_FTS_EPB) is expected
@pytest.mark.parametrize("candidate, alpha_long, expected", (
(CAR.VOLKSWAGEN_PASSAT_B7, False, False),
(CAR.VOLKSWAGEN_PASSAT_B7, True, True),
(CAR.VOLKSWAGEN_GOLF_MK7, False, False),
(CAR.VOLKSWAGEN_GOLF_MK7, True, True),
(CAR.AUDI_Q5_MK1, True, False),
(CAR.VOLKSWAGEN_ID4_MK1, True, False),
(CAR.VOLKSWAGEN_GOLF_MK8, True, False),
))
def test_supported_vw_longitudinal_stays_active_during_gas_override(self, candidate, alpha_long, expected):
fingerprints = {bus: {} for bus in range(7)}
params = CarInterface.get_params(candidate, fingerprints, [], alpha_long=alpha_long, is_release=False, docs=False)
params_iq = CarInterface.get_params_iq(params, candidate, fingerprints, [], alpha_long=alpha_long, is_release_iq=False, docs=False)
assert params_iq.longActiveWithGasOverride is expected
if expected:
assert bool(params.safetyConfigs[-1].safetyParam & VolkswagenSafetyFlags.ALLOW_LONG_ACCEL_WITH_GAS_PRESSED) is alpha_long
def _mlb_fingerprint(bus, msgs):
fingerprints = {b: {} for b in range(7)}
fingerprints[bus] = {msg: 8 for msg in msgs}
return fingerprints
A4_MK4_BUS_1 = (0x040, 0x080, 0x081, 0x086, 0x100, 0x101, 0x103, 0x104, 0x105, 0x106, 0x107, 0x10B,
0x10C, 0x10E, 0x114, 0x11D, 0x309, 0x30B, 0x30E, 0x312, 0x391, 0x392, 0x39C, 0x3BF,
0x3C0, 0x440, 0x471, 0x520, 0x585, 0x590, 0x5F0, 0x640, 0x641, 0x643, 0x644, 0x647,
0x670, 0x6B2, 0x6B4, 0x6B7, 0x6B8, 0x6C0, 0x6C1)
MLB_ECAN_GATEWAY = (0x086, 0x09F, 0x102, 0x103, 0x105, 0x106, 0x10B, 0x10C, 0x30B)
MLB_ECAN_CAMERA = (0x109, 0x10D, 0x117, 0x30C, 0x30F)
def test_mlb_no_ecan_car_moves_to_the_powertrain_bus():
fingerprints = _mlb_fingerprint(1, A4_MK4_BUS_1)
params = CarInterface.get_params(CAR.AUDI_A4_MK4, fingerprints, [], alpha_long=True, is_release=False, docs=False)
assert params.flags & VolkswagenFlagsIQ.IQ_MLB_NO_ECAN
assert params.safetyConfigs[-1].safetyParam & VolkswagenSafetyFlags.MLB_NO_ECAN
assert params.flags & VolkswagenFlagsIQ.IQ_CC_ONLY_NO_RADAR
assert params.flags & VolkswagenFlagsIQ.IQ_MLB_NO_HCA_EPS
assert params.transmissionType == CarParams.TransmissionType.manual
assert not params.alphaLongitudinalAvailable
assert not params.openpilotLongitudinalControl
assert params.dashcamOnly
def test_mlb_without_an_extended_can_is_not_controllable():
for msgs in (A4_MK4_BUS_1, A4_MK4_BUS_1 + (0x9F,)):
params = CarInterface.get_params(CAR.AUDI_A4_MK4, _mlb_fingerprint(1, msgs), [],
alpha_long=False, is_release=False, docs=False)
assert params.dashcamOnly
def test_mlb_gateway_car_keeps_the_extended_can():
fingerprints = _mlb_fingerprint(0, MLB_ECAN_GATEWAY)
fingerprints[2] = {msg: 8 for msg in MLB_ECAN_CAMERA}
params = CarInterface.get_params(CAR.AUDI_Q5_MK1, fingerprints, [], alpha_long=False, is_release=False, docs=False)
assert not params.flags & VolkswagenFlagsIQ.IQ_MLB_NO_ECAN
assert not params.flags & (VolkswagenFlagsIQ.IQ_CC_ONLY | VolkswagenFlagsIQ.IQ_CC_ONLY_NO_RADAR)
assert not params.flags & VolkswagenFlagsIQ.IQ_MLB_NO_HCA_EPS
assert params.transmissionType == CarParams.TransmissionType.automatic
assert params.alphaLongitudinalAvailable
assert not params.dashcamOnly
def test_mlb_flags_are_not_inferred_without_a_fingerprint():
fingerprints = {bus: {} for bus in range(7)}
for platform in (CAR.AUDI_Q5_MK1, CAR.PORSCHE_MACAN_MK1, CAR.AUDI_A4_MK4):
params = CarInterface.get_params(platform, fingerprints, [], alpha_long=False, is_release=False, docs=True)
assert not params.flags & VolkswagenFlagsIQ.IQ_MLB_NO_ECAN
assert not params.flags & (VolkswagenFlagsIQ.IQ_CC_ONLY | VolkswagenFlagsIQ.IQ_CC_ONLY_NO_RADAR)
assert not params.flags & VolkswagenFlagsIQ.IQ_MLB_NO_HCA_EPS
assert params.transmissionType == CarParams.TransmissionType.automatic
def _build_mlb_car(platform, fingerprints):
CP = CarInterface.get_params(platform, fingerprints, [], alpha_long=False, is_release=False, docs=False)
CP_IQ = CarInterface.get_params_iq(CP, platform, fingerprints, [], alpha_long=False, is_release_iq=False, docs=False)
return CarInterface(CP, CP_IQ)
def _a4_mk4_car():
return _build_mlb_car(CAR.AUDI_A4_MK4, _mlb_fingerprint(1, A4_MK4_BUS_1))
def _q5_mk1_car():
fingerprints = _mlb_fingerprint(0, MLB_ECAN_GATEWAY)
fingerprints[2] = {msg: 8 for msg in MLB_ECAN_CAMERA}
return _build_mlb_car(CAR.AUDI_Q5_MK1, fingerprints)
def _a4_mk4_frames(packer, reverse=False, eps_torque=None):
msgs = [
packer.make_can_msg("ESP_03", 1, {"ESP_%s_Radgeschw" % s: 30.0 for s in ("VL", "VR", "HL", "HR")}),
packer.make_can_msg("Motor_03", 1, {"MO_Fahrpedalrohwert_01": 0, "MO_BLS": 0}),
packer.make_can_msg("ESP_05", 1, {"ESP_Bremsdruck": 0, "ESP_Fahrer_bremst": 0}),
packer.make_can_msg("ESP_01", 1, {"ESP_Tastung_passiv": 0}),
packer.make_can_msg("ESP_02", 1, {"ESP_Stillstandsflag": 0}),
packer.make_can_msg("TSK_02", 1, {"TSK_Status": 0}),
packer.make_can_msg("LS_01", 1, {"LS_Hauptschalter": 1, "LS_Codierung": 2}),
packer.make_can_msg("LWI_01", 1, {"LWI_Lenkradwinkel": 12.0, "LWI_Lenkradw_Geschw": 4.0}),
packer.make_can_msg("Kombi_01", 1, {"KBI_angez_Geschw": 30.0, "KBI_Handbremse": 0}),
packer.make_can_msg("Kombi_02", 1, {"KBI_Inhalt_Tank": 40, "KBI_Kilometerstand": 100000}),
packer.make_can_msg("Airbag_02", 1, {"AB_Gurtschloss_FA": 3}),
packer.make_can_msg("Gateway_05", 1, {"BCM1_Rueckfahrlicht_Schalter": int(reverse)}),
packer.make_can_msg("LH_EPS_01", 1, {}),
]
if eps_torque is not None:
msgs.append(packer.make_can_msg("LH_EPS_03", 1, {"EPS_Lenkmoment": abs(eps_torque),
"EPS_VZ_Lenkmoment": eps_torque < 0,
"EPS_HCA_Status": 3}))
return msgs
def _run(car, build, frames=25):
nanos = 0
for _ in range(frames):
nanos += int(DT_CTRL * 1e9)
ret, _ = car.update([(nanos, build())])
return ret
def test_mlb_no_ecan_parsers_read_the_powertrain_bus():
a4 = _a4_mk4_car()
assert a4.CS.get_can_parsers(a4.CP, a4.CP_IQ)[Bus.pt].bus == 1
q5 = _q5_mk1_car()
assert q5.CS.get_can_parsers(q5.CP, q5.CP_IQ)[Bus.pt].bus == 0
def test_mlb_no_ecan_transmits_on_the_powertrain_bus():
a4 = _a4_mk4_car()
packer = CANPacker("vw_mlb")
CC = structs.CarControl()
CC.enabled = True
CC.latActive = True
CC.cruiseControl.cancel = True
CC = CC.as_reader()
CC_IQ = structs.IQCarControl()
sent = []
nanos = 0
for _ in range(20):
nanos += int(DT_CTRL * 1e9)
a4.update([(nanos, _a4_mk4_frames(packer, eps_torque=0))])
_, can_sends = a4.apply(CC, CC_IQ, nanos)
sent.extend(can_sends)
assert {addr for addr, _, _ in sent} == {0x126, 0x397, 0x10B}
assert {bus for _, _, bus in sent} == {1}
def test_mlb_no_hca_eps_reports_a_steer_fault_without_a_can_fault():
a4 = _a4_mk4_car()
assert a4.CP.flags & VolkswagenFlagsIQ.IQ_MLB_NO_HCA_EPS
packer = CANPacker("vw_mlb")
ret = _run(a4, lambda: _a4_mk4_frames(packer))
assert ret.steerFaultPermanent
assert not ret.steerFaultTemporary
assert ret.steeringTorque == 0.0
assert not ret.steeringPressed
assert ret.steeringAngleDeg == pytest.approx(12.0, abs=0.2)
pt = a4.can_parsers[Bus.pt]
assert pt.message_states[MLB_MSG_LH_EPS_03].ignore_alive
assert all(parser.can_valid for parser in a4.can_parsers.values())
def test_mlb_no_hca_eps_survives_the_parser_aliveness_timeout():
a4 = _a4_mk4_car()
packer = CANPacker("vw_mlb")
pt = a4.can_parsers[Bus.pt]
nanos = 0
for _ in range(int(15.0 / DT_CTRL)):
nanos += int(DT_CTRL * 1e9)
ret, _ = a4.update([(nanos, _a4_mk4_frames(packer))])
pt.vl["LH_EPS_03"]["EPS_HCA_Status"]
assert ret.steerFaultPermanent
assert all(parser.can_valid for parser in a4.can_parsers.values())
assert not any(parser.bus_timeout for parser in a4.can_parsers.values())
def test_mlb_lane_assist_eps_restores_the_normal_torque_path():
fingerprints = _mlb_fingerprint(1, A4_MK4_BUS_1 + (0x9F,))
a4 = _build_mlb_car(CAR.AUDI_A4_MK4, fingerprints)
assert not a4.CP.flags & VolkswagenFlagsIQ.IQ_MLB_NO_HCA_EPS
packer = CANPacker("vw_mlb")
ret = _run(a4, lambda: _a4_mk4_frames(packer, eps_torque=-140))
assert ret.steeringTorque == pytest.approx(-140, abs=1)
assert ret.steeringPressed
assert not ret.steerFaultPermanent
assert not a4.can_parsers[Bus.pt].message_states[MLB_MSG_LH_EPS_03].ignore_alive
def test_mlb_cc_only_never_reads_the_acc_coordinator():
a4 = _a4_mk4_car()
assert a4.CP.flags & VolkswagenFlagsIQ.IQ_CC_ONLY_NO_RADAR
packer = CANPacker("vw_mlb")
ret = _run(a4, lambda: _a4_mk4_frames(packer))
assert ret.cruiseState.available
assert not ret.cruiseState.enabled
assert ret.cruiseState.speed == 0.0
parsers = a4.can_parsers
for msg in MLB_ACC_COORDINATOR_MSGS + (MLB_MSG_ACC_10,):
for parser in parsers.values():
assert msg not in parser.addresses
def test_mlb_manual_gear_follows_the_reverse_light_switch():
a4 = _a4_mk4_car()
assert a4.CP.transmissionType == CarParams.TransmissionType.manual
packer = CANPacker("vw_mlb")
assert _run(a4, lambda: _a4_mk4_frames(packer, reverse=False)).gearShifter == structs.CarState.GearShifter.drive
assert _run(a4, lambda: _a4_mk4_frames(packer, reverse=True)).gearShifter == structs.CarState.GearShifter.reverse
assert _run(a4, lambda: _a4_mk4_frames(packer, reverse=False)).gearShifter == structs.CarState.GearShifter.drive
def _q5_params(bus0, bus1=(), car_fw=()):
fingerprints = _mlb_fingerprint(0, bus0)
fingerprints[1] = {msg: 8 for msg in bus1}
fingerprints[2] = {msg: 8 for msg in MLB_ECAN_CAMERA}
return CarInterface.get_params(CAR.AUDI_Q5_MK1, fingerprints, list(car_fw), alpha_long=False, is_release=False, docs=False)
MLB_ECAN_GATEWAY_NO_GEARBOX = tuple(msg for msg in MLB_ECAN_GATEWAY if msg not in MLB_GEARBOX_MSGS)
def test_mlb_gearbox_on_the_powertrain_bus_keeps_automatic():
params = _q5_params(MLB_ECAN_GATEWAY_NO_GEARBOX, bus1=(MLB_MSG_GETRIEBE_01, MLB_MSG_GATEWAY_05))
assert params.transmissionType == CarParams.TransmissionType.automatic
def test_mlb_transmission_ecu_firmware_keeps_automatic():
transmission = CarParams.CarFw.new_message(ecu=Ecu.transmission, address=0x7e1)
params = _q5_params(MLB_ECAN_GATEWAY_NO_GEARBOX, bus1=(MLB_MSG_GATEWAY_05,), car_fw=(transmission,))
assert params.transmissionType == CarParams.TransmissionType.automatic
def test_mlb_reverse_switch_on_the_extended_can_alone_keeps_automatic():
params = _q5_params(MLB_ECAN_GATEWAY_NO_GEARBOX + (MLB_MSG_GATEWAY_05,))
assert params.transmissionType == CarParams.TransmissionType.automatic
def test_mlb_manual_needs_no_gearbox_anywhere_and_a_reverse_switch_on_the_powertrain_bus():
params = _q5_params(MLB_ECAN_GATEWAY_NO_GEARBOX, bus1=(MLB_MSG_GATEWAY_05,))
assert params.transmissionType == CarParams.TransmissionType.manual
def test_mlb_manual_gateway_car_reads_reverse_from_the_powertrain_bus():
fingerprints = _mlb_fingerprint(0, MLB_ECAN_GATEWAY_NO_GEARBOX)
fingerprints[1] = {MLB_MSG_GATEWAY_05: 8}
fingerprints[2] = {msg: 8 for msg in MLB_ECAN_CAMERA}
q5 = _build_mlb_car(CAR.AUDI_Q5_MK1, fingerprints)
assert q5.CP.transmissionType == CarParams.TransmissionType.manual
packer = CANPacker("vw_mlb")
frames = lambda reverse: [packer.make_can_msg("Gateway_05", 1, {"BCM1_Rueckfahrlicht_Schalter": int(reverse)})]
assert _run(q5, lambda: frames(True)).gearShifter == structs.CarState.GearShifter.reverse
assert _run(q5, lambda: frames(False)).gearShifter == structs.CarState.GearShifter.drive
assert MLB_MSG_GATEWAY_05 not in q5.can_parsers[Bus.pt].addresses
def test_mlb_automatic_never_subscribes_gateway_05():
q5 = _q5_mk1_car()
q5.update([(int(DT_CTRL * 1e9), [])])
assert MLB_MSG_GATEWAY_05 not in q5.can_parsers[Bus.pt].addresses
assert MLB_MSG_GATEWAY_05 not in q5.can_parsers[Bus.aux].addresses
@pytest.mark.parametrize("button_type", (
structs.CarState.ButtonEvent.Type.setCruise,
structs.CarState.ButtonEvent.Type.resumeCruise,
))
def test_button_enable_is_blocked_while_cruise_fault_lateral_mode_is_still_faulted(button_type):
state = object.__new__(CarState)
state.CP = SimpleNamespace(pcmCruise=False)
state.cruise_fault_lateral_active = True
state.cruise_faulted = True
button_events = [structs.CarState.ButtonEvent(pressed=False, type=button_type)]
assert not state.update_button_enable(button_events)
def test_button_enable_recovers_once_cruise_fault_clears():
state = object.__new__(CarState)
state.CP = SimpleNamespace(pcmCruise=False)
state.cruise_fault_lateral_active = True
state.cruise_faulted = False
button_events = [structs.CarState.ButtonEvent(
pressed=False,
type=structs.CarState.ButtonEvent.Type.setCruise,
)]
assert state.update_button_enable(button_events)
def test_pq_hca_ready_does_not_complete_eps_initialization():
state = object.__new__(CarState)
state.eps_init_complete = False
state.frame = 0
assert state.update_hca_state("READY", ready_confirms_init=False) == (True, False)
state.frame = 317
assert state.update_hca_state("FAULT", ready_confirms_init=False) == (True, False)
state.frame = 1001
assert state.update_hca_state("FAULT", ready_confirms_init=False) == (False, True)
@pytest.mark.parametrize("platform", (CAR.AUDI_Q4_MK1, CAR.VOLKSWAGEN_ID4_MK2))
def test_meb_does_not_infer_mqb_cluster_from_address(platform):
fingerprints = {bus: {} for bus in range(7)}
fingerprints[0][0x30B] = 8
params = CarInterface.get_params(platform, fingerprints, [], alpha_long=False, is_release=False, docs=False)
assert not params.flags & VolkswagenFlags.KOMBI_PRESENT
def _subscribed_addresses(car, updates=5):
nanos = 0
for _ in range(updates):
nanos += int(DT_CTRL * 1e9)
car.update([(nanos, [])])
return {bus: set(parser.addresses) for bus, parser in car.can_parsers.items()}
def _frames_for(car, packer, subscribed):
frames = []
for bus, parser in car.can_parsers.items():
for addr in sorted(subscribed[bus]):
frames.append(packer.make_can_msg(parser.dbc.addr_to_msg[addr].name, parser.bus, {}))
return frames
def _run_past_aliveness_timeout(car, build, seconds=15.0):
nanos = 0
ret = None
for _ in range(int(seconds / DT_CTRL)):
nanos += int(DT_CTRL * 1e9)
ret, _ = car.update([(nanos, build())])
return ret
def _sparse_q5(car_fw=(), bus1=()):
fingerprints = _mlb_fingerprint(0, MLB_ECAN_GATEWAY_NO_GEARBOX)
fingerprints[1] = {msg: 8 for msg in bus1}
fingerprints[2] = {msg: 8 for msg in MLB_ECAN_CAMERA}
CP = CarInterface.get_params(CAR.AUDI_Q5_MK1, fingerprints, list(car_fw), alpha_long=False, is_release=False, docs=False)
CP_IQ = CarInterface.get_params_iq(CP, CAR.AUDI_Q5_MK1, fingerprints, list(car_fw), alpha_long=False, is_release_iq=False, docs=False)
return CarInterface(CP, CP_IQ)
@pytest.mark.parametrize("car_fw", (
(),
(CarParams.CarFw.new_message(ecu=Ecu.transmission, address=0x7e1),),
), ids=("no_fw", "transmission_fw"))
def test_mlb_gateway_car_with_a_sparse_fingerprint_snapshot_reads_only_what_an_automatic_reads(car_fw):
automatic = _subscribed_addresses(_q5_mk1_car())
assert all(MLB_MSG_GATEWAY_05 not in addrs for addrs in automatic.values())
q5 = _sparse_q5(car_fw)
packer = CANPacker("vw_mlb")
ret = _run_past_aliveness_timeout(q5, lambda: _frames_for(q5, packer, automatic))
assert ret.canValid
assert all(parser.can_valid for parser in q5.can_parsers.values())
assert not any(parser.bus_timeout for parser in q5.can_parsers.values())
assert {bus: set(parser.addresses) for bus, parser in q5.can_parsers.items()} == automatic
assert q5.CP.transmissionType == CarParams.TransmissionType.automatic
def test_mlb_manual_gateway_car_adds_only_the_reverse_switch_on_the_powertrain_bus():
automatic = _subscribed_addresses(_q5_mk1_car())
manual = _subscribed_addresses(_sparse_q5(bus1=(MLB_MSG_GATEWAY_05,)))
assert manual[Bus.pt] == automatic[Bus.pt]
assert manual[Bus.cam] == automatic[Bus.cam]
assert manual[Bus.aux] == automatic[Bus.aux] | {MLB_MSG_GATEWAY_05}
@pytest.mark.parametrize("build", (_a4_mk4_car, _q5_mk1_car), ids=("a4_mk4", "q5_mk1"))
def test_mlb_carstate_subscriptions_settle_and_stay_alive(build):
car = build()
subscribed = _subscribed_addresses(car)
packer = CANPacker("vw_mlb")
ret = _run_past_aliveness_timeout(car, lambda: _frames_for(car, packer, subscribed))
assert {bus: set(parser.addresses) for bus, parser in car.can_parsers.items()} == subscribed
assert ret.canValid
assert all(parser.can_valid for parser in car.can_parsers.values())
assert not any(parser.bus_timeout for parser in car.can_parsers.values())
def _steering_state(flags, lwi=(0.0, 0), eps=(0.0, 0)):
state = object.__new__(CarState)
state.CP = SimpleNamespace(flags=flags)
state.CCP = SimpleNamespace(STEER_DRIVER_ALLOWANCE=100, hca_status_values={5: "ACTIVE"})
state.eps_init_complete = True
state.frame = 0
pt_cp = SimpleNamespace(vl={
"LWI_01": {"LWI_Lenkradwinkel": lwi[0], "LWI_VZ_Lenkradwinkel": lwi[1],
"LWI_Lenkradw_Geschw": 4.0, "LWI_VZ_Lenkradw_Geschw": 0},
"LH_EPS_03": {"EPS_Berechneter_LW": eps[0], "EPS_VZ_BLW": eps[1],
"EPS_Lenkmoment": 0.0, "EPS_VZ_Lenkmoment": 0, "EPS_HCA_Status": 5},
})
ret = structs.CarState()
state.parse_mlb_mqb_steering_state(ret, pt_cp)
return ret
def test_mlb_takes_the_steering_angle_from_the_eps():
ret = _steering_state(VolkswagenFlags.MLB, lwi=(12.0, 0), eps=(9.6, 0))
assert ret.steeringAngleDeg == pytest.approx(9.6)
assert ret.steeringRateDeg == pytest.approx(4.0)
def test_mlb_eps_angle_honours_its_own_sign_bit():
ret = _steering_state(VolkswagenFlags.MLB, lwi=(12.0, 0), eps=(9.6, 1))
assert ret.steeringAngleDeg == pytest.approx(-9.6)
def test_mlb_without_hca_eps_keeps_the_steering_wheel_sensor():
flags = VolkswagenFlags.MLB | VolkswagenFlagsIQ.IQ_MLB_NO_HCA_EPS
ret = _steering_state(flags, lwi=(12.0, 1), eps=(9.6, 0))
assert ret.steeringAngleDeg == pytest.approx(-12.0)
def test_mqb_keeps_the_steering_wheel_sensor():
ret = _steering_state(0, lwi=(12.0, 0), eps=(9.6, 0))
assert ret.steeringAngleDeg == pytest.approx(12.0)

985
artifacts/package_runtime/iqdbc/car/volkswagen/values.py Normal file
View File

@@ -0,0 +1,985 @@
from collections import defaultdict, namedtuple
from dataclasses import dataclass, field
from enum import Enum, IntFlag, StrEnum
from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CanBusBase, CarSpecs, DbcDict, DT_CTRL, PlatformConfig, Platforms, structs, uds
from iqdbc.car.lateral import CurvatureSteeringLimits
from iqdbc.can import CANDefine
from iqdbc.car.common.conversions import Conversions as CV
from iqdbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
from iqdbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL, ISO_LATERAL_JERK
from iqdbc.car.fw_query_definitions import EcuAddrSubAddr, FwQueryConfig, Request, p16
from iqdbc.car.vin import Vin
Ecu = structs.CarParams.Ecu
NetworkLocation = structs.CarParams.NetworkLocation
TransmissionType = structs.CarParams.TransmissionType
DashcamOnlyReason = structs.CarParams.DashcamOnlyReason
GearShifter = structs.CarState.GearShifter
Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
class CanBus(CanBusBase):
def __init__(self, CP=None, fingerprint=None) -> None:
super().__init__(CP, fingerprint)
self.offset = 0
@property
def pt(self) -> int:
# ADAS / Extended CAN, gateway side of the relay
return 0
@property
def aux(self) -> int:
# NetworkLocation.fwdCamera: radar-camera object fusion CAN
# NetworkLocation.gateway: powertrain CAN
return 1
@property
def powertrain(self) -> int:
return 1
@property
def main(self) -> int:
return 1
@property
def cam(self) -> int:
# ADAS / Extended CAN, camera side of the relay
return 2
@property
def ext(self) -> int:
# ADAS / Extended CAN, side of the relay with the ACC radar
return 2
# Extra Tolerances For Road Variance
AVERAGE_ROAD_ROLL = 0.06
PQ_STOPPING_SPEED = 1.5 * CV.KPH_TO_MS
PASSAT_B7_STOPPING_SPEED = 0.55 * CV.KPH_TO_MS
PASSAT_B7_STOP_ACCEL = -0.55
class CarControllerParams:
ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
# Max Steering Angle Allowed
500, # deg
# Volkswagen uses a vehicle model
([], []),
([], []),
# Vehicle Model Angle Limits
# Add extra tolerance for average banked road since safety doesn't have the roll calculation
MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), # ~3.6 m/s^2
MAX_LATERAL_JERK=ISO_LATERAL_JERK + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL), # ~3.6 m/s^3
# Limit Angle Rate to both prevent a openpilot fault and for low speed comfort (~12 mph rate down to 0 mph)
MAX_ANGLE_RATE=5, # deg/20ms frame
)
STEER_STEP = 2 # HCA_01/HCA_1 message frequency 50Hz
ACC_CONTROL_STEP = 2 # ACC_06/ACC_07/ACC_System frequency 50Hz
AEB_CONTROL_STEP = 2 # ACC_10 frequency 50Hz
AEB_HUD_STEP = 20 # ACC_15 frequency 5Hz
VW_LOW_SPEED_STATE_SPEED = 0.5 # m/s; below this, always force starting or stopping in MQB legacy long
SNG_HANDOFF_SPEED = 5.0 * CV.KPH_TO_MS
SNG_HOLD_DECEL_MAX = -0.1
# Documented lateral limits: 3.00 Nm max, rate of change 5.00 Nm/sec.
# MQB vs PQ maximums are shared, but rate-of-change limited differently
# based on safety requirements driven by lateral accel testing.
STEER_MAX = 300 # Max heading control assist torque 3.00 Nm
STEER_DRIVER_MULTIPLIER = 3 # weight driver torque heavily
STEER_DRIVER_FACTOR = 1 # from dbc
STEER_TIME_MAX = 360 # Max time that EPS allows uninterrupted HCA steering control
STEER_TIME_BM = STEER_TIME_MAX - 120 # Attempts to mitigate the EPS max steer timer begin
STEER_TIME_ALERT = STEER_TIME_MAX - 10 # If mitigation fails, time to soft disengage before EPS timer expires
STEER_LOW_TORQUE = int(STEER_MAX * 0.20) # Steer timer mitigation performed when torque output under 20%
STEER_TIME_LOW_TORQUE = 0.5 # Wait for this duration of STEER_LOW_TORQUE to begin mitigation
STEER_TIME_STUCK_TORQUE = 1.9 # EPS limits same torque to 6 seconds, reset timer 3x within that period
STEER_TIME_RESET = 1.1 # Duration of HCA disable needed for effective EPS timer reset'
IQ_PQ_UNAVAILABLE_HUD_FRAMES = 25
GRA_CANCEL_TAP_ON = 10 # Stock GRA frames to hold Abbrechen, roughly a human button tap
GRA_CANCEL_TAP_OFF = 20 # Stock GRA frames to release between taps
GRA_CANCEL_MAX_TAPS = 3 # PQ engine faults its GRA input if Abbrechen is held indefinitely
DEFAULT_MIN_STEER_SPEED = 0.4 # m/s, newer EPS racks fault below this speed, don't show a low speed alert
ACCEL_MAX = 2.0 # 2.0 m/s max acceleration
ACCEL_MIN = -3.5 # 3.5 m/s max deceleration
VW_LOW_SPEED_STATE_SPEED = 0.5 # Always send either starting or stopping below this speed
def __init__(self, CP):
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
self.STEER_TIME_RESET = self.__class__.STEER_TIME_RESET
if CP.flags & VolkswagenFlags.PQ:
self.LDW_STEP = 5 # LDW_1 message frequency 20Hz
self.ACC_HUD_STEP = 4 # ACC_GRA_Anzeige frequency 25Hz
self.STEER_DRIVER_ALLOWANCE = 80
self.STEER_DELTA_UP = 6
self.STEER_DELTA_DOWN = 10
if CP.transmissionType == TransmissionType.automatic:
self.shifter_values = can_define.dv["Getriebe_1"]["GE1_Wahl_Pos"]
self.hca_status_values = can_define.dv["Lenkhilfe_2"]["LH2_Sta_HCA"]
self.BUTTONS = [
Button(structs.CarState.ButtonEvent.Type.setCruise, "GRA_Neu", "GRA_Neu_Setzen", [1]),
Button(structs.CarState.ButtonEvent.Type.resumeCruise, "GRA_Neu", "GRA_Recall", [1]),
Button(structs.CarState.ButtonEvent.Type.accelCruise, "GRA_Neu", "GRA_Up_kurz", [1]),
Button(structs.CarState.ButtonEvent.Type.decelCruise, "GRA_Neu", "GRA_Down_kurz", [1]),
Button(structs.CarState.ButtonEvent.Type.cancel, "GRA_Neu", "GRA_Abbrechen", [1]),
Button(structs.CarState.ButtonEvent.Type.gapAdjustCruise, "GRA_Neu", "GRA_Zeitluecke", [1, 2, 3]),
]
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"
}
elif CP.flags & (VolkswagenFlags.MEB | VolkswagenFlags.MQB_EVO):
self.LDW_STEP = 10
self.ACC_HUD_STEP = 6
self.STEER_DRIVER_ALLOWANCE = 80
self.STEER_DRIVER_MAX = 300
self.STEERING_POWER_MAX = 90
self.STEERING_POWER_MIN = 4
self.STEERING_POWER_STEP = 2
self.CURVATURE_HANDOFF_RATE = 0.20
self.CURVATURE_PID: structs.CarParams.LateralPIDTuning = structs.CarParams.LateralPIDTuning(
kpBP=[10., 40.],
kiBP=[10., 40.],
kf=1.,
kpV=[0., 1.45],
kiV=[0., 0.12],
)
self.CURVATURE_LIMITS: CurvatureSteeringLimits = CurvatureSteeringLimits(
0.195,
)
if CP.flags & VolkswagenFlags.ALT_GEAR:
self.shifter_values = can_define.dv["Gateway_73"]["GE_Fahrstufe"]
else:
self.shifter_values = can_define.dv["Getriebe_11"]["GE_Fahrstufe"]
self.hca_status_values = can_define.dv["QFK_01"]["LatCon_HCA_Status"]
BASE_BUTTONS = [
Button(structs.CarState.ButtonEvent.Type.setCruise, "GRA_ACC_01", "GRA_Tip_Setzen", [1]),
Button(structs.CarState.ButtonEvent.Type.resumeCruise, "GRA_ACC_01", "GRA_Tip_Wiederaufnahme", [1]),
Button(structs.CarState.ButtonEvent.Type.accelCruise, "GRA_ACC_01", "GRA_Tip_Hoch", [1]),
Button(structs.CarState.ButtonEvent.Type.decelCruise, "GRA_ACC_01", "GRA_Tip_Runter", [1]),
Button(structs.CarState.ButtonEvent.Type.gapAdjustCruise, "GRA_ACC_01", "GRA_Verstellung_Zeitluecke", [1, 2, 3]),
]
self.BUTTONS = BASE_BUTTONS + [
Button(structs.CarState.ButtonEvent.Type.cancel, "GRA_ACC_01", "GRA_Hauptschalter", [1]),
]
self.BUTTONS_ALT = BASE_BUTTONS + [
Button(structs.CarState.ButtonEvent.Type.cancel, "GRA_ACC_01", "GRA_Abbrechen", [1]),
]
self.LDW_MESSAGES = {
"none": 0,
"laneAssistTakeOverUrgent": 4,
"laneAssistTakeOver": 8,
}
self.LDW_SOUNDS = {
"None": 0,
"Chime": 1,
"Beep": 2,
}
else:
self.LDW_STEP = 10 # LDW_02 message frequency 10Hz
self.ACC_HUD_STEP = 6 # ACC_02 message frequency 16Hz
self.hca_status_values = can_define.dv["LH_EPS_03"]["EPS_HCA_Status"]
if CP.flags & VolkswagenFlags.MLB:
self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm
self.STEER_DELTA_UP = 9 # Max HCA reached in 0.66s (STEER_MAX / (50Hz * 0.66))
self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60))
self.ACC_HUD_TEXT_STEP = int(2.0 / DT_CTRL) # ACC_02 primary display text dwell time
if CP.carFingerprint == CAR.PORSCHE_MACAN_MK1:
self.shifter_values = can_define.dv["Getriebe_03"]["GE_Waehlhebel"]
else:
self.shifter_values = None
self.BUTTONS = [
Button(structs.CarState.ButtonEvent.Type.setCruise, "LS_01", "LS_Tip_Setzen", [1]),
Button(structs.CarState.ButtonEvent.Type.resumeCruise, "LS_01", "LS_Tip_Wiederaufnahme", [1]),
Button(structs.CarState.ButtonEvent.Type.accelCruise, "LS_01", "LS_Tip_Hoch", [1]),
Button(structs.CarState.ButtonEvent.Type.decelCruise, "LS_01", "LS_Tip_Runter", [1]),
Button(structs.CarState.ButtonEvent.Type.cancel, "LS_01", "LS_Abbrechen", [1]),
Button(structs.CarState.ButtonEvent.Type.gapAdjustCruise, "LS_01", "LS_Verstellung_Zeitluecke", [1, 2, 3]),
]
# ACC_02.ACC_Texte_Primaeranz, primary ACC display text at the bottom of the cluster
self.ACC_HUD_TEXTS = {
"none": 0,
"setSpeed": 21,
}
self.ACC_HUD_TEXT_DISTANCE = {1: 2, 2: 3, 3: 4, 4: 5} # follow distance bars to display text
else:
self.STEER_DRIVER_ALLOWANCE = 80
self.STEER_DELTA_UP = 4 # Max HCA reached in 1.50s (STEER_MAX / (50Hz * 1.50))
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["Gateway_73"]["GE_Fahrstufe"]
elif CP.transmissionType == TransmissionType.direct:
self.shifter_values = can_define.dv["Motor_EV_01"]["MO_Waehlpos"]
self.BUTTONS = [
Button(structs.CarState.ButtonEvent.Type.setCruise, "GRA_ACC_01", "GRA_Tip_Setzen", [1]),
Button(structs.CarState.ButtonEvent.Type.resumeCruise, "GRA_ACC_01", "GRA_Tip_Wiederaufnahme", [1]),
Button(structs.CarState.ButtonEvent.Type.accelCruise, "GRA_ACC_01", "GRA_Tip_Hoch", [1]),
Button(structs.CarState.ButtonEvent.Type.decelCruise, "GRA_ACC_01", "GRA_Tip_Runter", [1]),
Button(structs.CarState.ButtonEvent.Type.cancel, "GRA_ACC_01", "GRA_Abbrechen", [1]),
Button(structs.CarState.ButtonEvent.Type.gapAdjustCruise, "GRA_ACC_01", "GRA_Verstellung_Zeitluecke", [1, 2, 3]),
]
self.LDW_MESSAGES = {
"none": 0, # Nothing to display
"laneAssistUnavailChime": 1, # "Lane Assist currently not available." with chime
"laneAssistUnavailNoSensorChime": 3, # "Lane Assist not available. No sensor view." with chime
"laneAssistTakeOverUrgent": 4, # "Lane Assist: Please Take Over Steering" with urgent beep
"emergencyAssistUrgent": 6, # "Emergency Assist: Please Take Over Steering" with urgent beep
"laneAssistTakeOverChime": 7, # "Lane Assist: Please Take Over Steering" with chime
"laneAssistTakeOver": 8, # "Lane Assist: Please Take Over Steering" silent
"emergencyAssistChangingLanes": 9, # "Emergency Assist: Changing lanes..." with urgent beep
"laneAssistDeactivated": 10, # "Lane Assist deactivated." silent with persistent icon afterward
}
HOLD_MAX_FRAMES = 60
HOLD_TORQUE_DEADBAND_NM = 20
HOLD_ACCEL_KI = 0.00002
class WMI(StrEnum):
VOLKSWAGEN_USA_SUV = "1V2"
VOLKSWAGEN_USA_CAR = "1VW"
VOLKSWAGEN_MEXICO_SUV = "3VV"
VOLKSWAGEN_MEXICO_CAR = "3VW"
VOLKSWAGEN_ARGENTINA = "8AW"
VOLKSWAGEN_BRASIL = "9BW"
SAIC_VOLKSWAGEN = "LSV"
SKODA = "TMB"
SEAT = "VSS"
AUDI_EUROPE_MPV = "WA1"
AUDI_GERMANY_CAR = "WAU"
MAN = "WMA"
PORSCHE_SUV = "WP1"
AUDI_SPORT = "WUA"
VOLKSWAGEN_COMMERCIAL = "WV1"
VOLKSWAGEN_COMMERCIAL_BUS_VAN = "WV2"
VOLKSWAGEN_EUROPE_SUV = "WVG"
VOLKSWAGEN_EUROPE_CAR = "WVW"
VOLKSWAGEN_GROUP_RUS = "XW8"
class VolkswagenSafetyFlags(IntFlag):
LONG_CONTROL = 1
ALT_CRC_VARIANT_1 = 2
NO_GAS_OFFSET = 4
ALLOW_LONG_ACCEL_WITH_GAS_PRESSED = 8
DISABLE_RADAR = 16
PQ_ALC_MODULE = 32
PQ_LOWLINE = 64
PQ_NO_CAM_BUS = 128
PQ_ACC_FTS_EPB = 256
PQ_SNG_ECD = 512
MLB_NO_ECAN = 1024
class VolkswagenFlags(IntFlag):
# Detected flags
STOCK_HCA_PRESENT = 1
KOMBI_PRESENT = 4
# Static flags
PQ = 2
MLB = 8
MEB = 256
MEB_GEN2 = 512
MQB_EVO = 1024
# Detected flags (MEB/MQBevo)
STOCK_KLR_PRESENT = 2048
STOCK_PSD_PRESENT = 4096
STOCK_PSD_06_PRESENT = 8192
STOCK_DIAGNOSE_01_PRESENT = 16384
ALT_GEAR = 32768
DISABLE_RADAR = 65536
class VolkswagenFlagsIQ(IntFlag):
IQ_CC_ONLY = 1 << 5 # CC only mode with radar (has AEB)
IQ_CC_ONLY_NO_RADAR = 1 << 6 # CC only mode without radar
IQ_LVBS_ALC_MODULE = 1 << 7 # IQ.Lvbs VW ALC hardware module present / intended active path
IQ_PQ_LOWLINE = 1 << 17 # Non-ECAN lateral-only PQ: bus 0 dead, TX on bus 1 (ptCAN)
IQ_PQ_ACC_FTS_EPB = 1 << 18 # B7 TRW450: ACC FtS + EPB hold, Motor_1 resume spoof on bus 1
IQ_PQ_SNG_ECD = 1 << 19
IQ_PQ_TIMEBOMB = 1 << 20
IQ_MLB_NO_ECAN = 1 << 21
IQ_MLB_NO_HCA_EPS = 1 << 22
RADAR_DISABLE_STATE = {"error": False}
MLB_MSG_LH_EPS_03 = 0x9F
MLB_MSG_GETRIEBE_01 = 0x82
MLB_MSG_GETRIEBE_02 = 0x83
MLB_MSG_GETRIEBE_03 = 0x102
MLB_MSG_GETRIEBE_04 = 0x441
MLB_MSG_ACC_01 = 0x109
MLB_MSG_ACC_05 = 0x10D
MLB_MSG_ACC_02 = 0x30C
MLB_MSG_ACC_10 = 0x117
MLB_MSG_GATEWAY_05 = 0x39C
MLB_ACC_COORDINATOR_MSGS = (MLB_MSG_ACC_01, MLB_MSG_ACC_05, MLB_MSG_ACC_02)
MLB_GEARBOX_MSGS = (MLB_MSG_GETRIEBE_01, MLB_MSG_GETRIEBE_02, MLB_MSG_GETRIEBE_03, MLB_MSG_GETRIEBE_04)
@dataclass
class VolkswagenMLBPlatformConfig(PlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'vw_mlb'})
chassis_codes: set[str] = field(default_factory=set)
wmis: set[WMI] = field(default_factory=set)
def init(self):
self.flags |= VolkswagenFlags.MLB
@dataclass
class VolkswagenMQBPlatformConfig(PlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'vw_mqb'})
# Volkswagen uses the VIN WMI and chassis code to match in the absence of the comma power
# on camera-integrated cars, as we lose too many ECUs to reliably identify the vehicle
chassis_codes: set[str] = field(default_factory=set)
wmis: set[WMI] = field(default_factory=set)
model_years: set[str] = field(default_factory=set)
@dataclass
class VolkswagenPQPlatformConfig(VolkswagenMQBPlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'vw_pq'})
def init(self):
self.flags |= VolkswagenFlags.PQ
@dataclass
class VolkswagenMEBPlatformConfig(PlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'vw_meb', Bus.radar: 'vw_meb'})
chassis_codes: set[str] = field(default_factory=set)
wmis: set[WMI] = field(default_factory=set)
model_years: set[str] = field(default_factory=set)
def init(self):
self.flags |= VolkswagenFlags.MEB
if self.flags & VolkswagenFlags.MEB_GEN2:
self.dbc_dict = {Bus.pt: 'vw_meb_2024', Bus.radar: 'vw_meb_2024'}
@dataclass
class VolkswagenMQBevoPlatformConfig(PlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'vw_mqbevo', Bus.radar: 'vw_mqbevo'})
chassis_codes: set[str] = field(default_factory=set)
wmis: set[WMI] = field(default_factory=set)
def init(self):
self.flags |= VolkswagenFlags.MQB_EVO
@dataclass(frozen=True, kw_only=True)
class VolkswagenCarSpecs(CarSpecs):
centerToFrontRatio: float = 0.45
steerRatio: float = 18.4
minSteerSpeed: float = CarControllerParams.DEFAULT_MIN_STEER_SPEED
# FIXME: nuke this
class Footnote(Enum):
KAMIQ = CarFootnote(
"Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform.",
Column.MODEL)
PASSAT = CarFootnote(
"Refers only to the MQB-based European B8 Passat, not the NMS Passat in the USA/China/Mideast markets.",
Column.MODEL)
SKODA_HEATED_WINDSHIELD = CarFootnote(
"Some Škoda vehicles are equipped with heated windshields, which are known " +
"to block GPS signal needed for some comma four functionality.",
Column.MODEL)
VW_EXP_LONG = CarFootnote(
"Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness " +
"are limited to using stock ACC.",
Column.LONGITUDINAL, docs_only=True)
VW_MQB_A0 = CarFootnote(
"Model-years 2022 and beyond may have a combined CAN gateway and BCM, which is supported by openpilot " +
"in software, but doesn't yet have a harness available from the comma store.",
Column.HARDWARE)
@dataclass
class VWCarDocs(CarDocs):
package: str = "Adaptive Cruise Control (ACC) & Lane Assist"
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.vw_j533]))
def init_make(self, CP: structs.CarParams):
self.footnotes.append(Footnote.VW_EXP_LONG)
if "SKODA" in CP.carFingerprint:
self.footnotes.append(Footnote.SKODA_HEATED_WINDSHIELD)
if abs(CP.minSteerSpeed - CarControllerParams.DEFAULT_MIN_STEER_SPEED) < 1e-3:
self.min_steer_speed = 0
# Check the 7th and 8th characters of the VIN before adding a new CAR. If the
# chassis code is already listed below, don't add a new CAR, just add to the
# FW_VERSIONS for that existing CAR.
class CAR(Platforms):
config: VolkswagenMQBPlatformConfig | VolkswagenPQPlatformConfig | VolkswagenMLBPlatformConfig | VolkswagenMEBPlatformConfig | VolkswagenMQBevoPlatformConfig
VOLKSWAGEN_ARTEON_MK1 = VolkswagenMQBPlatformConfig(
[
VWCarDocs("Volkswagen Arteon 2018-23", video="https://youtu.be/FAomFKPFlDA"),
VWCarDocs("Volkswagen Arteon R 2020-23", video="https://youtu.be/FAomFKPFlDA"),
VWCarDocs("Volkswagen Arteon eHybrid 2020-23", video="https://youtu.be/FAomFKPFlDA"),
VWCarDocs("Volkswagen Arteon Shooting Brake 2020-23", video="https://youtu.be/FAomFKPFlDA"),
VWCarDocs("Volkswagen CC 2018-22", video="https://youtu.be/FAomFKPFlDA"),
],
VolkswagenCarSpecs(mass=1733, wheelbase=2.84),
chassis_codes={"AN", "3H"},
wmis={WMI.VOLKSWAGEN_EUROPE_CAR},
)
VOLKSWAGEN_ATLAS_MK1 = VolkswagenMQBPlatformConfig(
[
VWCarDocs("Volkswagen Atlas 2018-23"),
VWCarDocs("Volkswagen Atlas Cross Sport 2020-22"),
VWCarDocs("Volkswagen Teramont 2018-22"),
VWCarDocs("Volkswagen Teramont Cross Sport 2021-22"),
VWCarDocs("Volkswagen Teramont X 2021-22"),
],
VolkswagenCarSpecs(mass=2011, wheelbase=2.98),
chassis_codes={"CA"},
wmis={WMI.VOLKSWAGEN_USA_SUV, WMI.VOLKSWAGEN_EUROPE_SUV},
)
VOLKSWAGEN_CADDY_MK3 = VolkswagenPQPlatformConfig(
[
VWCarDocs("Volkswagen Caddy 2019"),
VWCarDocs("Volkswagen Caddy Maxi 2019"),
],
VolkswagenCarSpecs(mass=1613, wheelbase=2.6),
chassis_codes={"2K"},
wmis={WMI.VOLKSWAGEN_COMMERCIAL_BUS_VAN},
)
VOLKSWAGEN_CRAFTER_MK2 = VolkswagenMQBPlatformConfig(
[
VWCarDocs("Volkswagen Crafter 2017-24", video="https://youtu.be/4100gLeabmo"),
VWCarDocs("Volkswagen e-Crafter 2018-24", video="https://youtu.be/4100gLeabmo"),
VWCarDocs("Volkswagen Grand California 2019-24", video="https://youtu.be/4100gLeabmo"),
VWCarDocs("MAN TGE 2017-24", video="https://youtu.be/4100gLeabmo"),
VWCarDocs("MAN eTGE 2020-24", video="https://youtu.be/4100gLeabmo"),
],
VolkswagenCarSpecs(mass=2100, wheelbase=3.64, minSteerSpeed=50 * CV.KPH_TO_MS),
chassis_codes={"SY", "SZ", "UY", "UZ"},
wmis={WMI.VOLKSWAGEN_COMMERCIAL, WMI.MAN},
)
VOLKSWAGEN_GOLF_MK7 = VolkswagenMQBPlatformConfig(
[
VWCarDocs("Volkswagen e-Golf 2014-20"),
VWCarDocs("Volkswagen Golf (Mk7) 2015-20", auto_resume=False),
VWCarDocs("Volkswagen Golf Alltrack 2015-19", auto_resume=False),
VWCarDocs("Volkswagen Golf GTD 2015-20"),
VWCarDocs("Volkswagen Golf GTE 2015-20"),
VWCarDocs("Volkswagen Golf GTI 2015-21", auto_resume=False),
VWCarDocs("Volkswagen Golf R 2015-19"),
VWCarDocs("Volkswagen Golf SportsVan 2015-20"),
],
VolkswagenCarSpecs(mass=1397, wheelbase=2.62, steerRatio=17.0),
chassis_codes={"5G", "AU", "BA", "BE"},
wmis={WMI.VOLKSWAGEN_MEXICO_CAR, WMI.VOLKSWAGEN_EUROPE_CAR},
)
VOLKSWAGEN_GOLF_MK8 = VolkswagenMQBevoPlatformConfig(
[VWCarDocs("Volkswagen Golf (Mk8) 2020-25")],
VolkswagenCarSpecs(mass=1397, wheelbase=2.62),
chassis_codes={"CD"},
wmis={WMI.VOLKSWAGEN_EUROPE_CAR},
)
VOLKSWAGEN_ID3_MK1 = VolkswagenMEBPlatformConfig(
[
VWCarDocs("Volkswagen ID.3 2020-23"),
VWCarDocs("Volkswagen MEB Gen 1 2020-23"),
],
VolkswagenCarSpecs(mass=1935, wheelbase=2.77),
chassis_codes={"E1"},
wmis={WMI.VOLKSWAGEN_USA_SUV, WMI.VOLKSWAGEN_EUROPE_CAR},
model_years={"L", "M", "N", "P"},
)
VOLKSWAGEN_ID3_MK2 = VolkswagenMEBPlatformConfig(
[
VWCarDocs("Volkswagen ID.3 2024-25"),
VWCarDocs("Volkswagen MEB Gen 2 2024-25"),
],
VolkswagenCarSpecs(mass=1935, wheelbase=2.77),
chassis_codes={"E1"},
wmis={WMI.VOLKSWAGEN_USA_SUV, WMI.VOLKSWAGEN_EUROPE_CAR},
model_years={"R", "S"},
flags=VolkswagenFlags.MEB_GEN2,
)
VOLKSWAGEN_ID4_MK1 = VolkswagenMEBPlatformConfig(
[
VWCarDocs("Volkswagen ID.4 2021-23"),
VWCarDocs("Volkswagen ID.5 2022-23"),
],
VolkswagenCarSpecs(mass=2224, wheelbase=2.77),
chassis_codes={"E2"},
wmis={WMI.VOLKSWAGEN_USA_SUV, WMI.VOLKSWAGEN_EUROPE_CAR, WMI.VOLKSWAGEN_EUROPE_SUV},
)
VOLKSWAGEN_ID4_MK2 = VolkswagenMEBPlatformConfig(
[VWCarDocs("Volkswagen ID.4 2024-25")],
VolkswagenCarSpecs(mass=2224, wheelbase=2.77),
chassis_codes={"E8"},
wmis={WMI.VOLKSWAGEN_USA_SUV, WMI.VOLKSWAGEN_EUROPE_CAR, WMI.VOLKSWAGEN_EUROPE_SUV},
flags=VolkswagenFlags.MEB_GEN2,
)
VOLKSWAGEN_JETTA_MK6 = VolkswagenPQPlatformConfig(
[VWCarDocs("Volkswagen Jetta 2015-18")],
VolkswagenCarSpecs(mass=1518, wheelbase=2.65),
chassis_codes={"5K", "AJ"},
wmis={WMI.VOLKSWAGEN_MEXICO_CAR},
)
VOLKSWAGEN_JETTA_MK7 = VolkswagenMQBPlatformConfig(
[
VWCarDocs("Volkswagen Jetta 2019-23"),
VWCarDocs("Volkswagen Jetta GLI 2021-23"),
],
VolkswagenCarSpecs(mass=1328, wheelbase=2.71),
chassis_codes={"BU"},
wmis={WMI.VOLKSWAGEN_MEXICO_CAR, WMI.VOLKSWAGEN_EUROPE_CAR},
)
VOLKSWAGEN_PASSAT_MK8 = VolkswagenMQBPlatformConfig(
[
VWCarDocs("Volkswagen Passat 2015-22", footnotes=[Footnote.PASSAT]),
VWCarDocs("Volkswagen Passat Alltrack 2015-22"),
VWCarDocs("Volkswagen Passat GTE 2015-22"),
],
VolkswagenCarSpecs(mass=1551, wheelbase=2.79),
chassis_codes={"3C", "3G"},
wmis={WMI.VOLKSWAGEN_EUROPE_CAR},
model_years={"F", "G", "H", "J", "K", "L", "M", "N"}, # 2015-2022
)
VOLKSWAGEN_PASSAT_MK7 = VolkswagenPQPlatformConfig(
[VWCarDocs("Volkswagen Passat 2.0 TDI 2014")],
VolkswagenCarSpecs(mass=1836, wheelbase=2.70, steerRatio=13.0, minSteerSpeed=31 * CV.KPH_TO_MS),
chassis_codes={"3C"},
wmis={WMI.VOLKSWAGEN_EUROPE_CAR},
model_years={"E"}, # 2014
)
VOLKSWAGEN_PASSAT_NMS = VolkswagenPQPlatformConfig(
[VWCarDocs("Volkswagen Passat NMS 2015-17")],
VolkswagenCarSpecs(mass=1503, wheelbase=2.80),
chassis_codes={"A3"},
wmis={WMI.VOLKSWAGEN_USA_CAR},
# NMS and NMS+ share chassis code A3; disambiguate by model year
model_years={"F", "G", "H"}, # 2015-2017
)
VOLKSWAGEN_PASSAT_NMS_PLUS = VolkswagenPQPlatformConfig(
[VWCarDocs("Volkswagen Passat NMS 2018-22")],
VolkswagenCarSpecs(mass=1503, wheelbase=2.80, minEnableSpeed=20 * CV.KPH_TO_MS),
chassis_codes={"A3"},
wmis={WMI.VOLKSWAGEN_USA_CAR},
model_years={"J", "K", "L", "M", "N"}, # 2018-2022
)
VOLKSWAGEN_PASSAT_B7 = VolkswagenPQPlatformConfig(
[VWCarDocs("Volkswagen Passat B7 2011-15")],
VolkswagenCarSpecs(mass=1503, wheelbase=2.712, steerRatio=16.4, minSteerSpeed=0),
chassis_codes={"A3"},
wmis={WMI.VOLKSWAGEN_USA_CAR},
)
VOLKSWAGEN_POLO_MK6 = VolkswagenMQBPlatformConfig(
[
VWCarDocs("Volkswagen Polo 2018-23", footnotes=[Footnote.VW_MQB_A0]),
VWCarDocs("Volkswagen Polo GTI 2018-23", footnotes=[Footnote.VW_MQB_A0]),
],
VolkswagenCarSpecs(mass=1230, wheelbase=2.55),
chassis_codes={"AW"},
wmis={WMI.VOLKSWAGEN_EUROPE_CAR},
)
VOLKSWAGEN_SHARAN_MK2 = VolkswagenPQPlatformConfig(
[
VWCarDocs("Volkswagen Sharan 2018-22"),
],
VolkswagenCarSpecs(mass=1639, wheelbase=2.92),
chassis_codes={"7N"},
wmis={WMI.VOLKSWAGEN_EUROPE_CAR},
)
SEAT_ALHAMBRA_MK1 = VolkswagenPQPlatformConfig(
[
VWCarDocs("SEAT Alhambra 2018-20"),
],
VolkswagenCarSpecs(mass=1639, wheelbase=2.92, minSteerSpeed=50 * CV.KPH_TO_MS),
chassis_codes={"7N"},
wmis={WMI.SEAT},
)
VOLKSWAGEN_TAOS_MK1 = VolkswagenMQBPlatformConfig(
[VWCarDocs("Volkswagen Taos 2022-24")],
VolkswagenCarSpecs(mass=1498, wheelbase=2.69),
chassis_codes={"B2"},
wmis={WMI.VOLKSWAGEN_MEXICO_SUV, WMI.VOLKSWAGEN_ARGENTINA},
)
VOLKSWAGEN_TCROSS_MK1 = VolkswagenMQBPlatformConfig(
[VWCarDocs("Volkswagen T-Cross 2021", footnotes=[Footnote.VW_MQB_A0])],
VolkswagenCarSpecs(mass=1150, wheelbase=2.60),
chassis_codes={"C1"},
wmis={WMI.VOLKSWAGEN_EUROPE_SUV},
)
VOLKSWAGEN_TIGUAN_MK2 = VolkswagenMQBPlatformConfig(
[
VWCarDocs("Volkswagen Tiguan 2018-24"),
VWCarDocs("Volkswagen Tiguan eHybrid 2021-23"),
],
VolkswagenCarSpecs(mass=1715, wheelbase=2.74),
chassis_codes={"5N", "AD", "AX", "BW"},
wmis={WMI.VOLKSWAGEN_EUROPE_SUV, WMI.VOLKSWAGEN_MEXICO_SUV},
)
VOLKSWAGEN_TOURAN_MK2 = VolkswagenMQBPlatformConfig(
[VWCarDocs("Volkswagen Touran 2016-23")],
VolkswagenCarSpecs(mass=1516, wheelbase=2.79),
chassis_codes={"1T"},
wmis={WMI.VOLKSWAGEN_EUROPE_SUV},
)
VOLKSWAGEN_TRANSPORTER_T61 = VolkswagenMQBPlatformConfig(
[
VWCarDocs("Volkswagen Caravelle 2020"),
VWCarDocs("Volkswagen California 2021-23"),
],
VolkswagenCarSpecs(mass=1926, wheelbase=3.00, minSteerSpeed=14.0),
chassis_codes={"7H", "7L"},
wmis={WMI.VOLKSWAGEN_COMMERCIAL_BUS_VAN},
)
VOLKSWAGEN_TROC_MK1 = VolkswagenMQBPlatformConfig(
[VWCarDocs("Volkswagen T-Roc 2018-23")],
VolkswagenCarSpecs(mass=1413, wheelbase=2.63),
chassis_codes={"A1"},
wmis={WMI.VOLKSWAGEN_EUROPE_SUV},
)
AUDI_A3_MK3 = VolkswagenMQBPlatformConfig(
[
VWCarDocs("Audi A3 2014-19"),
VWCarDocs("Audi A3 Sportback e-tron 2017-18"),
VWCarDocs("Audi RS3 2018"),
VWCarDocs("Audi S3 2015-17"),
],
VolkswagenCarSpecs(mass=1335, wheelbase=2.61),
chassis_codes={"8V", "FF"},
wmis={WMI.AUDI_GERMANY_CAR, WMI.AUDI_SPORT},
)
AUDI_A4_MK4 = VolkswagenMLBPlatformConfig(
[VWCarDocs("Audi A4 2013-16", package="Cruise Control")],
VolkswagenCarSpecs(mass=1610, wheelbase=2.81, steerRatio=15.9),
chassis_codes={"8K", "FL"},
wmis={WMI.AUDI_GERMANY_CAR},
)
AUDI_Q2_MK1 = VolkswagenMQBPlatformConfig(
[VWCarDocs("Audi Q2 2018")],
VolkswagenCarSpecs(mass=1205, wheelbase=2.61),
chassis_codes={"GA"},
wmis={WMI.AUDI_GERMANY_CAR},
)
AUDI_Q3_MK2 = VolkswagenMQBPlatformConfig(
[VWCarDocs("Audi Q3 2019-24")],
VolkswagenCarSpecs(mass=1623, wheelbase=2.68),
chassis_codes={"8U", "F3", "FS"},
wmis={WMI.AUDI_EUROPE_MPV, WMI.AUDI_GERMANY_CAR},
)
AUDI_Q4_MK1 = VolkswagenMEBPlatformConfig(
[VWCarDocs("Audi Q4 2021-23")],
VolkswagenCarSpecs(mass=1965, wheelbase=2.764),
chassis_codes={"FZ"},
wmis={WMI.AUDI_EUROPE_MPV},
model_years={"M", "N", "P"},
)
AUDI_Q4_MK2 = VolkswagenMEBPlatformConfig(
[VWCarDocs("Audi Q4 2024-25")],
VolkswagenCarSpecs(mass=1965, wheelbase=2.764),
chassis_codes={"FZ"},
wmis={WMI.AUDI_EUROPE_MPV},
model_years={"R", "S"},
flags=VolkswagenFlags.MEB_GEN2,
)
AUDI_Q5_MK1 = VolkswagenMLBPlatformConfig(
[VWCarDocs("Audi Q5 2013-17")],
VolkswagenCarSpecs(mass=1895, wheelbase=2.81),
chassis_codes={"8R"},
wmis={WMI.AUDI_EUROPE_MPV, WMI.AUDI_GERMANY_CAR},
)
PORSCHE_MACAN_MK1 = VolkswagenMLBPlatformConfig(
[VWCarDocs("Porsche Macan 2017-24")],
VolkswagenCarSpecs(mass=1895, wheelbase=2.81, steerRatio=16.2),
chassis_codes={"95", "A5"},
wmis={WMI.PORSCHE_SUV},
)
SEAT_ATECA_MK1 = VolkswagenMQBPlatformConfig(
[
VWCarDocs("CUPRA Ateca 2018-23"),
VWCarDocs("SEAT Ateca 2016-23"),
VWCarDocs("SEAT Leon (Mk3) 2014-20"),
],
VolkswagenCarSpecs(mass=1300, wheelbase=2.64),
chassis_codes={"5F"},
wmis={WMI.SEAT},
)
SEAT_LEON_MK4 = VolkswagenMQBevoPlatformConfig(
[VWCarDocs("SEAT Leon (Mk4) 2020-25")],
VolkswagenCarSpecs(mass=1300, wheelbase=2.685),
chassis_codes={"KL"},
wmis={WMI.SEAT},
)
CUPRA_BORN_MK1 = VolkswagenMEBPlatformConfig(
[VWCarDocs("CUPRA Born 2022-23")],
VolkswagenCarSpecs(mass=1950, wheelbase=2.766, steerRatio=15.9),
chassis_codes={"K1"},
model_years={"N", "P"},
wmis={WMI.SEAT},
)
SKODA_ENYAQ_MK1 = VolkswagenMEBPlatformConfig(
[VWCarDocs("Škoda Enyaq 2021-23")],
VolkswagenCarSpecs(mass=1965, wheelbase=2.77),
chassis_codes={"NY"},
model_years={"M", "N", "P"},
wmis={WMI.SKODA},
)
SKODA_ENYAQ_MK2 = VolkswagenMEBPlatformConfig(
[VWCarDocs("Škoda Enyaq 2024-25")],
VolkswagenCarSpecs(mass=1965, wheelbase=2.77),
chassis_codes={"NY"},
model_years={"R", "S"},
wmis={WMI.SKODA},
flags=VolkswagenFlags.MEB_GEN2,
)
SKODA_FABIA_MK4 = VolkswagenMQBPlatformConfig(
[VWCarDocs("Škoda Fabia 2022-23", footnotes=[Footnote.VW_MQB_A0])],
VolkswagenCarSpecs(mass=1266, wheelbase=2.56),
chassis_codes={"PJ"},
wmis={WMI.SKODA},
)
SKODA_KAMIQ_MK1 = VolkswagenMQBPlatformConfig(
[
VWCarDocs("Škoda Kamiq 2021-23", footnotes=[Footnote.VW_MQB_A0, Footnote.KAMIQ]),
VWCarDocs("Škoda Scala 2020-23", footnotes=[Footnote.VW_MQB_A0]),
],
VolkswagenCarSpecs(mass=1230, wheelbase=2.66),
chassis_codes={"NW"},
wmis={WMI.SKODA},
)
SKODA_KAROQ_MK1 = VolkswagenMQBPlatformConfig(
[VWCarDocs("Škoda Karoq 2019-23")],
VolkswagenCarSpecs(mass=1278, wheelbase=2.66),
chassis_codes={"NU"},
wmis={WMI.SKODA},
)
SKODA_KODIAQ_MK1 = VolkswagenMQBPlatformConfig(
[VWCarDocs("Škoda Kodiaq 2017-23")],
VolkswagenCarSpecs(mass=1569, wheelbase=2.79),
chassis_codes={"NS"},
wmis={WMI.SKODA, WMI.VOLKSWAGEN_GROUP_RUS},
)
SKODA_OCTAVIA_MK3 = VolkswagenMQBPlatformConfig(
[
VWCarDocs("Škoda Octavia 2015-19"),
VWCarDocs("Škoda Octavia RS 2016"),
VWCarDocs("Škoda Octavia Scout 2017-19"),
],
VolkswagenCarSpecs(mass=1388, wheelbase=2.68),
chassis_codes={"NE"},
wmis={WMI.SKODA},
)
SKODA_SUPERB_MK3 = VolkswagenMQBPlatformConfig(
[VWCarDocs("Škoda Superb 2015-22")],
VolkswagenCarSpecs(mass=1505, wheelbase=2.84),
chassis_codes={"3V", "NP"},
wmis={WMI.SKODA},
)
def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str]:
candidates = set()
# Compile all FW versions for each ECU
all_ecu_versions: dict[EcuAddrSubAddr, set[str]] = defaultdict(set)
for ecus in offline_fw_versions.values():
for ecu, versions in ecus.items():
all_ecu_versions[ecu] |= set(versions)
# Check the WMI and chassis code to determine the platform
# https://www.clubvw.org.au/vwreference/vwvin
vin_obj = Vin(vin)
vin_wmi = vin_obj.wmi if len(vin_obj.wmi) == 3 else None
chassis_code = vin_obj.vds[3:5] if len(vin_obj.vds) >= 5 else None
model_year_code = vin_obj.vis[0] if len(vin_obj.vis) > 0 else None
vin_available = vin_wmi is not None and chassis_code is not None
fallback_ecus = CHECK_FUZZY_ECUS | {Ecu.fwdCamera, Ecu.adas, Ecu.cornerRadar, Ecu.parkingAdas}
fallback_optional_ecus = fallback_ecus - CHECK_FUZZY_ECUS
for platform in CAR:
if not vin_available:
matched_ecus = set()
optional_ecu_seen = False
for ecu, versions in offline_fw_versions.get(platform, {}).items():
if ecu[0] not in fallback_ecus:
continue
found_versions = live_fw_versions.get(ecu[1:], [])
if len(found_versions) == 0:
continue
if ecu[0] in fallback_optional_ecus:
optional_ecu_seen = True
if any(found_version in versions for found_version in found_versions):
matched_ecus.add(ecu[0])
else:
matched_ecus = set()
break
if Ecu.fwdRadar in matched_ecus and (len(matched_ecus) >= 2 or not optional_ecu_seen):
candidates.add(platform)
continue
valid_ecus = set()
for ecu in offline_fw_versions.get(platform, {}):
addr = ecu[1:]
if ecu[0] not in CHECK_FUZZY_ECUS:
continue
# Sanity check that live FW is in the superset of all FW, Volkswagen ECU part numbers are commonly shared
found_versions = live_fw_versions.get(addr, [])
expected_versions = all_ecu_versions[ecu]
if not any(found_version in expected_versions for found_version in found_versions):
break
valid_ecus.add(ecu[0])
if valid_ecus != CHECK_FUZZY_ECUS:
continue
model_years = getattr(platform.config, "model_years", set())
if vin_wmi in platform.config.wmis and chassis_code in platform.config.chassis_codes:
if len(model_years) > 0 and model_year_code is not None and model_year_code not in model_years:
continue
candidates.add(platform)
return {str(c) for c in candidates}
def refine_fw_matches(matches, vin) -> set[str]:
vin_obj = Vin(vin)
if len(vin_obj.wmi) != 3 or len(vin_obj.vds) < 5:
return matches
chassis_code = vin_obj.vds[3:5]
model_year_code = vin_obj.vis[0] if len(vin_obj.vis) > 0 else None
candidates = set()
for platform in CAR:
if platform not in matches:
continue
model_years = getattr(platform.config, "model_years", set())
if vin_obj.wmi not in platform.config.wmis or chassis_code not in platform.config.chassis_codes:
continue
if model_years and model_year_code not in model_years:
continue
candidates.add(platform)
return {str(candidate) for candidate in candidates} or matches
# These ECUs are required to match to gain a VIN match
CHECK_FUZZY_ECUS = {Ecu.fwdRadar}
# All supported cars should return FW from the engine, srs, eps, and fwdRadar. Cars
# with a manual trans won't return transmission firmware, but all other cars will.
#
# The 0xF187 SW part number query should return in the form of N[NX][NX] NNN NNN [X[X]],
# where N=number, X=letter, and the trailing two letters are optional. Performance
# tuners sometimes tamper with that field (e.g. 8V0 9C0 BB0 1 from COBB/EQT). Tampered
# ECU SW part numbers are invalid for vehicle ID and compatibility checks. Try to have
# them repaired by the tuner before including them in openpilot.
VOLKSWAGEN_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_VERSION_NUMBER) + \
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION)
VOLKSWAGEN_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
VOLKSWAGEN_RX_OFFSET = 0x6a
VOLKSWAGEN_RX_OFFSET_CANFD = 0x20000
FW_QUERY_CONFIG = FwQueryConfig(
requests=[request for bus, obd_multiplexing in [(1, True), (1, False), (0, False)] for request in [
Request(
[VOLKSWAGEN_VERSION_REQUEST_MULTI],
[VOLKSWAGEN_VERSION_RESPONSE],
whitelist_ecus=[Ecu.srs, Ecu.eps, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.parkingAdas, Ecu.cornerRadar, Ecu.adas],
rx_offset=VOLKSWAGEN_RX_OFFSET,
bus=bus,
obd_multiplexing=obd_multiplexing,
),
Request(
[VOLKSWAGEN_VERSION_REQUEST_MULTI],
[VOLKSWAGEN_VERSION_RESPONSE],
whitelist_ecus=[Ecu.engine, Ecu.transmission],
bus=bus,
obd_multiplexing=obd_multiplexing,
),
Request(
[VOLKSWAGEN_VERSION_REQUEST_MULTI],
[VOLKSWAGEN_VERSION_RESPONSE],
whitelist_ecus=[Ecu.engine, Ecu.inverter],
rx_offset=VOLKSWAGEN_RX_OFFSET_CANFD,
bus=bus,
obd_multiplexing=obd_multiplexing,
),
]],
non_essential_ecus={Ecu.eps: list(CAR)},
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
refine_fw_matches=refine_fw_matches,
)
MQB_A0_CARS = {
CAR.VOLKSWAGEN_POLO_MK6,
CAR.VOLKSWAGEN_TCROSS_MK1,
CAR.SKODA_FABIA_MK4,
CAR.SKODA_KAMIQ_MK1,
}
def get_longitudinal_stopping_speed_override(candidate: CAR, flags: int) -> float:
if candidate == CAR.VOLKSWAGEN_PASSAT_B7:
return PASSAT_B7_STOPPING_SPEED
if flags & VolkswagenFlags.PQ:
return PQ_STOPPING_SPEED
return 0.0
def apply_pq_stopping_accel(candidate: CAR, accel: float, stopping: bool) -> float:
return PASSAT_B7_STOP_ACCEL if candidate == CAR.VOLKSWAGEN_PASSAT_B7 and stopping else accel
DBC = CAR.create_dbc_map()