forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Release Commit @ 0798119
This commit is contained in:
0
iqdbc_repo/iqdbc/car/byd/__init__.py
Normal file
0
iqdbc_repo/iqdbc/car/byd/__init__.py
Normal file
98
iqdbc_repo/iqdbc/car/byd/bydcan.py
Normal file
98
iqdbc_repo/iqdbc/car/byd/bydcan.py
Normal file
@@ -0,0 +1,98 @@
|
||||
# iqdbc/can/dbc.py imports byd_checksum from here, so this module must not import from
|
||||
# iqdbc.car (circular import at DBC parse time).
|
||||
|
||||
# stock camera saturates the 0x1E2 rate limits here while steering
|
||||
ANGLE_RATE_LIMIT_UPPER = 251
|
||||
ANGLE_RATE_LIMIT_LOWER = -252
|
||||
|
||||
# COUNTER and CHECKSUM are filled in by the packer from the DBC signal types, so they are
|
||||
# stripped from any stock frame we pass through rather than inherited.
|
||||
_GENERATED = ("COUNTER", "CHECKSUM")
|
||||
|
||||
|
||||
def byd_checksum(address: int, sig, d: bytearray) -> int:
|
||||
return (~sum(d[:7])) & 0xFF
|
||||
|
||||
|
||||
def _passthrough(stock: dict) -> dict:
|
||||
return {k: v for k, v in stock.items() if k not in _GENERATED}
|
||||
|
||||
|
||||
def create_steering_control(packer, apply_angle: float, lat_active: bool):
|
||||
values = {
|
||||
"STEER_REQ": 1 if lat_active else 0,
|
||||
"STEER_REQ_ACTIVE_LOW": 0 if lat_active else 1,
|
||||
"STEER_ANGLE": apply_angle,
|
||||
"ANGLE_RATE_LIMIT_UPPER": ANGLE_RATE_LIMIT_UPPER if lat_active else 0,
|
||||
"ANGLE_RATE_LIMIT_LOWER": ANGLE_RATE_LIMIT_LOWER if lat_active else 0,
|
||||
"E2E_ALIVE_1": 1,
|
||||
"E2E_ALIVE_2": 1,
|
||||
"SET_ME_FF": 0xFF,
|
||||
"SET_ME_F": 0xF,
|
||||
}
|
||||
return packer.make_can_msg("STEERING_MODULE_ADAS", 0, values)
|
||||
|
||||
|
||||
def create_lkas_hud(packer, lat_active: bool, stock_lkas_hud: dict, hud_control):
|
||||
# The ADAS modules cross-check this frame's exact bit pattern every cycle and fail-safe on a
|
||||
# mismatch, so only the bits proven to arm the EPS are asserted; everything else passes through.
|
||||
values = _passthrough(stock_lkas_hud)
|
||||
if lat_active:
|
||||
values["LKAS_STATE"] = (int(stock_lkas_hud["LKAS_STATE"]) & 0b1100) | 0b0010
|
||||
values["LEFT_LANE_STATE"] = int(stock_lkas_hud["LEFT_LANE_STATE"]) | 2
|
||||
values["RIGHT_LANE_STATE"] = int(stock_lkas_hud["RIGHT_LANE_STATE"]) | 2
|
||||
|
||||
if hud_control is not None:
|
||||
if hud_control.leftLaneDepart:
|
||||
values["LEFT_LANE_STATE"] = 2
|
||||
if hud_control.rightLaneDepart:
|
||||
values["RIGHT_LANE_STATE"] = 2
|
||||
|
||||
return packer.make_can_msg("LKAS_HUD_ADAS", 0, values)
|
||||
|
||||
|
||||
def create_acc_cmd(packer, accel: float, long_active: bool, stock_acc_cmd: dict,
|
||||
standstill: bool = False, resume: bool = False):
|
||||
# ACCEL_FACTOR/DECEL_FACTOR select the IPB gain profile: coast, soft accel, soft decel,
|
||||
# sustained brake. Pairs are stock's modal values per accel band.
|
||||
holding = long_active and standstill and not resume
|
||||
|
||||
if not long_active or abs(accel) < 0.1:
|
||||
accel_fac, decel_fac = 0, 0
|
||||
elif accel > 0:
|
||||
accel_fac, decel_fac = 12, 5
|
||||
elif accel > -1.5:
|
||||
accel_fac, decel_fac = 13, 1
|
||||
else:
|
||||
accel_fac, decel_fac = 1, 1
|
||||
|
||||
values = {
|
||||
**_passthrough(stock_acc_cmd),
|
||||
"ACCEL_CMD": accel if long_active else 0.0,
|
||||
"ACC_ON_1": 1 if long_active else 0,
|
||||
"ACC_ON_2": 1 if long_active else 0,
|
||||
"ACC_CONTROLLABLE_AND_ON": 1 if long_active else 0,
|
||||
"ACC_REQ_NOT_STANDSTILL": 0 if holding else (1 if long_active else 0),
|
||||
"CMD_REQ_ACTIVE_LOW": 0 if long_active else 1,
|
||||
"ACC_OVERRIDE_OR_STANDSTILL": 1 if holding else 0,
|
||||
"STANDSTILL_RESUME": 1 if (long_active and resume) else 0,
|
||||
"STANDSTILL_STATE": 1 if holding else 0,
|
||||
"ACCEL_FACTOR": accel_fac,
|
||||
"DECEL_FACTOR": decel_fac,
|
||||
"SET_ME_25_1": 25,
|
||||
"SET_ME_25_2": 25,
|
||||
"SET_ME_1": 1,
|
||||
"SET_ME_X8": 8,
|
||||
"SET_ME_XF": 15,
|
||||
}
|
||||
return packer.make_can_msg("ACC_CMD", 0, values)
|
||||
|
||||
|
||||
def create_buttons(packer, stock_buttons: dict, cancel: bool):
|
||||
values = {
|
||||
**_passthrough(stock_buttons),
|
||||
"SET_ME_1_1": 1,
|
||||
"SET_ME_1_2": 1,
|
||||
"ACC_ON_BTN": 1 if cancel else 0,
|
||||
}
|
||||
return packer.make_can_msg("PCM_BUTTONS", 0, values)
|
||||
91
iqdbc_repo/iqdbc/car/byd/carcontroller.py
Normal file
91
iqdbc_repo/iqdbc/car/byd/carcontroller.py
Normal file
@@ -0,0 +1,91 @@
|
||||
import numpy as np
|
||||
|
||||
from iqdbc.can.packer import CANPacker
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.car.lateral import apply_steer_angle_limits_vm
|
||||
from iqdbc.car.interfaces import CarControllerBase
|
||||
from iqdbc.car.byd import bydcan
|
||||
from iqdbc.car.byd.values import CarControllerParams
|
||||
from iqdbc.car.vehicle_model import VehicleModel
|
||||
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
ACC_STEP = 3 # ~33 Hz
|
||||
ACC_DT = ACC_STEP * 0.01
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_names, CP, CP_IQ):
|
||||
super().__init__(dbc_names, CP, CP_IQ)
|
||||
self.packer = CANPacker(dbc_names[Bus.pt])
|
||||
self.apply_angle_last = 0.0
|
||||
self.accel_last = 0.0
|
||||
self.VM = VehicleModel(CP)
|
||||
|
||||
def update(self, CC, CC_IQ, CS, now_nanos):
|
||||
can_sends = []
|
||||
actuators = CC.actuators
|
||||
|
||||
# 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:
|
||||
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))
|
||||
|
||||
accel = 0.0
|
||||
if self.CP.openpilotLongitudinalControl and self.frame % ACC_STEP == 0:
|
||||
if CC.longActive:
|
||||
accel = self._apply_long_limits(actuators, CS, CC)
|
||||
else:
|
||||
self.accel_last = float(np.clip(CS.out.aEgo, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
|
||||
lcs = actuators.longControlState
|
||||
stopping = (lcs == LongCtrlState.stopping) or (CS.out.standstill and accel <= 0.0)
|
||||
resume = (lcs == LongCtrlState.starting) or CC.cruiseControl.resume
|
||||
can_sends.append(bydcan.create_acc_cmd(self.packer, accel, CC.longActive, CS.acc_cmd,
|
||||
standstill=stopping and CS.out.standstill, resume=resume))
|
||||
|
||||
new_actuators = actuators.as_builder()
|
||||
new_actuators.steeringAngleDeg = float(self.apply_angle_last)
|
||||
new_actuators.accel = accel
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
def _apply_long_limits(self, actuators, CS, CC) -> float:
|
||||
target = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
|
||||
launch = CS.out.vEgo < 2.0 and target > 0.0
|
||||
up = (CarControllerParams.JERK_UP_LAUNCH if launch else CarControllerParams.JERK_UP) * ACC_DT
|
||||
down = CarControllerParams.JERK_DOWN * ACC_DT
|
||||
|
||||
# the hold parks the ramp at the stopping brake; snap to 0 so the launch kick applies
|
||||
# immediately instead of ramping back through the negative band while ESC-held
|
||||
resume = (actuators.longControlState == LongCtrlState.starting) or CC.cruiseControl.resume
|
||||
if resume and CS.out.standstill and self.accel_last < 0.0:
|
||||
self.accel_last = 0.0
|
||||
|
||||
accel = float(np.clip(target, self.accel_last - down, self.accel_last + up))
|
||||
self.accel_last = accel
|
||||
return accel
|
||||
139
iqdbc_repo/iqdbc/car/byd/carstate.py
Normal file
139
iqdbc_repo/iqdbc/car/byd/carstate.py
Normal file
@@ -0,0 +1,139 @@
|
||||
import copy
|
||||
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.can.parser import CANParser
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.byd.values import DBC, CarControllerParams as CCP
|
||||
from iqdbc.car.interfaces import CarStateBase
|
||||
|
||||
GearShifter = structs.CarState.GearShifter
|
||||
|
||||
GEAR_MAP = {
|
||||
1: GearShifter.park,
|
||||
2: GearShifter.reverse,
|
||||
3: GearShifter.neutral,
|
||||
4: GearShifter.drive,
|
||||
}
|
||||
|
||||
# STEERING_TORQUE low nibble, rebuilt from LKS_PREPARED + CRUISE_ACTIVATED
|
||||
EPS_STATE_OFF = 8
|
||||
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):
|
||||
super().__init__(CP, CP_IQ)
|
||||
self.lkas_hud = {}
|
||||
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]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
ret = structs.CarState()
|
||||
ret_iq = structs.IQCarState()
|
||||
|
||||
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["FL"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["FR"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["RL"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["RR"] * CV.KPH_TO_MS
|
||||
self.parse_wheel_speeds(ret,
|
||||
cp.vl["WHEEL_SPEEDS"]["FL"],
|
||||
cp.vl["WHEEL_SPEEDS"]["FR"],
|
||||
cp.vl["WHEEL_SPEEDS"]["RL"],
|
||||
cp.vl["WHEEL_SPEEDS"]["RR"],
|
||||
)
|
||||
ret.standstill = ret.vEgoRaw < 0.01
|
||||
ret.vEgoCluster = ret.vEgo
|
||||
|
||||
# both torques come from STEERING_TORQUE; 0x11F's 16|8 field is unsigned and reads as a
|
||||
# steering rate in BYD's own firmware, so it is not used for override detection
|
||||
ret.steeringAngleDeg = cp.vl["STEER_MODULE_2"]["STEER_ANGLE_2"]
|
||||
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)
|
||||
# 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.
|
||||
lks_prepared = bool(cp.vl["STEERING_TORQUE"]["LKS_PREPARED"])
|
||||
cruise_activated = bool(cp.vl["STEERING_TORQUE"]["CRUISE_ACTIVATED"])
|
||||
self.eps_state = EPS_STATE_OFF + int(lks_prepared) + 2 * int(cruise_activated)
|
||||
|
||||
ret.steerFaultTemporary = self.eps_state == EPS_STATE_LATCHED_FAULT
|
||||
ret.steerFaultTemporary |= int(cp_cam.vl["LKAS_HUD_ADAS"]["LKAS_STATE"]) == 4
|
||||
ret.steerFaultPermanent = bool(cp.vl["STEERING_TORQUE"]["TORQUE_FAILED"])
|
||||
|
||||
# DRIVE_STATE.RAW_THROTTLE is powertrain torque demand, not the pedal
|
||||
ret.gasPressed = cp.vl["PEDAL"]["GAS_PEDAL"] > 0.10
|
||||
ret.brake = cp.vl["PEDAL"]["BRAKE_PEDAL"]
|
||||
# must stay the same bit byd_rx_hook reads, or the two engage latches desync on a light
|
||||
# brake graze and controlsd raises "Controls Mismatch"
|
||||
ret.brakePressed = bool(cp.vl["DRIVE_STATE"]["BRAKE_PRESSED"])
|
||||
|
||||
ret.gearShifter = GEAR_MAP.get(int(cp.vl["DRIVE_STATE"]["GEAR"]), GearShifter.unknown)
|
||||
|
||||
ret.leftBlinker = bool(cp.vl["STALKS"]["LEFT_BLINKER"])
|
||||
ret.rightBlinker = bool(cp.vl["STALKS"]["RIGHT_BLINKER"])
|
||||
|
||||
ret.leftBlindspot = cp.vl["BSD_RADAR"]["LEFT_APPROACH"] != 0
|
||||
ret.rightBlindspot = cp.vl["BSD_RADAR"]["RIGHT_APPROACH"] != 0
|
||||
|
||||
ret.doorOpen = any((
|
||||
cp.vl["METER_CLUSTER"]["FRONT_LEFT_DOOR"],
|
||||
cp.vl["METER_CLUSTER"]["FRONT_RIGHT_DOOR"],
|
||||
cp.vl["METER_CLUSTER"]["BACK_LEFT_DOOR"],
|
||||
cp.vl["METER_CLUSTER"]["BACK_RIGHT_DOOR"],
|
||||
))
|
||||
ret.seatbeltUnlatched = not bool(cp.vl["METER_CLUSTER"]["SEATBELT_DRIVER"])
|
||||
|
||||
# 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.vl["ACC_CMD"])
|
||||
self.buttons = copy.copy(cp.vl["PCM_BUTTONS"])
|
||||
|
||||
return ret, ret_iq
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers(CP, CP_IQ):
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 0),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
|
||||
}
|
||||
23
iqdbc_repo/iqdbc/car/byd/fingerprints.py
Normal file
23
iqdbc_repo/iqdbc/car/byd/fingerprints.py
Normal file
@@ -0,0 +1,23 @@
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.car.byd.values import CAR
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
# Placeholder until a real car is captured in bring-up B2. Ecu.engine is essential, so a version
|
||||
# no car reports keeps the platform from ever exact-matching.
|
||||
#
|
||||
# Do NOT replace this with an empty ECU dict: match_fw_to_car_exact only invalidates a platform
|
||||
# when an expected version fails to match, so an empty dict leaves the platform a candidate for
|
||||
# every car on the road.
|
||||
#
|
||||
# Until populated, fingerprint explicitly: FINGERPRINT=BYD_SEALION_7
|
||||
#
|
||||
# ECUs answering 0xF195 on this platform: 0x704 MPC camera (non-essential), 0x782 brake/IPB,
|
||||
# 0x783 EPS, 0x7f2 forward radar, 0x7e0 drive unit, 0x7f1 SRS airbag.
|
||||
FW_VERSIONS = {
|
||||
CAR.BYD_SEALION_7: {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'PLACEHOLDER_UNTIL_CAPTURED',
|
||||
],
|
||||
},
|
||||
}
|
||||
73
iqdbc_repo/iqdbc/car/byd/interface.py
Normal file
73
iqdbc_repo/iqdbc/car/byd/interface.py
Normal file
@@ -0,0 +1,73 @@
|
||||
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 BydFlags, BydSafetyFlags
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
CarState = CarState
|
||||
CarController = CarController
|
||||
|
||||
@staticmethod
|
||||
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
|
||||
ret.brand = "byd"
|
||||
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.byd)]
|
||||
|
||||
ret.steerControlType = structs.CarParams.SteerControlType.angle
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerLimitTimer = 0.4
|
||||
|
||||
# the Veoneer tracks live on a private CAN-FD pair that the BYD-6 harness jumpers straight
|
||||
# through, so the panda never sees them
|
||||
ret.radarUnavailable = True
|
||||
|
||||
# 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
|
||||
|
||||
ret.longitudinalActuatorDelay = 0.5 # the IPB closes its own loop
|
||||
ret.vEgoStarting = 0.3
|
||||
ret.stopAccel = -0.5
|
||||
ret.startAccel = 1.5
|
||||
# without this longcontrol goes stopping -> pid directly and neither the startAccel kick
|
||||
# nor the STANDSTILL_RESUME pulse ever fires
|
||||
ret.startingState = True
|
||||
|
||||
# ACCEL_CMD is feedforward; high feedback gain on a 0.5s-lag actuator winds up the
|
||||
# integrator before the car responds
|
||||
ret.longitudinalTuning.kpBP = [0.0, 5.0, 35.0]
|
||||
ret.longitudinalTuning.kpV = [0.5, 0.4, 0.3]
|
||||
ret.longitudinalTuning.kiBP = [0.0, 35.0]
|
||||
ret.longitudinalTuning.kiV = [0.03, 0.02]
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def _get_params_iq(stock_cp: structs.CarParams, ret: structs.IQCarParams, candidate, fingerprint,
|
||||
car_fw, alpha_long: bool, is_release_iq: bool, docs: bool) -> structs.IQCarParams:
|
||||
if stock_cp.openpilotLongitudinalControl:
|
||||
ret.longitudinalStoppingSpeedOverride = 0.3
|
||||
|
||||
return ret
|
||||
0
iqdbc_repo/iqdbc/car/byd/tests/__init__.py
Normal file
0
iqdbc_repo/iqdbc/car/byd/tests/__init__.py
Normal file
427
iqdbc_repo/iqdbc/car/byd/tests/test_byd.py
Normal file
427
iqdbc_repo/iqdbc/car/byd/tests/test_byd.py
Normal file
@@ -0,0 +1,427 @@
|
||||
#!/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
|
||||
from iqdbc.car.byd.carstate import (EPS_STATE_OFF, EPS_STATE_PREPARED, EPS_STATE_ACTUATING,
|
||||
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, 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
|
||||
|
||||
DBC_NAME = DBC[CAR.BYD_SEALION_7]['pt']
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
|
||||
def _unpack(dbc_name, msg_name, dat):
|
||||
"""Decode one frame with the DBC, bypassing the parser's liveness tracking."""
|
||||
dbc = CANParser(dbc_name, [], 0).dbc
|
||||
msg = dbc.name_to_msg[msg_name]
|
||||
out = {}
|
||||
for sig in msg.sigs.values():
|
||||
val = 0
|
||||
if sig.is_little_endian:
|
||||
for i in range(sig.size):
|
||||
bit = sig.lsb + i
|
||||
val |= ((dat[bit // 8] >> (bit % 8)) & 1) << i
|
||||
else:
|
||||
be_bits = [j + i * 8 for i in range(64) for j in range(7, -1, -1)]
|
||||
idx = be_bits.index(sig.start_bit)
|
||||
for i in range(sig.size):
|
||||
bit = be_bits[idx + i]
|
||||
val = (val << 1) | ((dat[bit // 8] >> (bit % 8)) & 1)
|
||||
if sig.is_signed and (val & (1 << (sig.size - 1))):
|
||||
val -= (1 << sig.size)
|
||||
out[sig.name] = val * sig.factor + sig.offset
|
||||
return out
|
||||
|
||||
|
||||
class TestBydChecksum(unittest.TestCase):
|
||||
def test_checksum_is_inverted_sum(self):
|
||||
for dat in (bytearray(8), bytearray(b'\x01' * 8), bytearray(b'\xff' * 8),
|
||||
bytearray(b'\x12\x34\x56\x78\x9a\xbc\xde\x00')):
|
||||
self.assertEqual(bydcan.byd_checksum(0, None, dat), (~sum(dat[:7])) & 0xFF)
|
||||
|
||||
def test_packer_fills_checksum_and_counter(self):
|
||||
packer = CANPacker(DBC_NAME)
|
||||
seen = []
|
||||
for _ in range(18):
|
||||
_, dat, _ = packer.make_can_msg("STEERING_MODULE_ADAS", 0, {"STEER_REQ": 1})
|
||||
self.assertEqual(dat[7], (~sum(dat[:7])) & 0xFF, "checksum not filled by the DBC layer")
|
||||
seen.append(dat[6] >> 4) # COUNTER is 55|4@0
|
||||
# rolls 0..15 and wraps, never repeating within a cycle
|
||||
self.assertEqual(seen[:16], list(range(16)))
|
||||
self.assertEqual(seen[16:], [0, 1])
|
||||
|
||||
|
||||
class TestBydSteeringControl(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.packer = CANPacker(DBC_NAME)
|
||||
|
||||
def test_steer_req_and_angle_round_trip(self):
|
||||
for angle in (-390.0, -100.5, 0.0, 12.3, 390.0):
|
||||
for lat_active in (True, False):
|
||||
_, dat, _ = bydcan.create_steering_control(self.packer, angle, lat_active)
|
||||
vals = _unpack(DBC_NAME, "STEERING_MODULE_ADAS", dat)
|
||||
self.assertAlmostEqual(vals["STEER_ANGLE"], angle, places=4)
|
||||
self.assertEqual(vals["STEER_REQ"], 1 if lat_active else 0)
|
||||
# STEER_REQ_ACTIVE_LOW is the inverse of STEER_REQ
|
||||
self.assertEqual(vals["STEER_REQ_ACTIVE_LOW"], 0 if lat_active else 1)
|
||||
self.assertEqual(vals["E2E_ALIVE_1"], 1)
|
||||
self.assertEqual(vals["E2E_ALIVE_2"], 1)
|
||||
|
||||
def test_rate_limits_zeroed_when_inactive(self):
|
||||
_, dat, _ = bydcan.create_steering_control(self.packer, 0.0, True)
|
||||
vals = _unpack(DBC_NAME, "STEERING_MODULE_ADAS", dat)
|
||||
self.assertEqual(vals["ANGLE_RATE_LIMIT_UPPER"], bydcan.ANGLE_RATE_LIMIT_UPPER)
|
||||
self.assertEqual(vals["ANGLE_RATE_LIMIT_LOWER"], bydcan.ANGLE_RATE_LIMIT_LOWER)
|
||||
|
||||
_, dat, _ = bydcan.create_steering_control(self.packer, 0.0, False)
|
||||
vals = _unpack(DBC_NAME, "STEERING_MODULE_ADAS", dat)
|
||||
self.assertEqual(vals["ANGLE_RATE_LIMIT_UPPER"], 0)
|
||||
self.assertEqual(vals["ANGLE_RATE_LIMIT_LOWER"], 0)
|
||||
|
||||
|
||||
class TestBydLkasHud(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.packer = CANPacker(DBC_NAME)
|
||||
# a stock frame with bits set in every field we touch and several we must not
|
||||
self.stock = {
|
||||
"HMA_STATE": 3, "LEFT_LANE_STATE": 1, "LKS_MODE": 2, "HANDS_ON_WHEEL_REQ": 1,
|
||||
"TJA_ICA_STATE": 5, "HMA_ON_OFF": 1, "LKAS_OUTPUT": -20, "LKAS_REQ_PREPARE": 1,
|
||||
"LKAS_ACTIVE": 1, "SLA_STATE": 3, "RIGHT_LANE_STATE": 1, "LKAS_STATE": 0b1000,
|
||||
"SPEED_LIMIT_VALUE": 100, "LDSW_TYPE": 2, "COUNTER": 9, "CHECKSUM": 0x11,
|
||||
}
|
||||
|
||||
def test_passes_stock_bits_through(self):
|
||||
# The ADAS modules cross-check this frame; every bit we do not own must survive.
|
||||
_, dat, _ = bydcan.create_lkas_hud(self.packer, False, self.stock, None)
|
||||
vals = _unpack(DBC_NAME, "LKAS_HUD_ADAS", dat)
|
||||
for name in ("HMA_STATE", "LKS_MODE", "HANDS_ON_WHEEL_REQ", "TJA_ICA_STATE", "HMA_ON_OFF",
|
||||
"LKAS_OUTPUT", "LKAS_REQ_PREPARE", "LKAS_ACTIVE", "SLA_STATE",
|
||||
"SPEED_LIMIT_VALUE", "LDSW_TYPE"):
|
||||
self.assertEqual(vals[name], self.stock[name], f"{name} was modified")
|
||||
|
||||
def test_hands_on_wheel_req_never_cleared(self):
|
||||
for lat_active in (True, False):
|
||||
_, dat, _ = bydcan.create_lkas_hud(self.packer, lat_active, self.stock, None)
|
||||
vals = _unpack(DBC_NAME, "LKAS_HUD_ADAS", dat)
|
||||
self.assertEqual(vals["HANDS_ON_WHEEL_REQ"], 1)
|
||||
|
||||
def test_active_asserts_eps_arming_bits_only(self):
|
||||
_, dat, _ = bydcan.create_lkas_hud(self.packer, True, self.stock, None)
|
||||
vals = _unpack(DBC_NAME, "LKAS_HUD_ADAS", dat)
|
||||
# low 2 bits become 0b10, the stock upper 2 bits are preserved
|
||||
self.assertEqual(int(vals["LKAS_STATE"]), 0b1010)
|
||||
self.assertEqual(int(vals["LEFT_LANE_STATE"]), 1 | 2)
|
||||
self.assertEqual(int(vals["RIGHT_LANE_STATE"]), 1 | 2)
|
||||
|
||||
def test_counter_not_inherited_from_stock(self):
|
||||
# inheriting the camera's counter would make our 50 Hz stream non-monotonic
|
||||
counters = []
|
||||
for _ in range(4):
|
||||
_, dat, _ = bydcan.create_lkas_hud(self.packer, True, self.stock, None)
|
||||
counters.append(int(_unpack(DBC_NAME, "LKAS_HUD_ADAS", dat)["COUNTER"]))
|
||||
self.assertNotEqual(counters, [self.stock["COUNTER"]] * 4)
|
||||
self.assertEqual(counters, [(counters[0] + i) % 16 for i in range(4)])
|
||||
|
||||
def test_checksum_recomputed_not_inherited(self):
|
||||
_, dat, _ = bydcan.create_lkas_hud(self.packer, True, self.stock, None)
|
||||
self.assertEqual(dat[7], (~sum(dat[:7])) & 0xFF)
|
||||
|
||||
|
||||
class TestBydAccCmd(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.packer = CANPacker(DBC_NAME)
|
||||
self.stock = {"ACCEL_CMD": 0.0, "COUNTER": 7, "CHECKSUM": 0x22}
|
||||
|
||||
def test_accel_scale_is_physical(self):
|
||||
# raw x 0.05 - 5 m/s^2, so 0 m/s^2 is raw 100
|
||||
for accel in (-3.0, -1.5, 0.0, 0.5, 1.5):
|
||||
_, dat, _ = bydcan.create_acc_cmd(self.packer, accel, True, self.stock)
|
||||
self.assertEqual(dat[0], round((accel + 5.0) / 0.05))
|
||||
vals = _unpack(DBC_NAME, "ACC_CMD", dat)
|
||||
self.assertAlmostEqual(vals["ACCEL_CMD"], accel, places=6)
|
||||
|
||||
def test_inactive_commands_zero_accel(self):
|
||||
_, dat, _ = bydcan.create_acc_cmd(self.packer, -2.0, False, self.stock)
|
||||
vals = _unpack(DBC_NAME, "ACC_CMD", dat)
|
||||
self.assertEqual(vals["ACCEL_CMD"], 0.0)
|
||||
self.assertEqual(dat[0], 100)
|
||||
self.assertEqual(vals["ACC_ON_1"], 0)
|
||||
self.assertEqual(vals["ACC_ON_2"], 0)
|
||||
self.assertEqual(vals["ACC_CONTROLLABLE_AND_ON"], 0)
|
||||
self.assertEqual(vals["CMD_REQ_ACTIVE_LOW"], 1)
|
||||
|
||||
def test_standstill_hold_and_resume(self):
|
||||
_, dat, _ = bydcan.create_acc_cmd(self.packer, -0.5, True, self.stock, standstill=True)
|
||||
vals = _unpack(DBC_NAME, "ACC_CMD", dat)
|
||||
self.assertEqual(vals["STANDSTILL_STATE"], 1)
|
||||
self.assertEqual(vals["ACC_OVERRIDE_OR_STANDSTILL"], 1)
|
||||
self.assertEqual(vals["ACC_REQ_NOT_STANDSTILL"], 0)
|
||||
self.assertEqual(vals["STANDSTILL_RESUME"], 0)
|
||||
|
||||
_, dat, _ = bydcan.create_acc_cmd(self.packer, 0.5, True, self.stock, standstill=True, resume=True)
|
||||
vals = _unpack(DBC_NAME, "ACC_CMD", dat)
|
||||
self.assertEqual(vals["STANDSTILL_RESUME"], 1)
|
||||
self.assertEqual(vals["STANDSTILL_STATE"], 0)
|
||||
self.assertEqual(vals["ACC_REQ_NOT_STANDSTILL"], 1)
|
||||
|
||||
def test_regime_pairs(self):
|
||||
for accel, expected in ((0.0, (0, 0)), (0.05, (0, 0)), (0.8, (12, 5)),
|
||||
(-1.0, (13, 1)), (-2.5, (1, 1))):
|
||||
_, dat, _ = bydcan.create_acc_cmd(self.packer, accel, True, self.stock)
|
||||
vals = _unpack(DBC_NAME, "ACC_CMD", dat)
|
||||
self.assertEqual((int(vals["ACCEL_FACTOR"]), int(vals["DECEL_FACTOR"])), expected, f"{accel=}")
|
||||
|
||||
def test_accel_within_safety_bounds(self):
|
||||
# the comfort envelope must stay inside what byd.h allows (-3.5 .. +2.0)
|
||||
self.assertGreaterEqual(CarControllerParams.ACCEL_MIN, -3.5)
|
||||
self.assertLessEqual(CarControllerParams.ACCEL_MAX, 2.0)
|
||||
|
||||
|
||||
class TestBydEpsState(unittest.TestCase):
|
||||
"""The 0x1FC decode is the core fix over the Sealion 7 PR, which inherited a stub that
|
||||
packed these status bits into a fake 16-bit torque value."""
|
||||
|
||||
def test_state_nibble_table(self):
|
||||
for prepared, activated, expected in (
|
||||
(0, 0, EPS_STATE_OFF),
|
||||
(1, 0, EPS_STATE_PREPARED),
|
||||
(0, 1, EPS_STATE_ACTUATING),
|
||||
(1, 1, EPS_STATE_LATCHED_FAULT),
|
||||
):
|
||||
self.assertEqual(EPS_STATE_OFF + prepared + 2 * activated, expected)
|
||||
|
||||
def test_steering_torque_signals_exist_and_are_signed(self):
|
||||
dbc = CANParser(DBC_NAME, [], 0).dbc
|
||||
sigs = dbc.name_to_msg["STEERING_TORQUE"].sigs
|
||||
for name in ("LKS_PREPARED", "CRUISE_ACTIVATED", "TORQUE_FAILED", "DRIVER_TORQUE",
|
||||
"TARGET_ANGLE", "MAIN_TORQUE"):
|
||||
self.assertIn(name, sigs, f"{name} missing from STEERING_TORQUE")
|
||||
# driver torque must be signed or override detection cannot see direction
|
||||
self.assertTrue(sigs["DRIVER_TORQUE"].is_signed)
|
||||
self.assertTrue(sigs["MAIN_TORQUE"].is_signed)
|
||||
self.assertEqual(sigs["DRIVER_TORQUE"].start_bit, 4)
|
||||
self.assertEqual(sigs["DRIVER_TORQUE"].size, 12)
|
||||
self.assertEqual(sigs["MAIN_TORQUE"].start_bit, 32)
|
||||
self.assertEqual(sigs["MAIN_TORQUE"].size, 12)
|
||||
|
||||
def test_driver_torque_decodes_negative(self):
|
||||
packer = CANPacker(DBC_NAME)
|
||||
for torque in (-20.0, -0.5, 0.0, 0.5, 20.0):
|
||||
_, dat, _ = packer.make_can_msg("STEERING_TORQUE", 0, {"DRIVER_TORQUE": torque})
|
||||
vals = _unpack(DBC_NAME, "STEERING_TORQUE", dat)
|
||||
self.assertAlmostEqual(vals["DRIVER_TORQUE"], torque, places=4)
|
||||
|
||||
|
||||
class TestBydWheelSpeeds(unittest.TestCase):
|
||||
def test_four_independent_wheels(self):
|
||||
dbc = CANParser(DBC_NAME, [], 0).dbc
|
||||
sigs = dbc.name_to_msg["WHEEL_SPEEDS"].sigs
|
||||
for name, start in (("FL", 0), ("FR", 16), ("RL", 28), ("RR", 40)):
|
||||
self.assertEqual(sigs[name].start_bit, start)
|
||||
self.assertEqual(sigs[name].size, 12)
|
||||
self.assertAlmostEqual(sigs[name].factor, 0.0725)
|
||||
|
||||
def test_wheel_speeds_round_trip(self):
|
||||
packer = CANPacker(DBC_NAME)
|
||||
_, dat, _ = packer.make_can_msg("WHEEL_SPEEDS", 0, {"FL": 50.0, "FR": 51.0, "RL": 52.0, "RR": 53.0})
|
||||
vals = _unpack(DBC_NAME, "WHEEL_SPEEDS", dat)
|
||||
for name, expected in (("FL", 50.0), ("FR", 51.0), ("RL", 52.0), ("RR", 53.0)):
|
||||
self.assertAlmostEqual(vals[name], expected, delta=0.0725)
|
||||
|
||||
|
||||
class TestBydFingerprint(unittest.TestCase):
|
||||
def test_placeholder_never_matches_a_real_car(self):
|
||||
# a platform whose ECU dict is empty survives as a candidate for EVERY car, so the
|
||||
# placeholder must be a version no car reports rather than an empty dict
|
||||
self.assertTrue(FW_VERSIONS[CAR.BYD_SEALION_7], "empty ECU dict would match every car")
|
||||
|
||||
live = build_fw_dict([CarParams.CarFw(ecu=Ecu.engine, fwVersion=b'REAL_CAR_FW', brand='byd',
|
||||
address=0x7e0, subAddress=0)])
|
||||
self.assertNotIn(str(CAR.BYD_SEALION_7), match_fw_to_car_exact(live, 'byd'))
|
||||
|
||||
def test_fuzzy_match_requires_vds(self):
|
||||
# WMI + model year alone would claim every BYD of that year
|
||||
from iqdbc.car.byd.values import match_fw_to_car_fuzzy
|
||||
self.assertEqual(CAR.BYD_SEALION_7.config.vds_prefixes, set())
|
||||
vin = "LGX" + "A" * 6 + "R" + "A" * 7 # LGX, 2024 model year
|
||||
self.assertEqual(match_fw_to_car_fuzzy({}, vin, {}), set())
|
||||
|
||||
|
||||
class TestBydCarController(unittest.TestCase):
|
||||
"""The EPS latches a fault (state 11) if the 0x1E2 stream stops while it is actuating, and
|
||||
re-arms only on a STEER_REQ rising edge over a continuous stream. The safety also statically
|
||||
blocks the camera's own 0x1E2/0x316, so openpilot is the only source of both."""
|
||||
|
||||
def _run(self, lat_active, long_active=False, frames=20):
|
||||
CP = CarInterface.get_non_essential_params("BYD_SEALION_7")
|
||||
CP_IQ = CarInterface.get_non_essential_params_iq(CP, "BYD_SEALION_7")
|
||||
CC_obj = structs.CarControl()
|
||||
CC_obj.enabled = lat_active
|
||||
CC_obj.latActive = lat_active
|
||||
CC_obj.longActive = long_active
|
||||
CC = CC_obj.as_reader()
|
||||
CC_IQ = structs.IQCarControl()
|
||||
|
||||
carcontroller = CarInterface.CarController({'pt': DBC_NAME}, CP, CP_IQ)
|
||||
carstate = CarInterface.CarState(CP, CP_IQ)
|
||||
parsers = CarInterface.CarState.get_can_parsers(CP, CP_IQ)
|
||||
cs_out, _ = carstate.update(parsers)
|
||||
|
||||
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
|
||||
|
||||
sent = []
|
||||
for i in range(frames):
|
||||
_, can_sends = carcontroller.update(CC, CC_IQ, cs, i * 10_000_000)
|
||||
sent.append([addr for addr, _, _ in can_sends])
|
||||
return sent
|
||||
|
||||
def test_steering_stream_is_continuous_when_inactive(self):
|
||||
for lat_active in (True, False):
|
||||
sent = self._run(lat_active)
|
||||
steering = [i for i, addrs in enumerate(sent) if 0x1E2 in addrs]
|
||||
hud = [i for i, addrs in enumerate(sent) if 0x316 in addrs]
|
||||
# every other frame, whether or not lateral is active
|
||||
self.assertEqual(steering, list(range(0, 20, 2)), f"{lat_active=}")
|
||||
self.assertEqual(hud, list(range(0, 20, 2)), f"{lat_active=}")
|
||||
|
||||
def test_steer_req_gates_actuation_not_transmission(self):
|
||||
CP = CarInterface.get_non_essential_params("BYD_SEALION_7")
|
||||
CP_IQ = CarInterface.get_non_essential_params_iq(CP, "BYD_SEALION_7")
|
||||
carcontroller = CarInterface.CarController({'pt': DBC_NAME}, CP, CP_IQ)
|
||||
for lat_active in (False, True):
|
||||
_, dat, _ = bydcan.create_steering_control(carcontroller.packer, 0.0, lat_active)
|
||||
vals = _unpack(DBC_NAME, "STEERING_MODULE_ADAS", dat)
|
||||
self.assertEqual(vals["STEER_REQ"], 1 if lat_active else 0)
|
||||
|
||||
def test_no_acc_cmd_without_openpilot_longitudinal(self):
|
||||
sent = self._run(True, long_active=True)
|
||||
self.assertFalse(any(0x32E in addrs for addrs in sent),
|
||||
"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")
|
||||
self.assertEqual(CP.brand, "byd")
|
||||
self.assertEqual(CP.steerControlType, CarParams.SteerControlType.angle)
|
||||
self.assertEqual(CP.safetyConfigs[0].safetyModel, CarParams.SafetyModel.byd)
|
||||
# the BYD-6 harness jumpers the Veoneer private CAN-FD pair straight through
|
||||
self.assertTrue(CP.radarUnavailable)
|
||||
self.assertFalse(CP.dashcamOnly)
|
||||
|
||||
def test_steer_step_matches_safety_frequency(self):
|
||||
# byd.h declares .frequency = 50U for the angle limiter
|
||||
self.assertEqual(CarControllerParams.STEER_STEP, 2)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
143
iqdbc_repo/iqdbc/car/byd/values.py
Normal file
143
iqdbc_repo/iqdbc/car/byd/values.py
Normal file
@@ -0,0 +1,143 @@
|
||||
from dataclasses import dataclass, field
|
||||
from enum import IntFlag, StrEnum
|
||||
|
||||
from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, structs
|
||||
from iqdbc.car.lateral import AngleSteeringLimits, AVERAGE_ROAD_ROLL, ISO_LATERAL_ACCEL
|
||||
from iqdbc.car.docs_definitions import CarDocs, CarHarness, CarParts
|
||||
from iqdbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
|
||||
from iqdbc.car.vin import Vin
|
||||
|
||||
Ecu = structs.CarParams.Ecu
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
STEER_STEP = 2 # 50 Hz
|
||||
|
||||
ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
|
||||
390, # deg
|
||||
([], []),
|
||||
([], []),
|
||||
|
||||
MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
|
||||
MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
|
||||
|
||||
# deg/20ms. EPS faults at 12 at a standstill; 5 caused command spikes, 2 under-tracked
|
||||
# sharp curves. Stock Veoneer max is 4.8.
|
||||
MAX_ANGLE_RATE=3,
|
||||
)
|
||||
|
||||
# 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
|
||||
ACCEL_MAX = 1.5
|
||||
|
||||
JERK_UP = 2.5
|
||||
JERK_UP_LAUNCH = 4.0 # below 2 m/s, to beat the ~0.5s IPB lag off the line
|
||||
JERK_DOWN = 5.0
|
||||
|
||||
|
||||
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"
|
||||
|
||||
|
||||
class ModelYear(StrEnum):
|
||||
R_2024 = "R"
|
||||
S_2025 = "S"
|
||||
T_2026 = "T"
|
||||
|
||||
|
||||
@dataclass
|
||||
class BydCarDocs(CarDocs):
|
||||
package: str = "All"
|
||||
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.custom]))
|
||||
|
||||
|
||||
@dataclass
|
||||
class BydPlatformConfig(PlatformConfig):
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'byd_sealion_7'})
|
||||
wmis: set[WMI] = field(default_factory=set)
|
||||
years: set[ModelYear] = field(default_factory=set)
|
||||
vds_prefixes: set[str] = field(default_factory=set)
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
BYD_SEALION_7 = BydPlatformConfig(
|
||||
[BydCarDocs("BYD Sealion 7 2024-25")],
|
||||
CarSpecs(mass=2090., wheelbase=2.93, steerRatio=16.0, centerToFrontRatio=0.44),
|
||||
wmis={WMI.BYD_AUTO},
|
||||
years={ModelYear.R_2024, ModelYear.S_2025, ModelYear.T_2026},
|
||||
)
|
||||
|
||||
|
||||
def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str]:
|
||||
# VIN: LGX (WMI) + <VDS> + <year><plant><seq> (VIS). Matching on WMI + year alone would claim
|
||||
# every BYD of that year, so a platform only matches once its VDS prefix is known.
|
||||
vin_obj = Vin(vin)
|
||||
year = vin_obj.vis[:1]
|
||||
|
||||
candidates = set()
|
||||
for platform in CAR:
|
||||
cfg = platform.config
|
||||
if not cfg.vds_prefixes or vin_obj.wmi not in cfg.wmis or year not in cfg.years:
|
||||
continue
|
||||
if any(vin_obj.vds.startswith(p) for p in cfg.vds_prefixes):
|
||||
candidates.add(platform)
|
||||
|
||||
return {str(c) for c in candidates}
|
||||
|
||||
|
||||
FW_QUERY_CONFIG = FwQueryConfig(
|
||||
# BYD ECUs NRC 0xF188 (openpilot's default) but answer 0xF195
|
||||
requests=[
|
||||
Request(
|
||||
[StdQueries.SUPPLIER_SOFTWARE_VERSION_REQUEST],
|
||||
[StdQueries.SUPPLIER_SOFTWARE_VERSION_RESPONSE],
|
||||
bus=0,
|
||||
),
|
||||
],
|
||||
# the MPC camera answers OBD DTC scans but not the bus-0 DID sweep
|
||||
non_essential_ecus={Ecu.fwdCamera: list(CAR)},
|
||||
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
|
||||
)
|
||||
|
||||
|
||||
DBC = CAR.create_dbc_map()
|
||||
Reference in New Issue
Block a user