mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-22 06:22:06 +08:00
VW PQ: Volkswagen Passat NMS (#24768)
* VW PQ: Volkswagen Passat NMS * regen CARS.md * vEgo from Bremse_1 vehicle speed * sync opendbc to master * handle checksums and counters in opendbc * LDW HUD message handling * GRA_Neu_Zaehler -> COUNTER * bump opendbc * stub in till we find platform ACC standstill * bump opendbc * bump opendbc * placeholder lateral accel data * regen CARS.md * counters now directly supported in opendbc * additional door-open signals * add trunk lid state * add doors and trunk lid to signals list * LDW_Status updates and passthrough * bump opendbc for typo fix * update AWV comment * another comment update * regen CARS.md with PQ in dashcam only * don't show NMS footnotes while still in dashcam * polish * add stubbed-out dashcamOnly prep * VW MQB: Cleanup stock ACC button handling * bump opendbc and panda * use controls resume output as trigger * these can wait until taco bell * bump opendbc * pass through of previously fixed value * retry CI * checks already done in carcontroller * don't need these anymore * reduce diff for now * slightly better abstraction * more engine and trans FW * turn signal is instantaneous stalk position * weak sauce :( * better clarity * try torque tune * add test route references * bump opendbc and panda for OP long * don't show steering faults for 3 seconds after start * longitudinal control senders * a little more torque * test hax to torque control * test a little more delay * allow use of manufacturer ramp-up rate * soften wheel-touch threshold * Revert "test hax to torque control" This reverts commit d1af459c29e36264aae406f72b8fcbc9ef22b9e0. * punch it Chewie * better ACC state and mainswitch handling * a little more * tweak max accel gradient * oops * also oops * stuff * srsly * that's not how this works * regen CARS.md * footnotes now properly excluded for dashcam cars * this wasn't a problem * update network location detection * bump submodules for ACC main switch * clean up DBC references and long flag * bump one more time * one more time * follow CANPacker counter refactor * bump opendbc * sync opendbc to master * bump panda to fix Subaru tests * DBC handling cleanup * fix * model-year stretch * cleanup and rate bugfixes * better abstractions * simplify create_lka_hud_control * volkswagencan -> mqbcan * bump panda * fix doc data bug, regen CARS.md * style updates; diff reduction * use common button enable logic * not needed anymore * refactor TSK and HUD enum values * make common button events function * consistency * bump panda * bump panda * dashcam only * don't need process_replay yet * regen CARS.md with Passat NMS in dashcam * can't handle dashcam-orphaned footnotes yet * remove outdated standstill handling * editor tried to be too helpful at some point * don't need to import this anymore * Apply suggestions from code review Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com> * follow parkingBrake refactor Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
This commit is contained in:
Jason Young
+1
-1
Submodule panda updated: 5e0dde7b48...199bc10db3
@@ -552,6 +552,7 @@ opendbc/toyota_nodsu_pt_generated.dbc
|
||||
opendbc/toyota_adas.dbc
|
||||
opendbc/toyota_tss2_adas.dbc
|
||||
|
||||
opendbc/vw_golf_mk4.dbc
|
||||
opendbc/vw_mqb_2010.dbc
|
||||
|
||||
opendbc/tesla_can.dbc
|
||||
|
||||
@@ -168,6 +168,7 @@ routes = [
|
||||
TestRoute("cae14e88932eb364|2021-03-26--14-43-28", VOLKSWAGEN.GOLF_MK7),
|
||||
TestRoute("58a7d3b707987d65|2021-03-25--17-26-37", VOLKSWAGEN.JETTA_MK7),
|
||||
TestRoute("4d134e099430fba2|2021-03-26--00-26-06", VOLKSWAGEN.PASSAT_MK8),
|
||||
TestRoute("3cfdec54aa035f3f|2022-07-19--23-45-10", VOLKSWAGEN.PASSAT_NMS),
|
||||
TestRoute("0cd0b7f7e31a3853|2021-11-03--19-30-22", VOLKSWAGEN.POLO_MK6),
|
||||
TestRoute("7d82b2f3a9115f1f|2021-10-21--15-39-42", VOLKSWAGEN.TAOS_MK1),
|
||||
TestRoute("2744c89a8dda9a51|2021-07-24--21-28-06", VOLKSWAGEN.TCROSS_MK1),
|
||||
|
||||
@@ -25,6 +25,7 @@ RAM 1500 5TH GEN: [2.0, 2.0, 0.0]
|
||||
RAM HD 5TH GEN: [1.4, 1.4, 0.0]
|
||||
SUBARU OUTBACK 6TH GEN: [2.3, 2.3, 0.11]
|
||||
CHEVROLET BOLT EUV 2022: [2.0, 2.0, 0.05]
|
||||
VOLKSWAGEN PASSAT NMS: [2.5, 2.5, 0.1]
|
||||
|
||||
# Dashcam or fallback configured as ideal car
|
||||
mock: [10.0, 10, 0.0]
|
||||
|
||||
@@ -1,8 +1,10 @@
|
||||
from cereal import car
|
||||
from opendbc.can.packer import CANPacker
|
||||
from common.numpy_fast import clip
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.volkswagen import mqbcan
|
||||
from selfdrive.car.volkswagen.values import CANBUS, CarControllerParams
|
||||
from selfdrive.car.volkswagen import mqbcan, pqcan
|
||||
from selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
@@ -11,7 +13,7 @@ class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.CCP = CarControllerParams(CP)
|
||||
self.CCS = mqbcan
|
||||
self.CCS = pqcan if CP.carFingerprint in PQ_CARS else mqbcan
|
||||
self.packer_pt = CANPacker(dbc_name)
|
||||
|
||||
self.apply_steer_last = 0
|
||||
@@ -65,6 +67,13 @@ class CarController:
|
||||
self.apply_steer_last = apply_steer
|
||||
can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer, hcaEnabled))
|
||||
|
||||
# **** Acceleration Controls ******************************************** #
|
||||
|
||||
if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl:
|
||||
tsk_status = self.CCS.tsk_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
|
||||
accel = clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0
|
||||
can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, CANBUS.pt, tsk_status, accel))
|
||||
|
||||
# **** HUD Controls ***************************************************** #
|
||||
|
||||
if self.frame % self.CCP.LDW_STEP == 0:
|
||||
@@ -74,6 +83,12 @@ class CarController:
|
||||
can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, CANBUS.pt, CS.ldw_stock_values, CC.enabled,
|
||||
CS.out.steeringPressed, hud_alert, hud_control))
|
||||
|
||||
if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl:
|
||||
acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
|
||||
set_speed = hud_control.setSpeed * CV.MS_TO_KPH # FIXME: follow the recent displayed-speed updates, also use mph_kmh toggle to fix display rounding problem?
|
||||
can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, CANBUS.pt, acc_hud_status, set_speed,
|
||||
hud_control.leadVisible))
|
||||
|
||||
# **** Stock ACC Button Controls **************************************** #
|
||||
|
||||
if self.CP.pcmCruise and self.frame % self.CCP.GRA_ACC_STEP == 0:
|
||||
|
||||
@@ -3,7 +3,7 @@ from cereal import car
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.car.interfaces import CarStateBase
|
||||
from opendbc.can.parser import CANParser
|
||||
from selfdrive.car.volkswagen.values import DBC, CANBUS, NetworkLocation, TransmissionType, GearShifter, \
|
||||
from selfdrive.car.volkswagen.values import DBC, CANBUS, PQ_CARS, NetworkLocation, TransmissionType, GearShifter, \
|
||||
CarControllerParams
|
||||
|
||||
|
||||
@@ -28,6 +28,9 @@ class CarState(CarStateBase):
|
||||
return button_events
|
||||
|
||||
def update(self, pt_cp, cam_cp, ext_cp, trans_type):
|
||||
if self.CP.carFingerprint in PQ_CARS:
|
||||
return self.update_pq(pt_cp, cam_cp, ext_cp, trans_type)
|
||||
|
||||
ret = car.CarState.new_message()
|
||||
# Update vehicle speed and acceleration from ABS wheel speeds.
|
||||
ret.wheelSpeeds = self.get_wheel_speeds(
|
||||
@@ -135,8 +138,111 @@ class CarState(CarStateBase):
|
||||
|
||||
return ret
|
||||
|
||||
def update_pq(self, pt_cp, cam_cp, ext_cp, trans_type):
|
||||
ret = car.CarState.new_message()
|
||||
# Update vehicle speed and acceleration from ABS wheel speeds.
|
||||
ret.wheelSpeeds = self.get_wheel_speeds(
|
||||
pt_cp.vl["Bremse_3"]["Radgeschw__VL_4_1"],
|
||||
pt_cp.vl["Bremse_3"]["Radgeschw__VR_4_1"],
|
||||
pt_cp.vl["Bremse_3"]["Radgeschw__HL_4_1"],
|
||||
pt_cp.vl["Bremse_3"]["Radgeschw__HR_4_1"],
|
||||
)
|
||||
|
||||
# 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"]["Geschwindigkeit_neu__Bremse_1_"] * CV.KPH_TO_MS
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = ret.vEgo < 0.1
|
||||
|
||||
# Update steering angle, rate, yaw rate, and driver input torque. VW send
|
||||
# the sign/direction in a separate signal so they must be recombined.
|
||||
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"]["Lenkradwinkel_Geschwindigkeit"] * (1, -1)[int(pt_cp.vl["Lenkwinkel_1"]["Lenkradwinkel_Geschwindigkeit_S"])]
|
||||
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
|
||||
ret.yawRate = pt_cp.vl["Bremse_5"]["Giergeschwindigkeit"] * (1, -1)[int(pt_cp.vl["Bremse_5"]["Vorzeichen_der_Giergeschwindigk"])] * CV.DEG_TO_RAD
|
||||
|
||||
# Verify EPS readiness to accept steering commands
|
||||
hca_status = self.CCP.hca_status_values.get(pt_cp.vl["Lenkhilfe_2"]["LH2_Sta_HCA"])
|
||||
ret.steerFaultPermanent = hca_status in ("DISABLED", "FAULT")
|
||||
ret.steerFaultTemporary = hca_status in ("INITIALIZING", "REJECTED")
|
||||
|
||||
# Update gas, brakes, and gearshift.
|
||||
ret.gas = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] / 100.0
|
||||
ret.gasPressed = ret.gas > 0
|
||||
ret.brake = pt_cp.vl["Bremse_5"]["Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects
|
||||
ret.brakePressed = bool(pt_cp.vl["Motor_2"]["Bremstestschalter"])
|
||||
ret.parkingBrake = bool(pt_cp.vl["Kombi_1"]["Bremsinfo"])
|
||||
|
||||
# Update gear and/or clutch position data.
|
||||
if trans_type == TransmissionType.automatic:
|
||||
ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Getriebe_1"]["Waehlhebelposition__Getriebe_1_"], None))
|
||||
elif trans_type == TransmissionType.manual:
|
||||
ret.clutchPressed = not pt_cp.vl["Motor_1"]["Kupplungsschalter"]
|
||||
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.
|
||||
ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["GRA_Hauptschalter"])
|
||||
ret.cruiseState.enabled = bool(pt_cp.vl["Motor_2"]["GRA_Status"])
|
||||
if self.CP.pcmCruise:
|
||||
ret.accFaulted = ext_cp.vl["ACC_GRA_Anziege"]["ACA_StaACC"] in (6, 7)
|
||||
# TODO: update opendbc with PQ TSK state for OP long accFaulted
|
||||
|
||||
# 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_Anziege"]["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"]["ESP_Passiv_getastet"])
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def get_can_parser(CP):
|
||||
if CP.carFingerprint in PQ_CARS:
|
||||
return CarState.get_can_parser_pq(CP)
|
||||
|
||||
signals = [
|
||||
# sig_name, sig_address
|
||||
("LWI_Lenkradwinkel", "LWI_01"), # Absolute steering angle
|
||||
@@ -222,6 +328,9 @@ class CarState(CarStateBase):
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
if CP.carFingerprint in PQ_CARS:
|
||||
return CarState.get_cam_can_parser_pq(CP)
|
||||
|
||||
signals = []
|
||||
checks = []
|
||||
|
||||
@@ -248,6 +357,123 @@ class CarState(CarStateBase):
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.cam)
|
||||
|
||||
@staticmethod
|
||||
def get_can_parser_pq(CP):
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("LH3_BLW", "Lenkhilfe_3"), # Absolute steering angle
|
||||
("LH3_BLWSign", "Lenkhilfe_3"), # Steering angle sign
|
||||
("LH3_LM", "Lenkhilfe_3"), # Absolute driver torque input
|
||||
("LH3_LMSign", "Lenkhilfe_3"), # Driver torque input sign
|
||||
("LH2_Sta_HCA", "Lenkhilfe_2"), # Steering rack HCA status
|
||||
("Lenkradwinkel_Geschwindigkeit", "Lenkwinkel_1"), # Absolute steering rate
|
||||
("Lenkradwinkel_Geschwindigkeit_S", "Lenkwinkel_1"), # Steering rate sign
|
||||
("Geschwindigkeit_neu__Bremse_1_", "Bremse_1"), # Vehicle speed from ABS
|
||||
("Radgeschw__VL_4_1", "Bremse_3"), # ABS wheel speed, front left
|
||||
("Radgeschw__VR_4_1", "Bremse_3"), # ABS wheel speed, front right
|
||||
("Radgeschw__HL_4_1", "Bremse_3"), # ABS wheel speed, rear left
|
||||
("Radgeschw__HR_4_1", "Bremse_3"), # ABS wheel speed, rear right
|
||||
("Giergeschwindigkeit", "Bremse_5"), # Absolute yaw rate
|
||||
("Vorzeichen_der_Giergeschwindigk", "Bremse_5"), # Yaw rate sign
|
||||
("Gurtschalter_Fahrer", "Airbag_1"), # Seatbelt status, driver
|
||||
("Gurtschalter_Beifahrer", "Airbag_1"), # Seatbelt status, passenger
|
||||
("Bremstestschalter", "Motor_2"), # Brake pedal pressed (brake light test switch)
|
||||
("Bremslichtschalter", "Motor_2"), # Brakes applied (brake light switch)
|
||||
("Bremsdruck", "Bremse_5"), # Brake pressure applied
|
||||
("Vorzeichen_Bremsdruck", "Bremse_5"), # Brake pressure applied sign (???)
|
||||
("Fahrpedal_Rohsignal", "Motor_3"), # Accelerator pedal value
|
||||
("ESP_Passiv_getastet", "Bremse_1"), # Stability control disabled
|
||||
("GRA_Hauptschalter", "Motor_5"), # ACC main switch
|
||||
("GRA_Status", "Motor_2"), # ACC engagement status
|
||||
("GK1_Fa_Tuerkont", "Gate_Komf_1"), # Door open, driver
|
||||
("BSK_BT_geoeffnet", "Gate_Komf_1"), # Door open, passenger
|
||||
("BSK_HL_geoeffnet", "Gate_Komf_1"), # Door open, rear left
|
||||
("BSK_HR_geoeffnet", "Gate_Komf_1"), # Door open, rear right
|
||||
("BSK_HD_Hauptraste", "Gate_Komf_1"), # Trunk or hatch open
|
||||
("GK1_Blinker_li", "Gate_Komf_1"), # Left turn signal on
|
||||
("GK1_Blinker_re", "Gate_Komf_1"), # Right turn signal on
|
||||
("Bremsinfo", "Kombi_1"), # Manual handbrake applied
|
||||
("GRA_Hauptschalt", "GRA_Neu"), # ACC button, on/off
|
||||
("GRA_Typ_Hauptschalt", "GRA_Neu"), # ACC button, momentary vs latching
|
||||
("GRA_Kodierinfo", "GRA_Neu"), # ACC button, configuration
|
||||
("GRA_Abbrechen", "GRA_Neu"), # ACC button, cancel
|
||||
("GRA_Neu_Setzen", "GRA_Neu"), # ACC button, set
|
||||
("GRA_Up_lang", "GRA_Neu"), # ACC button, increase or accel, long press
|
||||
("GRA_Down_lang", "GRA_Neu"), # ACC button, decrease or decel, long press
|
||||
("GRA_Up_kurz", "GRA_Neu"), # ACC button, increase or accel, short press
|
||||
("GRA_Down_kurz", "GRA_Neu"), # ACC button, decrease or decel, short press
|
||||
("GRA_Recall", "GRA_Neu"), # ACC button, resume
|
||||
("GRA_Zeitluecke", "GRA_Neu"), # ACC button, time gap adj
|
||||
("COUNTER", "GRA_Neu"), # ACC button, message counter
|
||||
("GRA_Sender", "GRA_Neu"), # ACC button, CAN message originator
|
||||
]
|
||||
|
||||
checks = [
|
||||
# sig_address, frequency
|
||||
("Bremse_1", 100), # From J104 ABS/ESP controller
|
||||
("Bremse_3", 100), # From J104 ABS/ESP controller
|
||||
("Lenkhilfe_3", 100), # From J500 Steering Assist with integrated sensors
|
||||
("Lenkwinkel_1", 100), # From J500 Steering Assist with integrated sensors
|
||||
("Motor_3", 100), # From J623 Engine control module
|
||||
("Airbag_1", 50), # From J234 Airbag control module
|
||||
("Bremse_5", 50), # From J104 ABS/ESP controller
|
||||
("GRA_Neu", 50), # From J??? steering wheel control buttons
|
||||
("Kombi_1", 50), # From J285 Instrument cluster
|
||||
("Motor_2", 50), # From J623 Engine control module
|
||||
("Motor_5", 50), # From J623 Engine control module
|
||||
("Lenkhilfe_2", 20), # From J500 Steering Assist with integrated sensors
|
||||
("Gate_Komf_1", 10), # From J533 CAN gateway
|
||||
]
|
||||
|
||||
if CP.transmissionType == TransmissionType.automatic:
|
||||
signals += [("Waehlhebelposition__Getriebe_1_", "Getriebe_1", 0)] # Auto trans gear selector position
|
||||
checks += [("Getriebe_1", 100)] # From J743 Auto transmission control module
|
||||
elif CP.transmissionType == TransmissionType.manual:
|
||||
signals += [("Kupplungsschalter", "Motor_1", 0), # Clutch switch
|
||||
("GK1_Rueckfahr", "Gate_Komf_1", 0)] # Reverse light from BCM
|
||||
checks += [("Motor_1", 100)] # From J623 Engine control module
|
||||
|
||||
if CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
# Extended CAN devices other than the camera are here on CANBUS.pt
|
||||
signals += PqExtraSignals.fwd_radar_signals
|
||||
checks += PqExtraSignals.fwd_radar_checks
|
||||
if CP.enableBsm:
|
||||
signals += PqExtraSignals.bsm_radar_signals
|
||||
checks += PqExtraSignals.bsm_radar_checks
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.pt)
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser_pq(CP):
|
||||
|
||||
signals = []
|
||||
checks = []
|
||||
|
||||
if CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
signals += [
|
||||
# sig_name, sig_address
|
||||
("LDW_SW_Warnung_links", "LDW_Status"), # Blind spot in warning mode on left side due to lane departure
|
||||
("LDW_SW_Warnung_rechts", "LDW_Status"), # Blind spot in warning mode on right side due to lane departure
|
||||
("LDW_Seite_DLCTLC", "LDW_Status"), # Direction of most likely lane departure (left or right)
|
||||
("LDW_DLC", "LDW_Status"), # Lane departure, distance to line crossing
|
||||
("LDW_TLC", "LDW_Status"), # Lane departure, time to line crossing
|
||||
]
|
||||
checks += [
|
||||
# sig_address, frequency
|
||||
("LDW_Status", 10) # From R242 Driver assistance camera
|
||||
]
|
||||
|
||||
if CP.networkLocation == NetworkLocation.gateway:
|
||||
# Radars are here on CANBUS.cam
|
||||
signals += PqExtraSignals.fwd_radar_signals
|
||||
checks += PqExtraSignals.fwd_radar_checks
|
||||
if CP.enableBsm:
|
||||
signals += PqExtraSignals.bsm_radar_signals
|
||||
checks += PqExtraSignals.bsm_radar_checks
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.cam)
|
||||
|
||||
|
||||
class MqbExtraSignals:
|
||||
# Additional signal and message lists for optional or bus-portable controllers
|
||||
fwd_radar_signals = [
|
||||
@@ -269,3 +495,22 @@ class MqbExtraSignals:
|
||||
bsm_radar_checks = [
|
||||
("SWA_01", 20), # From J1086 Lane Change Assist
|
||||
]
|
||||
|
||||
class PqExtraSignals:
|
||||
# Additional signal and message lists for optional or bus-portable controllers
|
||||
fwd_radar_signals = [
|
||||
("ACA_StaACC", "ACC_GRA_Anziege", 0), # ACC drivetrain coordinator status
|
||||
("ACA_V_Wunsch", "ACC_GRA_Anziege", 0), # ACC set speed
|
||||
]
|
||||
fwd_radar_checks = [
|
||||
("ACC_GRA_Anziege", 25), # From J428 ACC radar control module
|
||||
]
|
||||
bsm_radar_signals = [
|
||||
("SWA_Infostufe_SWA_li", "SWA_1", 0), # Blind spot object info, left
|
||||
("SWA_Warnung_SWA_li", "SWA_1", 0), # Blind spot object warning, left
|
||||
("SWA_Infostufe_SWA_re", "SWA_1", 0), # Blind spot object info, right
|
||||
("SWA_Warnung_SWA_re", "SWA_1", 0), # Blind spot object warning, right
|
||||
]
|
||||
bsm_radar_checks = [
|
||||
("SWA_1", 20), # From J1086 Lane Change Assist
|
||||
]
|
||||
|
||||
@@ -1,8 +1,10 @@
|
||||
from cereal import car
|
||||
from panda import Panda
|
||||
from common.conversions import Conversions as CV
|
||||
from selfdrive.car import STD_CARGO_KG, create_button_enable_events, scale_rot_inertia, scale_tire_stiffness, \
|
||||
gen_empty_fingerprint, get_safety_config
|
||||
from selfdrive.car.interfaces import CarInterfaceBase
|
||||
from selfdrive.car.volkswagen.values import CAR, CANBUS, NetworkLocation, TransmissionType, GearShifter
|
||||
from selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter
|
||||
|
||||
|
||||
EventName = car.CarEvent.EventName
|
||||
@@ -25,7 +27,36 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.carName = "volkswagen"
|
||||
ret.radarOffCan = True
|
||||
|
||||
if True: # pylint: disable=using-constant-test
|
||||
if candidate in PQ_CARS:
|
||||
# Set global PQ35/PQ46/NMS parameters
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagenPq)]
|
||||
ret.enableBsm = 0x3BA in fingerprint[0] # SWA_1
|
||||
|
||||
if 0x440 in fingerprint[0]: # Getriebe_1
|
||||
ret.transmissionType = TransmissionType.automatic
|
||||
else:
|
||||
ret.transmissionType = TransmissionType.manual
|
||||
|
||||
if any(msg in fingerprint[1] for msg in (0x1A0, 0xC2)): # Bremse_1, Lenkwinkel_1
|
||||
ret.networkLocation = NetworkLocation.gateway
|
||||
else:
|
||||
ret.networkLocation = NetworkLocation.fwdCamera
|
||||
|
||||
# The PQ port is in dashcam-only mode due to a fixed six-minute maximum timer on HCA steering. An unsupported
|
||||
# EPS flash update to work around this timer, and enable steering down to zero, is available from:
|
||||
# https://github.com/pd0wm/pq-flasher
|
||||
# It is documented in a four-part blog series:
|
||||
# https://blog.willemmelching.nl/carhacking/2022/01/02/vw-part1/
|
||||
# Panda ALLOW_DEBUG firmware required.
|
||||
ret.dashcamOnly = True
|
||||
|
||||
if disable_radar and ret.networkLocation == NetworkLocation.gateway:
|
||||
# Proof-of-concept, prep for E2E only. No radar points available. Follow-to-stop not yet supported, but should
|
||||
# be simple to add when a suitable test car becomes available. Panda ALLOW_DEBUG firmware required.
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_VOLKSWAGEN_LONG_CONTROL
|
||||
|
||||
else:
|
||||
# Set global MQB parameters
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagen)]
|
||||
ret.enableBsm = 0x30F in fingerprint[0] # SWA_01
|
||||
@@ -83,6 +114,14 @@ class CarInterface(CarInterfaceBase):
|
||||
ret.mass = 1551 + STD_CARGO_KG
|
||||
ret.wheelbase = 2.79
|
||||
|
||||
elif candidate == CAR.PASSAT_NMS:
|
||||
ret.mass = 1503 + STD_CARGO_KG
|
||||
ret.wheelbase = 2.80
|
||||
ret.minEnableSpeed = 20 * CV.KPH_TO_MS # ACC "basic", no FtS
|
||||
ret.minSteerSpeed = 50 * CV.KPH_TO_MS
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.POLO_MK6:
|
||||
ret.mass = 1230 + STD_CARGO_KG
|
||||
ret.wheelbase = 2.55
|
||||
|
||||
@@ -0,0 +1,84 @@
|
||||
def create_steering_control(packer, bus, apply_steer, lkas_enabled):
|
||||
values = {
|
||||
"LM_Offset": abs(apply_steer),
|
||||
"LM_OffSign": 1 if apply_steer < 0 else 0,
|
||||
"HCA_Status": 5 if (lkas_enabled and apply_steer != 0) else 3,
|
||||
"Vib_Freq": 16,
|
||||
}
|
||||
|
||||
return packer.make_can_msg("HCA_1", bus, values)
|
||||
|
||||
|
||||
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control):
|
||||
values = ldw_stock_values.copy()
|
||||
|
||||
values.update({
|
||||
"LDW_Lampe_gelb": 1 if enabled and steering_pressed else 0,
|
||||
"LDW_Lampe_gruen": 1 if enabled and not steering_pressed else 0,
|
||||
"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, idx, cancel=False, resume=False):
|
||||
values = gra_stock_values.copy()
|
||||
|
||||
values.update({
|
||||
"COUNTER": idx,
|
||||
"GRA_Abbrechen": cancel,
|
||||
"GRA_Recall": resume,
|
||||
})
|
||||
|
||||
return packer.make_can_msg("GRA_Neu", bus, values)
|
||||
|
||||
|
||||
def tsk_status_value(main_switch_on, acc_faulted, long_active):
|
||||
if long_active:
|
||||
tsk_status = 1
|
||||
elif main_switch_on:
|
||||
tsk_status = 2
|
||||
else:
|
||||
tsk_status = 0
|
||||
|
||||
return tsk_status
|
||||
|
||||
|
||||
def acc_hud_status_value(main_switch_on, acc_faulted, long_active):
|
||||
if acc_faulted:
|
||||
hud_status = 6
|
||||
elif long_active:
|
||||
hud_status = 3
|
||||
elif main_switch_on:
|
||||
hud_status = 2
|
||||
else:
|
||||
hud_status = 0
|
||||
|
||||
return hud_status
|
||||
|
||||
|
||||
def create_acc_accel_control(packer, bus, adr_status, accel):
|
||||
values = {
|
||||
"ACS_Sta_ADR": adr_status,
|
||||
"ACS_StSt_Info": adr_status != 1,
|
||||
"ACS_Typ_ACC": 0, # TODO: this is ACC "basic", find a way to detect FtS support (1)
|
||||
"ACS_Sollbeschl": accel if adr_status == 1 else 3.01,
|
||||
"ACS_zul_Regelabw": 0.2 if adr_status == 1 else 1.27,
|
||||
"ACS_max_AendGrad": 3.0 if adr_status == 1 else 5.08,
|
||||
}
|
||||
|
||||
return packer.make_can_msg("ACC_System", bus, values)
|
||||
|
||||
|
||||
def create_acc_hud_control(packer, bus, acc_status, set_speed, lead_visible):
|
||||
values = {
|
||||
"ACA_StaACC": acc_status,
|
||||
"ACA_Zeitluecke": 2,
|
||||
"ACA_V_Wunsch": set_speed,
|
||||
"ACA_gemZeitl": 8 if lead_visible else 0,
|
||||
}
|
||||
# TODO: ACA_ID_StaACC, ACA_AnzDisplay, ACA_kmh_mph, ACA_PrioDisp, ACA_Aend_Zeitluecke
|
||||
|
||||
return packer.make_can_msg("ACC_GRA_Anziege", bus, values)
|
||||
@@ -18,6 +18,11 @@ Button = namedtuple('Button', ['event_type', 'can_addr', 'can_msg', 'values'])
|
||||
class CarControllerParams:
|
||||
HCA_STEP = 2 # HCA_01/HCA_1 message frequency 50Hz
|
||||
GRA_ACC_STEP = 3 # GRA_ACC_01/GRA_Neu message frequency 33Hz
|
||||
ACC_CONTROL_STEP = 2 # ACC_06/ACC_07/ACC_System frequency 50Hz
|
||||
ACC_HUD_STEP = 4 # ACC_GRA_Anziege frequency 25Hz
|
||||
|
||||
ACCEL_MAX = 2.0 # 2.0 m/s max acceleration
|
||||
ACCEL_MIN = -3.5 # 3.5 m/s max deceleration
|
||||
|
||||
def __init__(self, CP):
|
||||
# Documented lateral limits: 3.00 Nm max, rate of change 5.00 Nm/sec.
|
||||
@@ -29,7 +34,35 @@ class CarControllerParams:
|
||||
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
|
||||
if True: # pylint: disable=using-constant-test
|
||||
if CP.carFingerprint in PQ_CARS:
|
||||
self.LDW_STEP = 5 # LDW_1 message frequency 20Hz
|
||||
self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm
|
||||
self.STEER_DELTA_UP = 6 # Max HCA reached in 1.00s (STEER_MAX / (50Hz * 1.00))
|
||||
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["Getriebe_1"]["Waehlhebelposition__Getriebe_1_"]
|
||||
self.hca_status_values = can_define.dv["Lenkhilfe_2"]["LH2_Sta_HCA"]
|
||||
|
||||
self.BUTTONS = [
|
||||
Button(car.CarState.ButtonEvent.Type.setCruise, "GRA_Neu", "GRA_Neu_Setzen", [1]),
|
||||
Button(car.CarState.ButtonEvent.Type.resumeCruise, "GRA_Neu", "GRA_Recall", [1]),
|
||||
Button(car.CarState.ButtonEvent.Type.accelCruise, "GRA_Neu", "GRA_Up_kurz", [1]),
|
||||
Button(car.CarState.ButtonEvent.Type.decelCruise, "GRA_Neu", "GRA_Down_kurz", [1]),
|
||||
Button(car.CarState.ButtonEvent.Type.cancel, "GRA_Neu", "GRA_Abbrechen", [1]),
|
||||
Button(car.CarState.ButtonEvent.Type.gapAdjustCruise, "GRA_Neu", "GRA_Zeitluecke", [1]),
|
||||
]
|
||||
|
||||
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"
|
||||
}
|
||||
|
||||
else:
|
||||
self.LDW_STEP = 10 # LDW_02 message frequency 10Hz
|
||||
self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm
|
||||
self.STEER_DELTA_UP = 4 # Max HCA reached in 1.50s (STEER_MAX / (50Hz * 1.50))
|
||||
@@ -79,6 +112,7 @@ class CAR:
|
||||
GOLF_MK7 = "VOLKSWAGEN GOLF 7TH GEN" # Chassis 5G/AU/BA/BE, Mk7 VW Golf and variants
|
||||
JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN" # Chassis BU, Mk7 VW Jetta
|
||||
PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN" # Chassis 3G, Mk8 VW Passat and variants
|
||||
PASSAT_NMS = "VOLKSWAGEN PASSAT NMS" # Chassis A3, North America/China/Mideast NMS Passat, incl. facelift
|
||||
POLO_MK6 = "VOLKSWAGEN POLO 6TH GEN" # Chassis AW, Mk6 VW Polo
|
||||
TAOS_MK1 = "VOLKSWAGEN TAOS 1ST GEN" # Chassis B2, Mk1 VW Taos and Tharu
|
||||
TCROSS_MK1 = "VOLKSWAGEN T-CROSS 1ST GEN" # Chassis C1, Mk1 VW T-Cross SWB and LWB variants
|
||||
@@ -99,7 +133,12 @@ class CAR:
|
||||
SKODA_OCTAVIA_MK3 = "SKODA OCTAVIA 3RD GEN" # Chassis NE, Mk3 Skoda Octavia and variants
|
||||
|
||||
|
||||
PQ_CARS = {CAR.PASSAT_NMS}
|
||||
|
||||
|
||||
DBC: Dict[str, Dict[str, str]] = defaultdict(lambda: dbc_dict("vw_mqb_2010", None))
|
||||
for car_type in PQ_CARS:
|
||||
DBC[car_type] = dbc_dict("vw_golf_mk4", None)
|
||||
|
||||
|
||||
class Footnote(Enum):
|
||||
@@ -160,6 +199,7 @@ CAR_INFO: Dict[str, Union[VWCarInfo, List[VWCarInfo]]] = {
|
||||
VWCarInfo("Volkswagen Passat Alltrack 2015-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Passat GTE 2015-22", footnotes=[Footnote.VW_HARNESS, Footnote.VW_VARIANT], harness=Harness.j533),
|
||||
],
|
||||
CAR.PASSAT_NMS: VWCarInfo("Volkswagen Passat NMS 2017-22", harness=Harness.j533),
|
||||
CAR.POLO_MK6: [
|
||||
VWCarInfo("Volkswagen Polo 2020-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
VWCarInfo("Volkswagen Polo GTI 2020-22", footnotes=[Footnote.VW_HARNESS], harness=Harness.j533),
|
||||
@@ -514,6 +554,26 @@ FW_VERSIONS = {
|
||||
b'\xf1\x875Q0907572R \xf1\x890771',
|
||||
],
|
||||
},
|
||||
CAR.PASSAT_NMS: {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xf1\x8706K906016C \xf1\x899609',
|
||||
b'\xf1\x8706K906016G \xf1\x891124',
|
||||
b'\xf1\x8706K906071BJ\xf1\x894891',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xf1\x8709G927158AB\xf1\x893318',
|
||||
b'\xf1\x8709G927158BD\xf1\x893121',
|
||||
b'\xf1\x8709G927158FQ\xf1\x893745',
|
||||
],
|
||||
(Ecu.srs, 0x715, None): [
|
||||
b'\xf1\x87561959655 \xf1\x890210\xf1\x82\02212121111113000102011--121012--101312',
|
||||
b'\xf1\x87561959655C \xf1\x890508\xf1\x82\02215141111121100314919--153015--304831',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x757, None): [
|
||||
b'\xf1\x87561907567A \xf1\x890132',
|
||||
b'\xf1\x877N0907572C \xf1\x890211\xf1\x82\00152',
|
||||
],
|
||||
},
|
||||
CAR.POLO_MK6: {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xf1\x8704C906025H \xf1\x895177',
|
||||
|
||||
@@ -32,8 +32,9 @@ original_segments = [
|
||||
("VOLKSWAGEN", "de9592456ad7d144|2021-06-29--11-00-15--6"), # VOLKSWAGEN.GOLF
|
||||
("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--2"), # MAZDA.CX9_2021
|
||||
|
||||
# Enable when port is tested and dascamOnly is no longer set
|
||||
# Enable when port is tested and dashcamOnly is no longer set
|
||||
#("TESLA", "bb50caf5f0945ab1|2021-06-19--17-20-18--3"), # TESLA.AP2_MODELS
|
||||
#("VOLKSWAGEN2", "3cfdec54aa035f3f|2022-07-19--23-45-10--2"), # VOLKSWAGEN.PASSAT_NMS
|
||||
]
|
||||
|
||||
segments = [
|
||||
|
||||
Reference in New Issue
Block a user