forked from IQ.Lvbs/IQ.Pilot
144 lines
5.1 KiB
Python
144 lines
5.1 KiB
Python
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()
|