feat: Squash all pre-brand features into pre

This commit is contained in:
Rick Lan
2025-09-11 13:52:44 +08:00
parent 5e0f34905d
commit 92b7644924
20 changed files with 439 additions and 31 deletions

View File

@@ -153,4 +153,10 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"dp_ui_radar_tracks", {PERSISTENT, BOOL, "0"}},
{"dp_dev_dashy", {PERSISTENT, INT, "0"}},
{"dp_dev_delay_loggerd", {PERSISTENT, INT, "0"}},
{"dp_toyota_door_auto_lock_unlock", {PERSISTENT, BOOL, "0"}},
{"dp_toyota_tss1_sng", {PERSISTENT, BOOL, "0"}},
{"dp_toyota_stock_lon", {PERSISTENT, BOOL, "0"}},
{"dp_vag_a0_sng", {PERSISTENT, BOOL, "0"}},
{"dp_vag_pq_steering_patch", {PERSISTENT, BOOL, "0"}},
{"dp_vag_avoid_eps_lockout", {PERSISTENT, BOOL, "0"}},
};

View File

@@ -91,6 +91,7 @@ class Bus(StrEnum):
party = auto()
ap_party = auto()
zss = auto()
def rate_limit(new_value, last_value, dw_step, up_step):
return float(np.clip(new_value, last_value + dw_step, last_value + up_step))

View File

@@ -95,13 +95,25 @@ class CarInterface(CarInterfaceBase):
ret.longitudinalTuning.kiV = [1.2, 0.8, 0.5]
# Disable control if EPS mod detected
eps_modified = False
for fw in car_fw:
if fw.ecu == "eps" and b"," in fw.fwVersion:
ret.dashcamOnly = True
# ret.dashcamOnly = True
eps_modified = True
if candidate == CAR.HONDA_CIVIC:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]]
if eps_modified:
# stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE
# stock request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x1680, 0x1680
# modified request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x2880, 0x3180
# stock filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108
# modified filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0400, 0x0480
# note: max request allowed is 4096, but request is capped at 3840 in firmware, so modifications result in 2x max
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]]
else:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]]
elif candidate in (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_BOSCH_DIESEL):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
@@ -114,7 +126,10 @@ class CarInterface(CarInterfaceBase):
elif candidate == CAR.HONDA_ACCORD:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
if eps_modified:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]]
else:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
elif candidate == CAR.HONDA_ACCORD_11G:
ret.steerActuatorDelay = 0.22
@@ -131,8 +146,15 @@ class CarInterface(CarInterfaceBase):
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.HONDA_CRV_5G:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
if eps_modified:
# stock request input values: 0x0000, 0x00DB, 0x01BB, 0x0296, 0x0377, 0x0454, 0x0532, 0x0610, 0x067F
# stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400
# modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]]
else:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.HONDA_CRV_HYBRID:

View File

@@ -152,6 +152,13 @@ class CarInterface(CarInterfaceBase):
if candidate in (CAR.KIA_OPTIMA_H,):
ret.dashcamOnly = True
# w/ SMDPS, allow steering to 0
if 0x2AA in fingerprint[0]:
ret.minSteerSpeed = 0.
print("----------------------------------------------")
print("dragonpilot: SMDPS detected!")
print("----------------------------------------------")
return ret
@staticmethod

View File

@@ -22,4 +22,10 @@ CarParamsT = capnp.lib.capnp._StructModule
class DPFlags:
LateralALKA = 1
ExtRadar = 2
ToyotaLockCtrl = 2 ** 2
ToyotaTSS1SnG = 2 ** 3
ToyotaStockLon = 2 ** 4
VagA0SnG = 2 ** 5
VAGPQSteeringPatch = 2 ** 6
VagAvoidEPSLockout = 2 ** 7
pass

View File

