diff --git a/panda b/panda index d0184f7266..2a0536c631 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit d0184f7266571ba563e5f992b6648ff4089b2dba +Subproject commit 2a0536c63148a02add52555386b5533f3555ef58 diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 2bc03a0e69..0cee2c7fce 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -5,7 +5,7 @@ from openpilot.common.conversions import Conversions as CV from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_driver_steer_torque_limits from openpilot.selfdrive.car.volkswagen import mqbcan, pqcan -from openpilot.selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams +from openpilot.selfdrive.car.volkswagen.values import CANBUS, PQ_CARS, CarControllerParams, VolkswagenFlags VisualAlert = car.CarControl.HUDControl.VisualAlert LongCtrlState = car.CarControl.Actuators.LongControlState @@ -65,6 +65,15 @@ class CarController: self.apply_steer_last = apply_steer can_sends.append(self.CCS.create_steering_control(self.packer_pt, CANBUS.pt, apply_steer, 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 = clip(apply_steer * 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, CANBUS.cam, CS.eps_stock_values, ea_simulated_torque)) + # **** Acceleration Controls ******************************************** # if self.frame % self.CCP.ACC_CONTROL_STEP == 0 and self.CP.openpilotLongitudinalControl: diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index cbe8918d48..25c5bc04bc 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -4,7 +4,7 @@ from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from openpilot.selfdrive.car.volkswagen.values import DBC, CANBUS, PQ_CARS, NetworkLocation, TransmissionType, GearShifter, \ - CarControllerParams + CarControllerParams, VolkswagenFlags class CarState(CarStateBase): @@ -14,6 +14,7 @@ class CarState(CarStateBase): 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 def create_button_events(self, pt_cp, buttons): button_events = [] @@ -59,6 +60,11 @@ class CarState(CarStateBase): ret.steerFaultPermanent = hca_status in ("DISABLED", "FAULT") ret.steerFaultTemporary = hca_status in ("INITIALIZING", "REJECTED") + # VW Emergency Assist status tracking and mitigation + self.eps_stock_values = pt_cp.vl["LH_EPS_03"] + 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 + # Update gas, brakes, and gearshift. ret.gas = pt_cp.vl["Motor_20"]["MO_Fahrpedalrohwert_01"] / 100.0 ret.gasPressed = ret.gas > 0 @@ -293,6 +299,11 @@ class CarState(CarStateBase): messages = [] + if CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT: + messages += [ + ("HCA_01", 1), # From R242 Driver assistance camera, 50Hz if steering/1Hz if not + ] + if CP.networkLocation == NetworkLocation.fwdCamera: messages += [ # sig_address, frequency diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 0a6573217c..710e779d0a 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -3,7 +3,7 @@ from panda import Panda from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car import get_safety_config from openpilot.selfdrive.car.interfaces import CarInterfaceBase -from openpilot.selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter +from openpilot.selfdrive.car.volkswagen.values import CAR, PQ_CARS, CANBUS, NetworkLocation, TransmissionType, GearShifter, VolkswagenFlags ButtonType = car.CarState.ButtonEvent.Type EventName = car.CarEvent.EventName @@ -67,6 +67,9 @@ class CarInterface(CarInterfaceBase): else: ret.networkLocation = NetworkLocation.fwdCamera + if 0x126 in fingerprint[2]: # HCA_01 + ret.flags |= VolkswagenFlags.STOCK_HCA_PRESENT.value + # Global lateral tuning defaults, can be overridden per-vehicle ret.steerActuatorDelay = 0.1 diff --git a/selfdrive/car/volkswagen/mqbcan.py b/selfdrive/car/volkswagen/mqbcan.py index 96e05aa8f0..787c7de530 100644 --- a/selfdrive/car/volkswagen/mqbcan.py +++ b/selfdrive/car/volkswagen/mqbcan.py @@ -10,6 +10,24 @@ def create_steering_control(packer, bus, apply_steer, lkas_enabled): return packer.make_can_msg("HCA_01", bus, values) +def create_eps_update(packer, bus, eps_stock_values, ea_simulated_torque): + values = {s: eps_stock_values[s] for s in [ + "COUNTER", # Sync counter value to EPS output + "EPS_Lenkungstyp", # EPS rack type + "EPS_Berechneter_LW", # Absolute raw steering angle + "EPS_VZ_BLW", # Raw steering angle sign + "EPS_HCA_Status", # EPS HCA control status + ]} + + values.update({ + # Absolute driver torque input and sign, with EA inactivity mitigation + "EPS_Lenkmoment": abs(ea_simulated_torque), + "EPS_VZ_Lenkmoment": 1 if ea_simulated_torque < 0 else 0, + }) + + return packer.make_can_msg("LH_EPS_03", bus, values) + + def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control): values = {} if len(ldw_stock_values): diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 35e88895db..f4809e4523 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -1,6 +1,6 @@ from collections import defaultdict, namedtuple from dataclasses import dataclass, field -from enum import Enum, StrEnum +from enum import Enum, IntFlag, StrEnum from typing import Dict, List, Union from cereal import car @@ -109,6 +109,10 @@ class CANBUS: cam = 2 +class VolkswagenFlags(IntFlag): + STOCK_HCA_PRESENT = 1 + + # Check the 7th and 8th characters of the VIN before adding a new CAR. If the # chassis code is already listed below, don't add a new CAR, just add to the # FW_VERSIONS for that existing CAR. diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 474d5056fd..9163bcae75 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -80c700f9b87e069dcb5b7ec76cd6036667615f2e \ No newline at end of file +1b981ce7f817974d4a7a28b06f01f727a5a7ea7b \ No newline at end of file