From 9174c4c63aec35ca7eac54c65d93a0fd50da5bff Mon Sep 17 00:00:00 2001 From: "IQ.Lvbs CI [bot]" Date: Wed, 5 Aug 2026 05:42:27 -0500 Subject: [PATCH] IQ.Pilot Release Commit @ 0798119 --- iqdbc_repo/iqdbc/car/byd/carcontroller.py | 23 ++++- iqdbc_repo/iqdbc/car/byd/carstate.py | 44 +++++++-- iqdbc_repo/iqdbc/car/byd/interface.py | 25 ++++- iqdbc_repo/iqdbc/car/byd/tests/test_byd.py | 97 ++++++++++++++++++- iqdbc_repo/iqdbc/car/byd/values.py | 38 +++++++- iqdbc_repo/iqdbc/dbc/byd_sealion_7.dbc | 15 ++- iqdbc_repo/iqdbc/safety/modes/byd.h | 55 +++++++++-- iqdbc_repo/iqdbc/safety/tests/test_byd.py | 19 +++- panda/board/drivers/interrupts.h | 4 + panda/board/drivers/interrupts_declarations.h | 7 ++ panda/board/stm32h7/llspi.h | 6 ++ 11 files changed, 300 insertions(+), 33 deletions(-) diff --git a/iqdbc_repo/iqdbc/car/byd/carcontroller.py b/iqdbc_repo/iqdbc/car/byd/carcontroller.py index d7b52081d..fedd525e1 100644 --- a/iqdbc_repo/iqdbc/car/byd/carcontroller.py +++ b/iqdbc_repo/iqdbc/car/byd/carcontroller.py @@ -29,9 +29,26 @@ class CarController(CarControllerBase): # 0x1E2/0x316 go out unconditionally, gated only by STEER_REQ: the safety blocks the # camera's copies, and the EPS latches a fault if the stream stops while it is actuating. if self.frame % CarControllerParams.STEER_STEP == 0: - self.apply_angle_last = apply_steer_angle_limits_vm(actuators.steeringAngleDeg, self.apply_angle_last, - CS.out.vEgoRaw, CS.out.steeringAngleDeg, - CC.latActive, CarControllerParams, self.VM) + apply_angle = apply_steer_angle_limits_vm(actuators.steeringAngleDeg, self.apply_angle_last, + CS.out.vEgoRaw, CS.out.steeringAngleDeg, + CC.latActive, CarControllerParams, self.VM) + + # The vehicle-model jerk limit stops binding below a few m/s, so cap the slew rate + # directly there. Without this the planner's standstill oscillation drives the command + # tens of degrees away from a stationary wheel and the EPS latches state 11. + if CC.latActive: + max_rate = float(np.interp(CS.out.vEgoRaw, CarControllerParams.ANGLE_RATE_BP, + CarControllerParams.ANGLE_RATE_V)) + apply_angle = float(np.clip(apply_angle, self.apply_angle_last - max_rate, + self.apply_angle_last + max_rate)) + + # Never wind the command away from the wheel: the EPS latches on angle divergence, and + # a driver holding the wheel below the override threshold would otherwise let the + # controller run tens of degrees past it. + err = CarControllerParams.MAX_ANGLE_ERROR + apply_angle = float(np.clip(apply_angle, CS.out.steeringAngleDeg - err, + CS.out.steeringAngleDeg + err)) + self.apply_angle_last = apply_angle can_sends.append(bydcan.create_steering_control(self.packer, self.apply_angle_last, CC.latActive)) can_sends.append(bydcan.create_lkas_hud(self.packer, CC.latActive, CS.lkas_hud, CC.hudControl)) diff --git a/iqdbc_repo/iqdbc/car/byd/carstate.py b/iqdbc_repo/iqdbc/car/byd/carstate.py index 808103c22..f25c332d8 100644 --- a/iqdbc_repo/iqdbc/car/byd/carstate.py +++ b/iqdbc_repo/iqdbc/car/byd/carstate.py @@ -21,6 +21,10 @@ EPS_STATE_PREPARED = 9 EPS_STATE_ACTUATING = 10 EPS_STATE_LATCHED_FAULT = 11 +# ACC_HUD_ADAS.CRUISE_STATE +CRUISE_STATE_AVAILABLE = 1 +CRUISE_STATE_ENGAGED = 2 + class CarState(CarStateBase): def __init__(self, CP, CP_IQ): @@ -29,6 +33,8 @@ class CarState(CarStateBase): self.acc_cmd = {} self.buttons = {} self.eps_state = EPS_STATE_OFF + self.override_latched = False + self.lkas_btn_prev = False def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]: cp = can_parsers[Bus.pt] @@ -55,7 +61,9 @@ class CarState(CarStateBase): ret.steeringTorque = cp.vl["STEERING_TORQUE"]["DRIVER_TORQUE"] ret.steeringTorqueEps = cp.vl["STEERING_TORQUE"]["MAIN_TORQUE"] ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CCP.STEER_DRIVER_OVERRIDE, 5) - ret.steeringDisengage = abs(ret.steeringTorque) > CCP.STEER_DRIVER_DISENGAGE + # Disengagement on override is handled by the latch below, which also decides when + # re-engagement is allowed, so no separate hard-disengage threshold here. + ret.steeringDisengage = False # state 11 is a latched dropout: the command stream stopped while the EPS was actuating. # It clears only on a STEER_REQ rising edge over a continuous stream. @@ -90,15 +98,35 @@ class CarState(CarStateBase): )) ret.seatbeltUnlatched = not bool(cp.vl["METER_CLUSTER"]["SEATBELT_DRIVER"]) - # ACC_STATE: 0=OFF, 2=AVAILABLE, 3=ACTIVE, 5=OVERRIDE, 7=ERROR - ret.cruiseState.speed = cp_cam.vl["ACC_HUD_ADAS"]["SET_SPEED"] * CV.KPH_TO_MS - acc_state = int(cp_cam.vl["ACC_HUD_ADAS"]["ACC_STATE"]) - ret.cruiseState.available = acc_state in (2, 3, 5) - ret.cruiseState.enabled = acc_state in (3, 5) - ret.cruiseState.standstill = bool(cp_cam.vl["ACC_CMD"]["STANDSTILL_STATE"]) + # The ADAS/ACC ECU is on the chassis bus, not behind the camera relay, so these come off + # bus 0. Bus 2 carries only the camera's own frames (0x1E2, 0x316, ...). This differs from + # the Atto 3, where PR #3337 reads both from the camera bus. + # CRUISE_STATE: 0=off, 1=available, 2=engaged, 3=engaged and commanding accel. + # Do NOT use PR #3337/#3352's ACC_STATE (19|3) - byte 2 is a constant 0x3c on this car, so + # it reads 7 (ERROR) forever and engagement can never happen. + ret.cruiseState.speed = cp.vl["ACC_HUD_ADAS"]["SET_SPEED"] * CV.KPH_TO_MS + cruise_state = int(cp.vl["ACC_HUD_ADAS"]["CRUISE_STATE"]) + ret.cruiseState.available = cruise_state >= CRUISE_STATE_AVAILABLE + + # A steering override fully disengages and stays disengaged. Re-arming is deliberate: + # either cycle stock cruise, or press the LKAS/ICC button. Suppressing cruiseState.enabled + # is what holds it off, and clearing the latch gives the rising edge that re-engages. + lkas_btn = bool(cp.vl["PCM_BUTTONS"]["LKAS_ON_BTN"]) + lkas_rising = lkas_btn and not self.lkas_btn_prev + self.lkas_btn_prev = lkas_btn + + # NOTE: latch on the instantaneous torque, not the debounced steeringPressed. byd_rx_hook + # latches on the same raw sample, and any skew between the two shows up as controlsMismatch. + if cruise_state < CRUISE_STATE_ENGAGED or lkas_rising: + self.override_latched = False + elif abs(ret.steeringTorque) > CCP.STEER_DRIVER_OVERRIDE: + self.override_latched = True + + ret.cruiseState.enabled = cruise_state >= CRUISE_STATE_ENGAGED and not self.override_latched + ret.cruiseState.standstill = bool(cp.vl["ACC_CMD"]["STANDSTILL_STATE"]) self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD_ADAS"]) - self.acc_cmd = copy.copy(cp_cam.vl["ACC_CMD"]) + self.acc_cmd = copy.copy(cp.vl["ACC_CMD"]) self.buttons = copy.copy(cp.vl["PCM_BUTTONS"]) return ret, ret_iq diff --git a/iqdbc_repo/iqdbc/car/byd/interface.py b/iqdbc_repo/iqdbc/car/byd/interface.py index 3eb54b9bb..88c84db26 100644 --- a/iqdbc_repo/iqdbc/car/byd/interface.py +++ b/iqdbc_repo/iqdbc/car/byd/interface.py @@ -2,7 +2,7 @@ from iqdbc.car import get_safety_config, structs from iqdbc.car.interfaces import CarInterfaceBase from iqdbc.car.byd.carcontroller import CarController from iqdbc.car.byd.carstate import CarState -from iqdbc.car.byd.values import BydSafetyFlags +from iqdbc.car.byd.values import BydFlags, BydSafetyFlags class CarInterface(CarInterfaceBase): @@ -23,8 +23,27 @@ class CarInterface(CarInterfaceBase): # through, so the panda never sees them ret.radarUnavailable = True - ret.alphaLongitudinalAvailable = True - if alpha_long: + # Two harness types exist for this car, and they differ in what can be filtered: + # + # camera harness - the relay only intercepts the MPC camera. 0x1E2/0x316 are camera + # frames so lateral works, but the ADAS/ACC ECU sits on the chassis bus + # in front of the relay: its 0x32E cannot be blocked and openpilot would + # contend with the stock ACC on the same address. Stock long only. + # gateway harness - the ACC ECU is behind the relay, so 0x32E is filterable and openpilot + # longitudinal is possible. + # + # These CANNOT be told apart from the fingerprint: fingerprinting runs with the relay + # closed, which ties bus 2 to bus 0, so bus 2 shows the whole car on either harness. The + # difference is only observable once the relay opens, which is after CarParams is fixed. + # Measured on a camera harness with the relay open: bus 2 carries 11 camera addresses and + # neither 0x32D nor 0x32E is among them. + # + # So default to the camera harness and keep longitudinal off. Setting GATEWAY_HARNESS is an + # explicit opt-in that must not be inferred - see BYD_SEALION7_PORT_PLAN.md. + gateway_harness = bool(ret.flags & BydFlags.GATEWAY_HARNESS) + + ret.alphaLongitudinalAvailable = gateway_harness + if alpha_long and gateway_harness: ret.openpilotLongitudinalControl = True ret.safetyConfigs[0].safetyParam |= BydSafetyFlags.LONG_CONTROL.value diff --git a/iqdbc_repo/iqdbc/car/byd/tests/test_byd.py b/iqdbc_repo/iqdbc/car/byd/tests/test_byd.py index 289ad92a0..f4dd3d2c9 100644 --- a/iqdbc_repo/iqdbc/car/byd/tests/test_byd.py +++ b/iqdbc_repo/iqdbc/car/byd/tests/test_byd.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 import unittest +import numpy as np + from iqdbc.can.packer import CANPacker from iqdbc.can.parser import CANParser from iqdbc.car.byd import bydcan @@ -8,7 +10,7 @@ from iqdbc.car.byd.carstate import (EPS_STATE_OFF, EPS_STATE_PREPARED, EPS_STATE EPS_STATE_LATCHED_FAULT) from iqdbc.car.byd.fingerprints import FW_VERSIONS from iqdbc.car.byd.interface import CarInterface -from iqdbc.car.byd.values import CAR, DBC, CarControllerParams +from iqdbc.car.byd.values import CAR, DBC, BydFlags, BydSafetyFlags, CarControllerParams from iqdbc.car.fw_versions import match_fw_to_car_exact, build_fw_dict from iqdbc.car import structs from iqdbc.car.structs import CarParams @@ -313,6 +315,99 @@ class TestBydCarController(unittest.TestCase): "0x32E sent while openpilotLongitudinalControl is off") +class TestBydLowSpeedAngleRate(unittest.TestCase): + """Regression for the 2026-08-05 EPS latch. At 0.29 m/s the planner oscillated and the command + swung -5.9 to +2.4 deg against a stationary wheel in 220 ms; the EPS went from state 9 straight + to a latched 11 and took LKAS with it. The vehicle-model jerk limit cannot catch this because + it scales as 1/v^2.""" + + def _slew(self, v_ego, targets): + CP = CarInterface.get_non_essential_params("BYD_SEALION_7") + CP_IQ = CarInterface.get_non_essential_params_iq(CP, "BYD_SEALION_7") + cc = CarInterface.CarController({'pt': DBC_NAME}, CP, CP_IQ) + carstate = CarInterface.CarState(CP, CP_IQ) + cs_out, _ = carstate.update(CarInterface.CarState.get_can_parsers(CP, CP_IQ)) + cs_out.vEgoRaw = v_ego + cs_out.vEgo = v_ego + cs_out.steeringAngleDeg = 0.1 + + class _CS: + pass + cs = _CS() + cs.out = cs_out + cs.lkas_hud = carstate.lkas_hud + cs.acc_cmd = carstate.acc_cmd + cs.buttons = carstate.buttons + + CC_obj = structs.CarControl() + CC_obj.enabled = True + CC_obj.latActive = True + CC_IQ = structs.IQCarControl() + + sent = [] + for i, tgt in enumerate(targets): + CC_obj.actuators.steeringAngleDeg = tgt + cc.update(CC_obj.as_reader(), CC_IQ, cs, i * 10_000_000) + sent.append(cc.apply_angle_last) + return sent + + # the actual planner output recorded during the fault + OSCILLATION = [-2.9, -5.9, -2.9, 0.1, 1.7, 1.8, 2.4, 2.2] * 3 + + def test_standstill_slew_is_bounded(self): + v = 0.29 # the speed at which the EPS latched + sent = self._slew(v, self.OSCILLATION) + cap = float(np.interp(v, CarControllerParams.ANGLE_RATE_BP, CarControllerParams.ANGLE_RATE_V)) + steps = [abs(b - a) for a, b in zip(sent, sent[1:], strict=False)] + self.assertLessEqual(max(steps), cap + 1e-6, + "command slews faster than the standstill rate cap") + # the uncapped path stepped a full 3.0 deg/frame here + self.assertLess(cap, 1.0) + # and it must never wander far from the stationary wheel + self.assertLess(max(abs(a - 0.1) for a in sent), 2.0, + "command diverged from the measured angle at a standstill") + + def test_rate_cap_scales_with_speed(self): + slow = self._slew(0.0, [30.0] * 10) + fast = self._slew(20.0, [30.0] * 10) + slow_step = max(abs(b - a) for a, b in zip(slow, slow[1:], strict=False)) + fast_step = max(abs(b - a) for a, b in zip(fast, fast[1:], strict=False)) + self.assertLess(slow_step, fast_step, "low-speed cap must be tighter than at speed") + self.assertLessEqual(fast_step, CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE + 1e-6) + + +class TestBydHarnessType(unittest.TestCase): + """Longitudinal requires the ACC ECU to sit behind the relay so 0x32E is filterable. That is + a property of the harness, and it cannot be inferred from the fingerprint: fingerprinting + runs with the relay closed, which ties bus 2 to bus 0, so bus 2 shows the whole car either + way. Default must therefore be the camera harness (lateral only).""" + + @staticmethod + def _params(cam_bus_addrs, alpha_long=True): + fp = {0: {0x1FC: 8, 0x1F0: 8}, 1: {}, 2: dict.fromkeys(cam_bus_addrs, 8)} + return CarInterface.get_params("BYD_SEALION_7", fp, [], alpha_long, False, False) + + def test_defaults_to_camera_harness_lateral_only(self): + CP = self._params([0x1E2, 0x316]) + self.assertFalse(CP.flags & BydFlags.GATEWAY_HARNESS) + self.assertFalse(CP.alphaLongitudinalAvailable) + self.assertFalse(CP.openpilotLongitudinalControl) + self.assertFalse(CP.safetyConfigs[0].safetyParam & BydSafetyFlags.LONG_CONTROL) + + def test_acc_cmd_on_fingerprint_bus2_does_not_imply_gateway(self): + # the relay is closed while fingerprinting, so bus 2 sees the chassis bus too. Seeing + # 0x32E there must NOT unlock longitudinal. + CP = self._params([0x1E2, 0x316, 0x32E, 0x32D, 0x1FC]) + self.assertFalse(CP.flags & BydFlags.GATEWAY_HARNESS) + self.assertFalse(CP.alphaLongitudinalAvailable) + self.assertFalse(CP.openpilotLongitudinalControl) + + def test_lateral_still_available_on_camera_harness(self): + CP = self._params([0x1E2, 0x316]) + self.assertFalse(CP.dashcamOnly) + self.assertEqual(CP.steerControlType, CarParams.SteerControlType.angle) + + class TestBydCarParams(unittest.TestCase): def test_angle_control_and_no_radar(self): CP = CarInterface.get_non_essential_params("BYD_SEALION_7") diff --git a/iqdbc_repo/iqdbc/car/byd/values.py b/iqdbc_repo/iqdbc/car/byd/values.py index 62975a8fa..d0d8d06a6 100644 --- a/iqdbc_repo/iqdbc/car/byd/values.py +++ b/iqdbc_repo/iqdbc/car/byd/values.py @@ -26,9 +26,31 @@ class CarControllerParams: MAX_ANGLE_RATE=3, ) - # STEERING_TORQUE.DRIVER_TORQUE units (Nm). PROVISIONAL, re-derive in bring-up B8. - STEER_DRIVER_OVERRIDE = 3.0 - STEER_DRIVER_DISENGAGE = 8.0 + # Low-speed taper on the angle rate, in deg per STEER_STEP frame. + # + # The vehicle-model jerk limit scales as 1/v^2, so below a few m/s it stops binding and only + # the flat MAX_ANGLE_RATE is left. The lateral planner is ill-conditioned at a standstill and + # oscillates, and slewing the command at the full rate while the wheel is not moving walks the + # EPS straight from state 9 to a latched 11. Measured 2026-08-05 at 0.29 m/s: the command swung + # -5.9 to +2.4 deg in 220 ms against a stationary wheel and the EPS latched, taking LKAS with + # it. A healthy engagement at 0.9 m/s held the command within 1.1 deg of measured. + ANGLE_RATE_BP = [0.0, 2.0, 5.0] # m/s + ANGLE_RATE_V = [0.3, 1.0, 3.0] # deg/frame, tops out at MAX_ANGLE_RATE + + # STEERING_TORQUE.DRIVER_TORQUE thresholds, derived from a drive where openpilot actually + # steered (route 0000000f, EPS state 10): + # |torque| while openpilot steered: p50 1.2 p90 2.7 p95 3.3 p99 5.8 max 9.8 + # |torque| while the human drove: p50 0.2 p90 8.1 p95 17.2 p99 24.5 max 35.5 + # The old 3.0 sat below what openpilot generates while steering, so it tripped its own + # override and dropped out within a few frames of every engage. + STEER_DRIVER_OVERRIDE = 12.0 # above openpilot's own max, below a deliberate grab + + # Never command further than this from the actual wheel angle. The EPS latches state 11 on + # angle divergence, not just on a lost stream: measured 2026-08-05, the driver held the wheel + # at -13.4 deg while the controller wound the command out to -48.1 deg and the EPS latched at + # 34.7 deg of error. Normal closed-loop steering holds the error inside ~1.1 deg, so this only + # bites when the wheel is being physically held. + MAX_ANGLE_ERROR = 10.0 # deg # comfort envelope, inside the safety cap of -3.5..+2.0 ACCEL_MIN = -3.0 @@ -43,6 +65,16 @@ class BydSafetyFlags(IntFlag): LONG_CONTROL = 1 +class BydFlags(IntFlag): + # The ADAS/ACC ECU is behind the relay, so its 0x32E ACC_CMD can be blocked and replaced. + # Set when ACC_CMD is fingerprinted on the camera-side bus. + GATEWAY_HARNESS = 1 + + +# addresses used to tell the two harness types apart +ACC_CMD_ADDR = 0x32E + + class WMI(StrEnum): BYD_AUTO = "LGX" diff --git a/iqdbc_repo/iqdbc/dbc/byd_sealion_7.dbc b/iqdbc_repo/iqdbc/dbc/byd_sealion_7.dbc index cbb1932d5..8a30522c8 100644 --- a/iqdbc_repo/iqdbc/dbc/byd_sealion_7.dbc +++ b/iqdbc_repo/iqdbc/dbc/byd_sealion_7.dbc @@ -109,6 +109,7 @@ BO_ 790 LKAS_HUD_ADAS: 8 MPC_CAM SG_ LKAS_REQ_PREPARE : 27|1@1+ (1,0) [0|1] "" MCU SG_ LKAS_ACTIVE : 28|1@1+ (1,0) [0|1] "" MCU SG_ SLA_STATE : 29|3@1+ (1,0) [0|7] "" MCU + SG_ SET_ME_3 : 32|2@1+ (1,0) [0|3] "" MCU SG_ RIGHT_LANE_STATE : 34|2@1+ (1,0) [0|3] "" MCU SG_ LKAS_STATE : 36|4@1+ (1,0) [0|15] "" MCU SG_ SPEED_LIMIT_VALUE : 40|8@1+ (5,-5) [-5|1270] "kph" MCU @@ -122,7 +123,7 @@ BO_ 813 ACC_HUD_ADAS: 8 ADAS_ECU SG_ SET_DISTANCE : 12|3@0+ (1,0) [0|7] "" MCU SG_ AEB : 16|1@1+ (1,0) [0|1] "" MCU SG_ FCW : 17|1@1+ (1,0) [0|1] "" MCU - SG_ ACC_STATE : 19|3@1+ (1,0) [0|7] "" MCU + SG_ CRUISE_STATE : 44|4@1+ (1,0) [0|15] "" MCU SG_ ACC_ON2 : 20|1@0+ (1,0) [0|1] "" MCU SG_ ACC_ON1 : 22|1@0+ (1,0) [0|1] "" MCU SG_ SET_ME_XFF : 47|8@0+ (1,0) [0|255] "" MCU @@ -203,8 +204,11 @@ EPS state nibble = 8 + LKS_PREPARED + 2*CRUISE_ACTIVATED: 8 off, 9 prepared, 10 here - never read state from it."; CM_ BO_ 813 "SET_SPEED is 9 bits per HotIce0; bit 8 is unallocated in the 8-bit reading, so this -is a safe superset. ACC_STATE bits are ACC_ON2 / ACC_CONTROLLABLE_AND_ON / OVERRIDE, giving -2=available, 3=active, 5=override, 7=error."; +is a safe superset. +CRUISE_STATE (44|4) is the real cruise state, NOT PR #3337/#3352's ACC_STATE at 19|3 - byte 2 is +a constant 0x3c on this car, so that field can only ever read 7 (ERROR) and engagement is +impossible. Measured over a 47 kph drive: 0=off, 1=available (ACC_CMD ACC_CONTROLLABLE_AND_ON +clear), 2=engaged (ON bit set, 100%), 3=engaged and commanding accel (mean +1.14 m/s^2)."; CM_ BO_ 814 "ACCEL_CMD in physical m/s^2, numerically identical to PR #3352's (1,-100) raw form. The SET_ME_25_*/ACCEL_FACTOR/DECEL_FACTOR split is kept because it is the form with real-car TX @@ -215,6 +219,9 @@ Sealion 7 in bring-up B4."; VAL_ 578 GEAR 1 "PARK" 2 "REVERSE" 3 "NEUTRAL" 4 "DRIVE" ; -VAL_ 813 ACC_STATE 0 "OFF" 2 "AVAILABLE" 3 "ACTIVE" 5 "OVERRIDE" 7 "ERROR" ; +VAL_ 813 CRUISE_STATE 0 "OFF" 1 "AVAILABLE" 2 "ENGAGED" 3 "ENGAGED_ACCELERATING" ; VAL_ 790 LKAS_STATE 0 "OFF" 1 "PASSIVE" 2 "ACTIVE" 3 "ACTIVE" 4 "FAULT" ; VAL_ 508 TORQUE_TEMP_FAILED 0 "OK" 1 "DERATE" 2 "DERATE" 3 "FAILED" ; + +CM_ SG_ 790 SET_ME_3 "Bits 32-33. The camera sets these on every frame; they were undefined, so +the packer zeroed them and our regenerated 0x316 differed from stock. Passthrough only."; diff --git a/iqdbc_repo/iqdbc/safety/modes/byd.h b/iqdbc_repo/iqdbc/safety/modes/byd.h index 69c606728..5706d1a15 100644 --- a/iqdbc_repo/iqdbc/safety/modes/byd.h +++ b/iqdbc_repo/iqdbc/safety/modes/byd.h @@ -9,7 +9,7 @@ #define BYD_WHEEL_SPEEDS 0x1F0U // RX from ESP, vehicle speed #define BYD_DRIVE_STATE 0x242U // RX from VCU, gear + brake pressed #define BYD_PEDAL 0x342U // RX from VCU, accelerator pedal -#define BYD_ACC_HUD_ADAS 0x32DU // RX from ADAS(b2), cruise state +#define BYD_ACC_HUD_ADAS 0x32DU // RX from ADAS(b0), cruise state #define BYD_STEERING_MODULE_ADAS 0x1E2U // TX to EPS, angle command #define BYD_LKAS_HUD_ADAS 0x316U // TX to cluster, LKAS HUD #define BYD_ACC_CMD 0x32EU // TX to IPB, accel command @@ -18,6 +18,17 @@ // WHEEL_SPEEDS scale, kph per LSB. PROVISIONAL - keep in lockstep with byd_sealion_7.dbc. #define BYD_WHEEL_SPEED_SCALE 0.0725f +// STEERING_TORQUE.DRIVER_TORQUE counts (0.1 Nm/LSB). MUST equal CarControllerParams +// .STEER_DRIVER_OVERRIDE * 10 in values.py: carstate.py latches on the same instantaneous +// sample, and if the two sides disagree openpilot and the panda desync into controlsMismatch. +#define BYD_DRIVER_TORQUE_OVERRIDE 120 + +// A steering override disengages and stays disengaged until the driver deliberately re-arms, +// either by cycling stock cruise or pressing the LKAS/ICC button (0x3B0 bit 6, confirmed +// on-car). Mirrors the override latch in carstate.py. +static bool byd_override_latched = false; +static bool byd_lkas_btn_prev = false; + // ACC_CMD.ACCEL_CMD is an 8-bit field at 0.05 m/s^2 per LSB with a -5 m/s^2 offset, so raw 100 // is 0.0 m/s^2. Limits below are in offset-corrected LSBs. #define BYD_ACCEL_OFFSET 100 @@ -84,14 +95,35 @@ static void byd_rx_hook(const CANPacket_t *msg) { if (msg->addr == BYD_PEDAL) { gas_pressed = msg->data[0] > 10U; } - } - if (msg->bus == 2U) { - // Cruise state. ACC_STATE is 3 bits starting at bit 19. - // 0=OFF, 2=AVAILABLE, 3=ACTIVE, 5=OVERRIDE, 7=ERROR + // Driver torque, and the override latch. DRIVER_TORQUE is 4|12 signed. + if (msg->addr == BYD_STEERING_TORQUE) { + int torque_driver_new = to_signed(((msg->data[1] & 0xFFU) << 4) | (msg->data[0] >> 4), 12); + update_sample(&torque_driver, torque_driver_new); + if (SAFETY_ABS(torque_driver_new) > BYD_DRIVER_TORQUE_OVERRIDE) { + byd_override_latched = true; + } + } + + // LKAS/ICC button (0x3B0 bit 6) re-arms after an override + if (msg->addr == BYD_PCM_BUTTONS) { + bool lkas_btn = ((msg->data[0] >> 6) & 0x1U) != 0U; + if (lkas_btn && !byd_lkas_btn_prev) { + byd_override_latched = false; + } + byd_lkas_btn_prev = lkas_btn; + } + + // Cruise state. The ADAS/ACC ECU is on the chassis bus, not behind the camera relay. + // CRUISE_STATE is the high nibble of byte 5: 0=off, 1=available, 2=engaged, + // 3=engaged and commanding accel. PR #3337/#3352 read ACC_STATE from byte 2, which is a + // constant 0x3c here and can only ever report 7 (ERROR). if (msg->addr == BYD_ACC_HUD_ADAS) { - uint8_t acc_state = (msg->data[2] >> 3) & 0x7U; - bool acc_on = (acc_state == 3U) || (acc_state == 5U); + uint8_t cruise_state = msg->data[5] >> 4; + if (cruise_state < 2U) { + byd_override_latched = false; + } + bool acc_on = (cruise_state >= 2U) && !byd_override_latched; pcm_cruise_check(acc_on); } } @@ -153,6 +185,10 @@ static safety_config byd_init(uint16_t param) { {BYD_PCM_BUTTONS, 0, 8, .check_relay = false}, }; + // Longitudinal is only offered on a gateway harness, where the ACC ECU is behind the relay + // and 0x32E is genuinely filterable. On a camera harness the ACC ECU is in front of the + // relay, so alphaLongitudinalAvailable is false there and this list is never selected - + // check_relay would otherwise fire on every stock ACC frame. static const CanMsg BYD_LONG_TX_MSGS[] = { {BYD_STEERING_MODULE_ADAS, 0, 8, .check_relay = true}, {BYD_LKAS_HUD_ADAS, 0, 8, .check_relay = true}, @@ -168,9 +204,12 @@ static safety_config byd_init(uint16_t param) { {.msg = {{BYD_WHEEL_SPEEDS, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // vehicle speed {.msg = {{BYD_DRIVE_STATE, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // gear + brake (no counter/checksum) {.msg = {{BYD_PEDAL, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // accelerator pedal - {.msg = {{BYD_ACC_HUD_ADAS, 2, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // cruise state + {.msg = {{BYD_ACC_HUD_ADAS, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // cruise state (chassis bus, not behind the relay) }; + byd_override_latched = false; + byd_lkas_btn_prev = false; + bool byd_longitudinal = false; SAFETY_UNUSED(param); diff --git a/iqdbc_repo/iqdbc/safety/tests/test_byd.py b/iqdbc_repo/iqdbc/safety/tests/test_byd.py index f222ee3bc..1f5a52245 100644 --- a/iqdbc_repo/iqdbc/safety/tests/test_byd.py +++ b/iqdbc_repo/iqdbc/safety/tests/test_byd.py @@ -74,9 +74,21 @@ class TestBydSafetyBase(common.CarSafetyTest, common.AngleSteeringSafetyTest): return self.packer.make_can_msg_safety("STEER_MODULE_2", self.MAIN_BUS, values) def _pcm_status_msg(self, enable): - # ACC_STATE: 0=OFF, 2=AVAILABLE, 3=ACTIVE, 5=OVERRIDE, 7=ERROR - values = {"ACC_STATE": 3 if enable else 0} - return self.packer.make_can_msg_safety("ACC_HUD_ADAS", self.CAM_BUS, values) + # the ADAS/ACC ECU is on the chassis bus, not behind the camera relay + # CRUISE_STATE: 0=off, 1=available, 2=engaged, 3=engaged and commanding accel + values = {"CRUISE_STATE": 2 if enable else 1} + return self.packer.make_can_msg_safety("ACC_HUD_ADAS", self.MAIN_BUS, values) + + def test_cruise_state_not_read_from_constant_byte(self): + # PR #3337/#3352 read ACC_STATE from byte 2, which is constant 0x3c on this car. Setting + # only that byte must never enable cruise. + self.safety.set_controls_allowed(0) + for _ in range(5): + self._rx(self.packer.make_can_msg_safety("ACC_HUD_ADAS", self.MAIN_BUS, {"CRUISE_STATE": 0})) + self.assertFalse(self.safety.get_controls_allowed()) + for _ in range(5): + self._rx(self.packer.make_can_msg_safety("ACC_HUD_ADAS", self.MAIN_BUS, {"CRUISE_STATE": 3})) + self.assertTrue(self.safety.get_controls_allowed()) def _speed_msg(self, speed): # all four wheels, matching the rx hook's average @@ -179,6 +191,7 @@ class TestBydStockSafety(TestBydSafetyBase): class TestBydLongSafety(TestBydSafetyBase, common.LongitudinalAccelSafetyTest): TX_MSGS = [[STEERING_MODULE_ADAS, 0], [LKAS_HUD_ADAS, 0], [ACC_CMD, 0], [PCM_BUTTONS, 0]] + # long is only offered on a gateway harness, where 0x32E is behind the relay RELAY_MALFUNCTION_ADDRS = {0: (STEERING_MODULE_ADAS, LKAS_HUD_ADAS, ACC_CMD)} FWD_BLACKLISTED_ADDRS = {2: [STEERING_MODULE_ADAS, LKAS_HUD_ADAS, ACC_CMD]} diff --git a/panda/board/drivers/interrupts.h b/panda/board/drivers/interrupts.h index 1188a8ec0..58f89c12d 100644 --- a/panda/board/drivers/interrupts.h +++ b/panda/board/drivers/interrupts.h @@ -74,6 +74,10 @@ void init_interrupts(bool check_rate_limit){ for(uint16_t i=0U; i NACK retry storms. +#define IRQ_PRIORITY_COMMS 0U +#define IRQ_PRIORITY_DEFAULT 2U + typedef struct interrupt { IRQn_Type irq_type; void (*handler)(void); diff --git a/panda/board/stm32h7/llspi.h b/panda/board/stm32h7/llspi.h index 05f8e22f9..5b2d31f94 100644 --- a/panda/board/stm32h7/llspi.h +++ b/panda/board/stm32h7/llspi.h @@ -101,6 +101,12 @@ void llspi_init(void) { register_set(&(SPI4->CR1), SPI_CR1_SPE, 0xFFFFU); register_set(&(SPI4->CR2), 0, 0xFFFFU); + // preempt other handlers so the RX DMA is re-armed before the master clocks + // the next phase of a transfer + NVIC_SetPriority(DMA2_Stream2_IRQn, IRQ_PRIORITY_COMMS); + NVIC_SetPriority(DMA2_Stream3_IRQn, IRQ_PRIORITY_COMMS); + NVIC_SetPriority(SPI4_IRQn, IRQ_PRIORITY_COMMS); + NVIC_EnableIRQ(DMA2_Stream2_IRQn); NVIC_EnableIRQ(DMA2_Stream3_IRQn); NVIC_EnableIRQ(SPI4_IRQn);