@@ -253,6 +253,7 @@ routes = [
CarTestRoute("6719965b0e1d1737/2023-02-09--22-44-05", TOYOTA.TOYOTA_CHR_TSS2), # hybrid
CarTestRoute("6719965b0e1d1737/2023-08-29--06-40-05", TOYOTA.TOYOTA_CHR_TSS2), # hybrid, openpilot longitudinal, radar disabled
CarTestRoute("14623aae37e549f3/2021-10-24--01-20-49", TOYOTA.TOYOTA_PRIUS_V),
CarTestRoute("ea8fbe72b96a185c|2023-02-22--09-20-34", TOYOTA.TOYOTA_CHR_TSS2), # openpilot longitudinal, with Radar Filter
CarTestRoute("202c40641158a6e5/2021-09-21--09-43-24", VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1),
CarTestRoute("2c68dda277d887ac/2021-05-11--15-22-20", VOLKSWAGEN.VOLKSWAGEN_ATLAS_MK1),

View File

@@ -33,6 +33,17 @@ MAX_STEER_RATE_FRAMES = 18 # tx control frames needed before torque can be cut
# EPS allows user torque above threshold for 50 frames before permanently faulting
MAX_USER_TORQUE = 500
# Lock / unlock door commands - Credit goes to AlexandreSato!
from opendbc.car.common.conversions import Conversions as CV
LOCK_SPEED = 20 * CV.KPH_TO_MS
LOCK_UNLOCK_CAN_ID = 0x750
UNLOCK_CMD = b'\x40\x05\x30\x11\x00\x40\x00\x00'
LOCK_CMD = b'\x40\x05\x30\x11\x00\x80\x00\x00'
from cereal import car
PARK = car.CarState.GearShifter.park
DRIVE = car.CarState.GearShifter.drive
def get_long_tune(CP, params):
if CP.carFingerprint in TSS2_CAR:
@@ -75,6 +86,8 @@ class CarController(CarControllerBase):
self.secoc_lta_message_counter = 0
self.secoc_prev_reset_counter = 0
self.doors_locked = False
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
stopping = actuators.longControlState == LongCtrlState.stopping
@@ -172,6 +185,9 @@ class CarController(CarControllerBase):
# pcm entered standstill or it's disabled
self.standstill_req = False
if (self.CP.flags & ToyotaFlags.TSS1_SNG.value) and CS.out.standstill and not self.last_standstill:
self.standstill_req = False
self.last_standstill = CS.out.standstill
# handle UI messages
@@ -268,7 +284,7 @@ class CarController(CarControllerBase):
hud_control.rightLaneVisible, hud_control.leftLaneDepart,
hud_control.rightLaneDepart, lat_active, CS.lkas_hud))
if (self.frame % 100 == 0 or send_ui) and (self.CP.enableDsu or self.CP.flags & ToyotaFlags.DISABLE_RADAR.value):
if not self.CP.flags & ToyotaFlags.RADAR_FILTER.value and (self.frame % 100 == 0 or send_ui) and (self.CP.enableDsu or self.CP.flags & ToyotaFlags.DISABLE_RADAR.value):
can_sends.append(toyotacan.create_fcw_command(self.packer, fcw_alert))
# *** static msgs ***
@@ -278,7 +294,7 @@ class CarController(CarControllerBase):
can_sends.append(CanData(addr, vl, bus))
# keep radar disabled
if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
if not self.CP.flags & ToyotaFlags.RADAR_FILTER.value and self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
can_sends.append(make_tester_present_msg(0x750, 0, 0xF))
new_actuators = actuators.as_builder()
@@ -287,5 +303,13 @@ class CarController(CarControllerBase):
new_actuators.steeringAngleDeg = self.last_angle
new_actuators.accel = self.accel
if self.CP.flags & ToyotaFlags.LOCK_CTRL.value:
if not self.doors_locked and CS.out.gearShifter == DRIVE and CS.out.vEgo >= LOCK_SPEED:
can_sends.append(CanData(LOCK_UNLOCK_CAN_ID, LOCK_CMD, 0))
self.doors_locked = True
elif self.doors_locked and CS.out.gearShifter == PARK:
can_sends.append(CanData(LOCK_UNLOCK_CAN_ID, UNLOCK_CMD, 0))
self.doors_locked = False
self.frame += 1
return new_actuators, can_sends

View File

