mirror of
https://jihulab.com/mr-one/openpilot.git
synced 2026-10-26 20:43:49 +08:00
Revert new_vw brand (back to 770c82446 state)
This commit is contained in:
@@ -188,7 +188,7 @@ def get_checksum_state(dbc_name: str) -> ChecksumState | None:
|
||||
return ChecksumState(SignalType.TOYOTA_CHECKSUM, toyota_checksum)
|
||||
elif dbc_name.startswith("hyundai_canfd_generated"):
|
||||
return ChecksumState(SignalType.HKG_CAN_FD_CHECKSUM, hkg_can_fd_checksum)
|
||||
elif dbc_name.startswith(("vw_meb_2024", "vw_mqbevo_2024")):
|
||||
elif dbc_name.startswith("vw_meb_2024"):
|
||||
return ChecksumState(SignalType.VOLKSWAGEN_MQB_MEB_CHECKSUM, volkswagen_meb_alt_crc_checksum)
|
||||
elif dbc_name.startswith(("vw_mqb", "vw_mqbevo", "vw_meb")):
|
||||
return ChecksumState(SignalType.VOLKSWAGEN_MQB_MEB_CHECKSUM, volkswagen_mqb_meb_checksum)
|
||||
|
||||
@@ -1,225 +0,0 @@
|
||||
import numpy as np
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, DT_CTRL, structs
|
||||
from opendbc.car.lateral import apply_driver_steer_torque_limits
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
from opendbc.car.volkswagen import mlbcan, mqbcan, pqcan
|
||||
from opendbc.car.new_vw import mebcan
|
||||
from opendbc.car.new_vw.values import CanBus, CarControllerParams, VolkswagenFlags
|
||||
from opendbc.sunnypilot.car.volkswagen.values import VolkswagenFlagsSP
|
||||
|
||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
class HCAMitigation:
|
||||
"""
|
||||
Manages HCA fault mitigations for VW/Audi EPS racks:
|
||||
* Reduces torque for a single frame after commanding the same torque value for too long
|
||||
"""
|
||||
|
||||
def __init__(self, CCP, torque_reduction=1):
|
||||
self._max_same_torque_frames = CCP.STEER_TIME_STUCK_TORQUE / (DT_CTRL * CCP.STEER_STEP)
|
||||
self._same_torque_frames = 0
|
||||
self._torque_reduction = torque_reduction
|
||||
|
||||
def update(self, apply_torque, apply_torque_last):
|
||||
if apply_torque != 0 and apply_torque_last == apply_torque:
|
||||
self._same_torque_frames += 1
|
||||
if self._same_torque_frames > self._max_same_torque_frames:
|
||||
apply_torque -= (self._torque_reduction, -self._torque_reduction)[apply_torque < 0]
|
||||
self._same_torque_frames = 0
|
||||
else:
|
||||
self._same_torque_frames = 0
|
||||
|
||||
return apply_torque
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_names, CP, CP_SP):
|
||||
super().__init__(dbc_names, CP, CP_SP)
|
||||
self.CCP = CarControllerParams(CP)
|
||||
self.CAN = CanBus(CP)
|
||||
self.packer_pt = CANPacker(dbc_names[Bus.pt])
|
||||
self.aeb_available = not CP.flags & VolkswagenFlags.PQ
|
||||
self.dp_avoid_eps_lockout = bool(self.CP_SP.flags & VolkswagenFlagsSP.AVOID_EPS_LOCKOUT)
|
||||
|
||||
if CP.flags & VolkswagenFlags.MEB:
|
||||
self.meb_long_state = mebcan.MebLongStateMachine(self.CP, self.CCP)
|
||||
|
||||
if CP.flags & VolkswagenFlags.PQ:
|
||||
self.CCS = pqcan
|
||||
elif CP.flags & VolkswagenFlags.MLB:
|
||||
self.CCS = mlbcan
|
||||
else:
|
||||
self.CCS = mqbcan
|
||||
|
||||
self.apply_torque_last = 0
|
||||
self.apply_curvature_last = 0.
|
||||
self.steering_power_last = 0
|
||||
self.accel_last = 0.
|
||||
self.lead_distance_bars_last = None
|
||||
self.distance_bar_frame = 0
|
||||
self.gra_acc_counter_last = None
|
||||
self.hca_mitigation = HCAMitigation(self.CCP, torque_reduction=3 if self.dp_avoid_eps_lockout else 1)
|
||||
|
||||
def update(self, CC, CC_SP, CS, now_nanos):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
can_sends = []
|
||||
|
||||
# **** Steering Controls ************************************************ #
|
||||
|
||||
if self.frame % self.CCP.STEER_STEP == 0:
|
||||
apply_torque = 0
|
||||
if self.CP.flags & VolkswagenFlags.MEB:
|
||||
# Logic to avoid HCA refused state:
|
||||
# * steering power as counter and near zero before OP lane assist deactivation
|
||||
# MEB rack can be used continuously without time limits
|
||||
# maximum real steering angle change ~ 120-130 deg/s
|
||||
|
||||
if CC.latActive:
|
||||
hca_enabled = True
|
||||
# compensate the gap between measured and current curvature
|
||||
apply_curvature = actuators.curvature + (CS.curvature_meas - CC.currentCurvature)
|
||||
apply_curvature = self.CCP.CURVATURE_LIMITS.apply_limits(apply_curvature, self.apply_curvature_last, CS.out.vEgoRaw, CS.curvature_meas,
|
||||
CC.latActive, self.CCP.STEER_STEP)
|
||||
|
||||
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.steering_power_last > 0: # keep HCA alive until steering power has reduced to zero
|
||||
hca_enabled = True
|
||||
apply_curvature = float(np.clip(CS.curvature_meas, -self.CCP.CURVATURE_MAX, self.CCP.CURVATURE_MAX))
|
||||
steering_power = max(self.steering_power_last - self.CCP.STEERING_POWER_STEP, 0)
|
||||
else:
|
||||
hca_enabled = False
|
||||
apply_curvature = 0. # inactive curvature
|
||||
steering_power = 0
|
||||
|
||||
can_sends.append(mebcan.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:
|
||||
if self.dp_avoid_eps_lockout:
|
||||
# Scale steering torque down at low speeds and sharp steering angles to avoid EPS lockout.
|
||||
# Speed range extends to 8 m/s (~29 km/h) to cover sharp 90+ degree turns taken at speed.
|
||||
speed_scale = np.interp(CS.out.vEgo, [0.4, 3.5, 8.0], [0.6, 0.85, 1.0])
|
||||
angle_scale = np.interp(abs(CS.out.steeringAngleDeg), [50.0, 100.0], [1.0, 0.75])
|
||||
torque_scale = min(speed_scale, angle_scale)
|
||||
scaled_steer_max = self.CCP.STEER_MAX * torque_scale
|
||||
new_torque = int(round(actuators.torque * scaled_steer_max))
|
||||
else:
|
||||
new_torque = int(round(actuators.torque * self.CCP.STEER_MAX))
|
||||
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.CCP)
|
||||
|
||||
apply_torque = self.hca_mitigation.update(apply_torque, self.apply_torque_last)
|
||||
hca_enabled = apply_torque != 0
|
||||
self.apply_torque_last = apply_torque
|
||||
can_sends.append(self.CCS.create_steering_control(self.packer_pt, self.CAN.pt, apply_torque, hca_enabled))
|
||||
|
||||
if self.CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT:
|
||||
# Pacify VW Emergency Assist driver inactivity detection by changing its view of driver steering input torque
|
||||
# to the greatest of actual driver input or 2x openpilot's output (1x openpilot output is not enough to
|
||||
# consistently reset inactivity detection on straight level roads). See commaai/openpilot#23274 for background.
|
||||
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))
|
||||
|
||||
# Emergency Assist intervention
|
||||
if self.CP.flags & VolkswagenFlags.MEB and self.CP.flags & VolkswagenFlags.STOCK_KLR_PRESENT:
|
||||
# send capacitive steering wheel hands-on message to keep ACC resume active and control Emergency Assist
|
||||
# MEB Emergency Assist brake jerks after 30s of continued hands-off time.
|
||||
# We send the stock wheeltouch message to start the stock DM timer when openpilot latches the critical driver monitoring alert
|
||||
if self.frame % self.CCP.KLR_01_STEP == 0:
|
||||
lat_active = CC.latActive and not CC.driverMonitoringEscalation
|
||||
can_sends.append(mebcan.create_capacitive_wheel_touch(self.packer_pt, self.CAN.cam, lat_active, CS.klr_stock_values))
|
||||
can_sends.append(mebcan.create_capacitive_wheel_touch(self.packer_pt, self.CAN.pt, lat_active, CS.klr_stock_values))
|
||||
|
||||
# **** Acceleration Controls ******************************************** #
|
||||
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if self.frame % self.CCP.ACC_CONTROL_STEP == 0:
|
||||
if self.CP.flags & VolkswagenFlags.MEB:
|
||||
accel = float(np.clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX))
|
||||
accel, acc_status, acc_hold_type, braking_to_stop, leaving_standstill = self.meb_long_state.update(CS, CC, accel)
|
||||
can_sends.extend(mebcan.create_acc_accel_control(self.packer_pt, self.CAN.pt, self.CCP, CS.acc_type, CC.enabled,
|
||||
accel, acc_status, acc_hold_type, braking_to_stop, leaving_standstill,
|
||||
CS.out.vEgoRaw * CV.MS_TO_KPH, CS.travel_assist_available))
|
||||
|
||||
else:
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
|
||||
accel = float(np.clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0)
|
||||
starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < 0.25)
|
||||
can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, self.CAN.pt, CS.acc_type, CC.longActive, accel,
|
||||
acc_control, stopping, starting, CS.esp_hold_confirmation))
|
||||
|
||||
self.accel_last = accel
|
||||
|
||||
#if self.aeb_available:
|
||||
# if self.frame % self.CCP.AEB_CONTROL_STEP == 0:
|
||||
# can_sends.append(self.CCS.create_aeb_control(self.packer_pt, False, False, 0.0))
|
||||
# if self.frame % self.CCP.AEB_HUD_STEP == 0:
|
||||
# can_sends.append(self.CCS.create_aeb_hud(self.packer_pt, False, False))
|
||||
|
||||
# **** HUD Controls ***************************************************** #
|
||||
|
||||
if self.frame % self.CCP.LDW_STEP == 0:
|
||||
hud_alert = 0
|
||||
if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw):
|
||||
hud_alert = self.CCP.LDW_MESSAGES["laneAssistTakeOver"]
|
||||
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))
|
||||
|
||||
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:
|
||||
if self.CP.flags & VolkswagenFlags.MEB:
|
||||
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
|
||||
show_distance_bars = self.frame - self.distance_bar_frame < 400
|
||||
lead_distance = 0
|
||||
if hud_control.leadVisible and self.frame * DT_CTRL > 1.0:
|
||||
lead_distance = 8
|
||||
can_sends.append(mebcan.create_acc_hud_control(self.packer_pt, self.CAN.pt, self.meb_long_state.acc_status, hud_control.setSpeed * CV.MS_TO_KPH,
|
||||
hud_control.leadVisible, hud_control.leadDistanceBars, show_distance_bars,
|
||||
lead_distance, fcw_alert))
|
||||
|
||||
else:
|
||||
lead_distance = 0
|
||||
if hud_control.leadVisible and self.frame * DT_CTRL > 1.0: # Don't display lead until we know the scaling factor
|
||||
lead_distance = 512 if CS.upscale_lead_car_signal else 8
|
||||
acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
|
||||
# FIXME: PQ may need to use the on-the-wire mph/kmh toggle to fix rounding errors
|
||||
# FIXME: Detect clusters with vEgoCluster offsets and apply an identical vCruiseCluster offset
|
||||
set_speed = hud_control.setSpeed * CV.MS_TO_KPH
|
||||
can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, self.CAN.pt, acc_hud_status, set_speed,
|
||||
lead_distance, hud_control.leadDistanceBars))
|
||||
|
||||
# **** Stock ACC Button Controls **************************************** #
|
||||
|
||||
gra_send_ready = self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last
|
||||
if gra_send_ready and (CC.cruiseControl.cancel or CC.cruiseControl.resume):
|
||||
can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, self.CAN.ext, CS.gra_stock_values,
|
||||
cancel=CC.cruiseControl.cancel, resume=CC.cruiseControl.resume))
|
||||
|
||||
new_actuators = actuators.as_builder()
|
||||
new_actuators.torque = self.apply_torque_last / self.CCP.STEER_MAX
|
||||
new_actuators.torqueOutputCan = self.apply_torque_last
|
||||
new_actuators.curvature = self.apply_curvature_last
|
||||
new_actuators.accel = self.accel_last
|
||||
|
||||
self.lead_distance_bars_last = hud_control.leadDistanceBars
|
||||
self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"]
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
@@ -1,465 +0,0 @@
|
||||
from opendbc.can import CANParser
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.new_vw.values import DBC, CanBus, NetworkLocation, TransmissionType, GearShifter, \
|
||||
CarControllerParams, VolkswagenFlags
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP, CP_SP):
|
||||
super().__init__(CP, CP_SP)
|
||||
self.frame = 0
|
||||
self.eps_init_complete = False
|
||||
self.tsk_recovery_timer = 0
|
||||
self.CCP = CarControllerParams(CP)
|
||||
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.acc_type = 0
|
||||
self.travel_assist_available = False
|
||||
self.curvature_meas = 0.
|
||||
|
||||
def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]):
|
||||
if not self.CP.pcmCruise:
|
||||
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(self, can_parsers) -> tuple[structs.CarState, structs.CarStateSP]:
|
||||
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
|
||||
alt_cp = can_parsers[Bus.alt]
|
||||
|
||||
if self.CP.flags & VolkswagenFlags.PQ:
|
||||
return self.update_pq(pt_cp, cam_cp, ext_cp)
|
||||
elif self.CP.flags & VolkswagenFlags.MLB:
|
||||
return self.update_mlb(pt_cp, cam_cp, ext_cp, alt_cp)
|
||||
elif self.CP.flags & VolkswagenFlags.MEB:
|
||||
return self.update_meb(pt_cp, cam_cp, ext_cp)
|
||||
else:
|
||||
return self.update_mqb(pt_cp, cam_cp, ext_cp)
|
||||
|
||||
def update_mqb(self, pt_cp, cam_cp, ext_cp) -> structs.CarState:
|
||||
ret = structs.CarState()
|
||||
ret_sp = structs.CarStateSP()
|
||||
|
||||
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 self.CP.flags & VolkswagenFlags.KOMBI_PRESENT:
|
||||
self.upscale_lead_car_signal = bool(pt_cp.vl["Kombi_03"]["KBI_Variante"]) # Analog vs 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"],
|
||||
)
|
||||
|
||||
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
|
||||
|
||||
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"])
|
||||
|
||||
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"])
|
||||
|
||||
self.acc_type = ext_cp.vl["ACC_06"]["ACC_Typ"]
|
||||
self.esp_hold_confirmation = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"])
|
||||
acc_limiter_mode = ext_cp.vl["ACC_02"]["ACC_Gesetzte_Zeitluecke"] == 0
|
||||
speed_limiter_mode = bool(pt_cp.vl["TSK_06"]["TSK_Limiter_ausgewaehlt"])
|
||||
|
||||
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 = 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
|
||||
|
||||
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)
|
||||
|
||||
self.frame += 1
|
||||
return ret, ret_sp
|
||||
|
||||
def update_pq(self, pt_cp, cam_cp, ext_cp) -> tuple[structs.CarState, structs.CarStateSP]:
|
||||
ret = structs.CarState()
|
||||
ret_sp = structs.CarStateSP()
|
||||
|
||||
# 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"])]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE
|
||||
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)
|
||||
|
||||
# Update gas, brakes, and gearshift.
|
||||
ret.gasPressed = pt_cp.vl["Motor_3"]["MO3_Pedalwert"] > 0
|
||||
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"])
|
||||
|
||||
# Consume blind-spot monitoring info/warning LED states, if available.
|
||||
# Infostufe: BSM LED on, Warnung: BSM LED flashing
|
||||
if self.CP.enableBsm:
|
||||
ret.leftBlindspot = bool(ext_cp.vl["SWA_1"]["SWA_Infostufe_SWA_li"]) or bool(ext_cp.vl["SWA_1"]["SWA_Warnung_SWA_li"])
|
||||
ret.rightBlindspot = bool(ext_cp.vl["SWA_1"]["SWA_Infostufe_SWA_re"]) or bool(ext_cp.vl["SWA_1"]["SWA_Warnung_SWA_re"])
|
||||
|
||||
# Consume factory LDW data relevant for factory SWA (Lane Change Assist)
|
||||
# and capture it for forwarding to the blind spot radar controller
|
||||
self.ldw_stock_values = cam_cp.vl["LDW_Status"] if self.CP.networkLocation == NetworkLocation.fwdCamera else {}
|
||||
|
||||
# Stock FCW is considered active if the release bit for brake-jerk warning
|
||||
# is set. Stock AEB considered active if the partial braking or target
|
||||
# braking release bits are set.
|
||||
# Refer to VW Self Study Program 890253: Volkswagen Driver Assistance
|
||||
# Systems, chapters on Front Assist with Braking and City Emergency
|
||||
# Braking for the 2016 Passat NMS
|
||||
# TODO: deferred until we can collect data on pre-MY2016 behavior, AWV message may be shorter with fewer signals
|
||||
ret.stockFcw = False
|
||||
ret.stockAeb = False
|
||||
|
||||
# Update ACC radar status.
|
||||
self.acc_type = ext_cp.vl["ACC_System"]["ACS_Typ_ACC"]
|
||||
ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["MO5_GRA_Hauptsch"])
|
||||
ret.cruiseState.enabled = pt_cp.vl["Motor_2"]["MO2_Sta_GRA"] in (1, 2)
|
||||
if self.CP.pcmCruise:
|
||||
ret.accFaulted = ext_cp.vl["ACC_GRA_Anzeige"]["ACA_StaACC"] in (6, 7)
|
||||
else:
|
||||
ret.accFaulted = pt_cp.vl["Motor_2"]["MO2_Sta_GRA"] == 3
|
||||
|
||||
# Update ACC setpoint. When the setpoint reads as 255, the driver has not
|
||||
# yet established an ACC setpoint, so treat it as zero.
|
||||
ret.cruiseState.speed = ext_cp.vl["ACC_GRA_Anzeige"]["ACA_V_Wunsch"] * CV.KPH_TO_MS
|
||||
if ret.cruiseState.speed > 70: # 255 kph in m/s == no current setpoint
|
||||
ret.cruiseState.speed = 0
|
||||
|
||||
# Update button states for turn signals and ACC controls, capture all ACC button state/config for passthrough
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(300, pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_li"],
|
||||
pt_cp.vl["Gate_Komf_1"]["GK1_Blinker_re"])
|
||||
ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS)
|
||||
self.gra_stock_values = pt_cp.vl["GRA_Neu"]
|
||||
|
||||
# Additional safety checks performed in CarInterface.
|
||||
ret.espDisabled = bool(pt_cp.vl["Bremse_1"]["BR1_ESPASR_passive"])
|
||||
|
||||
ret.lowSpeedAlert = self.update_low_speed_alert(ret.vEgo)
|
||||
|
||||
self.frame += 1
|
||||
return ret, ret_sp
|
||||
|
||||
def update_meb(self, pt_cp, cam_cp, ext_cp) -> tuple[structs.CarState, structs.CarStateSP]:
|
||||
ret = structs.CarState()
|
||||
ret_sp = structs.CarStateSP()
|
||||
|
||||
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"],
|
||||
)
|
||||
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["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"])]
|
||||
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE, 5)
|
||||
self.curvature_meas = -pt_cp.vl["QFK_01"]["Curvature"] * (1, -1)[int(pt_cp.vl["QFK_01"]["Curvature_VZ"])]
|
||||
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:
|
||||
ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Gateway_73"]["GE_Fahrstufe"], None))
|
||||
else:
|
||||
ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Getriebe_11"]["GE_Fahrstufe"], None))
|
||||
in_drive = 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, in_drive)
|
||||
|
||||
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
|
||||
ret.brakePressed = bool(pt_cp.vl["Motor_14"]["MO_Fahrer_bremst"])
|
||||
ret.parkingBrake = pt_cp.vl["ESC_50"]["EPB_Status"] in (1, 4) # EPB closing or closed
|
||||
ret.seatbeltUnlatched = pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] != 3
|
||||
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.espDisabled = bool(pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"])
|
||||
ret.espActive = bool(pt_cp.vl["ESP_21"]["ESP_Eingriff"])
|
||||
|
||||
self.acc_type = ext_cp.vl["ACC_18"]["ACC_Typ"]
|
||||
self.esp_hold_confirmation = bool(pt_cp.vl["ESC_50"]["Standstill"])
|
||||
self.travel_assist_available = bool(cam_cp.vl["TA_01"]["Travel_Assist_Available"])
|
||||
ret.stockFcw = bool(ext_cp.vl["AWV_03"]["FCW_Active"])
|
||||
ret.stockAeb = bool(ext_cp.vl["AWV_03"]["AEB_Active"])
|
||||
|
||||
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)
|
||||
ret.cruiseState.nonAdaptive = bool(pt_cp.vl["Motor_51"]["TSK_Limiter_ausgewaehlt"])
|
||||
|
||||
tsk_faulted = pt_cp.vl["Motor_51"]["TSK_Status"] in (6, 7)
|
||||
engine_off = pt_cp.vl["Motor_54"]["Engine_On"] == 0
|
||||
# MQB EVO Gen2 does not define Long_Control_Inhibit in its DBC
|
||||
long_control_inhibit = False
|
||||
ret.accFaulted = (self.update_acc_fault(tsk_faulted, engine_off, long_control_inhibit) or
|
||||
ext_cp.vl["AWV_03"]["AWV_Unavailable"] == 1) # AEB unavailable (i.e. radar covered)
|
||||
|
||||
# TSK winds braking down through brake_only after driver brakes at low speeds. Requesting drive-off in this
|
||||
# state can fault TSK, and stock refuses to engage here as well, so block entry until it clears.
|
||||
# Secondly, after harshly braking, long control is temporarily inhibited (ignored as a fault above)
|
||||
ret.carNotReady = pt_cp.vl["Motor_51"]["TSK_Status"] == 5 or long_control_inhibit
|
||||
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(240, pt_cp.vl["SMLS_01"]["BH_Blinker_li"],
|
||||
pt_cp.vl["SMLS_01"]["BH_Blinker_re"])
|
||||
|
||||
if self.CP.enableBsm:
|
||||
bsm_cp = pt_cp if self.CP.flags & VolkswagenFlags.MEB_GEN2 else ext_cp
|
||||
ret.leftBlindspot = (bool(bsm_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Info_Driver"]) or
|
||||
bool(bsm_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Warn_Driver"]))
|
||||
ret.rightBlindspot = (bool(bsm_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Info_Passenger"]) or
|
||||
bool(bsm_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Warn_Passenger"]))
|
||||
|
||||
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"]
|
||||
self.klr_stock_values = pt_cp.vl["KLR_01"] if self.CP.flags & VolkswagenFlags.STOCK_KLR_PRESENT else {}
|
||||
|
||||
ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS)
|
||||
ret.lowSpeedAlert = self.update_low_speed_alert(ret.vEgo)
|
||||
|
||||
self.frame += 1
|
||||
return ret, ret_sp
|
||||
|
||||
def update_mlb(self, pt_cp, cam_cp, ext_cp, alt_cp) -> tuple[structs.CarState, structs.CarStateSP]:
|
||||
ret = structs.CarState()
|
||||
ret_sp = structs.CarStateSP()
|
||||
|
||||
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
|
||||
ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(alt_cp.vl["Getriebe_03"]["GE_Waehlhebel"], None))
|
||||
|
||||
# TODO: We don't have a true mainswitch state yet, might need stateful tracking on LS_01 if momentary-press is a thing
|
||||
# TSK_04.TSK_Status_GRA_ACC_02 0 = not engaged, 1 = engaged, 2 = engaged with driver accel override, 3 = fault
|
||||
ret.cruiseState.available = alt_cp.vl["TSK_04"]["TSK_Status_GRA_ACC_02"] in (0, 1, 2)
|
||||
ret.cruiseState.enabled = alt_cp.vl["TSK_04"]["TSK_Status_GRA_ACC_02"] in (1, 2)
|
||||
ret.accFaulted = alt_cp.vl["TSK_04"]["TSK_Status_GRA_ACC_02"] == 3
|
||||
ret.cruiseState.speed = ext_cp.vl["ACC_02"]["ACC_Wunschgeschw_02"] * CV.KPH_TO_MS
|
||||
|
||||
self.parse_mlb_mqb_steering_state(ret, pt_cp)
|
||||
|
||||
brake_pedal_pressed = bool(pt_cp.vl["Motor_03"]["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"])
|
||||
ret.espDisabled = pt_cp.vl["ESP_01"]["ESP_Tastung_passiv"] != 0
|
||||
|
||||
ret.leftBlinker = bool(pt_cp.vl["BCM"]["BLINKER_LEFT"])
|
||||
ret.rightBlinker = bool(pt_cp.vl["BCM"]["BLINKER_RIGHT"])
|
||||
|
||||
ret.seatbeltUnlatched = bool(pt_cp.vl["Airbag_01"]["AB_Gurtwarn_VF"])
|
||||
ret.doorOpen = any([alt_cp.vl["Gateway_05"]["FT_Tuer_geoeffnet"],
|
||||
alt_cp.vl["Gateway_05"]["BT_Tuer_geoeffnet"],
|
||||
alt_cp.vl["Gateway_05"]["HL_Tuer_geoeffnet"],
|
||||
alt_cp.vl["Gateway_05"]["HR_Tuer_geoeffnet"]])
|
||||
|
||||
# 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.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
|
||||
|
||||
self.frame += 1
|
||||
return ret, ret_sp
|
||||
|
||||
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):
|
||||
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"])]
|
||||
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)
|
||||
|
||||
def update_hca_state(self, hca_status, in_drive=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
|
||||
self.eps_init_complete = self.eps_init_complete or (hca_status in ("DISABLED", "READY", "ACTIVE") or self.frame > 600)
|
||||
perm_fault = in_drive and hca_status == "DISABLED" or (self.eps_init_complete and hca_status == "FAULT")
|
||||
temp_fault = in_drive and hca_status in ("REJECTED", "PREEMPTED") or not self.eps_init_complete
|
||||
return temp_fault, perm_fault
|
||||
|
||||
def update_acc_fault(self, acc_fault, engine_off, long_inhibit, recovery_frames=10):
|
||||
# TSK temporarily faults when car is "off" (no power steering), and shortly after driver harshly brakes.
|
||||
# Both conditions rise with or slightly before TSK fault, and the fault trails the conditions clearing by under 100 ms
|
||||
if engine_off or long_inhibit:
|
||||
self.tsk_recovery_timer = self.frame
|
||||
return acc_fault and self.frame - self.tsk_recovery_timer >= recovery_frames
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers(CP, CP_SP):
|
||||
if CP.flags & VolkswagenFlags.PQ:
|
||||
return CarState.get_can_parsers_pq(CP)
|
||||
elif CP.flags & VolkswagenFlags.MEB:
|
||||
return CarState.get_can_parsers_meb(CP)
|
||||
|
||||
# manually configure some optional and variable-rate/edge-triggered messages
|
||||
pt_messages, cam_messages, alt_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.STOCK_HCA_PRESENT:
|
||||
cam_messages += [
|
||||
("HCA_01", 1), # From R242 Driver assistance camera, 50Hz if steering/1Hz if not
|
||||
]
|
||||
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, CanBus(CP).pt),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CanBus(CP).cam),
|
||||
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], alt_messages, CanBus(CP).alt),
|
||||
}
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers_pq(CP):
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).pt),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).cam),
|
||||
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).alt),
|
||||
}
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers_meb(CP):
|
||||
pt_messages = [
|
||||
# frequency changes too much for the CANParser to figure out
|
||||
("Blinkmodi_02", 1), # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active)
|
||||
("SMLS_01", 1), # From Stalk Controls
|
||||
]
|
||||
if CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
pt_messages.append(("AWV_03", 1)) # Front Collision Detection (1 Hz when inactive, 50 Hz when active)
|
||||
|
||||
cam_messages = []
|
||||
if CP.networkLocation == NetworkLocation.gateway:
|
||||
cam_messages.append(("AWV_03", 1)) # Front Collision Detection (1 Hz when inactive, 50 Hz when active)
|
||||
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, CanBus(CP).pt),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CanBus(CP).cam),
|
||||
Bus.alt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).alt),
|
||||
}
|
||||
@@ -1,469 +0,0 @@
|
||||
from opendbc.car.new_vw.values import CAR
|
||||
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.NEW_VW_AUDI_A3_MK4: [
|
||||
{ # bus 0 (160 messages)
|
||||
0x40: 8,
|
||||
0x86: 8,
|
||||
0x9F: 8,
|
||||
0xA5: 16,
|
||||
0xAD: 8,
|
||||
0xDB: 48,
|
||||
0xFC: 64,
|
||||
0xFD: 8,
|
||||
0x102: 48,
|
||||
0x10B: 48,
|
||||
0x116: 8,
|
||||
0x12B: 8,
|
||||
0x139: 32,
|
||||
0x13B: 64,
|
||||
0x13C: 48,
|
||||
0x13D: 32,
|
||||
0x14C: 32,
|
||||
0x14D: 32,
|
||||
0x184: 8,
|
||||
0x193: 8,
|
||||
0x200: 32,
|
||||
0x20D: 16,
|
||||
0x20E: 8,
|
||||
0x24C: 16,
|
||||
0x24D: 64,
|
||||
0x25D: 8,
|
||||
0x268: 32,
|
||||
0x26B: 8,
|
||||
0x300: 48,
|
||||
0x30B: 8,
|
||||
0x318: 8,
|
||||
0x31C: 32,
|
||||
0x31D: 32,
|
||||
0x32A: 8,
|
||||
0x366: 8,
|
||||
0x36F: 8,
|
||||
0x374: 8,
|
||||
0x378: 8,
|
||||
0x37B: 8,
|
||||
0x380: 8,
|
||||
0x397: 8,
|
||||
0x3BE: 8,
|
||||
0x3C0: 4,
|
||||
0x3D4: 8,
|
||||
0x3D5: 8,
|
||||
0x3D6: 8,
|
||||
0x3DB: 8,
|
||||
0x3DC: 8,
|
||||
0x3DF: 8,
|
||||
0x3E5: 8,
|
||||
0x3FB: 8,
|
||||
0x462: 8,
|
||||
0x463: 8,
|
||||
0x464: 8,
|
||||
0x481: 8,
|
||||
0x484: 8,
|
||||
0x485: 8,
|
||||
0x486: 8,
|
||||
0x520: 8,
|
||||
0x53F: 8,
|
||||
0x545: 8,
|
||||
0x583: 8,
|
||||
0x585: 8,
|
||||
0x5A0: 8,
|
||||
0x5A7: 8,
|
||||
0x5BF: 4,
|
||||
0x5E1: 8,
|
||||
0x5F0: 8,
|
||||
0x641: 8,
|
||||
0x643: 8,
|
||||
0x645: 8,
|
||||
0x658: 8,
|
||||
0x65A: 8,
|
||||
0x65D: 8,
|
||||
0x65F: 8,
|
||||
0x663: 8,
|
||||
0x670: 8,
|
||||
0x6A6: 8,
|
||||
0x6B2: 8,
|
||||
0x6B4: 8,
|
||||
0x6B5: 8,
|
||||
0x6B7: 8,
|
||||
0x6B8: 8,
|
||||
0x6F95410: 48,
|
||||
0x6F95411: 32,
|
||||
0x6F95412: 48,
|
||||
0x6F95413: 32,
|
||||
0x6F95414: 48,
|
||||
0x6F95415: 32,
|
||||
0x6F95416: 48,
|
||||
0x6F95417: 32,
|
||||
0x6F95440: 8,
|
||||
0x6F95444: 20,
|
||||
0x6F95448: 16,
|
||||
0x6F95449: 16,
|
||||
0x6F9544A: 16,
|
||||
0x6F9544B: 16,
|
||||
0xAF95400: 64,
|
||||
0xAF95401: 64,
|
||||
0xAF95402: 64,
|
||||
0xAF95403: 64,
|
||||
0xAF95404: 64,
|
||||
0xAF95405: 20,
|
||||
0xAF95406: 20,
|
||||
0xAF95409: 64,
|
||||
0xAF9540A: 64,
|
||||
0xAF9540B: 64,
|
||||
0xAF9540C: 64,
|
||||
0xAF9540D: 64,
|
||||
0xAF9540E: 64,
|
||||
0xAF9540F: 64,
|
||||
0xAF95431: 8,
|
||||
0xAF95432: 8,
|
||||
0xAF9544B: 8,
|
||||
0xAF9544C: 8,
|
||||
0xAF954D8: 64,
|
||||
0xAF954D9: 64,
|
||||
0xAF954DA: 64,
|
||||
0x12DD5496: 24,
|
||||
0x12DD54D6: 8,
|
||||
0x12DD54E2: 8,
|
||||
0x12DD54E3: 8,
|
||||
0x12DD5501: 8,
|
||||
0x12DD55D3: 8,
|
||||
0x12DD55D4: 8,
|
||||
0x16A9540A: 8,
|
||||
0x16A95494: 8,
|
||||
0x16A95497: 8,
|
||||
0x16A954AD: 8,
|
||||
0x16A954C4: 8,
|
||||
0x16A95590: 8,
|
||||
0x16A95591: 8,
|
||||
0x17330510: 5,
|
||||
0x17331910: 3,
|
||||
0x17331B10: 3,
|
||||
0x17332110: 5,
|
||||
0x17337610: 5,
|
||||
0x17F00010: 8,
|
||||
0x17F00039: 8,
|
||||
0x17F0004F: 8,
|
||||
0x17F00057: 8,
|
||||
0x17F000BB: 8,
|
||||
0x17F000BC: 8,
|
||||
0x1A5554A8: 8,
|
||||
0x1A555548: 8,
|
||||
0x1A555564: 8,
|
||||
0x1A555592: 8,
|
||||
0x1A5555AC: 8,
|
||||
0x1A555645: 8,
|
||||
0x1A555646: 8,
|
||||
0x1A555647: 8,
|
||||
0x1A555648: 8,
|
||||
0x1A555649: 8,
|
||||
0x1A55564A: 8,
|
||||
0x1A55567D: 8,
|
||||
0x1B000010: 8,
|
||||
0x1B00004F: 8,
|
||||
0x1B000057: 8,
|
||||
0x1B0000BB: 8,
|
||||
0x1B0000BC: 8,
|
||||
},
|
||||
{ # bus 1 (136 messages)
|
||||
0x40: 8,
|
||||
0x86: 8,
|
||||
0xAD: 8,
|
||||
0xB5: 8,
|
||||
0xE4: 8,
|
||||
0xF6: 48,
|
||||
0xFC: 64,
|
||||
0xFD: 8,
|
||||
0x102: 48,
|
||||
0x10B: 48,
|
||||
0x10D: 48,
|
||||
0x10E: 48,
|
||||
0x116: 8,
|
||||
0x11D: 8,
|
||||
0x12B: 8,
|
||||
0x139: 32,
|
||||
0x144: 8,
|
||||
0x148: 8,
|
||||
0x149: 8,
|
||||
0x14A: 32,
|
||||
0x14C: 32,
|
||||
0x14D: 32,
|
||||
0x14E: 8,
|
||||
0x153: 8,
|
||||
0x176: 12,
|
||||
0x184: 8,
|
||||
0x187: 8,
|
||||
0x1A4: 8,
|
||||
0x1FD: 32,
|
||||
0x200: 32,
|
||||
0x206: 24,
|
||||
0x272: 8,
|
||||
0x2C2: 8,
|
||||
0x30B: 8,
|
||||
0x318: 8,
|
||||
0x331: 8,
|
||||
0x35B: 64,
|
||||
0x35C: 64,
|
||||
0x365: 8,
|
||||
0x394: 8,
|
||||
0x3A3: 8,
|
||||
0x3B5: 8,
|
||||
0x3BE: 8,
|
||||
0x3C0: 4,
|
||||
0x3C7: 8,
|
||||
0x3D0: 8,
|
||||
0x3D4: 8,
|
||||
0x3D6: 8,
|
||||
0x3E5: 8,
|
||||
0x3FB: 8,
|
||||
0x462: 8,
|
||||
0x463: 8,
|
||||
0x464: 8,
|
||||
0x465: 8,
|
||||
0x481: 8,
|
||||
0x48B: 8,
|
||||
0x497: 8,
|
||||
0x504: 8,
|
||||
0x520: 8,
|
||||
0x545: 8,
|
||||
0x551: 8,
|
||||
0x556: 8,
|
||||
0x557: 8,
|
||||
0x569: 8,
|
||||
0x56F: 8,
|
||||
0x583: 8,
|
||||
0x585: 8,
|
||||
0x58C: 8,
|
||||
0x592: 8,
|
||||
0x5A7: 8,
|
||||
0x5AC: 8,
|
||||
0x5E1: 8,
|
||||
0x5E7: 8,
|
||||
0x5E9: 8,
|
||||
0x5EA: 8,
|
||||
0x5F0: 8,
|
||||
0x5F7: 8,
|
||||
0x640: 8,
|
||||
0x641: 8,
|
||||
0x643: 8,
|
||||
0x647: 8,
|
||||
0x65A: 8,
|
||||
0x65D: 8,
|
||||
0x65E: 8,
|
||||
0x65F: 8,
|
||||
0x668: 8,
|
||||
0x670: 8,
|
||||
0x676: 8,
|
||||
0x6A6: 8,
|
||||
0x6B2: 8,
|
||||
0x6B4: 8,
|
||||
0x6B7: 8,
|
||||
0x6B8: 8,
|
||||
0x6BA: 8,
|
||||
0xAF95452: 64,
|
||||
0xAF95453: 64,
|
||||
0xAF954D5: 8,
|
||||
0x12DD5496: 24,
|
||||
0x12DD5497: 16,
|
||||
0x12DD54A7: 32,
|
||||
0x12DD54BF: 8,
|
||||
0x12DD54C1: 8,
|
||||
0x12DD54D6: 8,
|
||||
0x12DD54DF: 8,
|
||||
0x12DD5501: 8,
|
||||
0x12DD553D: 8,
|
||||
0x12DD553F: 8,
|
||||
0x12DD5562: 8,
|
||||
0x17333910: 8,
|
||||
0x17333C10: 8,
|
||||
0x17335D10: 5,
|
||||
0x17F00010: 8,
|
||||
0x17F00076: 8,
|
||||
0x17F00077: 8,
|
||||
0x17FC0742: 8,
|
||||
0x17FE0742: 8,
|
||||
0x1A555444: 8,
|
||||
0x1A555445: 32,
|
||||
0x1A555480: 8,
|
||||
0x1A555493: 8,
|
||||
0x1A555494: 8,
|
||||
0x1A5554F6: 16,
|
||||
0x1A5554F7: 8,
|
||||
0x1A555521: 8,
|
||||
0x1A55552B: 8,
|
||||
0x1A55552F: 8,
|
||||
0x1A555548: 8,
|
||||
0x1A555563: 8,
|
||||
0x1A555564: 8,
|
||||
0x1A5555AC: 8,
|
||||
0x1A555682: 16,
|
||||
0x1A5556E8: 32,
|
||||
0x1B000010: 8,
|
||||
0x1B000076: 8,
|
||||
0x1B000077: 8,
|
||||
0x1B0003D2: 8,
|
||||
},
|
||||
{ # bus 2 (160 messages)
|
||||
0x40: 8,
|
||||
0x86: 8,
|
||||
0x9F: 8,
|
||||
0xA5: 16,
|
||||
0xAD: 8,
|
||||
0xDB: 48,
|
||||
0xFC: 64,
|
||||
0xFD: 8,
|
||||
0x102: 48,
|
||||
0x10B: 48,
|
||||
0x116: 8,
|
||||
0x12B: 8,
|
||||
0x139: 32,
|
||||
0x13B: 64,
|
||||
0x13C: 48,
|
||||
0x13D: 32,
|
||||
0x14C: 32,
|
||||
0x14D: 32,
|
||||
0x184: 8,
|
||||
0x193: 8,
|
||||
0x200: 32,
|
||||
0x20D: 16,
|
||||
0x20E: 8,
|
||||
0x24C: 16,
|
||||
0x24D: 64,
|
||||
0x25D: 8,
|
||||
0x268: 32,
|
||||
0x26B: 8,
|
||||
0x300: 48,
|
||||
0x30B: 8,
|
||||
0x318: 8,
|
||||
0x31C: 32,
|
||||
0x31D: 32,
|
||||
0x32A: 8,
|
||||
0x366: 8,
|
||||
0x36F: 8,
|
||||
0x374: 8,
|
||||
0x378: 8,
|
||||
0x37B: 8,
|
||||
0x380: 8,
|
||||
0x397: 8,
|
||||
0x3BE: 8,
|
||||
0x3C0: 4,
|
||||
0x3D4: 8,
|
||||
0x3D5: 8,
|
||||
0x3D6: 8,
|
||||
0x3DB: 8,
|
||||
0x3DC: 8,
|
||||
0x3DF: 8,
|
||||
0x3E5: 8,
|
||||
0x3FB: 8,
|
||||
0x462: 8,
|
||||
0x463: 8,
|
||||
0x464: 8,
|
||||
0x481: 8,
|
||||
0x484: 8,
|
||||
0x485: 8,
|
||||
0x486: 8,
|
||||
0x520: 8,
|
||||
0x53F: 8,
|
||||
0x545: 8,
|
||||
0x583: 8,
|
||||
0x585: 8,
|
||||
0x5A0: 8,
|
||||
0x5A7: 8,
|
||||
0x5BF: 4,
|
||||
0x5E1: 8,
|
||||
0x5F0: 8,
|
||||
0x641: 8,
|
||||
0x643: 8,
|
||||
0x645: 8,
|
||||
0x658: 8,
|
||||
0x65A: 8,
|
||||
0x65D: 8,
|
||||
0x65F: 8,
|
||||
0x663: 8,
|
||||
0x670: 8,
|
||||
0x6A6: 8,
|
||||
0x6B2: 8,
|
||||
0x6B4: 8,
|
||||
0x6B5: 8,
|
||||
0x6B7: 8,
|
||||
0x6B8: 8,
|
||||
0x6F95410: 48,
|
||||
0x6F95411: 32,
|
||||
0x6F95412: 48,
|
||||
0x6F95413: 32,
|
||||
0x6F95414: 48,
|
||||
0x6F95415: 32,
|
||||
0x6F95416: 48,
|
||||
0x6F95417: 32,
|
||||
0x6F95440: 8,
|
||||
0x6F95444: 20,
|
||||
0x6F95448: 16,
|
||||
0x6F95449: 16,
|
||||
0x6F9544A: 16,
|
||||
0x6F9544B: 16,
|
||||
0xAF95400: 64,
|
||||
0xAF95401: 64,
|
||||
0xAF95402: 64,
|
||||
0xAF95403: 64,
|
||||
0xAF95404: 64,
|
||||
0xAF95405: 20,
|
||||
0xAF95406: 20,
|
||||
0xAF95409: 64,
|
||||
0xAF9540A: 64,
|
||||
0xAF9540B: 64,
|
||||
0xAF9540C: 64,
|
||||
0xAF9540D: 64,
|
||||
0xAF9540E: 64,
|
||||
0xAF9540F: 64,
|
||||
0xAF95431: 8,
|
||||
0xAF95432: 8,
|
||||
0xAF9544B: 8,
|
||||
0xAF9544C: 8,
|
||||
0xAF954D8: 64,
|
||||
0xAF954D9: 64,
|
||||
0xAF954DA: 64,
|
||||
0x12DD5496: 24,
|
||||
0x12DD54D6: 8,
|
||||
0x12DD54E2: 8,
|
||||
0x12DD54E3: 8,
|
||||
0x12DD5501: 8,
|
||||
0x12DD55D3: 8,
|
||||
0x12DD55D4: 8,
|
||||
0x16A9540A: 8,
|
||||
0x16A95494: 8,
|
||||
0x16A95497: 8,
|
||||
0x16A954AD: 8,
|
||||
0x16A954C4: 8,
|
||||
0x16A95590: 8,
|
||||
0x16A95591: 8,
|
||||
0x17330510: 5,
|
||||
0x17331910: 3,
|
||||
0x17331B10: 3,
|
||||
0x17332110: 5,
|
||||
0x17337610: 5,
|
||||
0x17F00010: 8,
|
||||
0x17F00039: 8,
|
||||
0x17F0004F: 8,
|
||||
0x17F00057: 8,
|
||||
0x17F000BB: 8,
|
||||
0x17F000BC: 8,
|
||||
0x1A5554A8: 8,
|
||||
0x1A555548: 8,
|
||||
0x1A555564: 8,
|
||||
0x1A555592: 8,
|
||||
0x1A5555AC: 8,
|
||||
0x1A555645: 8,
|
||||
0x1A555646: 8,
|
||||
0x1A555647: 8,
|
||||
0x1A555648: 8,
|
||||
0x1A555649: 8,
|
||||
0x1A55564A: 8,
|
||||
0x1A55567D: 8,
|
||||
0x1B000010: 8,
|
||||
0x1B00004F: 8,
|
||||
0x1B000057: 8,
|
||||
0x1B0000BB: 8,
|
||||
0x1B0000BC: 8,
|
||||
},
|
||||
],
|
||||
}
|
||||
@@ -1,51 +0,0 @@
|
||||
from opendbc.car import get_safety_config, structs
|
||||
from opendbc.car.interfaces import CarInterfaceBase
|
||||
from opendbc.car.new_vw.carcontroller import CarController
|
||||
from opendbc.car.new_vw.carstate import CarState
|
||||
from opendbc.car.new_vw.radar_interface import RadarInterface
|
||||
from opendbc.car.volkswagen.values import NetworkLocation, TransmissionType, VolkswagenSafetyFlags
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
CarState = CarState
|
||||
CarController = CarController
|
||||
RadarInterface = RadarInterface
|
||||
|
||||
@staticmethod
|
||||
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
|
||||
ret.brand = "new_vw"
|
||||
|
||||
# MQB EVO Gen2 shares the MEB safety hooks in the panda firmware, so use the existing
|
||||
# volkswagenMeb safety model plus the gen2 CRC/DLC variant. No safety change and no panda
|
||||
# firmware rebuild is required for this brand.
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.volkswagenMeb)]
|
||||
ret.safetyConfigs[0].safetyParam |= VolkswagenSafetyFlags.MEB_ALT_CRC.value
|
||||
|
||||
ret.transmissionType = TransmissionType.direct
|
||||
ret.steerControlType = structs.CarParams.SteerControlType.curvature
|
||||
ret.steerAtStandstill = True
|
||||
ret.steerActuatorDelay = 0.3
|
||||
|
||||
ret.lateralTuning.init('pid')
|
||||
ret.lateralTuning.pid.kpBP = [10., 40.]
|
||||
ret.lateralTuning.pid.kpV = [0., 1.45]
|
||||
ret.lateralTuning.pid.kiBP = [10., 40.]
|
||||
ret.lateralTuning.pid.kiV = [0., 0.12]
|
||||
ret.lateralTuning.pid.kf = 1.
|
||||
|
||||
ret.networkLocation = NetworkLocation.gateway
|
||||
# 0x24F (radar objects) is not published by this car
|
||||
ret.radarUnavailable = True
|
||||
ret.enableBsm = 0x24C in fingerprint[0] # MEB_Side_Assist_01
|
||||
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.longitudinalActuatorDelay = 0.3
|
||||
ret.radarDelay = 0.15
|
||||
ret.longitudinalTuning.kiBP = [0., 30.]
|
||||
ret.longitudinalTuning.kiV = [0.4, 0.]
|
||||
|
||||
ret.pcmCruise = False
|
||||
ret.stopAccel = -0.55
|
||||
ret.autoResumeSng = False
|
||||
|
||||
return ret
|
||||
@@ -1,263 +0,0 @@
|
||||
from opendbc.car import Bus, structs
|
||||
from opendbc.can import CANDefine
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.new_vw.values import DBC
|
||||
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
def create_steering_control(packer, bus, apply_curvature, lkas_enabled, power=0):
|
||||
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_lka_hud_control(packer, bus, ldw_stock_values, lat_active, steering_pressed, hud_alert, hud_control, sound_alert=False):
|
||||
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)
|
||||
|
||||
|
||||
ACC_HUD_ERROR = 6
|
||||
ACC_HUD_OVERRIDE = 4
|
||||
ACC_HUD_ACTIVE = 3
|
||||
ACC_HUD_ENABLED = 2
|
||||
ACC_HUD_DISABLED = 0
|
||||
|
||||
|
||||
class MebLongStateMachine:
|
||||
HOLD_RELEASE_SPEED = 5 * CV.KPH_TO_MS
|
||||
|
||||
def __init__(self, CP, CCP):
|
||||
self.CCP = CCP
|
||||
self.RAMP_FRAMES = 10 // CCP.ACC_CONTROL_STEP # 100 ms
|
||||
|
||||
self.disengage_ramp_counter = 0 # always ramp when disengaging
|
||||
|
||||
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
|
||||
self.acc_status_vals = {v: k for k, v in can_define.dv['ACC_18']['ACC_Status_ACC'].items()}
|
||||
self.acc_hold_type_vals = {v: k for k, v in can_define.dv['ACC_18']['ACC_Anforderung_HMS'].items()}
|
||||
|
||||
self.prev_acc_hold_type = self.acc_hold_type_vals['KEINE_ANFORDERUNG'] # no request
|
||||
self.acc_status = self.acc_status_vals['ACC_OFF_HAUPTSCHALTER_AUS'] # last acc status, read by HUD msg
|
||||
|
||||
def _get_acc_status(self, CS, CC) -> int:
|
||||
# stateless
|
||||
# NOTE: stock TSK and camera goes to 5 on disengage independently which we don't model, but hasn't been shown to fault without it
|
||||
if CS.out.accFaulted:
|
||||
return self.acc_status_vals['REVERSIBLER_FEHLER_IM_ACC_SYSTEM']
|
||||
elif CC.enabled:
|
||||
return self.acc_status_vals['ACC_OVERRIDE' if CC.cruiseControl.override else 'ACC_AKTIV_REGELT']
|
||||
elif CS.out.cruiseState.available:
|
||||
return self.acc_status_vals['ACC_STANDBY']
|
||||
else:
|
||||
return self.acc_status_vals['ACC_OFF_HAUPTSCHALTER_AUS'] # disabled
|
||||
|
||||
def _get_hold_type(self, CS, CC) -> int:
|
||||
# warning: car is reacting to hold mechanic even with long control off
|
||||
# HALTEN -> KEINE_ANFORDERUNG causes the car to fault into park, so both branches below put a ramp in
|
||||
# between: disengaging always ramps, and while engaged a release ramps until 5 kph
|
||||
# NOTE: this allows KEINE_ANFORDERUNG -> ANFAHREN, but we haven't observed a fault due to this yet
|
||||
# TODO: camera can send 7 on disengage at a stop which we don't fully understand yet
|
||||
stopping = CC.actuators.longControlState == LongCtrlState.stopping
|
||||
starting = CC.actuators.longControlState == LongCtrlState.pid and CS.esp_hold_confirmation
|
||||
long_active = CC.longActive and not CS.out.accFaulted # catches it one frame earlier, not sure if needed
|
||||
|
||||
if not long_active:
|
||||
# Stock goes to RAMP for as long as TSK_Status is 5 usually, 100ms seems fine to mimic that behavior.
|
||||
# Stock stays active for gas press, but we go inactive
|
||||
if self.disengage_ramp_counter > 0:
|
||||
acc_hold_type = self.acc_hold_type_vals['LOESEN_UEBER_RAMPE'] # ramp
|
||||
self.disengage_ramp_counter -= 1
|
||||
else:
|
||||
acc_hold_type = self.acc_hold_type_vals['KEINE_ANFORDERUNG'] # no request
|
||||
|
||||
else:
|
||||
was_engaged = self.disengage_ramp_counter == self.RAMP_FRAMES
|
||||
self.disengage_ramp_counter = self.RAMP_FRAMES # prep ramp if we disengage
|
||||
|
||||
if stopping:
|
||||
acc_hold_type = self.acc_hold_type_vals['HALTEN'] # stopping/stopped, allowed at any time
|
||||
elif starting:
|
||||
acc_hold_type = self.acc_hold_type_vals['ANFAHREN'] # resume after reaching full stop
|
||||
else:
|
||||
# After aborting a stop or finishing starting, we need to send RAMP until we hit 5 kph or go long inactive,
|
||||
# only if we didn't just re-engage
|
||||
releasing = was_engaged and self.prev_acc_hold_type in (self.acc_hold_type_vals['HALTEN'],
|
||||
self.acc_hold_type_vals['ANFAHREN'],
|
||||
self.acc_hold_type_vals['LOESEN_UEBER_RAMPE'])
|
||||
|
||||
if releasing and CS.out.vEgo < self.HOLD_RELEASE_SPEED:
|
||||
acc_hold_type = self.acc_hold_type_vals['LOESEN_UEBER_RAMPE'] # ramp
|
||||
else:
|
||||
acc_hold_type = self.acc_hold_type_vals['KEINE_ANFORDERUNG'] # no request
|
||||
|
||||
return acc_hold_type
|
||||
|
||||
def update(self, CS, CC, accel) -> tuple[float, int, int, bool, bool]:
|
||||
acc_status = self._get_acc_status(CS, CC)
|
||||
acc_hold_type = self._get_hold_type(CS, CC)
|
||||
|
||||
# transition to inactive accel and jerks as soon as we enter ESP standstill
|
||||
requesting_hold = acc_hold_type == self.acc_hold_type_vals['HALTEN']
|
||||
held = requesting_hold and CS.esp_hold_confirmation
|
||||
if not CC.enabled or held:
|
||||
accel = self.CCP.ACCEL_INACTIVE
|
||||
|
||||
# hold requested but the car hasn't reached standstill yet
|
||||
braking_to_stop = requesting_hold and not CS.esp_hold_confirmation
|
||||
|
||||
# driving off from a hold
|
||||
leaving_standstill = acc_hold_type == self.acc_hold_type_vals['ANFAHREN']
|
||||
|
||||
self.prev_acc_hold_type = acc_hold_type
|
||||
self.acc_status = acc_status
|
||||
return accel, acc_status, acc_hold_type, braking_to_stop, leaving_standstill
|
||||
|
||||
|
||||
def create_acc_accel_control(packer, bus, CCP, acc_type, acc_enabled, accel, acc_status, acc_hold_type,
|
||||
braking_to_stop, leaving_standstill, speed, travel_assist_available):
|
||||
# active longitudinal control disables one pedal driving (regen mode) while using overriding mechanism
|
||||
# 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 inactive together 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 while requesting stop but ESP is not in standstill
|
||||
commands = []
|
||||
|
||||
# ACC_Anhalteweg: when stopping: MEB: values <> 0 the car can execute a hard brake probably if target is too close, MQBEvo: value 0 results in hard brake
|
||||
terminal_rollout = 0
|
||||
|
||||
values = {
|
||||
"ACC_Typ": acc_type,
|
||||
"ACC_Status_ACC": acc_status,
|
||||
"ACC_StartStopp_Info": acc_enabled,
|
||||
"ACC_Sollbeschleunigung_02": accel,
|
||||
"ACC_zul_Regelabw_unten": 0,
|
||||
"ACC_zul_Regelabw_oben": 0,
|
||||
"ACC_neg_Sollbeschl_Grad_02": CCP.JERK_LIMIT if accel != CCP.ACCEL_INACTIVE else 0,
|
||||
"ACC_pos_Sollbeschl_Grad_02": CCP.JERK_LIMIT if accel != CCP.ACCEL_INACTIVE else 0,
|
||||
# NOTE: gen1 stock sets this while launching, gen2 stock never does
|
||||
"ACC_Anfahren": 1 if leaving_standstill else 0,
|
||||
"ACC_Anhalten": 1 if braking_to_stop else 0,
|
||||
"ACC_Anhalteweg": terminal_rollout if braking_to_stop else 20.46,
|
||||
"ACC_Anforderung_HMS": acc_hold_type,
|
||||
"ACC_AKTIV_regelt": 0, # always zero, stock uses ACC_Status_ACC
|
||||
"Speed": speed,
|
||||
"SET_ME_0XFE": 0xFE,
|
||||
"SET_ME_0X1": 0x1,
|
||||
"SET_ME_0X9": 0x9,
|
||||
}
|
||||
|
||||
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 create_acc_hud_control(packer, bus, acc_status, set_speed, lead_visible, distance_bars, show_distance_bars, distance, fcw_alert):
|
||||
values = {
|
||||
"ACC_Status_ACC": acc_status,
|
||||
"ACC_Tempolimit": 0,
|
||||
"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_status in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 1, # probably keeping warning in front
|
||||
"ACC_Optischer_Fahrerhinweis": 1 if fcw_alert and acc_status in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0, # enables optical warning
|
||||
"ACC_Akustischer_Fahrerhinweis": 3 if fcw_alert and acc_status in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0, # enables sound warning
|
||||
"ACC_Texte_Zusatzanz_02": 11 if fcw_alert and acc_status 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_status in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else
|
||||
(1 if acc_status == 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_status in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0,
|
||||
"ACC_Standby_Override": 1 if acc_status != ACC_HUD_ACTIVE else 0,
|
||||
"Street_Color": 1 if acc_status in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0, # light grey (1) or dark (0) street
|
||||
"Lead_Brightness": 3 if acc_status == ACC_HUD_ACTIVE else 0, # object shows in color
|
||||
# TODO: a nice speed dependent bar distance
|
||||
"Zeitluecke_1": 0, # desired distance to lead object for distance bar 1
|
||||
"Zeitluecke_2": 0, # desired distance to lead object for distance bar 2
|
||||
"Zeitluecke_3": 0, # desired distance to lead object for distance bar 3
|
||||
"Zeitluecke_4": 0, # desired distance to lead object for distance bar 4
|
||||
"Zeitluecke_5": 0, # desired distance to lead object for distance bar 5
|
||||
"Zeitluecke_Farbe": 1 if acc_status 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_status != 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_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)
|
||||
@@ -1,9 +0,0 @@
|
||||
from opendbc.car.interfaces import RadarInterfaceBase
|
||||
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
"""MQB EVO Gen2 does not publish radar objects: 0x24F (MEB_Distance_01) is never seen on the
|
||||
bus, so there is nothing to parse. Publish empty radar data at the base class rate."""
|
||||
|
||||
def update(self, can_strings):
|
||||
return super().update(None)
|
||||
@@ -1,29 +0,0 @@
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms
|
||||
from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts
|
||||
# Reuse the VW/MEB scaffolding instead of duplicating it. MQB EVO Gen2 uses the MEB gateway
|
||||
# harness, the MEB safety hooks (see interface.py) and the same CAN bus layout.
|
||||
from opendbc.car.volkswagen.values import (CanBus, CarControllerParams, FW_QUERY_CONFIG, NetworkLocation,
|
||||
TransmissionType, GearShifter, VolkswagenCarSpecs, VolkswagenFlags)
|
||||
|
||||
|
||||
@dataclass
|
||||
class NewVwCarDocs(CarDocs):
|
||||
package: str = "All"
|
||||
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.vw_j533]))
|
||||
|
||||
|
||||
@dataclass
|
||||
class NewVwPlatformConfig(PlatformConfig):
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'vw_mqbevo_2024', Bus.radar: 'vw_mqbevo_2024'})
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
NEW_VW_AUDI_A3_MK4 = NewVwPlatformConfig(
|
||||
[NewVwCarDocs("Audi RS3 2026")],
|
||||
VolkswagenCarSpecs(mass=1650., wheelbase=2.631),
|
||||
)
|
||||
|
||||
|
||||
DBC = CAR.create_dbc_map()
|
||||
@@ -16,9 +16,8 @@ from opendbc.car.subaru.values import CAR as SUBARU
|
||||
from opendbc.car.tesla.values import CAR as TESLA
|
||||
from opendbc.car.toyota.values import CAR as TOYOTA
|
||||
from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN
|
||||
from opendbc.car.new_vw.values import CAR as NEW_VW
|
||||
|
||||
Platform = BODY | CHRYSLER | FORD | GM | GWM | HONDA | HYUNDAI | MAZDA | MG | MOCK | NISSAN | PSA | RIVIAN | SUBARU | TESLA | TOYOTA | VOLKSWAGEN | NEW_VW
|
||||
Platform = BODY | CHRYSLER | FORD | GM | GWM | HONDA | HYUNDAI | MAZDA | MG | MOCK | NISSAN | PSA | RIVIAN | SUBARU | TESLA | TOYOTA | VOLKSWAGEN
|
||||
BRANDS = get_args(Platform)
|
||||
|
||||
PLATFORMS: dict[str, Platform] = {str(platform): platform for brand in BRANDS for platform in brand}
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -1,12 +1,4 @@
|
||||
{
|
||||
"NEW_VW Audi RS3 2026": {
|
||||
"platform": "NEW_VW_AUDI_A3_MK4",
|
||||
"make": "NEW_VW",
|
||||
"brand": "new_vw",
|
||||
"model": "RS3",
|
||||
"year": ["2026"],
|
||||
"package": "All"
|
||||
},
|
||||
"Acura ILX 2016-18": {
|
||||
"platform": "ACURA_ILX",
|
||||
"make": "Acura",
|
||||
|
||||
Reference in New Issue
Block a user