@@ -52,12 +52,29 @@ class CarState(CarStateBase):
self.gvc = 0.0
self.secoc_synchronization = None
# radar filter (mainly for CHR/Camry)
# the idea is to place a Panda in between Radar and camera/body (engine room) to block 0x343 (longitudinal)
# depends on the firmware, we should be able to read most CAN directly from cp (not cp_cam, its empty)
self.dp_radar_filter = bool(self.CP.flags & ToyotaFlags.RADAR_FILTER.value)
# rick - dsu_bypass from cydia2020: https://github.com/cydia2020/toyota-dsu-reroute-harness/
# the idea is to "re-route" the DSU to Panda CAN2 (Which connects to ADAS Camera)
# * when comma device is not available, the DSU message can still communicate with ADAS Camera, and over to car.
# * when comma device is active, CAN message of DSU and ADAS camera will the be blocked by Panda, only forward some CAN messages over to car (from CAN0).
self.dp_dsu_bypass = self.CP.flags & ToyotaFlags.DSU_BYPASS.value
from opendbc.car.toyota.zss import ZSS
self.zss = ZSS(CP.flags)
def update(self, can_parsers) -> structs.CarState:
cp = can_parsers[Bus.pt]
cp_cam = can_parsers[Bus.cam]
ret = structs.CarState()
cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp
if self.dp_dsu_bypass:
cp_acc = cp_cam
else:
cp_acc = cp_cam if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) else cp
if not self.CP.flags & ToyotaFlags.SECOC.value:
self.gvc = cp.vl["VSC1S07"]["GVC"]
@@ -77,7 +94,7 @@ class CarState(CarStateBase):
else:
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 # TODO: these also have GAS_PEDAL, come back and unify
can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"])
if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
if (not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value) or self.dp_radar_filter:
ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5)
self.parse_wheel_speeds(ret,
@@ -94,6 +111,14 @@ class CarState(CarStateBase):
ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"]
torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]
if self.zss.enabled:
self.zss.set_values(can_parsers[Bus.zss])
if self.CP.carFingerprint in UNSUPPORTED_DSU_CAR:
main_on = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
else:
main_on = cp.vl["PCM_CRUISE_2"]["MAIN_ON"] != 0
ret.steeringAngleDeg = self.zss.get_steering_angle_deg(main_on, cp.vl["PCM_CRUISE"]["CRUISE_ACTIVE"], ret.steeringAngleDeg)
# On some cars, the angle measurement is non-zero while initializing
if abs(torque_sensor_angle_deg) > 1e-3 and not bool(cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE_INITIALIZING"]):
self.accurate_steer_angle_seen = True
@@ -146,8 +171,10 @@ class CarState(CarStateBase):
conversion_factor = CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS
ret.cruiseState.speedCluster = cluster_set_speed * conversion_factor
if self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value:
self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"]
if (self.CP.carFingerprint in TSS2_CAR and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value) or \
self.dp_dsu_bypass:
if not (self.CP.flags & ToyotaFlags.SDSU.value):
self.acc_type = cp_acc.vl["ACC_CONTROL"]["ACC_TYPE"]
ret.stockFcw = bool(cp_acc.vl["PCS_HUD"]["FCW"])
# some TSS2 cars have low speed lockout permanently set, so ignore on those cars
@@ -206,7 +233,12 @@ class CarState(CarStateBase):
("BLINKERS_STATE", float('nan')),
]
return {
parsers = {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
}
if CP.flags & ToyotaFlags.ZSS:
parsers[Bus.zss] = CANParser("toyota_zss", [("SECONDARY_STEER_ANGLE", float('nan'))], 0)
return parsers

View File

@@ -57,13 +57,32 @@ class CarInterface(CarInterfaceBase):
if Ecu.hybrid in found_ecus:
ret.flags |= ToyotaFlags.HYBRID.value
# 0x343 should not be present on bus 2 on cars other than TSS2_CAR unless we are re-routing DSU
dsu_bypass = False
if (0x343 in fingerprint[2] or 0x4CB in fingerprint[2]) and candidate not in TSS2_CAR:
print("----------------------------------------------")
print("dragonpilot: DSU_BYPASS detected!")
print("----------------------------------------------")
# rick - disable for now, breaks TOYOTA_AVALON_2019 model tests.
# dsu_bypass = True
# ret.flags |= ToyotaFlags.DSU_BYPASS.value
if 0x23 in fingerprint[0]:
print("----------------------------------------------")
print("dragonpilot: ZSS detected!")
print("----------------------------------------------")
ret.flags |= ToyotaFlags.ZSS.value
if candidate == CAR.TOYOTA_PRIUS:
stop_and_go = True
# Only give steer angle deadzone to for bad angle sensor prius
for fw in car_fw:
if fw.ecu == "eps" and not fw.fwVersion == b'8965B47060\x00\x00\x00\x00\x00\x00':
ret.steerActuatorDelay = 0.25
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
if ret.flags & ToyotaFlags.ZSS.value:
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
else:
ret.steerActuatorDelay = 0.25
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg=0.2)
elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RX_TSS2):
stop_and_go = True
@@ -118,6 +137,30 @@ class CarInterface(CarInterfaceBase):
if alpha_long and candidate in RADAR_ACC_CAR:
ret.flags |= ToyotaFlags.DISABLE_RADAR.value
# RADAR_ACC_CAR = CHR TSS2 / RAV4 TSS2
# NO_DSU_CAR = CAMRY / CHR
if 0x2FF in fingerprint[0] or 0x2AA in fingerprint[0]:
print("----------------------------------------------")
print("dragonpilot: RADAR_FILTER detected!")
print("----------------------------------------------")
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.LONG_FILTER.value
ret.alphaLongitudinalAvailable = False
ret.flags |= ToyotaFlags.RADAR_FILTER.value | ToyotaFlags.DISABLE_RADAR.value
sdsu_active = False
if not (candidate in (RADAR_ACC_CAR | NO_DSU_CAR)) and 0x2FF in fingerprint[0]:
print("----------------------------------------------")
print("dragonpilot: SDSU detected!")
print("----------------------------------------------")
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.LONG_FILTER.value
ret.enableDsu = False
sdsu_active = True
stop_and_go = True
ret.flags |= ToyotaFlags.SDSU.value
ret.alphaLongitudinalAvailable = False
# openpilot longitudinal enabled by default:
# - cars w/ DSU disconnected
# - TSS2 cars with camera sending ACC_CONTROL where we can block it
@@ -129,7 +172,13 @@ class CarInterface(CarInterfaceBase):
else:
ret.openpilotLongitudinalControl = ret.enableDsu or \
candidate in (TSS2_CAR - RADAR_ACC_CAR) or \
bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value)
bool(ret.flags & ToyotaFlags.DISABLE_RADAR.value) or \
sdsu_active or \
dsu_bypass
if dp_params & structs.DPFlags.ToyotaStockLon:
ret.openpilotLongitudinalControl = False
ret.alphaLongitudinalAvailable = False
ret.autoResumeSng = ret.openpilotLongitudinalControl and candidate in NO_STOP_TIMER_CAR
@@ -151,12 +200,19 @@ class CarInterface(CarInterfaceBase):
if ret.flags & ToyotaFlags.HYBRID.value:
ret.longitudinalActuatorDelay = 0.05
if dp_params & structs.DPFlags.ToyotaLockCtrl:
ret.flags |= ToyotaFlags.LOCK_CTRL.value
ret.safetyConfigs[0].safetyParam |= ToyotaSafetyFlags.LOCK_CTRL.value
if dp_params & structs.DPFlags.ToyotaTSS1SnG:
ret.flags |= ToyotaFlags.TSS1_SNG.value
return ret
@staticmethod
def init(CP, can_recv, can_send, communication_control=None):
# disable radar if alpha longitudinal toggled on radar-ACC car
if CP.flags & ToyotaFlags.DISABLE_RADAR.value:
if not CP.flags & ToyotaFlags.RADAR_FILTER.value and CP.flags & ToyotaFlags.DISABLE_RADAR.value:
if communication_control is None:
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX, uds.MESSAGE_TYPE.NORMAL])
disable_ecu(can_recv, can_send, bus=0, addr=0x750, sub_addr=0xf, com_cont_req=communication_control)

View File

@@ -56,11 +56,15 @@ class ToyotaSafetyFlags(IntFlag):
STOCK_LONGITUDINAL = (2 << 8)
LTA = (4 << 8)
SECOC = (8 << 8)
LOCK_CTRL = (16 << 8)
LONG_FILTER = (32 << 8)
class ToyotaFlags(IntFlag):
# Detected flags
HYBRID = 1
# use legacy id
SDSU = 2
DISABLE_RADAR = 4
# Static flags
@@ -78,6 +82,11 @@ class ToyotaFlags(IntFlag):
SECOC = 2048
ALKA = 2 ** 12
LOCK_CTRL = 2 ** 13
TSS1_SNG = 2 ** 14
RADAR_FILTER = 2 ** 15
DSU_BYPASS = 2 ** 16
ZSS = 2 ** 17
def dbc_dict(pt, radar):

View File

@@ -0,0 +1,79 @@
from opendbc.car.toyota.values import ToyotaFlags
from opendbc.can.parser import CANParser
ANGLE_DIFF_THRESHOLD = 4.0
THRESHOLD_COUNT = 10
class ZSS:
def __init__(self, flags: int):
self.enabled = flags & ToyotaFlags.ZSS.value
self._alka_enabled = flags & ToyotaFlags.ALKA.value
self._offset_compute_once = True
self._alka_active_prev = False
self._cruise_active_prev = False
self._offset = 0.
self._threshold_count = 0
self._zss_value = 0.
self._print_allow = True
def set_values(self, cp: CANParser):
self._zss_value = cp.vl["SECONDARY_STEER_ANGLE"]["ZORRO_STEER"]
def get_enabled(self):
return self.enabled
def get_steering_angle_deg(self, main_on, cruise_active, stock_steering_angle_deg):
# off, fall back to stock
if not self.enabled:
return stock_steering_angle_deg
# when lka control is off, use stock
alka_active = self._is_alka_active(main_on)
if not cruise_active and not alka_active:
return stock_steering_angle_deg
# lka just activated
if not self._offset_compute_once and ((alka_active and not self._alka_active_prev) or (cruise_active and not self._cruise_active_prev)):
self._threshold_count = 0
self._offset_compute_once = True
self._print_allow = True
self._alka_active_prev = alka_active
self._cruise_active_prev = cruise_active
# compute offset when required
if self._offset_compute_once:
self._offset_compute_once = not self._compute_offset(stock_steering_angle_deg)
# error checking
if self._threshold_count >= THRESHOLD_COUNT:
if self._print_allow:
print("ZSS: Too many large diff, fallback to stock.")
self._print_allow = False
return stock_steering_angle_deg
if self._offset_compute_once:
print("ZSS: Compute offset required, fallback to stock.")
return stock_steering_angle_deg
zss_steering_angle_deg = self._zss_value - self._offset
angle_diff = abs(stock_steering_angle_deg - zss_steering_angle_deg)
if angle_diff > ANGLE_DIFF_THRESHOLD:
print(f"ZSS: Diff too large ({angle_diff}), fallback to stock. ")
if self._is_alka_active(main_on) or cruise_active:
self._threshold_count += 1
return stock_steering_angle_deg
return zss_steering_angle_deg
def _is_alka_active(self, main_on):
return self._alka_enabled and main_on != 0
def _compute_offset(self, steering_angle_deg):
if abs(steering_angle_deg) > 1e-3 and abs(self._zss_value) > 1e-3:
self._offset = self._zss_value - steering_angle_deg
print(f"ZSS: offset computed: {self._offset}")
return True
return False

View File

@@ -25,6 +25,9 @@ class CarController(CarControllerBase):
self.eps_timer_soft_disable_alert = False
self.hca_frame_timer_running = 0
self.hca_frame_same_torque = 0
self._dp_vag_a0_sng = bool(self.CP.flags & VolkswagenFlags.A0SnG)
self.dp_vag_pq_steering_patch = 7 if CP.flags & VolkswagenFlags.PQSteeringPatch else 5
self.dp_avoid_eps_lockout = CP.flags & VolkswagenFlags.AVOID_EPS_LOCKOUT
def update(self, CC, CS, now_nanos):
actuators = CC.actuators
@@ -44,7 +47,13 @@ class CarController(CarControllerBase):
# of HCA disabled; this is done whenever output happens to be zero.
if CC.latActive:
new_torque = int(round(actuators.torque * self.CCP.STEER_MAX))
if self.dp_avoid_eps_lockout:
#根據速度縮放new_torque扭力上限
torque_scale = np.interp(CS.out.vEgo, [0.4, 3.5, 4.0], [0.8, 0.95, 1.0])
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)
self.hca_frame_timer_running += self.CCP.STEER_STEP
if self.apply_torque_last == apply_torque:
@@ -64,7 +73,7 @@ class CarController(CarControllerBase):
self.eps_timer_soft_disable_alert = self.hca_frame_timer_running > self.CCP.STEER_TIME_ALERT / DT_CTRL
self.apply_torque_last = apply_torque
can_sends.append(self.CCS.create_steering_control(self.packer_pt, self.CAN.pt, apply_torque, hca_enabled))
can_sends.append(self.CCS.create_steering_control(self.packer_pt, self.CAN.pt, apply_torque, hca_enabled, self.dp_vag_pq_steering_patch))
if self.CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT:
# Pacify VW Emergency Assist driver inactivity detection by changing its view of driver steering input torque
@@ -113,11 +122,22 @@ class CarController(CarControllerBase):
lead_distance, hud_control.leadDistanceBars))
# **** Stock ACC Button Controls **************************************** #
if self._dp_vag_a0_sng:
if self.CP.pcmCruise and CS.gra_stock_values["COUNTER"] != self.gra_acc_counter_last:
standing_resume_spam = CS.out.standstill
spam_window = self.frame % 50 < 15
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))
send_cancel = CC.cruiseControl.cancel
send_resume = CC.cruiseControl.resume or (standing_resume_spam and spam_window)
if send_cancel or send_resume:
can_sends.append(self.CCS.create_acc_buttons_control(self.packer_pt, self.CAN.ext, CS.gra_stock_values,
cancel=send_cancel, resume=send_resume))
else:
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

View File

@@ -35,7 +35,7 @@ class CarInterface(CarInterfaceBase):
# 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
# ret.dashcamOnly = True
else:
# Set global MQB parameters
@@ -94,4 +94,13 @@ class CarInterface(CarInterfaceBase):
safety_configs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput))
ret.safetyConfigs = safety_configs
if dp_params & structs.DPFlags.VagA0SnG:
ret.flags |= VolkswagenFlags.A0SnG.value
if ret.flags & VolkswagenFlags.PQ and dp_params & structs.DPFlags.VAGPQSteeringPatch:
ret.flags |= VolkswagenFlags.PQSteeringPatch.value
if dp_params & structs.DPFlags.VagAvoidEPSLockout:
ret.flags |= VolkswagenFlags.AVOID_EPS_LOCKOUT.value
return ret

View File

@@ -1,7 +1,7 @@
from opendbc.car.crc import CRC8H2F
def create_steering_control(packer, bus, apply_torque, lkas_enabled):
def create_steering_control(packer, bus, apply_torque, lkas_enabled, dp_vag_pq_steering_patch):
values = {
"HCA_01_Status_HCA": 5 if lkas_enabled else 3,
"HCA_01_LM_Offset": abs(apply_torque),

View File

@@ -1,8 +1,8 @@
def create_steering_control(packer, bus, apply_torque, lkas_enabled):
def create_steering_control(packer, bus, apply_torque, lkas_enabled, dp_vag_pq_steering_patch = 5):
values = {
"LM_Offset": abs(apply_torque),
"LM_OffSign": 1 if apply_torque < 0 else 0,
"HCA_Status": 5 if (lkas_enabled and apply_torque != 0) else 3,
"HCA_Status": dp_vag_pq_steering_patch if (lkas_enabled and apply_torque != 0) else 3,
"Vib_Freq": 16,
}

View File

@@ -170,6 +170,9 @@ class VolkswagenFlags(IntFlag):
# Static flags
PQ = 2
A0SnG = 2 ** 10
PQSteeringPatch = 2 ** 11
AVOID_EPS_LOCKOUT = 2 ** 12
@dataclass
class VolkswagenMQBPlatformConfig(PlatformConfig):

View File

@@ -0,0 +1,4 @@
BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
SG_ ZORRO_STEER : 7|24@0- (0.004901594652,0) [-500|500] "" XXX
CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";

View File

@@ -31,6 +31,38 @@
/* ACC */ \
{0x343, 0, 8, .check_relay = true}, \
// lock ctrl
// we dont need one for TOYOTA_COMMON_LONG_TX_MSGS as it's already allowing 0x750
#define TOYOTA_COMMON_LOCK_TX_MSGS \
TOYOTA_COMMON_TX_MSGS \
/* radar diagnostic address */ \
{0x750, 0, 8, .check_relay = false}, \
#define TOYOTA_COMMON_SECOC_LOCK_TX_MSGS \
TOYOTA_COMMON_SECOC_TX_MSGS \
/* radar diagnostic address */ \
{0x750, 0, 8, .check_relay = false}, \
// long filter
// same as TOYOTA_COMMON_LONG_TX_MSGS except we don't need to 0x343 to check relay
#define TOYOTA_COMMON_LONG_FILTER_TX_MSGS \
TOYOTA_COMMON_TX_MSGS \
/* DSU bus 0 */ \
{0x283, 0, 7, .check_relay = false}, {0x2E6, 0, 8, .check_relay = false}, {0x2E7, 0, 8, .check_relay = false}, {0x33E, 0, 7, .check_relay = false}, \
{0x344, 0, 8, .check_relay = false}, {0x365, 0, 7, .check_relay = false}, {0x366, 0, 7, .check_relay = false}, {0x4CB, 0, 8, .check_relay = false}, \
/* DSU bus 1 */ \
{0x128, 1, 6, .check_relay = false}, {0x141, 1, 4, .check_relay = false}, {0x160, 1, 8, .check_relay = false}, {0x161, 1, 7, .check_relay = false}, \
{0x470, 1, 4, .check_relay = false}, \
/* PCS_HUD */ \
{0x411, 0, 8, .check_relay = false}, \
/* ACC */ \
{0x343, 0, 8, .check_relay = false}, \
// lock ctrl + long filter
#define TOYOTA_COMMON_LONG_FILTER_LOCK_TX_MSGS \
TOYOTA_COMMON_LONG_FILTER_TX_MSGS \
{0x750, 0, 8, .check_relay = false}, \
#define TOYOTA_COMMON_RX_CHECKS(lta) \
{.msg = {{ 0xaa, 0, 8, 83U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{0x260, 0, 8, 50U, .ignore_counter = true, .ignore_quality_flag=!(lta)}, { 0 }, { 0 }}}, \
@@ -57,6 +89,11 @@ static bool toyota_stock_longitudinal = false;
static bool toyota_lta = false;
static int toyota_dbc_eps_torque_factor = 100; // conversion factor for STEER_TORQUE_EPS in %: see dbc file
// lock ctrl
static bool toyota_lock_ctrl = false;
// long filter
static bool toyota_long_filter = false;
static uint32_t toyota_compute_checksum(const CANPacket_t *msg) {
int len = GET_LEN(msg);
uint8_t checksum = (uint8_t)(msg->addr) + (uint8_t)((unsigned int)(msg->addr) >> 8U) + (uint8_t)(len);
@@ -316,7 +353,7 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
}
// UDS: Only tester present ("\x0F\x02\x3E\x00\x00\x00\x00\x00") allowed on diagnostics address
if (msg->addr == 0x750U) {
if (!toyota_lock_ctrl && msg->addr == 0x750U) {
// this address is sub-addressed. only allow tester present to radar (0xF)
bool invalid_uds_msg = (GET_BYTES(msg, 0, 4) != 0x003E020FU) || (GET_BYTES(msg, 4, 4) != 0x0U);
if (invalid_uds_msg) {
@@ -340,6 +377,26 @@ static safety_config toyota_init(uint16_t param) {
TOYOTA_COMMON_LONG_TX_MSGS
};
// lock ctrl
// we dont need one for TOYOTA_COMMON_LONG_TX_MSGS as it's already allowing 0x750
static const CanMsg TOYOTA_LOCK_TX_MSGS[] = {
TOYOTA_COMMON_LOCK_TX_MSGS
};
static const CanMsg TOYOTA_SECOC_LOCK_TX_MSGS[] = {
TOYOTA_COMMON_SECOC_LOCK_TX_MSGS
};
// long filter
static const CanMsg TOYOTA_LONG_FILTER_TX_MSGS[] = {
TOYOTA_COMMON_LONG_FILTER_TX_MSGS
};
// lock ctrl + long filter
static const CanMsg TOYOTA_LONG_FILTER_LOCK_TX_MSGS[] = {
TOYOTA_COMMON_LONG_FILTER_LOCK_TX_MSGS
};
// safety param flags
// first byte is for EPS factor, second is for flags
const uint32_t TOYOTA_PARAM_OFFSET = 8U;
@@ -353,6 +410,14 @@ static safety_config toyota_init(uint16_t param) {
toyota_secoc = GET_FLAG(param, TOYOTA_PARAM_SECOC);
#endif
// lock ctrl
const uint32_t TOYOTA_PARAM_LOCK_CTRL = 16UL << TOYOTA_PARAM_OFFSET;
toyota_lock_ctrl = GET_FLAG(param, TOYOTA_PARAM_LOCK_CTRL);
// long filter
const uint32_t TOYOTA_PARAM_LONG_FILTER = 32U << TOYOTA_PARAM_OFFSET;
toyota_long_filter = GET_FLAG(param, TOYOTA_PARAM_LONG_FILTER);
toyota_alt_brake = GET_FLAG(param, TOYOTA_PARAM_ALT_BRAKE);
toyota_stock_longitudinal = GET_FLAG(param, TOYOTA_PARAM_STOCK_LONGITUDINAL);
toyota_lta = GET_FLAG(param, TOYOTA_PARAM_LTA);
@@ -361,12 +426,28 @@ static safety_config toyota_init(uint16_t param) {
safety_config ret;
if (toyota_stock_longitudinal) {
if (toyota_secoc) {
SET_TX_MSGS(TOYOTA_SECOC_TX_MSGS, ret);
if (toyota_lock_ctrl) {
SET_TX_MSGS(TOYOTA_SECOC_LOCK_TX_MSGS, ret);
} else {
SET_TX_MSGS(TOYOTA_SECOC_TX_MSGS, ret);
}
} else {
SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
if (toyota_lock_ctrl) {
SET_TX_MSGS(TOYOTA_LOCK_TX_MSGS, ret);
} else {
SET_TX_MSGS(TOYOTA_TX_MSGS, ret);
}
}
} else {
SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret);
if (toyota_long_filter) {
if (toyota_lock_ctrl) {
SET_TX_MSGS(TOYOTA_LONG_FILTER_LOCK_TX_MSGS, ret);
} else {
SET_TX_MSGS(TOYOTA_LONG_FILTER_TX_MSGS, ret);
}
} else {
SET_TX_MSGS(TOYOTA_LONG_TX_MSGS, ret);
}
}
if (toyota_secoc) {

View File

@@ -105,6 +105,24 @@ class Car:
if self.params.get_bool("dp_lat_alka"):
dp_params |= structs.DPFlags.LateralALKA
if self.params.get_bool("dp_toyota_door_auto_lock_unlock"):
dp_params |= structs.DPFlags.ToyotaLockCtrl
if self.params.get_bool("dp_toyota_tss1_sng"):
dp_params |= structs.DPFlags.ToyotaTSS1SnG
if self.params.get_bool("dp_toyota_stock_lon"):
dp_params |= structs.DPFlags.ToyotaStockLon
if self.params.get_bool("dp_vag_a0_sng"):
dp_params |= structs.DPFlags.VagA0SnG
if self.params.get_bool("dp_vag_pq_steering_patch"):
dp_params |= structs.DPFlags.VAGPQSteeringPatch
if self.params.get_bool("dp_vag_avoid_eps_lockout"):
dp_params |= structs.DPFlags.VagAvoidEPSLockout
self.CI = get_car(*self.can_callbacks, obd_callback(self.params), alpha_long_allowed, is_release, num_pandas, dp_params, cached_params)
self.RI = interfaces[self.CI.CP.carFingerprint].RadarInterface(self.CI.CP)
self.CP = self.CI.CP

View File

@@ -7,6 +7,21 @@ void DPPanel::add_toyota_toggles() {
QString::fromUtf8("🐉 ") + tr("Toyota / Lexus"),
"",
},
{
"dp_toyota_door_auto_lock_unlock",
tr("Enable Door Auto Lock/Unlock"),
"",
},
{
"dp_toyota_tss1_sng",
tr("Enable TSS1 SnG Mod"),
"",
},
{
"dp_toyota_stock_lon",
tr("Use Stock Longitudinal Control"),
"",
},
};
QWidget *label = nullptr;
@@ -40,6 +55,21 @@ void DPPanel::add_vag_toggles() {
QString::fromUtf8("🐉 ") + tr("VW / Audi / Skoda"),
"",
},
{
"dp_vag_a0_sng",
tr("Enable MQB A0 SnG Mod"),
"",
},
{
"dp_vag_pq_steering_patch",
tr("PQ Steering Patch"),
""
},
{
"dp_vag_avoid_eps_lockout",
tr("Avoid EPS Lockout"),
"",
},
};
QWidget *label = nullptr;