mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-20 07:43:48 +08:00
Code Issues Packages Projects Releases Wiki Activity

what a mornin

Browse Source
This commit is contained in:
firestar5683
2026-08-18 12:07:03 -05:00
parent ef956fb54e
commit 50e1c1d376
57 changed files with 13171 additions and 382 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/log.capnp
+27
View File
@@ -472,6 +472,10 @@ struct CanData {
struct DeviceState @0xa4d8b5af2aa492eb {
deviceType @45 :InitData.DeviceType;
# usb
chestnutPresent @51 :Bool;
usbState @52 :UsbState;
networkType @22 :NetworkType;
networkInfo @31 :NetworkInfo;
networkStrength @24 :NetworkStrength;
@@ -504,6 +508,7 @@ struct DeviceState @0xa4d8b5af2aa492eb {
intakeTempC @46 :Float32;
exhaustTempC @47 :Float32;
caseTempC @48 :Float32;
bottomSocTempC @50 :Float32;
maxTempC @44 :Float32; # max of other temps, used to control fan
thermalZones @38 :List(ThermalZone);
thermalStatus @14 :ThermalStatus;
@@ -736,6 +741,28 @@ struct PeripheralState {
}
}
struct UsbState {
devices @0 :List(Device);
struct Device {
busnum @0 :UInt8;
devnum @1 :UInt8;
vendorId @2 :UInt16;
productId @3 :UInt16;
speedMbps @4 :UInt16;
manufacturer @6 :Text;
product @5 :Text;
linkErrorCount @7 :UInt16;
usb3Lane @8 :Usb3Lane;
enum Usb3Lane {
unknown @0;
a @1;
b @2;
}
}
}
struct ChestnutState {
tempC @0 :Float32;
memoryTempC @1 :Float32;
opendbc_repo/docs/CARS.md
+33 -4
View File
@@ -1,6 +1,6 @@
<!--- AUTOGENERATED FROM selfdrive/car/CARS_template.md, DO NOT EDIT. --->
# Support Information for 489 Known Cars
# Support Information for 518 Known Cars
|Make|Model|Package|Support Level|
|---|---|---|:---:|
@@ -38,16 +38,26 @@
|Audi|A5 2016-24|All|[Not compatible](#flexray)|
|Audi|Q2 2018|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|Audi|Q3 2019-24|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|Audi|Q4 e-tron 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|Audi|Q4 e-tron 2024-25|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|Audi|Q5 2017-24|All|[Not compatible](#flexray)|
|Audi|RS3 2018|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|Audi|S3 2015-17|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|Buick|Baby Enclave 2020-23|Driver Assist Package|[Upstream](#upstream)|
|Buick|LaCrosse 2017-19|Driver Confidence Package 2|[Upstream](#upstream)|
|Buick|LaCrosse ASCM Harness 2017-19|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Buick|LaCrosse US ASCM Harness 2019|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Buick|Regal Essence 2018|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Cadillac|ATS Premium Performance 2018|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Cadillac|CT6 No-ACC 2016-20|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Cadillac|Escalade 2017|Driver Assist Package|[Upstream](#upstream)|
|Cadillac|Escalade ASCM Harness 2018|Driver Assist Package|[Upstream](#upstream)|
|Cadillac|Escalade ESV 2016|Adaptive Cruise Control (ACC) & LKAS|[Upstream](#upstream)|
|Cadillac|Escalade ESV 2019|Adaptive Cruise Control (ACC) & LKAS|[Upstream](#upstream)|
|Cadillac|Escalade ESV Platinum ASCM Harness 2019|Adaptive Cruise Control (ACC) & LKAS|[Upstream](#upstream)|
|Cadillac|XT4 2023|Driver Assist Package|[Upstream](#upstream)|
|Cadillac|XT4 No-ACC 2023|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Cadillac|XT5 2022|Driver Assist Package|[Upstream](#upstream)|
|Cadillac|XT5 No-ACC 2022|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Cadillac|XT6 2020|Driver Assist Package|[Upstream](#upstream)|
|Chevrolet|Blazer 2019-25|Driver Assist Package|[Upstream](#upstream)|
@@ -62,13 +72,17 @@
|Chevrolet|Malibu ASCM Harness 2017-19|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Chevrolet|Malibu Hybrid No-ACC 2017|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Chevrolet|Malibu No-ACC 2023|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Chevrolet|Malibu Premier 2017|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Chevrolet|Silverado 1500 2020-21|Safety Package II|[Upstream](#upstream)|
|Chevrolet|Silverado 1500 No-ACC 2020-21|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Chevrolet|Suburban Premier 2016-20|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Chevrolet|Suburban Premier No-ACC 2016-20|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Chevrolet|Trailblazer 2021-22|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Chevrolet|Trailblazer No-ACC 2021-22|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Chevrolet|TRAX 2024|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Chevrolet|Traverse 2022-23|RS, Premier, or High Country Trim|[Upstream](#upstream)|
|Chevrolet|TRAX 2024-25|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Chevrolet|Volt 2017-18|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Chevrolet|Volt 2019|Adaptive Cruise Control (ACC) & LKAS|[Upstream](#upstream)|
|Chevrolet|Volt ASCM Harness 2017-18|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Chevrolet|Volt Camera Harness 2017-18|Flashed camera-forward integration with ACC|[Upstream](#upstream)|
|Chevrolet|Volt No-ACC 2017-18|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
@@ -79,8 +93,10 @@
|Chrysler|Pacifica Hybrid 2019-25|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|comma|body|All|[Upstream](#upstream)|
|CUPRA|Ateca 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|CUPRA|Born 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|Dodge|Durango 2020-21|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Ford|Bronco Sport 2021-24|Co-Pilot360 Assist+|[Upstream](#upstream)|
|Ford|Edge 2022|Co-Pilot360 Assist+|[Upstream](#upstream)|
|Ford|Escape 2020-22|Co-Pilot360 Assist+|[Upstream](#upstream)|
|Ford|Escape 2023-24|Co-Pilot360 Assist+|[Upstream](#upstream)|
|Ford|Escape Hybrid 2020-22|Co-Pilot360 Assist+|[Upstream](#upstream)|
@@ -92,8 +108,9 @@
|Ford|Explorer Hybrid 2020-24|Co-Pilot360 Assist+|[Upstream](#upstream)|
|Ford|F-150 2021-23|Co-Pilot360 Assist 2.0|[Upstream](#upstream)|
|Ford|F-150 Hybrid 2021-23|Co-Pilot360 Assist 2.0|[Upstream](#upstream)|
|Ford|Focus 2018|Adaptive Cruise Control with Lane Centering|[Upstream](#upstream)|
|Ford|Focus Hybrid 2018|Adaptive Cruise Control with Lane Centering|[Upstream](#upstream)|
|Ford|F-150 Lightning 2022-25|Co-Pilot360 Assist 2.0|[Upstream](#upstream)|
|Ford|Focus 2018-22|Adaptive Cruise Control with Lane Centering|[Upstream](#upstream)|
|Ford|Focus Hybrid 2018-22|Adaptive Cruise Control with Lane Centering|[Upstream](#upstream)|
|Ford|Kuga 2020-23|Adaptive Cruise Control with Lane Centering|[Upstream](#upstream)|
|Ford|Kuga Hybrid 2020-23|Adaptive Cruise Control with Lane Centering|[Upstream](#upstream)|
|Ford|Kuga Hybrid 2024|All|[Upstream](#upstream)|
@@ -103,6 +120,7 @@
|Ford|Maverick 2023-24|Co-Pilot360 Assist|[Upstream](#upstream)|
|Ford|Maverick Hybrid 2022|LARIAT Luxury|[Upstream](#upstream)|
|Ford|Maverick Hybrid 2023-24|Co-Pilot360 Assist|[Upstream](#upstream)|
|Ford|Mondeo 2014-22|Adaptive Cruise Control with Lane Centering|[Upstream](#upstream)|
|Ford|Mustang Mach-E 2021-24|All|[Upstream](#upstream)|
|Ford|Ranger 2024|Adaptive Cruise Control with Lane Centering|[Upstream](#upstream)|
|Ford|Transit 2025|Co-Pilot360 Assist+|[Upstream](#upstream)|
@@ -124,11 +142,13 @@
|Genesis|GV80 2023|All|[Upstream](#upstream)|
|Genesis|GV80 (3.5T Prestige Trim, with HDA II & LFA2) 2025|Highway Driving Assist II & Lane Follow Assist 2|[Upstream](#upstream)|
|Genesis|GV80 Coupe (with HDA II & LFA2) 2025|Highway Driving Assist II & Lane Follow Assist 2|[Upstream](#upstream)|
|GMC|Acadia 2018|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|GMC|Acadia ASCM Harness 2018|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|GMC|Sierra 1500 2020-21|Driver Alert Package II|[Upstream](#upstream)|
|GMC|Sierra 1500 No-ACC 2020-21|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|GMC|Yukon 2019-20|Adaptive Cruise Control (ACC) & LKAS|[Upstream](#upstream)|
|GMC|Yukon No-ACC 2019-20|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Holden|Astra 2017|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Honda|Accord 2016-17|Honda Sensing|[Upstream](#upstream)|
|Honda|Accord 2016-17|Honda Sensing|[Community](#community)|
|Honda|Accord 2018-22|All|[Upstream](#upstream)|
@@ -353,7 +373,9 @@
|Ram|2500 2020-24|Adaptive Cruise Control (ACC)|[Dashcam mode](#dashcam)|
|Ram|3500 2019-22|Adaptive Cruise Control (ACC)|[Dashcam mode](#dashcam)|
|Rivian|R1S 2022-24|All|[Upstream](#upstream)|
|Rivian|R1S 2025|All|[Upstream](#upstream)|
|Rivian|R1T 2022-24|All|[Upstream](#upstream)|
|Rivian|R1T 2025|All|[Upstream](#upstream)|
|SEAT|Alhambra 2018-20|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|SEAT|Ateca 2016-23|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|SEAT|Leon 2014-20|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
@@ -379,6 +401,8 @@
|Subaru|Solterra 2023-25|Any|[Not compatible](#can-bus-security)|
|Subaru|XV 2018-19|EyeSight Driver Assistance|[Upstream](#upstream)|
|Subaru|XV 2020-21|EyeSight Driver Assistance|[Upstream](#upstream)|
|Škoda|Enyaq 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|Škoda|Enyaq 2024-25|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|Škoda|Fabia 2022-23|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|Škoda|Kamiq 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|Škoda|Karoq 2019-23|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
@@ -474,6 +498,11 @@
|Volkswagen|Golf R 2015-19|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|Volkswagen|Golf SportsVan 2015-20|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|Volkswagen|Grand California 2019-24|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|Volkswagen|ID.3 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|Volkswagen|ID.3 2024-25|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|Volkswagen|ID.4 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|Volkswagen|ID.4 2024-25|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|Volkswagen|ID.5 2022-23|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|Volkswagen|Jetta 2015-18|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|Volkswagen|Jetta 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|Volkswagen|Jetta GLI 2021-23|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
opendbc_repo/opendbc/car/ford/values.py
+1 -5
View File
@@ -132,11 +132,7 @@ class FordLKASteeringPlatformConfig(FordPlatformConfig):
@dataclass
class FordF150LightningPlatform(FordCANFDPlatformConfig):
def init(self):
super().init()
# Don't show in docs until this issue is resolved. See https://github.com/commaai/openpilot/issues/30302
self.car_docs = []
pass
MY_2020, MY_2021, MY_2022, MY_2023, MY_2024, MY_2025 = 'L', 'M', 'N', 'P', 'R', 'S'
opendbc_repo/opendbc/car/fw_query_definitions.py
+5
View File
@@ -110,8 +110,13 @@ class FwQueryConfig:
# Function a brand can implement to provide better fuzzy matching. Takes in FW versions and VIN,
# returns set of candidates. Only will match if one candidate is returned
match_fw_to_car_fuzzy: Callable[[LiveFwVersions, str, OfflineFwVersions], set[str]] | None = None
# Platforms whose shared firmware must be disambiguated by the brand fuzzy matcher.
fuzzy_only_platforms: set[str] = field(default_factory=set)
def __post_init__(self):
assert not self.fuzzy_only_platforms or self.match_fw_to_car_fuzzy is not None, \
"Fuzzy-only platforms require a brand fuzzy matcher"
# Asserts that a request exists if extra ecus are used
if len(self.extra_ecus):
assert len(self.requests), "Must define a request with extra ecus"
opendbc_repo/opendbc/car/fw_versions.py
+2 -1
View File
@@ -111,7 +111,8 @@ def match_fw_to_car_exact(live_fw_versions: LiveFwVersions, match_brand: str = N
invalid = set()
candidates = {c: f for c, f in FW_VERSIONS.items() if
is_brand(MODEL_TO_BRAND[c], match_brand)}
is_brand(MODEL_TO_BRAND[c], match_brand) and
c not in FW_QUERY_CONFIGS[MODEL_TO_BRAND[c]].fuzzy_only_platforms}
for candidate, fws in candidates.items():
config = FW_QUERY_CONFIGS[MODEL_TO_BRAND[candidate]]
opendbc_repo/opendbc/car/gm/values.py
+2 -6
View File
@@ -214,16 +214,12 @@ class GMPlatformConfig(PlatformConfig):
@dataclass
class GMASCMPlatformConfig(GMPlatformConfig):
def init(self):
# ASCM is supported, but due to a janky install and hardware configuration, we are not showing in the car docs
self.car_docs = []
pass
@dataclass
class GMSDGMPlatformConfig(GMPlatformConfig):
def init(self):
# Don't show in docs until the harness is sold. See https://github.com/commaai/openpilot/issues/32471
self.car_docs = []
pass
class CAR(Platforms):
opendbc_repo/opendbc/car/lateral.py
+30 -1
View File
@@ -1,7 +1,7 @@
import math
import numpy as np
from dataclasses import dataclass
from opendbc.car import structs, rate_limit, DT_CTRL
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, structs, rate_limit, DT_CTRL
from opendbc.car.vehicle_model import VehicleModel
FRICTION_THRESHOLD = 0.3
@@ -10,6 +10,12 @@ FRICTION_THRESHOLD = 0.3
ISO_LATERAL_ACCEL = 3.0 # m/s^2
ISO_LATERAL_JERK = 5.0 # m/s^3
# Common angle/curvature safety limits. The road-roll allowance keeps the
# controller and panda limits aligned on normally banked roads.
AVERAGE_ROAD_ROLL = 0.06
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)
@dataclass
class AngleSteeringLimits:
@@ -24,6 +30,29 @@ class AngleSteeringLimits:
MAX_ANGLE_RATE: float = math.inf
@dataclass
class CurvatureSteeringLimits:
CURVATURE_MAX: float
MAX_LATERAL_ACCEL: float = MAX_LATERAL_ACCEL
MAX_LATERAL_JERK: float = MAX_LATERAL_JERK
def apply_limits(self, apply_curvature: float, apply_curvature_last: float, v_ego: float, curvature: float,
lat_active: bool, steer_step: int) -> float:
"""Apply lateral acceleration and jerk constraints to curvature."""
v_ego = max(v_ego, 1)
max_curvature = self.MAX_LATERAL_ACCEL / (v_ego ** 2)
new_apply_curvature = float(np.clip(apply_curvature, -max_curvature, max_curvature))
max_jerk = (self.MAX_LATERAL_JERK / (v_ego ** 2)) * (steer_step * DT_CTRL)
new_apply_curvature = float(np.clip(new_apply_curvature, apply_curvature_last - max_jerk, apply_curvature_last + max_jerk))
if not lat_active:
new_apply_curvature = curvature
return float(np.clip(new_apply_curvature, -self.CURVATURE_MAX, self.CURVATURE_MAX))
def apply_driver_steer_torque_limits(apply_torque: int, apply_torque_last: int, driver_torque: float, LIMITS, steer_max: int = None):
# some safety modes utilize a dynamic max steer
if steer_max is None:
opendbc_repo/opendbc/car/tests/routes.py
+9
View File
@@ -85,6 +85,12 @@ non_tested_cars = [
TESLA.TESLA_MODEL_S_PREAP,
TOYOTA.TOYOTA_MATRIX_RETROFIT,
VOLKSWAGEN.VOLKSWAGEN_CRAFTER_MK2, # need a route from an ACC-equipped Crafter
VOLKSWAGEN.VOLKSWAGEN_ID3_MK1,
VOLKSWAGEN.VOLKSWAGEN_ID3_MK2,
VOLKSWAGEN.AUDI_Q4_MK1,
VOLKSWAGEN.AUDI_Q4_MK2,
VOLKSWAGEN.SKODA_ENYAQ_MK1,
VOLKSWAGEN.SKODA_ENYAQ_MK2,
SUBARU.SUBARU_FORESTER_HYBRID,
SUBARU.SUBARU_CROSSTREK_2025,
VOLKSWAGEN.PORSCHE_MACAN_MK1,
@@ -348,6 +354,8 @@ routes = [
CarTestRoute("202c40641158a6e5/2021-09-21--09-43-24", VOLKSWAGEN.VOLKSWAGEN_ARTEON_MK1),
CarTestRoute("2c68dda277d887ac/2021-05-11--15-22-20", VOLKSWAGEN.VOLKSWAGEN_ATLAS_MK1),
CarTestRoute("ffcd23abbbd02219/2024-02-28--14-59-38", VOLKSWAGEN.VOLKSWAGEN_CADDY_MK3),
CarTestRoute("aebd8f1d4ea16066/00000009--b31e222338", VOLKSWAGEN.VOLKSWAGEN_ID4_MK1),
CarTestRoute("f73c01590368ee5b/00000aea--dc31ef6d5f", VOLKSWAGEN.VOLKSWAGEN_ID4_MK2),
CarTestRoute("cae14e88932eb364/2021-03-26--14-43-28", VOLKSWAGEN.VOLKSWAGEN_GOLF_MK7), # Stock ACC
CarTestRoute("3cfdec54aa035f3f/2022-10-13--14-58-58", VOLKSWAGEN.VOLKSWAGEN_GOLF_MK7), # openpilot longitudinal
CarTestRoute("578742b26807f756|00000010--41ee3e5bec", VOLKSWAGEN.VOLKSWAGEN_JETTA_MK6),
@@ -367,6 +375,7 @@ routes = [
CarTestRoute("0cd0b7f7e31a3853/2021-12-03--03-12-05", VOLKSWAGEN.AUDI_Q3_MK2),
CarTestRoute("8f205bdd11bcbb65/2021-03-26--01-00-17", VOLKSWAGEN.SEAT_ATECA_MK1),
CarTestRoute("fc6b6c9a3471c846/2021-05-27--13-39-56", VOLKSWAGEN.SEAT_ATECA_MK1), # Leon
CarTestRoute("d4dd69160a48f11f/00000003--9cfe00cb74", VOLKSWAGEN.CUPRA_BORN_MK1),
CarTestRoute("0bbe367c98fa1538/2023-03-04--17-46-11", VOLKSWAGEN.SKODA_FABIA_MK4),
CarTestRoute("12d6ae3057c04b0d/2021-09-15--00-04-07", VOLKSWAGEN.SKODA_KAMIQ_MK1),
CarTestRoute("12d6ae3057c04b0d/2021-09-04--21-21-21", VOLKSWAGEN.SKODA_KAROQ_MK1),
opendbc_repo/opendbc/car/tests/test_docs.py
+20 -2
View File
@@ -5,9 +5,20 @@ from opendbc.car.car_helpers import interfaces
from opendbc.car.docs import get_all_car_docs
from opendbc.car.docs_definitions import Cable, Column, PartType, Star, SupportType
from opendbc.car.honda.values import CAR as HONDA
from opendbc.car.mock.values import CAR as MOCK
from opendbc.car.values import PLATFORMS
# These platform IDs use different messages or DBCs, but are represented by the
# same customer-facing model rows as their base platforms.
DOCUMENTED_PLATFORM_ALIASES = {
HONDA.HONDA_CIVIC_BOSCH_DIESEL: HONDA.HONDA_CIVIC_BOSCH,
HONDA.HONDA_CRV_EU: HONDA.HONDA_CRV,
HONDA.HONDA_CRV_SA: HONDA.HONDA_CRV,
HONDA.HONDA_E_ADVANCE: HONDA.HONDA_E,
}
class TestCarDocs:
@classmethod
def setup_class(cls):
@@ -26,10 +37,17 @@ class TestCarDocs:
make_model_years[make_model].append(year)
def test_missing_car_docs(self, subtests):
all_car_docs_platforms = [name for name, config in PLATFORMS.items()]
documented_platforms = {car.car_fingerprint for car in self.all_cars}
for platform in sorted(interfaces.keys()):
with subtests.test(platform=platform):
assert platform in all_car_docs_platforms, f"Platform: {platform} doesn't have a CarDocs entry"
assert platform in PLATFORMS, f"Platform: {platform} isn't registered"
if platform in DOCUMENTED_PLATFORM_ALIASES:
assert DOCUMENTED_PLATFORM_ALIASES[platform] in documented_platforms, \
f"Platform: {platform} has no documented base platform"
continue
if platform == MOCK.MOCK:
continue
assert platform in documented_platforms, f"Platform: {platform} doesn't have a generated CarDocs entry"
def test_naming_conventions(self, subtests):
# Asserts market-standard car naming conventions by brand
opendbc_repo/opendbc/car/tests/test_fw_fingerprint.py
+3
View File
@@ -32,6 +32,9 @@ class TestFwFingerprint:
[(b, c, e[c], n) for b, e in VERSIONS.items() for c in e for n in (True, False)])
def test_exact_match(self, brand, car_model, ecus, test_non_essential):
config = FW_QUERY_CONFIGS[brand]
if car_model in config.fuzzy_only_platforms:
pytest.skip("Platform requires VIN-aware fuzzy matching")
CP = CarParams()
for _ in range(20):
fw = []
opendbc_repo/opendbc/car/torque_data/override.toml
+9
View File
@@ -28,6 +28,15 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
"TESLA_MODEL_X" = [nan, 2.5, nan]
# Guess
"VOLKSWAGEN_ID3_MK1" = [nan, 2.5, nan]
"VOLKSWAGEN_ID3_MK2" = [nan, 2.5, nan]
"VOLKSWAGEN_ID4_MK1" = [nan, 2.5, nan]
"VOLKSWAGEN_ID4_MK2" = [nan, 2.5, nan]
"AUDI_Q4_MK1" = [nan, 2.5, nan]
"AUDI_Q4_MK2" = [nan, 2.5, nan]
"CUPRA_BORN_MK1" = [nan, 2.5, nan]
"SKODA_ENYAQ_MK1" = [nan, 2.5, nan]
"SKODA_ENYAQ_MK2" = [nan, 2.5, nan]
"FORD_BRONCO_SPORT_MK1" = [nan, 1.5, nan]
"FORD_ESCAPE_MK4" = [nan, 1.5, nan]
"FORD_ESCAPE_MK4_5" = [nan, 1.5, nan]
opendbc_repo/opendbc/car/volkswagen/carcontroller.py
+107 -42
View File
@@ -4,7 +4,7 @@ from opendbc.car import Bus, DT_CTRL, structs
from opendbc.car.lateral import apply_driver_steer_torque_limits
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.interfaces import CarControllerBase
from opendbc.car.volkswagen import mlbcan, mqbcan, pqcan
from opendbc.car.volkswagen import mebcan, mlbcan, mqbcan, pqcan
from opendbc.car.volkswagen.values import CanBus, CarControllerParams, VolkswagenFlags
VisualAlert = structs.CarControl.HUDControl.VisualAlert
@@ -19,6 +19,9 @@ class CarController(CarControllerBase):
self.packer_pt = CANPacker(dbc_names[Bus.pt])
self.aeb_available = not CP.flags & VolkswagenFlags.PQ
if CP.flags & VolkswagenFlags.MEB:
self.meb_long_state = mebcan.MebLongStateMachine(self.CP, self.CCP)
if CP.flags & VolkswagenFlags.PQ:
self.CCS = pqcan
elif CP.flags & VolkswagenFlags.MLB:
@@ -27,6 +30,11 @@ class CarController(CarControllerBase):
self.CCS = mqbcan
self.apply_torque_last = 0
self.apply_curvature_last = 0.
self.steering_power_last = 0
self.accel_last = 0.
self.lead_distance_bars_last = None
self.distance_bar_frame = 0
self.gra_acc_counter_last = None
self.eps_timer_soft_disable_alert = False
self.hca_frame_timer_running = 0
@@ -40,37 +48,66 @@ class CarController(CarControllerBase):
# **** Steering Controls ************************************************ #
if self.frame % self.CCP.STEER_STEP == 0:
# Logic to avoid HCA state 4 "refused":
# * Don't steer unless HCA is in state 3 "ready" or 5 "active"
# * Don't steer at standstill
# * Don't send > 3.00 Newton-meters torque
# * Don't send the same torque for > 6 seconds
# * Don't send uninterrupted steering for > 360 seconds
# MQB racks reset the uninterrupted steering timer after a single frame
# of HCA disabled; this is done whenever output happens to be zero.
apply_torque = 0
if self.CP.flags & VolkswagenFlags.MEB:
if CC.latActive:
hca_enabled = True
apply_curvature = actuators.curvature + (CS.curvature_meas - CC.currentCurvature)
apply_curvature = self.CCP.CURVATURE_LIMITS.apply_limits(apply_curvature, self.apply_curvature_last, CS.out.vEgoRaw,
CS.curvature_meas, CC.latActive, self.CCP.STEER_STEP)
if CC.latActive:
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:
self.hca_frame_same_torque += self.CCP.STEER_STEP
if self.hca_frame_same_torque > self.CCP.STEER_TIME_STUCK_TORQUE / DT_CTRL:
apply_torque -= (1, -1)[apply_torque < 0]
self.hca_frame_same_torque = 0
min_power = max(self.steering_power_last - self.CCP.STEERING_POWER_STEP, self.CCP.STEERING_POWER_MIN)
max_power = min(self.steering_power_last + self.CCP.STEERING_POWER_STEP, self.CCP.STEERING_POWER_MAX)
target_power_driver = int(np.interp(CS.out.steeringTorque, [self.CCP.STEER_DRIVER_ALLOWANCE, self.CCP.STEER_DRIVER_MAX],
[self.CCP.STEERING_POWER_MAX, self.CCP.STEERING_POWER_MIN]))
target_power = int(np.interp(CS.out.vEgo, [0., 0.5], [self.CCP.STEERING_POWER_MIN, target_power_driver]))
steering_power = min(max(target_power, min_power), max_power)
elif self.steering_power_last > 0:
# Wind steering authority down instead of abruptly faulting the EPS at disengagement.
hca_enabled = True
apply_curvature = float(np.clip(CS.curvature_meas, -self.CCP.CURVATURE_MAX, self.CCP.CURVATURE_MAX))
steering_power = max(self.steering_power_last - self.CCP.STEERING_POWER_STEP, 0)
else:
self.hca_frame_same_torque = 0
hca_enabled = abs(apply_torque) > 0
hca_enabled = False
apply_curvature = 0.
steering_power = 0
can_sends.append(mebcan.create_steering_control(self.packer_pt, self.CAN.pt, apply_curvature, hca_enabled, steering_power))
self.apply_curvature_last = apply_curvature
self.steering_power_last = steering_power
else:
hca_enabled = False
apply_torque = 0
# Logic to avoid HCA state 4 "refused":
# * Don't steer unless HCA is in state 3 "ready" or 5 "active"
# * Don't steer at standstill
# * Don't send > 3.00 Newton-meters torque
# * Don't send the same torque for > 6 seconds
# * Don't send uninterrupted steering for > 360 seconds
# MQB racks reset the uninterrupted steering timer after a single frame
# of HCA disabled; this is done whenever output happens to be zero.
if not hca_enabled:
self.hca_frame_timer_running = 0
if CC.latActive:
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:
self.hca_frame_same_torque += self.CCP.STEER_STEP
if self.hca_frame_same_torque > self.CCP.STEER_TIME_STUCK_TORQUE / DT_CTRL:
apply_torque -= (1, -1)[apply_torque < 0]
self.hca_frame_same_torque = 0
else:
self.hca_frame_same_torque = 0
hca_enabled = abs(apply_torque) > 0
else:
hca_enabled = False
apply_torque = 0
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))
if not hca_enabled:
self.hca_frame_timer_running = 0
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))
if self.CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT:
# Pacify VW Emergency Assist driver inactivity detection by changing its view of driver steering input torque
@@ -81,16 +118,29 @@ class CarController(CarControllerBase):
ea_simulated_torque = CS.out.steeringTorque
can_sends.append(self.CCS.create_eps_update(self.packer_pt, self.CAN.cam, CS.eps_stock_values, ea_simulated_torque))
if self.CP.flags & VolkswagenFlags.MEB and self.CP.flags & VolkswagenFlags.STOCK_KLR_PRESENT:
if self.frame % self.CCP.KLR_01_STEP == 0:
can_sends.append(mebcan.create_capacitive_wheel_touch(self.packer_pt, self.CAN.cam, CC.latActive, CS.klr_stock_values))
can_sends.append(mebcan.create_capacitive_wheel_touch(self.packer_pt, self.CAN.pt, CC.latActive, CS.klr_stock_values))
# **** Acceleration Controls ******************************************** #
if self.CP.openpilotLongitudinalControl:
if self.frame % self.CCP.ACC_CONTROL_STEP == 0:
acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
accel = float(np.clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0)
stopping = actuators.longControlState == LongCtrlState.stopping
starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < starpilot_toggles.vEgoStopping)
can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, self.CAN.pt, CS.acc_type, CC.longActive, accel,
acc_control, stopping, starting, CS.esp_hold_confirmation))
if self.CP.flags & VolkswagenFlags.MEB:
accel = float(np.clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX))
accel, acc_status, acc_hold_type, braking_to_stop = self.meb_long_state.update(CS, CC, accel)
can_sends.extend(mebcan.create_acc_accel_control(self.packer_pt, self.CAN.pt, self.CCP, CS.acc_type, CC.enabled,
accel, acc_status, acc_hold_type, braking_to_stop,
CS.out.vEgoRaw * CV.MS_TO_KPH, CS.travel_assist_available))
else:
acc_control = self.CCS.acc_control_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
accel = float(np.clip(actuators.accel, self.CCP.ACCEL_MIN, self.CCP.ACCEL_MAX) if CC.longActive else 0)
stopping = actuators.longControlState == LongCtrlState.stopping
starting = actuators.longControlState == LongCtrlState.pid and (CS.esp_hold_confirmation or CS.out.vEgo < starpilot_toggles.vEgoStopping)
can_sends.extend(self.CCS.create_acc_accel_control(self.packer_pt, self.CAN.pt, CS.acc_type, CC.longActive, accel,
acc_control, stopping, starting, CS.esp_hold_confirmation))
self.accel_last = accel
#if self.aeb_available:
# if self.frame % self.CCP.AEB_CONTROL_STEP == 0:
@@ -107,16 +157,27 @@ class CarController(CarControllerBase):
can_sends.append(self.CCS.create_lka_hud_control(self.packer_pt, self.CAN.pt, CS.ldw_stock_values, CC.latActive,
CS.out.steeringPressed, hud_alert, hud_control))
if hud_control.leadDistanceBars != self.lead_distance_bars_last:
self.distance_bar_frame = self.frame
if self.frame % self.CCP.ACC_HUD_STEP == 0 and self.CP.openpilotLongitudinalControl:
lead_distance = 0
if hud_control.leadVisible and self.frame * DT_CTRL > 1.0: # Don't display lead until we know the scaling factor
lead_distance = 512 if CS.upscale_lead_car_signal else 8
acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
# FIXME: PQ may need to use the on-the-wire mph/kmh toggle to fix rounding errors
# FIXME: Detect clusters with vEgoCluster offsets and apply an identical vCruiseCluster offset
set_speed = hud_control.setSpeed * CV.MS_TO_KPH
can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, self.CAN.pt, acc_hud_status, set_speed,
lead_distance, hud_control.leadDistanceBars))
if self.CP.flags & VolkswagenFlags.MEB:
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
show_distance_bars = self.frame - self.distance_bar_frame < 400
lead_distance = 8 if hud_control.leadVisible and self.frame * DT_CTRL > 1.0 else 0
can_sends.append(mebcan.create_acc_hud_control(self.packer_pt, self.CAN.pt, self.meb_long_state.acc_status,
hud_control.setSpeed * CV.MS_TO_KPH, hud_control.leadVisible,
hud_control.leadDistanceBars, show_distance_bars, lead_distance, fcw_alert))
else:
lead_distance = 0
if hud_control.leadVisible and self.frame * DT_CTRL > 1.0: # Don't display lead until we know the scaling factor
lead_distance = 512 if CS.upscale_lead_car_signal else 8
acc_hud_status = self.CCS.acc_hud_status_value(CS.out.cruiseState.available, CS.out.accFaulted, CC.longActive)
# FIXME: PQ may need to use the on-the-wire mph/kmh toggle to fix rounding errors
# FIXME: Detect clusters with vEgoCluster offsets and apply an identical vCruiseCluster offset
set_speed = hud_control.setSpeed * CV.MS_TO_KPH
can_sends.append(self.CCS.create_acc_hud_control(self.packer_pt, self.CAN.pt, acc_hud_status, set_speed,
lead_distance, hud_control.leadDistanceBars))
# **** Stock ACC Button Controls **************************************** #
@@ -128,7 +189,11 @@ class CarController(CarControllerBase):
new_actuators = actuators.as_builder()
new_actuators.torque = self.apply_torque_last / self.CCP.STEER_MAX
new_actuators.torqueOutputCan = self.apply_torque_last
if self.CP.flags & VolkswagenFlags.MEB:
new_actuators.curvature = self.apply_curvature_last
new_actuators.accel = self.accel_last
self.lead_distance_bars_last = hud_control.leadDistanceBars
self.gra_acc_counter_last = CS.gra_stock_values["COUNTER"]
self.frame += 1
return new_actuators, can_sends
opendbc_repo/opendbc/car/volkswagen/carstate.py
+120
View File
@@ -14,12 +14,16 @@ class CarState(CarStateBase):
super().__init__(CP, FPCP)
self.frame = 0
self.eps_init_complete = False
self.tsk_recovery_timer = 0
self.CCP = CarControllerParams(CP)
self.button_states = {button.event_type: False for button in self.CCP.BUTTONS}
self.esp_hold_confirmation = False
self.upscale_lead_car_signal = False
self.eps_stock_values = False
self.acc_type = 0
self.travel_assist_available = False
self.curvature_meas = 0.
self.klr_stock_values = {}
def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]):
if not self.CP.pcmCruise:
@@ -52,6 +56,8 @@ class CarState(CarStateBase):
return self.update_pq(pt_cp, cam_cp, ext_cp)
elif self.CP.flags & VolkswagenFlags.MLB:
return self.update_mlb(pt_cp, cam_cp, ext_cp)
elif self.CP.flags & VolkswagenFlags.MEB:
return self.update_meb(pt_cp, cam_cp, ext_cp)
ret = structs.CarState()
@@ -143,6 +149,93 @@ class CarState(CarStateBase):
return ret, fp_ret
def update_meb(self, pt_cp, cam_cp, ext_cp) -> structs.CarState:
ret = structs.CarState()
self.parse_wheel_speeds(ret,
pt_cp.vl["ESC_51"]["VL_Radgeschw"],
pt_cp.vl["ESC_51"]["VR_Radgeschw"],
pt_cp.vl["ESC_51"]["HL_Radgeschw"],
pt_cp.vl["ESC_51"]["HR_Radgeschw"],
)
ret.standstill = ret.vEgoRaw == 0
ret.steeringAngleDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradwinkel"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradwinkel"])]
ret.steeringRateDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradw_Geschw"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradw_Geschw"])]
ret.steeringTorque = pt_cp.vl["LH_EPS_03"]["EPS_Lenkmoment"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_Lenkmoment"])]
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE, 5)
self.curvature_meas = -pt_cp.vl["QFK_01"]["Curvature"] * (1, -1)[int(pt_cp.vl["QFK_01"]["Curvature_VZ"])]
ret.yawRate = -pt_cp.vl["ESC_50"]["Yaw_Rate"] * (1, -1)[int(pt_cp.vl["ESC_50"]["Yaw_Rate_Sign"])] * CV.DEG_TO_RAD
if self.CP.flags & VolkswagenFlags.ALT_GEAR:
ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Gateway_73"]["GE_Fahrstufe"], None))
else:
ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Getriebe_11"]["GE_Fahrstufe"], None))
in_drive = ret.gearShifter == GearShifter.drive
hca_status = self.CCP.hca_status_values.get(pt_cp.vl["QFK_01"]["LatCon_HCA_Status"])
ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status, in_drive)
ret.carFaultedNonCritical = cam_cp.vl["EA_01"]["EA_Funktionsstatus"] in (3, 4, 5, 6)
ret.gasPressed = pt_cp.vl["Motor_51"]["Accel_Pedal_Pressure"] > 0
ret.brakePressed = bool(pt_cp.vl["Motor_14"]["MO_Fahrer_bremst"])
ret.parkingBrake = pt_cp.vl["ESC_50"]["EPB_Status"] in (1, 4)
ret.seatbeltUnlatched = pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] != 3
doors = pt_cp.vl["ZV_02"] if bool(pt_cp.vl["Gateway_72"]["ZV_02_alt"]) else pt_cp.vl["Gateway_72"]
ret.doorOpen = any([doors["ZV_FT_offen"], doors["ZV_BT_offen"], doors["ZV_HFS_offen"],
doors["ZV_HBFS_offen"], doors["ZV_HD_offen"]])
ret.espDisabled = bool(pt_cp.vl["ESP_21"]["ESP_Tastung_passiv"])
ret.espActive = bool(pt_cp.vl["ESP_21"]["ESP_Eingriff"])
self.acc_type = ext_cp.vl["ACC_18"]["ACC_Typ"]
self.esp_hold_confirmation = bool(pt_cp.vl["ESC_50"]["Standstill"])
self.travel_assist_available = bool(cam_cp.vl["TA_01"]["Travel_Assist_Available"])
ret.stockFcw = bool(ext_cp.vl["AWV_03"]["FCW_Active"])
ret.stockAeb = bool(ext_cp.vl["AWV_03"]["AEB_Active"])
ret.cruiseState.available = pt_cp.vl["Motor_51"]["TSK_Status"] in (2, 3, 4, 5)
ret.cruiseState.enabled = pt_cp.vl["Motor_51"]["TSK_Status"] in (3, 4, 5)
ret.cruiseState.standstill = self.CP.pcmCruise and self.esp_hold_confirmation
if self.CP.pcmCruise:
ret.cruiseState.nonAdaptive = bool(ext_cp.vl["ACC_19"]["ACC_Limiter_Mode"])
ret.cruiseState.speed = ext_cp.vl["ACC_19"]["ACC_Wunschgeschw_02"] * CV.KPH_TO_MS
if ret.cruiseState.speed > 90:
ret.cruiseState.speed = 0
else:
ret.cruiseState.nonAdaptive = bool(pt_cp.vl["Motor_51"]["TSK_Limiter_ausgewaehlt"])
tsk_faulted = pt_cp.vl["Motor_51"]["TSK_Status"] in (6, 7)
engine_off = pt_cp.vl["Motor_54"]["Engine_On"] == 0
long_control_inhibit = pt_cp.vl["VMM_02"]["Long_Control_Inhibit"] == 2
ret.accFaulted = (self.update_acc_fault(tsk_faulted, engine_off, long_control_inhibit) or
ext_cp.vl["ACC_18"]["ACC_Status_ACC"] == 6)
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(240, pt_cp.vl["SMLS_01"]["BH_Blinker_li"],
pt_cp.vl["SMLS_01"]["BH_Blinker_re"])
if self.CP.enableBsm:
if self.CP.flags & VolkswagenFlags.MEB_GEN2:
ret.leftBlindspot = (bool(pt_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Info_Driver"]) or
bool(pt_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Warn_Driver"]))
ret.rightBlindspot = (bool(pt_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Info_Passenger"]) or
bool(pt_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Warn_Passenger"]))
else:
ret.leftBlindspot = (bool(ext_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Info_Left"]) or
bool(ext_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Warn_Left"]))
ret.rightBlindspot = (bool(ext_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Info_Right"]) or
bool(ext_cp.vl["MEB_Side_Assist_01"]["Blind_Spot_Warn_Right"]))
self.eps_stock_values = pt_cp.vl["LH_EPS_03"]
self.ldw_stock_values = cam_cp.vl["LDW_02"] if self.CP.networkLocation == NetworkLocation.fwdCamera else {}
self.gra_stock_values = pt_cp.vl["GRA_ACC_01"]
self.klr_stock_values = pt_cp.vl["KLR_01"] if self.CP.flags & VolkswagenFlags.STOCK_KLR_PRESENT else {}
ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS)
ret.lowSpeedAlert = self.update_low_speed_alert(ret.vEgo)
self.frame += 1
return ret, custom.StarPilotCarState.new_message()
def update_pq(self, pt_cp, cam_cp, ext_cp) -> structs.CarState:
ret = structs.CarState()
@@ -318,10 +411,19 @@ class CarState(CarStateBase):
temp_fault = drive_mode and hca_status in ("REJECTED", "PREEMPTED") or not self.eps_init_complete
return temp_fault, perm_fault
def update_acc_fault(self, acc_fault, engine_off, long_inhibit, recovery_frames=10):
# MEB briefly reports a drivetrain coordinator fault while the car powers down
# or after hard braking. Ignore only the short trailing state from those events.
if engine_off or long_inhibit:
self.tsk_recovery_timer = self.frame
return acc_fault and self.frame - self.tsk_recovery_timer >= recovery_frames
@staticmethod
def get_can_parsers(CP):
if CP.flags & VolkswagenFlags.PQ:
return CarState.get_can_parsers_pq(CP)
elif CP.flags & VolkswagenFlags.MEB:
return CarState.get_can_parsers_meb(CP)
# manually configure some optional and variable-rate/edge-triggered messages
pt_messages, cam_messages = [], []
@@ -346,3 +448,21 @@ class CarState(CarStateBase):
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).pt),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).cam),
}
@staticmethod
def get_can_parsers_meb(CP):
pt_messages = [
("Blinkmodi_02", 1),
("SMLS_01", 1),
]
if CP.networkLocation == NetworkLocation.fwdCamera:
pt_messages.append(("AWV_03", 1))
cam_messages = []
if CP.networkLocation == NetworkLocation.gateway:
cam_messages.append(("AWV_03", 1))
return {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, CanBus(CP).pt),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CanBus(CP).cam),
}
opendbc_repo/opendbc/car/volkswagen/fingerprints.py
+65
View File
@@ -8,6 +8,71 @@ Ecu = CarParams.Ecu
FW_VERSIONS = {
CAR.VOLKSWAGEN_ID3_MK1: {
(Ecu.srs, 0x715, None): [
b'\xf1\x871EA959655CD\xf1\x890366',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x871EA907572H \xf1\x890234',
],
},
CAR.VOLKSWAGEN_ID3_MK2: {
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x871EA907567D \xf1\x890250',
],
},
CAR.VOLKSWAGEN_ID4_MK1: {
(Ecu.srs, 0x715, None): [
b'\xf1\x871EA959655EA\xf1\x890376',
b'\xf1\x875WA959655R \xf1\x890717',
],
(Ecu.eps, 0x712, None): [
b'\xf1\x871EA907144AQ\xf1\x895033\xf1\x82\x000_BH0A0_ON',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x871EA907572H \xf1\x890234',
],
},
CAR.VOLKSWAGEN_ID4_MK2: {
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x871EA907567D \xf1\x890250',
b'\xf1\x871EA907567C \xf1\x890099',
],
},
CAR.AUDI_Q4_MK1: {
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x871EA907572H \xf1\x890234',
],
},
CAR.AUDI_Q4_MK2: {
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x871EA907567B \xf1\x890232',
],
},
CAR.CUPRA_BORN_MK1: {
(Ecu.srs, 0x715, None): [
b'\xf1\x871EA959655EH\xf1\x890381',
],
(Ecu.eps, 0x712, None): [
b'\xf1\x871EA907144AQ\xf1\x895033',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x871EA907572H \xf1\x890234',
],
},
CAR.SKODA_ENYAQ_MK1: {
(Ecu.srs, 0x715, None): [
b'\xf1\x871EA959655EA\xf1\x890376',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x871EA907572H \xf1\x890234',
],
},
CAR.SKODA_ENYAQ_MK2: {
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x871EA907567B \xf1\x890232',
],
},
CAR.VOLKSWAGEN_ARTEON_MK1: {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8704L906026TM\xf1\x896847',
opendbc_repo/opendbc/car/volkswagen/interface.py
+44 -4
View File
@@ -1,13 +1,15 @@
from opendbc.car import get_safety_config, structs
from opendbc.car import Bus, get_safety_config, structs
from opendbc.car.interfaces import CarInterfaceBase
from opendbc.car.volkswagen.carcontroller import CarController
from opendbc.car.volkswagen.carstate import CarState
from opendbc.car.volkswagen.values import CanBus, CAR, NetworkLocation, TransmissionType, VolkswagenFlags, VolkswagenSafetyFlags
from opendbc.car.volkswagen.radar_interface import RadarInterface
from opendbc.car.volkswagen.values import CanBus, CAR, DBC, NetworkLocation, TransmissionType, VolkswagenFlags, VolkswagenSafetyFlags
class CarInterface(CarInterfaceBase):
CarState = CarState
CarController = CarController
RadarInterface = RadarInterface
@staticmethod
def _get_params(ret: structs.CarParams, candidate: CAR, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
@@ -44,6 +46,37 @@ class CarInterface(CarInterfaceBase):
ret.networkLocation = NetworkLocation.gateway
ret.dashcamOnly = True # Pending HCA timeout fix, safety validation, harness termination, install procedure
elif ret.flags & VolkswagenFlags.MEB:
safety_configs = [get_safety_config(structs.CarParams.SafetyModel.volkswagenMeb)]
if ret.flags & VolkswagenFlags.MEB_GEN2:
safety_configs[0].safetyParam |= VolkswagenSafetyFlags.MEB_ALT_CRC.value
ret.transmissionType = TransmissionType.direct
ret.steerControlType = structs.CarParams.SteerControlType.curvatureDEPRECATED
ret.steerAtStandstill = True
ret.lateralTuning.init('pid')
ret.lateralTuning.pid.kpBP = [10., 40.]
ret.lateralTuning.pid.kpV = [0., 1.45]
ret.lateralTuning.pid.kiBP = [10., 40.]
ret.lateralTuning.pid.kiV = [0., 0.12]
ret.lateralTuning.pid.kf = 1.
if any(msg in fingerprint[1] for msg in (0x520, 0x86, 0xFD, 0x13D)):
ret.networkLocation = NetworkLocation.gateway
ret.radarUnavailable = Bus.radar not in DBC[candidate]
else:
ret.networkLocation = NetworkLocation.fwdCamera
ret.enableBsm = 0x24C in fingerprint[0]
if 0x25D in fingerprint[0]:
ret.flags |= VolkswagenFlags.STOCK_KLR_PRESENT.value
if 0x3DC in fingerprint[0]:
ret.flags |= VolkswagenFlags.ALT_GEAR.value
# MEB support requires the J533 gateway harness; camera installations remain passive.
ret.dashcamOnly = ret.networkLocation == NetworkLocation.fwdCamera
else:
# Set global MQB parameters
safety_configs = [get_safety_config(structs.CarParams.SafetyModel.volkswagen)]
@@ -72,6 +105,8 @@ class CarInterface(CarInterfaceBase):
if ret.flags & VolkswagenFlags.PQ or ret.flags & VolkswagenFlags.MLB:
ret.steerActuatorDelay = 0.2
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif ret.flags & VolkswagenFlags.MEB:
ret.steerActuatorDelay = 0.3
else:
ret.steerActuatorDelay = 0.1
ret.lateralTuning.pid.kpBP = [0.]
@@ -82,9 +117,14 @@ class CarInterface(CarInterfaceBase):
# Global longitudinal tuning defaults, can be overridden per-vehicle
if ret.flags & VolkswagenFlags.MEB:
ret.longitudinalActuatorDelay = 0.5
ret.longitudinalTuning.kiBP = [0., 30.]
ret.longitudinalTuning.kiV = [0.4, 0.]
ret.alphaLongitudinalAvailable = ret.networkLocation == NetworkLocation.gateway or docs
if alpha_long:
# Proof-of-concept, prep for E2E only. No radar points available. Panda ALLOW_DEBUG firmware required.
if alpha_long and (not ret.flags & VolkswagenFlags.MEB or ret.alphaLongitudinalAvailable):
# Panda ALLOW_DEBUG firmware is required for Volkswagen longitudinal control.
ret.openpilotLongitudinalControl = True
safety_configs[0].safetyParam |= VolkswagenSafetyFlags.LONG_CONTROL.value
if ret.transmissionType == TransmissionType.manual:
opendbc_repo/opendbc/car/volkswagen/mebcan.py
+277
View File
@@ -0,0 +1,277 @@
from opendbc.car import Bus, structs
from opendbc.can import CANDefine
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.volkswagen.values import DBC
LongCtrlState = structs.CarControl.Actuators.LongControlState
def create_steering_control(packer, bus, apply_curvature, lkas_enabled, power=0):
values = {
"Curvature": abs(apply_curvature), # in rad/m
"Curvature_VZ": 1 if apply_curvature > 0 and lkas_enabled else 0,
"Power": power if lkas_enabled else 0,
"RequestStatus": 4 if lkas_enabled else 2,
"HighSendRate": lkas_enabled,
}
return packer.make_can_msg("HCA_03", bus, values)
def create_eps_update(packer, bus, eps_stock_values, ea_simulated_torque):
values = {s: eps_stock_values[s] for s in [
"COUNTER", # Sync counter value to EPS output
"EPS_Lenkungstyp", # EPS rack type
"EPS_Berechneter_LW", # Absolute raw steering angle
"EPS_VZ_BLW", # Raw steering angle sign
"EPS_HCA_Status", # EPS HCA control status
]}
values.update({
# Absolute driver torque input and sign, with EA inactivity mitigation
"EPS_Lenkmoment": abs(ea_simulated_torque),
"EPS_VZ_Lenkmoment": 1 if ea_simulated_torque < 0 else 0,
})
return packer.make_can_msg("LH_EPS_03", bus, values)
def create_lka_hud_control(packer, bus, ldw_stock_values, lat_active, steering_pressed, hud_alert, hud_control, sound_alert=False):
display_mode = 1 if lat_active else 0 # travel assist style showing yellow lanes when op is active
values = {}
if len(ldw_stock_values):
values = {s: ldw_stock_values[s] for s in [
"LDW_SW_Warnung_links", # Blind spot in warning mode on left side due to lane departure
"LDW_SW_Warnung_rechts", # Blind spot in warning mode on right side due to lane departure
"LDW_Seite_DLCTLC", # Direction of most likely lane departure (left or right)
"LDW_DLC", # Lane departure, distance to line crossing
"LDW_TLC", # Lane departure, time to line crossing
]}
values.update({
"LDW_Gong": sound_alert,
"LDW_Status_LED_gelb": 1 if lat_active and steering_pressed else 0,
"LDW_Status_LED_gruen": 1 if lat_active and not steering_pressed else 0,
"LDW_Lernmodus_links": 3 + display_mode if hud_control.leftLaneDepart else 1 + hud_control.leftLaneVisible + display_mode,
"LDW_Lernmodus_rechts": 3 + display_mode if hud_control.rightLaneDepart else 1 + hud_control.rightLaneVisible + display_mode,
"LDW_Texte": hud_alert,
})
return packer.make_can_msg("LDW_02", bus, values)
def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resume=False, up=False, down=False):
values = {s: gra_stock_values[s] for s in [
"GRA_Hauptschalter", # ACC button, on/off
"GRA_Typ_Hauptschalter", # ACC main button type
"GRA_Codierung", # ACC button configuration/coding
"GRA_Tip_Stufe_2", # unknown related to stalk type
"GRA_ButtonTypeInfo", # unknown related to stalk type
]}
values.update({
"COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
"GRA_Abbrechen": cancel,
"GRA_Tip_Wiederaufnahme": resume or up,
"GRA_Tip_Setzen": down,
})
return packer.make_can_msg("GRA_ACC_01", bus, values)
ACC_HUD_ERROR = 6
ACC_HUD_OVERRIDE = 4
ACC_HUD_ACTIVE = 3
ACC_HUD_ENABLED = 2
ACC_HUD_DISABLED = 0
class MebLongStateMachine:
HOLD_RELEASE_SPEED = 5 * CV.KPH_TO_MS
def __init__(self, CP, CCP):
self.CCP = CCP
self.RAMP_FRAMES = 10 // CCP.ACC_CONTROL_STEP # 100 ms
self.disengage_ramp_counter = 0 # always ramp when disengaging
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
self.acc_status_vals = {v: k for k, v in can_define.dv['ACC_18']['ACC_Status_ACC'].items()}
self.acc_hold_type_vals = {v: k for k, v in can_define.dv['ACC_18']['ACC_Anforderung_HMS'].items()}
self.prev_acc_hold_type = self.acc_hold_type_vals['KEINE_ANFORDERUNG'] # no request
self.acc_status = self.acc_status_vals['ACC_OFF_HAUPTSCHALTER_AUS'] # last acc status, read by HUD msg
def _get_acc_status(self, CS, CC) -> int:
# stateless
# NOTE: stock TSK and camera goes to 5 on disengage independently which we don't model, but hasn't been shown to fault without it
if CS.out.accFaulted:
return self.acc_status_vals['REVERSIBLER_FEHLER_IM_ACC_SYSTEM']
elif CC.enabled:
return self.acc_status_vals['ACC_OVERRIDE' if CC.cruiseControl.override else 'ACC_AKTIV_REGELT']
elif CS.out.cruiseState.available:
return self.acc_status_vals['ACC_STANDBY']
else:
return self.acc_status_vals['ACC_OFF_HAUPTSCHALTER_AUS'] # disabled
def _get_hold_type(self, CS, CC) -> int:
# warning: car is reacting to hold mechanic even with long control off
# HALTEN -> KEINE_ANFORDERUNG causes the car to fault into park, so both branches below put a ramp in
# between: disengaging always ramps, and while engaged a release ramps until 5 kph
# NOTE: this allows KEINE_ANFORDERUNG -> ANFAHREN, but we haven't observed a fault due to this yet
# TODO: camera can send 7 on disengage at a stop which we don't fully understand yet
stopping = CC.actuators.longControlState == LongCtrlState.stopping
starting = CC.actuators.longControlState == LongCtrlState.pid and CS.esp_hold_confirmation
long_active = CC.longActive and not CS.out.accFaulted # catches it one frame earlier, not sure if needed
if not long_active:
# Stock goes to RAMP for as long as TSK_Status is 5 usually, 100ms seems fine to mimic that behavior.
# Stock stays active for gas press, but we go inactive
if self.disengage_ramp_counter > 0:
acc_hold_type = self.acc_hold_type_vals['LOESEN_UEBER_RAMPE'] # ramp
self.disengage_ramp_counter -= 1
else:
acc_hold_type = self.acc_hold_type_vals['KEINE_ANFORDERUNG'] # no request
else:
was_engaged = self.disengage_ramp_counter == self.RAMP_FRAMES
self.disengage_ramp_counter = self.RAMP_FRAMES # prep ramp if we disengage
if stopping:
acc_hold_type = self.acc_hold_type_vals['HALTEN'] # stopping/stopped, allowed at any time
elif starting:
acc_hold_type = self.acc_hold_type_vals['ANFAHREN'] # resume after reaching full stop
else:
# After aborting a stop or finishing starting, we need to send RAMP until we hit 5 kph or go long inactive,
# only if we didn't just re-engage
releasing = was_engaged and self.prev_acc_hold_type in (self.acc_hold_type_vals['HALTEN'],
self.acc_hold_type_vals['ANFAHREN'],
self.acc_hold_type_vals['LOESEN_UEBER_RAMPE'])
if releasing and CS.out.vEgo < self.HOLD_RELEASE_SPEED:
acc_hold_type = self.acc_hold_type_vals['LOESEN_UEBER_RAMPE'] # ramp
else:
acc_hold_type = self.acc_hold_type_vals['KEINE_ANFORDERUNG'] # no request
return acc_hold_type
def update(self, CS, CC, accel) -> tuple[float, int, int, bool]:
acc_status = self._get_acc_status(CS, CC)
acc_hold_type = self._get_hold_type(CS, CC)
# transition to inactive accel and jerks as soon as we enter ESP standstill
requesting_hold = acc_hold_type == self.acc_hold_type_vals['HALTEN']
held = requesting_hold and CS.esp_hold_confirmation
if not CC.enabled or held:
accel = self.CCP.ACCEL_INACTIVE
# hold requested but the car hasn't reached standstill yet
braking_to_stop = requesting_hold and not CS.esp_hold_confirmation
self.prev_acc_hold_type = acc_hold_type
self.acc_status = acc_status
return accel, acc_status, acc_hold_type, braking_to_stop
def create_acc_accel_control(packer, bus, CCP, acc_type, acc_enabled, accel, acc_status, acc_hold_type,
braking_to_stop, speed, travel_assist_available):
# active longitudinal control disables one pedal driving (regen mode) while using overriding mechanism
# error mitigation when stopping or stopped: (newer gen cars can be very sensitive)
# - send 0 m stopping distance for cars in kind of parameterized stopping mode (stopping accel -0.2 seen for those cars)
# -> this mode is seen for different cars with same firmware radars so could be a coded operational mode
# - jerk and control limits values set inactive together when fully stopped
# - set accel to 0 / no stop accel for full stop (seems to be compatible with old (non 0 stop accel) and new gen, because HMS state holds the car anyways)
# - stopping command sent while requesting stop but ESP is not in standstill
commands = []
# ACC_Anhalteweg: when stopping: MEB: values <> 0 the car can execute a hard brake probably if target is too close, MQBEvo: value 0 results in hard brake
terminal_rollout = 0
values = {
"ACC_Typ": acc_type,
"ACC_Status_ACC": acc_status,
"ACC_StartStopp_Info": acc_enabled,
"ACC_Sollbeschleunigung_02": accel,
"ACC_zul_Regelabw_unten": 0,
"ACC_zul_Regelabw_oben": 0,
"ACC_neg_Sollbeschl_Grad_02": CCP.JERK_LIMIT if accel != CCP.ACCEL_INACTIVE else 0,
"ACC_pos_Sollbeschl_Grad_02": CCP.JERK_LIMIT if accel != CCP.ACCEL_INACTIVE else 0,
"ACC_Anfahren": 0, # always zero, stock uses ACC_Anforderung_HMS
"ACC_Anhalten": 1 if braking_to_stop else 0,
"ACC_Anhalteweg": terminal_rollout if braking_to_stop else 20.46,
"ACC_Anforderung_HMS": acc_hold_type,
"ACC_AKTIV_regelt": 0, # always zero, stock uses ACC_Status_ACC
"Speed": speed,
"SET_ME_0XFE": 0xFE,
"SET_ME_0X1": 0x1,
"SET_ME_0X9": 0x9,
}
commands.append(packer.make_can_msg("ACC_18", bus, values))
if travel_assist_available:
# satisfy car to prevent errors when pressing Travel Assist Button
values_ta = {
"Travel_Assist_Status": 4 if acc_enabled else 2,
"Travel_Assist_Request": 0,
"Travel_Assist_Available": 1,
}
commands.append(packer.make_can_msg("TA_01", bus, values_ta))
return commands
def create_acc_hud_control(packer, bus, acc_status, set_speed, lead_visible, distance_bars, show_distance_bars, distance, fcw_alert):
values = {
"ACC_Status_ACC": acc_status,
"ACC_Tempolimit": 0,
"ACC_Wunschgeschw_02": set_speed if set_speed < 250 else 327.36,
"ACC_Gesetzte_Zeitluecke": distance_bars, # 5 distance bars available (3 are used by OP)
"ACC_Display_Prio": 0 if fcw_alert and acc_status in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 1, # probably keeping warning in front
"ACC_Optischer_Fahrerhinweis": 1 if fcw_alert and acc_status in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0, # enables optical warning
"ACC_Akustischer_Fahrerhinweis": 3 if fcw_alert and acc_status in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0, # enables sound warning
"ACC_Texte_Zusatzanz_02": 11 if fcw_alert and acc_status in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0, # type of warning: Break!
"ACC_Abstandsindex_02": 569, # seems to be default for MEB but is not static in every case
"ACC_EGO_Fahrzeug": 2 if fcw_alert and acc_status in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else
(1 if acc_status == ACC_HUD_ACTIVE else 0), # red car warn symbol for fcw
"Lead_Type_Detected": 1 if lead_visible else 0, # object should be displayed
"Lead_Type": 3 if lead_visible else 0, # displaying a car
"Lead_Distance": distance if lead_visible else 0, # hud distance of object
"ACC_Enabled": 1 if acc_status in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0,
"ACC_Standby_Override": 1 if acc_status != ACC_HUD_ACTIVE else 0,
"Street_Color": 1 if acc_status in (ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0, # light grey (1) or dark (0) street
"Lead_Brightness": 3 if acc_status == ACC_HUD_ACTIVE else 0, # object shows in color
# TODO: a nice speed dependent bar distance
"Zeitluecke_1": 0, # desired distance to lead object for distance bar 1
"Zeitluecke_2": 0, # desired distance to lead object for distance bar 2
"Zeitluecke_3": 0, # desired distance to lead object for distance bar 3
"Zeitluecke_4": 0, # desired distance to lead object for distance bar 4
"Zeitluecke_5": 0, # desired distance to lead object for distance bar 5
"Zeitluecke_Farbe": 1 if acc_status in (ACC_HUD_ENABLED, ACC_HUD_ACTIVE, ACC_HUD_OVERRIDE) else 0, # yellow (1) or white (0) time gap
"ACC_Anzeige_Zeitluecke": show_distance_bars if acc_status != ACC_HUD_DISABLED else 0, # show distance bar selection
"SET_ME_0X1": 0x1, # unknown
"SET_ME_0X6A": 0x6A, # unknown
"SET_ME_0XFFFF": 0xFFFF, # unknown
"SET_ME_0X7FFF": 0x7FFF, # unknown
}
return packer.make_can_msg("ACC_19", bus, values)
def create_capacitive_wheel_touch(packer, bus, lat_active, klr_stock_values):
values = {s: klr_stock_values[s] for s in [
"COUNTER",
"KLR_Touchintensitaet_1",
"KLR_Touchintensitaet_2",
"KLR_Touchintensitaet_3",
"KLR_Touchauswertung",
]}
if lat_active:
values.update({
"COUNTER": (klr_stock_values["COUNTER"] + 1) % 16,
"KLR_Touchintensitaet_1": 80,
"KLR_Touchintensitaet_2": 200,
"KLR_Touchintensitaet_3": 10,
"KLR_Touchauswertung": 10,
})
return packer.make_can_msg("KLR_01", bus, values)
opendbc_repo/opendbc/car/volkswagen/radar_interface.py
+85
View File
@@ -0,0 +1,85 @@
from opendbc.can import CANParser
from opendbc.car import Bus, structs
from opendbc.car.interfaces import RadarInterfaceBase
from opendbc.car.volkswagen.values import DBC, VolkswagenFlags, CanBus
NO_OBJECT_ID = 0
LANE_TYPES = ("Same_Lane", "Left_Lane", "Right_Lane")
SIGNAL_SETS = tuple(
(
f"{prefix}_ObjectID",
f"{prefix}_Long_Distance",
f"{prefix}_Lat_Distance",
f"{prefix}_Rel_Velo",
)
for lane in LANE_TYPES
for idx in (1, 2)
for prefix in (f"{lane}_0{idx}",)
)
class RadarInterface(RadarInterfaceBase):
def __init__(self, CP):
super().__init__(CP)
# With the MEB gateway harness, we do not have access to the raw points from the radar.
# However, the camera publishes decent, albeit filtered, tracks. Two for each lane; left, center, and right.
self.rcp: CANParser | None = None
if CP.flags & VolkswagenFlags.MEB and not self.CP.radarUnavailable:
self.rcp = CANParser(DBC[CP.carFingerprint][Bus.radar], [("MEB_Distance_01", 25)], CanBus(CP).cam)
def update(self, can_strings):
if self.rcp is None:
return super().update(None)
self.rcp.update(can_strings)
if len(self.rcp.vl_all["MEB_Distance_01"]["Distance_Status"]) == 0:
return None
return self._update()
def _update(self):
ret = structs.RadarData()
if not self.rcp.can_valid:
ret.errors.canError = True
return ret
msg = self.rcp.vl["MEB_Distance_01"]
# Can be 3 when radar sensor is obstructed
if msg["Distance_Status"] != 0:
ret.errors.radarUnavailableTemporary = True
seen_ids = set()
for obj_id_sig, long_sig, lat_sig, vel_sig in SIGNAL_SETS:
obj_id = int(msg[obj_id_sig])
if obj_id == NO_OBJECT_ID:
continue
# We shouldn't see duplicate track ids
if obj_id in seen_ids:
ret.errors.radarFault = True
return ret
seen_ids.add(obj_id)
if obj_id not in self.pts:
pt = structs.RadarData.RadarPoint()
pt.trackId = self.track_id
self.track_id += 1
self.pts[obj_id] = pt
else:
pt = self.pts[obj_id]
pt.dRel = msg[long_sig]
pt.yRel = msg[lat_sig]
pt.vRel = msg[vel_sig]
inactive_ids = self.pts.keys() - seen_ids
for obj_id in inactive_ids:
self.pts.pop(obj_id, None)
ret.points = list(self.pts.values())
return ret
opendbc_repo/opendbc/car/volkswagen/tests/test_volkswagen.py
+61 -16
View File
@@ -3,7 +3,7 @@ import re
from opendbc.car.structs import CarParams
from opendbc.car.volkswagen.interface import CarInterface
from opendbc.car.volkswagen.values import CAR, FW_QUERY_CONFIG, WMI
from opendbc.car.volkswagen.values import CAR, FW_QUERY_CONFIG, WMI, VolkswagenFlags, VolkswagenSafetyFlags
from opendbc.car.volkswagen.fingerprints import FW_VERSIONS
Ecu = CarParams.Ecu
@@ -14,6 +14,44 @@ SPARE_PART_FW_PATTERN = re.compile(b'\xf1\x87(?P<gateway>[0-9][0-9A-Z]{2})(?P<un
class TestVolkswagenPlatformConfigs:
MEB_CARS = {car for car in CAR if car.config.flags & VolkswagenFlags.MEB}
@staticmethod
def _get_meb_params(car, gateway=True, alpha_long=False):
fingerprint = {bus: {} for bus in range(8)}
if gateway:
fingerprint[1][0x13D] = 32
return CarInterface.get_params(car, fingerprint, [], alpha_long, False, False, None)
def test_meb_platform_params(self):
for car in self.MEB_CARS:
cp = self._get_meb_params(car)
assert cp.flags & VolkswagenFlags.MEB
assert cp.transmissionType == CarParams.TransmissionType.direct
assert cp.steerControlType == CarParams.SteerControlType.curvatureDEPRECATED
assert cp.steerAtStandstill
assert cp.safetyConfigs[-1].safetyModel == CarParams.SafetyModel.volkswagenMeb
assert not cp.dashcamOnly
assert not cp.radarUnavailable
has_gen2_crc = bool(cp.safetyConfigs[-1].safetyParam & VolkswagenSafetyFlags.MEB_ALT_CRC)
assert has_gen2_crc == bool(car.config.flags & VolkswagenFlags.MEB_GEN2)
def test_meb_camera_harness_is_passive(self):
cp = self._get_meb_params(CAR.VOLKSWAGEN_ID4_MK1, gateway=False, alpha_long=True)
assert cp.dashcamOnly
assert cp.radarUnavailable
assert not cp.alphaLongitudinalAvailable
assert not cp.openpilotLongitudinalControl
assert not (cp.safetyConfigs[-1].safetyParam & VolkswagenSafetyFlags.LONG_CONTROL)
def test_meb_gateway_longitudinal(self):
cp = self._get_meb_params(CAR.VOLKSWAGEN_ID4_MK1, gateway=True, alpha_long=True)
assert cp.alphaLongitudinalAvailable
assert cp.openpilotLongitudinalControl
assert not cp.pcmCruise
assert cp.safetyConfigs[-1].safetyParam & VolkswagenSafetyFlags.LONG_CONTROL
def test_taos_longitudinal_actuator_delay(self):
taos_cp = CarInterface.get_non_essential_params(CAR.VOLKSWAGEN_TAOS_MK1)
golf_cp = CarInterface.get_non_essential_params(CAR.VOLKSWAGEN_GOLF_MK7)
@@ -37,32 +75,39 @@ class TestVolkswagenPlatformConfigs:
assert all(CHASSIS_CODE_PATTERN.match(cc) for cc in
platform.config.chassis_codes), "Bad chassis codes"
# No two platforms should share chassis codes
# Shared MEB chassis codes are valid only when VIN model-year sets are disjoint.
for comp in CAR:
if platform == comp:
continue
assert set() == platform.config.chassis_codes & comp.config.chassis_codes, \
f"Shared chassis codes: {comp}"
shared_chassis = platform.config.chassis_codes & comp.config.chassis_codes
if shared_chassis:
both_meb = platform.config.flags & VolkswagenFlags.MEB and comp.config.flags & VolkswagenFlags.MEB
disjoint_years = (getattr(platform.config, "model_years", set()) and getattr(comp.config, "model_years", set()) and
not platform.config.model_years & comp.config.model_years)
assert both_meb and disjoint_years, f"Shared chassis codes: {comp}"
def test_custom_fuzzy_fingerprinting(self, subtests):
all_radar_fw = list({fw for ecus in FW_VERSIONS.values() for fw in ecus[Ecu.fwdRadar, 0x757, None]})
for platform in CAR:
with subtests.test(platform=platform.name):
model_years = getattr(platform.config, "model_years", set()) or {"0"}
for wmi in WMI:
for chassis_code in platform.config.chassis_codes | {"00"}:
vin = ["0"] * 17
vin[0:3] = wmi
vin[6:8] = chassis_code
vin = "".join(vin)
for model_year in model_years:
vin = ["0"] * 17
vin[0:3] = wmi
vin[6:8] = chassis_code
vin[9] = model_year
vin = "".join(vin)
# Check a few FW cases - expected, unexpected
for radar_fw in random.sample(all_radar_fw, 5) + [b'\xf1\x875Q0907572G \xf1\x890571', b'\xf1\x877H9907572AA\xf1\x890396']:
should_match = ((wmi in platform.config.wmis and chassis_code in platform.config.chassis_codes) and
radar_fw in all_radar_fw)
# Check a few FW cases - expected, unexpected
for radar_fw in random.sample(all_radar_fw, 5) + [b'\xf1\x875Q0907572G \xf1\x890571', b'\xf1\x877H9907572AA\xf1\x890396']:
should_match = ((wmi in platform.config.wmis and chassis_code in platform.config.chassis_codes) and
radar_fw in all_radar_fw)
live_fws = {(0x757, None): [radar_fw]}
matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fws, vin, FW_VERSIONS)
live_fws = {(0x757, None): [radar_fw]}
matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fws, vin, FW_VERSIONS)
expected_matches = {platform} if should_match else set()
assert expected_matches == matches, "Bad match"
expected_matches = {platform} if should_match else set()
assert expected_matches == matches, "Bad match"
opendbc_repo/opendbc/car/volkswagen/values.py
+125 -1
View File
@@ -3,6 +3,7 @@ from dataclasses import dataclass, field
from enum import Enum, IntFlag, StrEnum
from opendbc.car import Bus, CanBusBase, CarSpecs, DbcDict, PlatformConfig, Platforms, structs, uds
from opendbc.car.lateral import CurvatureSteeringLimits
from opendbc.can import CANDefine
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
@@ -101,6 +102,40 @@ class CarControllerParams:
"laneAssistDeactivTrailer": 5, # "Lane Assist: no function with trailer"
}
elif CP.flags & VolkswagenFlags.MEB:
self.LDW_STEP = 10 # LDW_02 message frequency 10Hz
self.ACC_HUD_STEP = 6
self.KLR_01_STEP = 6 # KLR_01 message frequency 17Hz
self.STEER_DRIVER_ALLOWANCE = 100 # Begin reducing steering power at 1.0 Nm driver torque
self.STEER_DRIVER_MAX = 300 # Reach minimum steering power at 3.0 Nm driver torque
self.STEERING_POWER_MAX = 50
self.STEERING_POWER_MIN = 4
self.STEERING_POWER_STEP = 2
self.CURVATURE_MAX = 0.195
self.CURVATURE_LIMITS = CurvatureSteeringLimits(self.CURVATURE_MAX)
self.ACCEL_INACTIVE = 3.01
self.JERK_LIMIT = 4.0
self.shifter_values = can_define.dv["Getriebe_11"]["GE_Fahrstufe"]
self.hca_status_values = can_define.dv["QFK_01"]["LatCon_HCA_Status"]
self.BUTTONS = [
Button(structs.CarState.ButtonEvent.Type.setCruise, "GRA_ACC_01", "GRA_Tip_Setzen", [1]),
Button(structs.CarState.ButtonEvent.Type.resumeCruise, "GRA_ACC_01", "GRA_Tip_Wiederaufnahme", [1]),
Button(structs.CarState.ButtonEvent.Type.accelCruise, "GRA_ACC_01", "GRA_Tip_Hoch", [1]),
Button(structs.CarState.ButtonEvent.Type.decelCruise, "GRA_ACC_01", "GRA_Tip_Runter", [1]),
Button(structs.CarState.ButtonEvent.Type.cancel, "GRA_ACC_01", "GRA_Abbrechen", [1]),
Button(structs.CarState.ButtonEvent.Type.gapAdjustCruise, "GRA_ACC_01", "GRA_Verstellung_Zeitluecke", [3]),
]
self.LDW_MESSAGES = {
"none": 0,
"laneAssistTakeOverUrgent": 4,
"laneAssistTakeOver": 8,
}
else:
self.LDW_STEP = 10 # LDW_02 message frequency 10Hz
self.ACC_HUD_STEP = 6 # ACC_02 message frequency 16Hz
@@ -179,16 +214,21 @@ class WMI(StrEnum):
class VolkswagenSafetyFlags(IntFlag):
LONG_CONTROL = 1
MEB_ALT_CRC = 2
class VolkswagenFlags(IntFlag):
# Detected flags
STOCK_HCA_PRESENT = 1
KOMBI_PRESENT = 4
ALT_GEAR = 32
STOCK_KLR_PRESENT = 64
# Static flags
PQ = 2
MLB = 8
MEB = 16
MEB_GEN2 = 128
@dataclass
@@ -210,6 +250,19 @@ class VolkswagenMQBPlatformConfig(PlatformConfig):
wmis: set[WMI] = field(default_factory=set)
@dataclass
class VolkswagenMEBPlatformConfig(PlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'vw_meb_generated', Bus.radar: 'vw_meb_generated'})
chassis_codes: set[str] = field(default_factory=set)
wmis: set[WMI] = field(default_factory=set)
model_years: set[str] = field(default_factory=set)
def init(self):
self.flags |= VolkswagenFlags.MEB
if self.flags & VolkswagenFlags.MEB_GEN2:
self.dbc_dict = {Bus.pt: 'vw_meb_2024_generated', Bus.radar: 'vw_meb_2024_generated'}
@dataclass
class VolkswagenPQPlatformConfig(VolkswagenMQBPlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'vw_pq'})
@@ -265,7 +318,7 @@ class VWCarDocs(CarDocs):
# FW_VERSIONS for that existing CAR.
class CAR(Platforms):
config: VolkswagenMQBPlatformConfig | VolkswagenPQPlatformConfig
config: VolkswagenMLBPlatformConfig | VolkswagenMQBPlatformConfig | VolkswagenPQPlatformConfig | VolkswagenMEBPlatformConfig
VOLKSWAGEN_ARTEON_MK1 = VolkswagenMQBPlatformConfig(
[
@@ -327,6 +380,37 @@ class CAR(Platforms):
chassis_codes={"5G", "AU", "BA", "BE"},
wmis={WMI.VOLKSWAGEN_MEXICO_CAR, WMI.VOLKSWAGEN_EUROPE_CAR},
)
VOLKSWAGEN_ID3_MK1 = VolkswagenMEBPlatformConfig(
[VWCarDocs("Volkswagen ID.3 2020-23")],
VolkswagenCarSpecs(mass=1935, wheelbase=2.77),
chassis_codes={"E1"},
wmis={WMI.VOLKSWAGEN_EUROPE_CAR},
model_years={"L", "M", "N", "P"},
)
VOLKSWAGEN_ID3_MK2 = VolkswagenMEBPlatformConfig(
[VWCarDocs("Volkswagen ID.3 2024-25")],
VolkswagenCarSpecs(mass=1935, wheelbase=2.77),
chassis_codes={"E1"},
wmis={WMI.VOLKSWAGEN_EUROPE_CAR},
model_years={"R", "S"},
flags=VolkswagenFlags.MEB_GEN2,
)
VOLKSWAGEN_ID4_MK1 = VolkswagenMEBPlatformConfig(
[
VWCarDocs("Volkswagen ID.4 2021-23"),
VWCarDocs("Volkswagen ID.5 2022-23"),
],
VolkswagenCarSpecs(mass=2224, wheelbase=2.77),
chassis_codes={"E2"},
wmis={WMI.VOLKSWAGEN_USA_SUV, WMI.VOLKSWAGEN_EUROPE_CAR, WMI.VOLKSWAGEN_EUROPE_SUV},
)
VOLKSWAGEN_ID4_MK2 = VolkswagenMEBPlatformConfig(
[VWCarDocs("Volkswagen ID.4 2024-25")],
VolkswagenCarSpecs(mass=2224, wheelbase=2.77),
chassis_codes={"E8"},
wmis={WMI.VOLKSWAGEN_USA_SUV, WMI.VOLKSWAGEN_EUROPE_CAR, WMI.VOLKSWAGEN_EUROPE_SUV},
flags=VolkswagenFlags.MEB_GEN2,
)
VOLKSWAGEN_JETTA_MK6 = VolkswagenPQPlatformConfig(
[VWCarDocs("Volkswagen Jetta 2015-18")],
VolkswagenCarSpecs(mass=1518, wheelbase=2.65, minSteerSpeed=50 * CV.KPH_TO_MS, minEnableSpeed=20 * CV.KPH_TO_MS),
@@ -441,6 +525,21 @@ class CAR(Platforms):
chassis_codes={"8U", "F3", "FS"},
wmis={WMI.AUDI_EUROPE_MPV, WMI.AUDI_GERMANY_CAR},
)
AUDI_Q4_MK1 = VolkswagenMEBPlatformConfig(
[VWCarDocs("Audi Q4 e-tron 2021-23")],
VolkswagenCarSpecs(mass=1965, wheelbase=2.764),
chassis_codes={"FZ"},
wmis={WMI.AUDI_EUROPE_MPV},
model_years={"M", "N", "P"},
)
AUDI_Q4_MK2 = VolkswagenMEBPlatformConfig(
[VWCarDocs("Audi Q4 e-tron 2024-25")],
VolkswagenCarSpecs(mass=1965, wheelbase=2.764),
chassis_codes={"FZ"},
wmis={WMI.AUDI_EUROPE_MPV},
model_years={"R", "S"},
flags=VolkswagenFlags.MEB_GEN2,
)
PORSCHE_MACAN_MK1 = VolkswagenMLBPlatformConfig(
[VWCarDocs("Porsche Macan 2017-24")],
VolkswagenCarSpecs(mass=1895, wheelbase=2.81, steerRatio=16.2),
@@ -457,6 +556,27 @@ class CAR(Platforms):
chassis_codes={"5F"},
wmis={WMI.SEAT},
)
CUPRA_BORN_MK1 = VolkswagenMEBPlatformConfig(
[VWCarDocs("CUPRA Born 2021-23")],
VolkswagenCarSpecs(mass=1956, wheelbase=2.766, steerRatio=15.9),
chassis_codes={"K1"},
wmis={WMI.SEAT},
)
SKODA_ENYAQ_MK1 = VolkswagenMEBPlatformConfig(
[VWCarDocs("Škoda Enyaq 2021-23")],
VolkswagenCarSpecs(mass=1965, wheelbase=2.77),
chassis_codes={"NY"},
model_years={"M", "N", "P"},
wmis={WMI.SKODA},
)
SKODA_ENYAQ_MK2 = VolkswagenMEBPlatformConfig(
[VWCarDocs("Škoda Enyaq 2024-25")],
VolkswagenCarSpecs(mass=1965, wheelbase=2.77),
chassis_codes={"NY"},
model_years={"R", "S"},
wmis={WMI.SKODA},
flags=VolkswagenFlags.MEB_GEN2,
)
SKODA_FABIA_MK4 = VolkswagenMQBPlatformConfig(
[VWCarDocs("Škoda Fabia 2022-23", footnotes=[Footnote.VW_MQB_A0])],
VolkswagenCarSpecs(mass=1266, wheelbase=2.56),
@@ -515,6 +635,7 @@ def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str
# https://www.clubvw.org.au/vwreference/vwvin
vin_obj = Vin(vin)
chassis_code = vin_obj.vds[3:5]
model_year = vin_obj.vis[0] if vin_obj.vis else ""
for platform in CAR:
valid_ecus = set()
@@ -535,6 +656,8 @@ def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str
continue
if vin_obj.wmi in platform.config.wmis and chassis_code in platform.config.chassis_codes:
if platform.config.flags & VolkswagenFlags.MEB and platform.config.model_years and model_year not in platform.config.model_years:
continue
candidates.add(platform)
return {str(c) for c in candidates}
@@ -582,6 +705,7 @@ FW_QUERY_CONFIG = FwQueryConfig(
non_essential_ecus={Ecu.eps: list(CAR)},
extra_ecus=[(Ecu.fwdCamera, 0x74f, None)],
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
fuzzy_only_platforms={car for car in CAR if car.config.flags & VolkswagenFlags.MEB},
)
DBC = CAR.create_dbc_map()
opendbc_repo/opendbc/dbc/generator/volkswagen/_vw_meb_common.dbc
+3321
View File
File diff suppressed because it is too large Load Diff
opendbc_repo/opendbc/dbc/generator/volkswagen/vw_meb.dbc
+132
View File
@@ -0,0 +1,132 @@
CM_ "IMPORT _vw_meb_common.dbc";
BO_ 190 MEB_HVEM_01: 48 XXX
SG_ CRC : 0|8@1+ (1,0) [0|255] "" XXX
SG_ CNT : 8|4@1+ (1,0) [0|15] "" XXX
SG_ Engine_RPM_Max : 12|14@1+ (2,-9658) [0|15] "RPM" XXX
SG_ Engine_RPM_Min : 26|14@1+ (2,-10300) [0|63] "RPM" XXX
SG_ In_Motion_04 : 48|3@1+ (1,0) [0|7] "" XXX
SG_ In_Motion_03 : 52|1@0+ (1,0) [0|1] "" XXX
SG_ In_Motion_02 : 54|1@0+ (1,0) [0|1] "" XXX
SG_ Engine_Power : 56|12@1+ (0.5,-1023) [0|255] "kW" XXX
SG_ In_Motion : 68|1@1+ (1,0) [0|3] "" XXX
SG_ Standstill : 71|1@0+ (1,0) [0|1] "" XXX
SG_ Unknown_04 : 72|10@1+ (1,0) [0|255] "" XXX
SG_ Battery_Voltage : 86|12@1+ (0.2,0) [0|3] "Volt" XXX
SG_ Unknown_01 : 100|9@1+ (1,0) [0|7] "" XXX
SG_ Battery_Voltage_02 : 113|11@1+ (0.24,0) [0|127] "Volt" XXX
SG_ Engine_Status : 296|2@1+ (1,0) [0|3] "" XXX
SG_ Inactive : 300|1@0+ (1,0) [0|1] "" XXX
SG_ Inactive_02 : 303|1@0+ (1,0) [0|1] "" XXX
BO_ 252 ESC_51: 48 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
SG_ AEB_Breaking_01 : 24|8@1+ (1,0) [0|255] "" XXX
SG_ AEB_Breaking_02 : 32|8@1+ (1,0) [0|255] "" XXX
SG_ Accelerator_Higher_Speed : 40|1@0+ (1,0) [0|1] "" XXX
SG_ Brake_Pressure : 42|9@1+ (0.195,0) [0|100] "Unit_Percent" XXX
SG_ HL_Radgeschw : 64|16@1+ (0.0075,0) [0|491.5125] "Unit_KilometerPerHour" XXX
SG_ HR_Radgeschw : 80|16@1+ (0.0075,0) [0|491.5125] "Unit_KilometerPerHour" XXX
SG_ VL_Radgeschw : 96|16@1+ (0.0075,0) [0|491.5125] "Unit_KilometerPerHour" XXX
SG_ VR_Radgeschw : 112|16@1+ (0.0075,0) [0|491.5125] "Unit_KilometerPerHour" XXX
SG_ HL_Brake_Pressure : 152|8@1+ (1,0) [0|100] "" XXX
SG_ HR_Brake_Pressure : 160|8@1+ (1,0) [0|100] "" XXX
SG_ VL_Brake_Pressure : 168|8@1+ (1,0) [0|100] "" XXX
SG_ VR_Brake_Pressure : 176|8@1+ (1,0) [0|100] "" XXX
SG_ Steering_Wheel_CW : 184|8@1+ (1.67,0) [0|255] "" XXX
SG_ Steering_Wheel_CCW : 192|8@1+ (1.67,0) [0|255] "" XXX
BO_ 267 Motor_51: 32 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
SG_ Accel_Pedal_Pressure : 12|9@1+ (0.4,0) [0|255] "" XXX
SG_ Accel_Low_Pressed_Support : 21|1@1+ (1,0) [0|7] "" XXX
SG_ TSK_Status : 88|3@1+ (1,0) [0|7] "" XXX
SG_ TSK_Limiter_ausgewaehlt : 95|1@1+ (1,0) [0|3] "" XXX
BO_ 496 EA_02: 8 Gateway
SG_ EA_02_CRC : 0|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ EA_02_BZ : 8|4@1+ (1,0) [0|15] "" Vector__XXX
SG_ EA_Texte : 12|4@1+ (1,0) [0|15] "" ZR_High
SG_ ACF_Lampe_Hands_Off : 16|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ EA_Infotainment_Anf : 22|2@1+ (1,0) [0|3] "" ZR_High,ZR_LIMU,ZR_MIB_TOP_ab_Gen3,ZR_Standard
SG_ EA_Tueren_Anf : 24|1@1+ (1,0) [0|1] "" ZR_High
SG_ EA_Innenraumlicht_Anf : 25|1@1+ (1,0) [0|1] "" ZR_High
SG_ zFAS_Warnblinken : 26|2@1+ (1,0) [0|3] "" ZR_High
SG_ STP_Primaeranz : 28|3@1+ (1,0) [0|7] "" Vector__XXX
SG_ EA_Bremslichtblinken : 31|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ EA_Blinken : 32|3@1+ (1,0) [0|7] "" Vector__XXX
SG_ EA_Unknown : 60|3@0+ (1,0) [0|7] "" XXX
BO_ 588 MEB_Side_Assist_01: 16 XXX
SG_ Blind_Spot_Right : 12|7@1+ (1,0) [0|15] "" XXX
SG_ Blind_Spot_Left : 19|7@1+ (1,0) [0|15] "" XXX
SG_ Blind_Spot_Info_Right : 26|1@0+ (1,0) [0|1] "" XXX
SG_ Blind_Spot_Warn_Right : 27|1@0+ (1,0) [0|1] "" XXX
SG_ Blind_Spot_Info_Left : 29|1@0+ (1,0) [0|1] "" XXX
SG_ Blind_Spot_Warn_Left : 30|1@0+ (1,0) [0|1] "" XXX
SG_ Lower_Speed_01 : 32|1@0+ (1,0) [0|1] "" XXX
SG_ Higher_Speed_01 : 33|1@0+ (1,0) [0|1] "" XXX
SG_ Higher_Speed_02 : 83|1@0+ (1,0) [0|1] "" XXX
SG_ Lower_Speed_02 : 84|1@0+ (1,0) [0|1] "" XXX
SG_ Standstill : 86|1@0+ (1,0) [0|1] "" XXX
BO_ 619 TA_01: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
SG_ Travel_Assist_Status : 13|3@1+ (1,0) [0|3] "" XXX
SG_ Travel_Assist_Request : 19|3@1+ (1,0) [0|7] "" XXX
SG_ Travel_Assist_Available : 23|1@1+ (1,0) [0|1] "" XXX
BO_ 768 ACC_19: 48 XXX
SG_ ACC_Tempolimit : 64|5@1+ (1,0) [0|31] "" OTA_FC
SG_ ACC_Wunschgeschw_Farbe : 69|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ACC_Warnung_Verkehrszeichen_1 : 70|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ACA_Querfuehrung : 71|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ Unknown_02 : 73|1@0+ (1,0) [0|1] "" XXX
SG_ ACC_Regelung_AIO : 75|1@1+ (1,0) [0|1] "" ZR_High,ZR_LIMU,ZR_MIB_TOP_ab_Gen3
SG_ ACC_Wunschgeschw_02 : 76|10@1+ (0.32,0) [0|327.04] "Unit_KiloMeterPerHour" Vector__XXX
SG_ ACC_Abstandsindex_02 : 86|10@1+ (1,0) [1|1021] "" Vector__XXX
SG_ ACC_Display_Prio : 96|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ ACC_rel_Objekt_Zusatzanz : 98|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ ACC_Gesetzte_Zeitluecke : 101|3@1+ (1,0) [0|7] "" Vector__XXX
SG_ ACC_Optischer_Fahrerhinweis : 104|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ACC_Warnhinweis : 105|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ACC_EGO_Fahrzeug : 106|3@1+ (1,0) [0|7] "" Vector__XXX
SG_ ACC_Relevantes_Objekt_02 : 109|2@1+ (1,0) [0|3] "" OTA_FC,ZR_High,ZR_LIMU,ZR_MIB_TOP_ab_Gen3
SG_ ACC_Wunschgeschw_erreicht : 112|1@1+ (1,0) [0|1] "" OTA_FC
SG_ ACC_Anzeige_Zeitluecke : 113|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ACC_Texte_Primaeranz_02 : 114|6@1+ (1,0) [0|63] "" Vector__XXX
SG_ ACC_Texte_Zusatzanz_02 : 120|6@1+ (1,0) [0|63] "" Vector__XXX
SG_ STA_Primaeranz : 126|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ SET_ME_0X3FF : 140|10@1+ (1,0) [0|15] "" XXX
SG_ Heartbeat : 150|9@1+ (1,0) [0|3] "" XXX
SG_ SET_ME_0XFFFF : 160|16@1+ (1,0) [0|65535] "" XXX
SG_ ACC_Enabled : 186|1@0+ (1,0) [0|1] "" XXX
SG_ Zeitluecke_Farbe : 189|1@0+ (1,0) [0|1] "" XXX
SG_ SET_ME_0X1 : 199|1@0+ (1,0) [0|1] "" XXX
SG_ ACC_Status_ACC : 208|3@1+ (1,0) [0|7] "" XXX
SG_ ACC_Akustischer_Fahrerhinweis : 211|2@1+ (1,0) [0|1] "" XXX
SG_ Unknown_08 : 224|1@0+ (1,0) [0|1] "" XXX
SG_ Unknown_01 : 225|1@0+ (1,0) [0|1] "" XXX
SG_ Unknown_06 : 226|1@0+ (1,0) [0|1] "" XXX
SG_ Unknown_07 : 228|1@0+ (1,0) [0|1] "" XXX
SG_ SET_ME_0X7FFF : 240|16@1+ (1,0) [0|65535] "" XXX
SG_ Unknown_09 : 262|1@0+ (1,0) [0|3] "" XXX
SG_ Lead_Type_Detected : 265|1@0+ (1,0) [0|1] "" XXX
SG_ ACC_Standby_Override : 266|1@0+ (1,0) [0|1] "" XXX
SG_ Street_Color : 267|1@0+ (1,0) [0|1] "" XXX
SG_ ACC_Limiter_Mode : 268|1@0+ (1,0) [0|1] "" XXX
SG_ Lead_Brightness : 269|4@1+ (1,0) [0|7] "" XXX
SG_ SET_ME_0X6A : 273|8@1+ (1,0) [0|7] "" XXX
SG_ Lead_Type : 287|3@1+ (1,0) [0|3] "" XXX
SG_ Lead_Distance : 290|10@1+ (0.2,0) [0|7] "Unit_Meter" XXX
SG_ ACC_Events : 332|4@0+ (1,0) [0|3] "Unit_Meter" XXX
SG_ Zeitluecke_1 : 334|9@1+ (0.171,0) [0|100] "Unit_Meter" XXX
SG_ Zeitluecke_2 : 344|9@1+ (0.171,0) [0|100] "Unit_Meter" XXX
SG_ Zeitluecke_3 : 354|9@1+ (0.171,0) [0|100] "Unit_Meter" XXX
SG_ Zeitluecke_4 : 364|9@1+ (0.171,0) [0|100] "Unit_Meter" XXX
SG_ Zeitluecke_5 : 374|9@1+ (0.171,0) [0|100] "Unit_Meter" XXX
VAL_ 768 ACC_Events 3 "Starting_Available" 0 "None" 5 "Speed_Limit_Camera" 9 "Street_Type" 4 "Speed_Limit_in_Nav";
opendbc_repo/opendbc/dbc/generator/volkswagen/vw_meb_2024.dbc
+134
View File
@@ -0,0 +1,134 @@
CM_ "IMPORT _vw_meb_common.dbc";
BO_ 190 MEB_HVEM_01: 48 XXX
SG_ CRC : 0|8@1+ (1,0) [0|255] "" XXX
SG_ CNT : 8|4@1+ (1,0) [0|15] "" XXX
SG_ Engine_RPM_Max : 12|14@1+ (2,-9658) [0|15] "RPM" XXX
SG_ Engine_RPM_Min : 26|14@1+ (2,-10300) [0|63] "RPM" XXX
SG_ In_Motion_04 : 48|3@1+ (1,0) [0|7] "" XXX
SG_ In_Motion_03 : 52|1@0+ (1,0) [0|1] "" XXX
SG_ In_Motion_02 : 54|1@0+ (1,0) [0|1] "" XXX
SG_ Engine_Power : 56|12@1+ (0.5,-1023) [0|255] "kW" XXX
SG_ In_Motion : 68|1@1+ (1,0) [0|3] "" XXX
SG_ Standstill : 71|1@0+ (1,0) [0|1] "" XXX
SG_ Voltage : 86|12@1+ (0.24,0) [0|3] "V" XXX
SG_ Battery_Voltage : 113|13@1+ (0.065,0) [0|127] "V" XXX
SG_ Engine_Status : 296|2@1+ (1,0) [0|3] "" XXX
SG_ Inactive : 300|1@0+ (1,0) [0|1] "" XXX
SG_ Inactive_02 : 303|1@0+ (1,0) [0|1] "" XXX
BO_ 496 EA_02: 8 Gateway
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" Vector__XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" Vector__XXX
SG_ EA_Texte : 12|4@1+ (1,0) [0|15] "" ZR_High
SG_ ACF_Lampe_Hands_Off : 16|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ EA_Infotainment_Anf : 22|2@1+ (1,0) [0|3] "" ZR_High,ZR_LIMU,ZR_MIB_TOP_ab_Gen3,ZR_Standard
SG_ EA_Tueren_Anf : 24|1@1+ (1,0) [0|1] "" ZR_High
SG_ EA_Innenraumlicht_Anf : 25|1@1+ (1,0) [0|1] "" ZR_High
SG_ zFAS_Warnblinken : 26|2@1+ (1,0) [0|3] "" ZR_High
SG_ STP_Primaeranz : 28|3@1+ (1,0) [0|7] "" Vector__XXX
SG_ EA_Bremslichtblinken : 31|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ EA_Blinken : 32|3@1+ (1,0) [0|7] "" Vector__XXX
BO_ 588 MEB_Side_Assist_01: 16 XXX
SG_ Blind_Spot_Passenger : 12|7@1+ (1,0) [0|15] "" XXX
SG_ Blind_Spot_Driver : 19|7@1+ (1,0) [0|15] "" XXX
SG_ Blind_Spot_Info_Passenger : 26|1@0+ (1,0) [0|1] "" XXX
SG_ Blind_Spot_Warn_Passenger : 27|1@0+ (1,0) [0|1] "" XXX
SG_ Blind_Spot_Info_Driver : 29|1@0+ (1,0) [0|1] "" XXX
SG_ Blind_Spot_Warn_Driver : 30|1@0+ (1,0) [0|1] "" XXX
SG_ Lower_Speed_01 : 32|1@0+ (1,0) [0|1] "" XXX
SG_ Higher_Speed_01 : 33|1@0+ (1,0) [0|1] "" XXX
SG_ Higher_Speed_02 : 83|1@0+ (1,0) [0|1] "" XXX
SG_ Lower_Speed_02 : 84|1@0+ (1,0) [0|1] "" XXX
SG_ Standstill : 86|1@0+ (1,0) [0|1] "" XXX
BO_ 768 ACC_19: 48 XXX
SG_ ACC_Tempolimit : 64|5@1+ (1,0) [0|31] "" OTA_FC
SG_ ACC_Wunschgeschw_Farbe : 69|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ACC_Warnung_Verkehrszeichen_1 : 70|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ACA_Querfuehrung : 71|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ ACC_Regelung_AIO : 75|1@1+ (1,0) [0|1] "" ZR_High,ZR_LIMU,ZR_MIB_TOP_ab_Gen3
SG_ ACC_Wunschgeschw_02 : 76|10@1+ (0.32,0) [0|327.04] "Unit_KiloMeterPerHour" Vector__XXX
SG_ ACC_Abstandsindex_02 : 86|10@1+ (1,0) [1|1021] "" Vector__XXX
SG_ ACC_Display_Prio : 96|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ ACC_rel_Objekt_Zusatzanz : 98|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ ACC_Gesetzte_Zeitluecke : 101|3@1+ (1,0) [0|7] "" Vector__XXX
SG_ ACC_Optischer_Fahrerhinweis : 104|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ACC_Warnhinweis : 105|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ACC_EGO_Fahrzeug : 106|3@1+ (1,0) [0|7] "" Vector__XXX
SG_ ACC_Relevantes_Objekt_02 : 109|2@1+ (1,0) [0|3] "" OTA_FC,ZR_High,ZR_LIMU,ZR_MIB_TOP_ab_Gen3
SG_ ACC_Wunschgeschw_erreicht : 112|1@1+ (1,0) [0|1] "" OTA_FC
SG_ ACC_Anzeige_Zeitluecke : 113|1@1+ (1,0) [0|1] "" Vector__XXX
SG_ ACC_Texte_Primaeranz_02 : 114|6@1+ (1,0) [0|63] "" Vector__XXX
SG_ ACC_Texte_Zusatzanz_02 : 120|6@1+ (1,0) [0|63] "" Vector__XXX
SG_ STA_Primaeranz : 126|2@1+ (1,0) [0|3] "" Vector__XXX
SG_ ACC_Event_Wunschgeschw : 140|10@1+ (0.32,0) [0|15] "" XXX
SG_ Heartbeat : 150|9@1+ (1,0) [0|3] "" XXX
SG_ SET_ME_0XFFFF : 160|16@1+ (1,0) [0|65535] "" XXX
SG_ ACC_Enabled : 186|1@0+ (1,0) [0|1] "" XXX
SG_ Zeitluecke_Farbe : 189|1@0+ (1,0) [0|1] "" XXX
SG_ SET_ME_0X1 : 199|1@0+ (1,0) [0|1] "" XXX
SG_ ACC_Status_ACC : 208|3@1+ (1,0) [0|7] "" XXX
SG_ ACC_Akustischer_Fahrerhinweis : 211|2@1+ (1,0) [0|1] "" XXX
SG_ SET_ME_0X7FFF : 240|16@1+ (1,0) [0|65535] "" XXX
SG_ Lead_Type_Detected : 265|1@0+ (1,0) [0|1] "" XXX
SG_ ACC_Standby_Override : 266|1@0+ (1,0) [0|1] "" XXX
SG_ Street_Color : 267|1@0+ (1,0) [0|1] "" XXX
SG_ ACC_Limiter_Mode : 268|1@0+ (1,0) [0|1] "" XXX
SG_ Lead_Brightness : 269|4@1+ (1,0) [0|7] "" XXX
SG_ SET_ME_0X6A : 273|8@1+ (1,0) [0|7] "" XXX
SG_ Lead_Position : 281|2@1+ (1,0) [0|1] "" XXX
SG_ Lead_Type : 287|3@1+ (1,0) [0|3] "" XXX
SG_ Lead_Distance : 290|10@1+ (0.2,0) [0|7] "Unit_Meter" XXX
SG_ Lead_Distance_Right : 300|10@1+ (0.2,0) [0|15] "Unit_Meter" XXX
SG_ Lead_Distance_Left : 310|10@1+ (0.2,0) [0|3] "Unit_Meter" XXX
SG_ ACC_Events : 332|4@0+ (1,0) [0|3] "Unit_Meter" XXX
SG_ Zeitluecke_1 : 334|9@1+ (0.171,0) [0|100] "Unit_Meter" XXX
SG_ Zeitluecke_2 : 344|9@1+ (0.171,0) [0|100] "Unit_Meter" XXX
SG_ Zeitluecke_3 : 354|9@1+ (0.171,0) [0|100] "Unit_Meter" XXX
SG_ Zeitluecke_4 : 364|9@1+ (0.171,0) [0|100] "Unit_Meter" XXX
SG_ Zeitluecke_5 : 374|9@1+ (0.171,0) [0|100] "Unit_Meter" XXX
VAL_ 768 ACC_Event_Wunschgeschw 1023 "None";
VAL_ 768 ACC_Events 3 "Starting_Available" 0 "None" 5 "Speed_Limit_Detected" 9 "Crossing" 4 "Speed_Limit_Ahead" 10 "Roundabout" 6 "Curve";
BO_ 252 ESC_51: 64 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
SG_ AEB_Breaking_01 : 24|8@1+ (1,0) [0|255] "" XXX
SG_ AEB_Breaking_02 : 32|8@1+ (1,0) [0|255] "" XXX
SG_ Accelerator_Higher_Speed : 40|1@0+ (1,0) [0|1] "" XXX
SG_ Brake_Pressure : 42|9@1+ (0.195,0) [0|100] "Unit_Percent" XXX
SG_ HL_Radgeschw : 64|16@1+ (0.0075,0) [0|491.5125] "Unit_KilometerPerHour" XXX
SG_ HR_Radgeschw : 80|16@1+ (0.0075,0) [0|491.5125] "Unit_KilometerPerHour" XXX
SG_ VL_Radgeschw : 96|16@1+ (0.0075,0) [0|491.5125] "Unit_KilometerPerHour" XXX
SG_ VR_Radgeschw : 112|16@1+ (0.0075,0) [0|491.5125] "Unit_KilometerPerHour" XXX
SG_ HL_Brake_Pressure : 152|8@1+ (1,0) [0|100] "" XXX
SG_ HR_Brake_Pressure : 160|8@1+ (1,0) [0|100] "" XXX
SG_ VL_Brake_Pressure : 168|8@1+ (1,0) [0|100] "" XXX
SG_ VR_Brake_Pressure : 176|8@1+ (1,0) [0|100] "" XXX
SG_ Steering_Wheel_CW : 184|8@1+ (1.67,0) [0|255] "" XXX
SG_ Steering_Wheel_CCW : 192|8@1+ (1.67,0) [0|255] "" XXX
SG_ ESC_v_Signal : 328|13@1+ (0.052,-107.016) [0|63] "" XXX
BO_ 267 Motor_51: 48 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
SG_ Accel_Pedal_Pressure : 12|9@1+ (0.4,0) [0|255] "" XXX
SG_ Accel_Low_Pressed_Support : 21|1@1+ (1,0) [0|7] "" XXX
SG_ TSK_Status : 88|3@1+ (1,0) [0|7] "" XXX
SG_ TSK_Limiter_ausgewaehlt : 95|1@1+ (1,0) [0|3] "" XXX
BO_ 619 TA_01: 8 XXX
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
SG_ Travel_Assist_Status : 13|3@1+ (1,0) [0|3] "" XXX
SG_ Speed_Mode_01 : 16|1@0+ (1,0) [0|1] "" XXX
SG_ Speed_Mode_02 : 18|1@0+ (1,0) [0|1] "" XXX
SG_ Travel_Assist_Request : 19|3@1+ (1,0) [0|7] "" XXX
SG_ Init : 22|1@0+ (1,0) [0|1] "" XXX
SG_ Travel_Assist_Available : 23|1@1+ (1,0) [0|1] "" XXX
SG_ Speed_Mode_Status : 32|3@1+ (1,0) [0|7] "" XXX
SG_ Lane_Unsure : 37|2@1+ (1,0) [0|3] "" XXX
opendbc_repo/opendbc/dbc/vw_meb_2024_generated.dbc
+3460
View File
File diff suppressed because it is too large Load Diff
opendbc_repo/opendbc/dbc/vw_meb_generated.dbc
+3458
View File
File diff suppressed because it is too large Load Diff
opendbc_repo/opendbc/safety/declarations.h
+23
View File
@@ -56,6 +56,7 @@
} while (0);
#define UPDATE_VEHICLE_SPEED(val_ms) (update_sample(&vehicle_speed, ROUND((val_ms) * VEHICLE_SPEED_FACTOR)))
#define UPDATE_VEHICLE_SPEED_2(val_ms) (update_sample(&vehicle_speed_2, ROUND((val_ms) * VEHICLE_SPEED_FACTOR)))
uint32_t GET_BYTES(const CANPacket_t *msg, int start, int len);
@@ -137,6 +138,15 @@ typedef struct {
const bool inactive_angle_is_zero; // if false, enforces angle near meas when disabled (default)
} AngleSteeringLimits;
typedef struct {
const int max_curvature; // rad/m * curvature_to_can
const float curvature_to_can; // CAN units per rad/m
const uint32_t frequency; // Hz
const int max_curvature_error; // max deviation from measured curvature (0 disables)
const float curvature_error_min_speed; // minimum speed for curvature error checks [m/s]
const int max_steer_power; // max steering authority value (0 disables)
} CurvatureSteeringLimits;
// parameters for lateral accel/jerk angle limiting using a simple vehicle model
typedef struct {
const float slip_factor;
@@ -237,6 +247,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee
bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const AngleSteeringLimits limits);
bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, const AngleSteeringLimits limits,
const AngleSteeringParams params);
bool steer_curvature_cmd_checks(int desired_curvature, int steer_power, bool steer_control_enabled, const CurvatureSteeringLimits limits);
bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits);
bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limits);
bool longitudinal_gas_checks(int desired_gas, const LongitudinalLimits limits);
@@ -262,6 +273,7 @@ extern bool steering_disengage;
extern bool steering_disengage_prev;
extern bool cruise_engaged_prev;
extern struct sample_t vehicle_speed;
extern struct sample_t vehicle_speed_2;
extern bool vehicle_moving;
extern bool acc_main_on; // referred to as "ACC off" in ISO 15622:2018
extern int cruise_button_prev;
@@ -292,6 +304,16 @@ extern uint32_t ts_angle_check_last;
extern int desired_angle_last;
extern struct sample_t angle_meas; // last 6 steer angles/curvatures
typedef struct {
int desired_last;
uint32_t rt_msgs;
uint32_t rt_msgs_prev;
uint32_t ts_check_last;
int steer_power_last;
struct sample_t meas;
} CurvatureSteeringState;
extern CurvatureSteeringState curvature_state;
extern bool enable_gas_interceptor;
extern int gas_interceptor_prev;
extern bool gm_remote_start_boots_comma;
@@ -353,6 +375,7 @@ extern const safety_hooks subaru_preglobal_hooks;
extern const safety_hooks tesla_hooks;
extern const safety_hooks toyota_hooks;
extern const safety_hooks volkswagen_mlb_hooks;
extern const safety_hooks volkswagen_meb_hooks;
extern const safety_hooks volkswagen_mqb_hooks;
extern const safety_hooks volkswagen_pq_hooks;
extern const safety_hooks rivian_hooks;
opendbc_repo/opendbc/safety/lateral.h
+79
View File
@@ -172,6 +172,26 @@ static bool rt_angle_rate_limit_check(AngleSteeringLimits limits) {
return violation;
}
static bool rt_curvature_rate_limit_check(CurvatureSteeringLimits limits) {
bool violation = false;
uint32_t ts = microsecond_timer_get();
int max_rt_msgs = ((float)limits.frequency * MAX_RT_INTERVAL / 1e6 * 1.2) + 1;
uint32_t rt_msgs = curvature_state.rt_msgs + curvature_state.rt_msgs_prev;
if ((int)rt_msgs > max_rt_msgs) {
violation = true;
}
curvature_state.rt_msgs += 1U;
if (safety_get_ts_elapsed(ts, curvature_state.ts_check_last) >= (MAX_RT_INTERVAL / 2U)) {
curvature_state.rt_msgs_prev = curvature_state.rt_msgs;
curvature_state.rt_msgs = 0U;
curvature_state.ts_check_last = ts;
}
return violation;
}
// Safety checks for angle-based steering commands
bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const AngleSteeringLimits limits) {
bool violation = false;
@@ -278,6 +298,65 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
return violation;
}
// Safety checks for curvature-based steering commands.
bool steer_curvature_cmd_checks(int desired_curvature, int steer_power, bool steer_control_enabled, const CurvatureSteeringLimits limits) {
static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL + (EARTH_G * AVERAGE_ROAD_ROLL);
static const float MAX_LATERAL_JERK = 3.0 + (EARTH_G * AVERAGE_ROAD_ROLL);
const float speed_1 = (float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR;
const float speed_2 = (float)vehicle_speed_2.values[0] / VEHICLE_SPEED_FACTOR;
const bool speed_sources_valid = SAFETY_ABS(speed_1 - speed_2) <= 2.0;
speed_mismatch_check(speed_2);
const bool lateral_allowed = (aol_allowed || controls_allowed) && speed_sources_valid;
const float fudged_speed = SAFETY_MAX((vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0, 1.0);
bool violation = false;
if (lateral_allowed && steer_control_enabled) {
violation |= safety_max_limit_check(desired_curvature, limits.max_curvature, -limits.max_curvature);
const float max_curvature_rate_sec = MAX_LATERAL_JERK / (fudged_speed * fudged_speed);
const float max_curvature_delta = max_curvature_rate_sec / (float)limits.frequency;
const int max_curvature_delta_can = (max_curvature_delta * limits.curvature_to_can) + 1.;
const int highest_desired_curvature = curvature_state.desired_last + max_curvature_delta_can;
const int lowest_desired_curvature = curvature_state.desired_last - max_curvature_delta_can;
violation |= safety_max_limit_check(desired_curvature, highest_desired_curvature, lowest_desired_curvature);
const float max_curvature = MAX_LATERAL_ACCEL / (fudged_speed * fudged_speed);
const int max_curvature_can = (max_curvature * limits.curvature_to_can) + 1.;
violation |= safety_max_limit_check(desired_curvature, max_curvature_can, -max_curvature_can);
if (limits.max_curvature_error && (speed_1 > limits.curvature_error_min_speed)) {
const int lowest_error = curvature_state.meas.min - limits.max_curvature_error - 1;
const int highest_error = curvature_state.meas.max + limits.max_curvature_error + 1;
violation |= safety_max_limit_check(desired_curvature, highest_error, lowest_error);
}
violation |= rt_curvature_rate_limit_check(limits);
}
curvature_state.desired_last = desired_curvature;
if (!steer_control_enabled) {
violation |= desired_curvature != 0;
}
if (limits.max_steer_power != 0) {
violation |= safety_max_limit_check(steer_power, limits.max_steer_power, 0);
violation |= (steer_power != 0) && !steer_control_enabled;
// Permit only a strict power wind-down after lateral control becomes inactive.
violation |= !lateral_allowed && (steer_power != 0) && (steer_power >= curvature_state.steer_power_last);
curvature_state.steer_power_last = steer_power;
} else {
violation |= !lateral_allowed && steer_control_enabled;
}
if (violation) {
curvature_state.desired_last = 0;
}
return violation;
}
static float get_curvature_factor(const float speed, const AngleSteeringParams params) {
// Matches VehicleModel.curvature_factor()
return 1. / (1. - (params.slip_factor * (speed * speed))) / params.wheelbase;
opendbc_repo/opendbc/safety/modes/volkswagen_meb.h
+287
View File
@@ -0,0 +1,287 @@
#pragma once
#include "opendbc/safety/declarations.h"
#include "opendbc/safety/modes/volkswagen_common.h"
#define MSG_ESC_51 0xFCU // RX, for wheel speeds
#define MSG_ACC_18 0x14DU // TX by OP, ACC control instructions to the drivetrain coordinator
#define MSG_ESP_21 0xFDU // RX, redundant vehicle speed source
#define MSG_HCA_03 0x303U
#define MSG_ACC_19 0x300U // TX by OP, ACC HUD data to the instrument cluster
#define MSG_QFK_01 0x13DU
#define MSG_Motor_51 0x10BU // RX for TSK state and accel pedal
#define MSG_KLR_01 0x25DU // TX, for capacitive steering wheel
#define MSG_TA_01 0x26BU // TX by OP, Travel Assist status
static bool volkswagen_meb_alt_crc = false;
#define VOLKSWAGEN_MEB_COMMON_RX_CHECKS \
{.msg = {{MSG_LH_EPS_03, 0, 8, 100U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_MOTOR_14, 0, 8, 10U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_GRA_ACC_01, 0, 8, 33U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_QFK_01, 0, 32, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
{.msg = {{MSG_ESP_21, 0, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
static uint32_t volkswagen_meb_compute_crc(const CANPacket_t *msg) {
int len = GET_LEN(msg);
uint8_t crc = 0xFFU;
for (int i = 1; i < len; i++) {
crc ^= (uint8_t)msg->data[i];
crc = volkswagen_crc8_lut_8h2f[crc];
}
uint8_t counter = volkswagen_mqb_meb_get_counter(msg);
if (msg->addr == MSG_LH_EPS_03) {
crc ^= (uint8_t[]){0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5, 0xF5}[counter];
} else if (msg->addr == MSG_MOTOR_14) {
crc ^= (uint8_t[]){0x1F, 0x28, 0xC6, 0x85, 0xE6, 0xF8, 0xB0, 0x19, 0x5B, 0x64, 0x35, 0x21, 0xE4, 0xF7, 0x9C, 0x24}[counter];
} else if (msg->addr == MSG_GRA_ACC_01) {
crc ^= (uint8_t[]){0x6A, 0x38, 0xB4, 0x27, 0x22, 0xEF, 0xE1, 0xBB, 0xF8, 0x80, 0x84, 0x49, 0xC7, 0x9E, 0x1E, 0x2B}[counter];
} else if (msg->addr == MSG_QFK_01) {
crc ^= (uint8_t[]){0x20, 0xCA, 0x68, 0xD5, 0x1B, 0x31, 0xE2, 0xDA, 0x08, 0x0A, 0xD4, 0xDE, 0x9C, 0xE4, 0x35, 0x5B}[counter];
} else if (msg->addr == MSG_ESC_51) {
crc ^= (uint8_t[]){0x77, 0x5C, 0xA0, 0x89, 0x4B, 0x7C, 0xBB, 0xD6, 0x1F, 0x6C, 0x4F, 0xF6, 0x20, 0x2B, 0x43, 0xDD}[counter];
} else if (msg->addr == MSG_ESP_21) {
crc ^= (uint8_t[]){0xB4, 0xEF, 0xF8, 0x49, 0x1E, 0xE5, 0xC2, 0xC0, 0x97, 0x19, 0x3C, 0xC9, 0xF1, 0x98, 0xD6, 0x61}[counter];
} else if (msg->addr == MSG_Motor_51) {
crc ^= (uint8_t[]){0x77, 0x5C, 0xA0, 0x89, 0x4B, 0x7C, 0xBB, 0xD6, 0x1F, 0x6C, 0x4F, 0xF6, 0x20, 0x2B, 0x43, 0xDD}[counter];
} else {
// Undefined CAN message, CRC check expected to fail
}
crc = volkswagen_crc8_lut_8h2f[crc];
return (uint8_t)(crc ^ 0xFFU);
}
static uint32_t volkswagen_meb_alt_crc_compute(const CANPacket_t *msg) {
uint32_t ret = volkswagen_meb_compute_crc(msg);
int len = 0;
if (volkswagen_meb_alt_crc) {
if (msg->addr == MSG_QFK_01) {
len = 28;
} else if (msg->addr == MSG_ESC_51) {
len = 60;
} else if (msg->addr == MSG_Motor_51) {
len = 44;
} else {
len = 0;
}
}
if (len > 0) {
uint8_t crc = 0xFFU;
for (int i = 1; i < len; i++) {
crc ^= (uint8_t)msg->data[i];
crc = volkswagen_crc8_lut_8h2f[crc];
}
uint8_t counter = volkswagen_mqb_meb_get_counter(msg);
if (msg->addr == MSG_QFK_01) {
crc ^= (uint8_t[]){0x18, 0x71, 0x10, 0x8D, 0xD7, 0xAA, 0xB0, 0x78, 0xAC, 0x12, 0xAE, 0x0C, 0xDD, 0xF1, 0x85, 0x68}[counter];
} else if (msg->addr == MSG_ESC_51) {
crc ^= (uint8_t[]){0x69, 0xDC, 0xF9, 0x64, 0x6A, 0xCE, 0x55, 0x2C, 0xC4, 0x38, 0x8F, 0xD1, 0xC6, 0x43, 0xB4, 0xB1}[counter];
} else if (msg->addr == MSG_Motor_51) {
crc ^= (uint8_t[]){0x2C, 0xB1, 0x1A, 0x75, 0xBB, 0x65, 0x79, 0x47, 0x81, 0x2B, 0xCC, 0x96, 0x17, 0xDB, 0xC0, 0x94}[counter];
} else {
// Undefined CAN message, CRC check expected to fail
}
crc = (uint8_t)(volkswagen_crc8_lut_8h2f[crc] ^ 0xFFU);
if (crc == msg->data[0]) {
ret = crc;
}
}
return ret;
}
static safety_config volkswagen_meb_init(uint16_t param) {
// Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
static const CanMsg VOLKSWAGEN_MEB_STOCK_TX_MSGS[] = {
{MSG_HCA_03, 0, 24, .check_relay = true},
{MSG_GRA_ACC_01, 0, 8, .check_relay = false},
{MSG_GRA_ACC_01, 2, 8, .check_relay = false},
{MSG_LDW_02, 0, 8, .check_relay = true},
{MSG_KLR_01, 0, 8, .check_relay = false},
{MSG_KLR_01, 2, 8, .check_relay = true},
};
static const CanMsg VOLKSWAGEN_MEB_LONG_TX_MSGS[] = {
{MSG_HCA_03, 0, 24, .check_relay = true},
{MSG_LDW_02, 0, 8, .check_relay = true},
{MSG_KLR_01, 0, 8, .check_relay = false},
{MSG_KLR_01, 2, 8, .check_relay = true},
{MSG_ACC_19, 0, 48, .check_relay = true},
{MSG_ACC_18, 0, 32, .check_relay = true},
{MSG_TA_01, 0, 8, .check_relay = true},
};
static RxCheck volkswagen_meb_rx_checks[] = {
VOLKSWAGEN_MEB_COMMON_RX_CHECKS
{.msg = {{MSG_Motor_51, 0, 32, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_ESC_51, 0, 48, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
};
static RxCheck volkswagen_meb_gen2_rx_checks[] = {
VOLKSWAGEN_MEB_COMMON_RX_CHECKS
{.msg = {{MSG_Motor_51, 0, 48, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
{.msg = {{MSG_ESC_51, 0, 64, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
};
volkswagen_common_init();
const uint16_t FLAG_VOLKSWAGEN_MEB_ALT_CRC = 2;
volkswagen_meb_alt_crc = GET_FLAG(param, FLAG_VOLKSWAGEN_MEB_ALT_CRC);
#ifdef ALLOW_DEBUG
volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL);
#endif
safety_config ret;
if (volkswagen_longitudinal && volkswagen_meb_alt_crc) {
ret = BUILD_SAFETY_CFG(volkswagen_meb_gen2_rx_checks, VOLKSWAGEN_MEB_LONG_TX_MSGS);
} else if (volkswagen_longitudinal) {
ret = BUILD_SAFETY_CFG(volkswagen_meb_rx_checks, VOLKSWAGEN_MEB_LONG_TX_MSGS);
} else if (volkswagen_meb_alt_crc) {
ret = BUILD_SAFETY_CFG(volkswagen_meb_gen2_rx_checks, VOLKSWAGEN_MEB_STOCK_TX_MSGS);
} else {
ret = BUILD_SAFETY_CFG(volkswagen_meb_rx_checks, VOLKSWAGEN_MEB_STOCK_TX_MSGS);
}
return ret;
}
static void volkswagen_meb_rx_hook(const CANPacket_t *msg) {
if (msg->bus == 0U) {
// Update in-motion state by sampling wheel speeds
if (msg->addr == MSG_ESC_51) {
uint32_t fl = msg->data[8] | (msg->data[9] << 8);
uint32_t fr = msg->data[10] | (msg->data[11] << 8);
uint32_t rl = msg->data[12] | (msg->data[13] << 8);
uint32_t rr = msg->data[14] | (msg->data[15] << 8);
vehicle_moving = (fr > 0U) || (rr > 0U) || (rl > 0U) || (fl > 0U);
UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4.0 * 0.0075 * KPH_TO_MS);
}
// Check vehicle speed with redundant source
if (msg->addr == MSG_ESP_21) {
// Signal: ESP_v_Signal
float esp_speed = ((msg->data[5] << 8) | msg->data[4]) * 0.01 * KPH_TO_MS;
UPDATE_VEHICLE_SPEED_2(esp_speed);
}
if (msg->addr == MSG_QFK_01) {
int current_curvature = ((msg->data[6] & 0x7FU) << 8) | msg->data[5];
current_curvature *= GET_BIT(msg, 55U) ? 1 : -1;
update_sample(&curvature_state.meas, current_curvature);
}
if (msg->addr == MSG_LH_EPS_03) {
update_sample(&torque_driver, volkswagen_mlb_mqb_driver_input_torque(msg));
}
if (msg->addr == MSG_Motor_51) {
int acc_status = (msg->data[11] & 0x07U);
bool cruise_engaged = (acc_status == 3) || (acc_status == 4) || (acc_status == 5);
acc_main_on = cruise_engaged || (acc_status == 2);
if (!volkswagen_longitudinal) {
pcm_cruise_check(cruise_engaged);
}
if (!acc_main_on) {
controls_allowed = false;
}
int accel_pedal_value = ((msg->data[1] >> 4) & 0x0FU) | ((msg->data[2] & 0x1FU) << 4);
gas_pressed = accel_pedal_value > 0;
}
if (msg->addr == MSG_GRA_ACC_01) {
// If using openpilot longitudinal, enter controls on falling edge of Set or Resume with main switch on
// Signal: GRA_ACC_01.GRA_Tip_Setzen
// Signal: GRA_ACC_01.GRA_Tip_Wiederaufnahme
if (volkswagen_longitudinal) {
bool set_button = GET_BIT(msg, 16U);
bool resume_button = GET_BIT(msg, 19U);
if ((volkswagen_set_button_prev && !set_button) || (volkswagen_resume_button_prev && !resume_button)) {
controls_allowed = acc_main_on;
}
volkswagen_set_button_prev = set_button;
volkswagen_resume_button_prev = resume_button;
}
// Always exit controls on rising edge of Cancel
if (GET_BIT(msg, 13U)) {
controls_allowed = false;
}
}
if (msg->addr == MSG_MOTOR_14) {
brake_pressed = GET_BIT(msg, 28U);
}
}
}
static bool volkswagen_meb_tx_hook(const CANPacket_t *msg) {
// acceleration in m/s2 * 1000 to avoid floating point math
const LongitudinalLimits VOLKSWAGEN_MEB_LONG_LIMITS = {
.max_accel = 2000,
.min_accel = -3500,
.inactive_accel = 3010, // VW sends one increment above the max range when inactive
};
bool tx = true;
// Safety check for MSG_ACC_18 acceleration requests
if (msg->addr == MSG_ACC_18) {
// Signal: ACC_18.ACC_Sollbeschleunigung_02 (acceleration in m/s2, scale 0.005, offset -7.22)
int desired_accel = ((((msg->data[4] & 0x7U) << 8) | msg->data[3]) * 5U) - 7220U;
// MEB inactive accel is 3.01, but we also need to send 0.0 for gas override
bool accel_override = controls_allowed && (desired_accel == 0);
if (!accel_override && longitudinal_accel_checks(desired_accel, VOLKSWAGEN_MEB_LONG_LIMITS)) {
tx = false;
}
}
if (msg->addr == MSG_HCA_03) {
const CurvatureSteeringLimits VOLKSWAGEN_MEB_STEERING_LIMITS = {
.max_curvature = 29105,
.curvature_to_can = 149253.7313f,
.frequency = 50, // Hz
.max_curvature_error = 0, // disabled, MEB doesn't track rack
.curvature_error_min_speed = 0.0, // disabled
.max_steer_power = 125,
};
int desired_curvature_raw = GET_BYTES(msg, 3, 2) & 0x7FFFU;
bool desired_curvature_sign = GET_BIT(msg, 39U);
if (!desired_curvature_sign) {
desired_curvature_raw *= -1;
}
bool steer_req = (((msg->data[1] >> 4) & 0x0FU) == 4U);
int steer_power = msg->data[2];
if (steer_curvature_cmd_checks(desired_curvature_raw, steer_power, steer_req, VOLKSWAGEN_MEB_STEERING_LIMITS)) {
tx = false;
}
}
if ((msg->addr == MSG_GRA_ACC_01) && !controls_allowed) {
// only allow cancel button: bit 13
if (!GET_BIT(msg, 13U)) {
tx = false;
}
}
return tx;
}
const safety_hooks volkswagen_meb_hooks = {
.init = volkswagen_meb_init,
.rx = volkswagen_meb_rx_hook,
.tx = volkswagen_meb_tx_hook,
.get_counter = volkswagen_mqb_meb_get_counter,
.get_checksum = volkswagen_mqb_meb_get_checksum,
.compute_checksum = volkswagen_meb_alt_crc_compute,
};
opendbc_repo/opendbc/safety/safety.h
+12
View File
@@ -30,6 +30,7 @@
#ifdef CANFD
#include "opendbc/safety/modes/hyundai_canfd.h"
#include "opendbc/safety/modes/volkswagen_meb.h"
#endif
uint32_t GET_BYTES(const CANPacket_t *msg, int start, int len) {
@@ -56,6 +57,7 @@ bool steering_disengage;
bool steering_disengage_prev;
bool cruise_engaged_prev = false;
struct sample_t vehicle_speed;
struct sample_t vehicle_speed_2;
bool vehicle_moving = false;
bool acc_main_on = false; // referred to as "ACC off" in ISO 15622:2018
int cruise_button_prev = 0;
@@ -86,6 +88,8 @@ uint32_t ts_angle_check_last = 0;
int desired_angle_last = 0;
struct sample_t angle_meas; // last 6 steer angles/curvatures
CurvatureSteeringState curvature_state;
int alternative_experience = 0;
@@ -425,6 +429,7 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
{SAFETY_TESLA_PREAP, &tesla_preap_hooks},
#ifdef CANFD
{SAFETY_HYUNDAI_CANFD, &hyundai_canfd_hooks},
{SAFETY_VOLKSWAGEN_MEB, &volkswagen_meb_hooks},
#endif
#ifdef ALLOW_DEBUG
{SAFETY_PSA, &psa_hooks},
@@ -455,6 +460,11 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
rt_angle_msgs = 0;
ts_angle_check_last = 0;
desired_angle_last = 0;
curvature_state.desired_last = 0;
curvature_state.rt_msgs = 0;
curvature_state.rt_msgs_prev = 0;
curvature_state.ts_check_last = 0;
curvature_state.steer_power_last = 0;
ts_torque_check_last = 0;
ts_steer_req_mismatch_last = 0;
valid_steer_req_count = 0;
@@ -462,9 +472,11 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
// reset samples
reset_sample(&vehicle_speed);
reset_sample(&vehicle_speed_2);
reset_sample(&torque_meas);
reset_sample(&torque_driver);
reset_sample(&angle_meas);
reset_sample(&curvature_state.meas);
controls_allowed = false;
relay_malfunction_reset();
opendbc_repo/opendbc/safety/tests/common.py
+109 -1
View File
@@ -9,6 +9,7 @@ from collections.abc import Callable
from opendbc.can import CANPacker
from opendbc.safety import ALTERNATIVE_EXPERIENCE
from opendbc.safety.tests.libsafety import libsafety_py
from opendbc.car.lateral import MAX_LATERAL_ACCEL, MAX_LATERAL_JERK
MAX_WRONG_COUNTERS = 5
MAX_SAMPLE_VALS = 6
@@ -888,6 +889,113 @@ class AngleSteeringSafetyTest(VehicleSpeedSafetyTest):
self.assertFalse(self._tx(self._angle_cmd_msg(angle=angle_cmd, enabled=True)))
class CurvatureSteeringSafetyTest(VehicleSpeedSafetyTest):
MAX_CURVATURE: float
MAX_CURVATURE_TEST: float
CURVATURE_TO_CAN: float
SEND_RATE: float
@classmethod
def setUpClass(cls):
if cls.__name__ == "CurvatureSteeringSafetyTest":
cls.safety = None
raise unittest.SkipTest
@abc.abstractmethod
def _curvature_cmd_msg(self, curvature: float, steer_req: bool):
pass
@abc.abstractmethod
def _curvature_meas_msg(self, curvature: float):
pass
def _set_prev_desired_curvature(self, curvature: float):
curvature_can = int(round(curvature * self.CURVATURE_TO_CAN))
self.safety.set_desired_curvature_last(curvature_can)
def _reset_curvature_measurement(self, curvature: float):
for _ in range(MAX_SAMPLE_VALS):
self._rx(self._curvature_meas_msg(curvature))
def _reset_speed_measurement(self, speed: float):
for _ in range(MAX_SAMPLE_VALS):
self._rx(self._speed_msg(speed))
self._rx(self._speed_msg_2(speed))
def test_curvature_measurements(self):
self._common_measurement_test(self._curvature_meas_msg, -self.MAX_CURVATURE, self.MAX_CURVATURE, self.CURVATURE_TO_CAN,
self.safety.get_curvature_meas_min, self.safety.get_curvature_meas_max)
def test_curvature_limit(self):
v = 1
for sign in (1, -1):
max_curvature = self.MAX_CURVATURE_TEST * sign
max_curvature_rate = MAX_LATERAL_JERK / v**2
max_curvature_delta = max_curvature_rate * self.SEND_RATE * sign
self._reset_speed_measurement(v)
self.safety.set_controls_allowed(True)
self._set_prev_desired_curvature(max_curvature)
self.assertTrue(self._tx(self._curvature_cmd_msg(max_curvature, True)), f"{v} {max_curvature} {max_curvature_delta}")
self.assertTrue(self.safety.get_controls_allowed())
self.assertTrue(self._tx(self._curvature_cmd_msg(max_curvature - max_curvature_delta, True)), f"{v} {max_curvature} {max_curvature_delta}")
self.assertTrue(self.safety.get_controls_allowed())
self.assertTrue(self._tx(self._curvature_cmd_msg(max_curvature, True)), f"{v} {max_curvature} {max_curvature_delta}")
self.assertTrue(self.safety.get_controls_allowed())
self.assertFalse(self._tx(self._curvature_cmd_msg(max_curvature + max_curvature_delta, True)), f"{v} {max_curvature} {max_curvature_delta}")
def test_iso_accel_limit(self):
speeds = [2., 5., 10., 15., 50.]
for v in speeds:
for sign in (1, -1):
max_curvature = np.clip(((MAX_LATERAL_ACCEL / (v - 1)**2) * sign), -self.MAX_CURVATURE_TEST, self.MAX_CURVATURE_TEST)
max_curvature_rate = MAX_LATERAL_JERK / (v - 1)**2
max_curvature_delta = max_curvature_rate * self.SEND_RATE * sign
self._reset_speed_measurement(v)
self.safety.set_controls_allowed(True)
self._set_prev_desired_curvature(max_curvature)
self.assertTrue(self._tx(self._curvature_cmd_msg(max_curvature, True)), f"{v} {max_curvature} {max_curvature_delta}")
self.assertTrue(self.safety.get_controls_allowed())
self.assertTrue(self._tx(self._curvature_cmd_msg(max_curvature - max_curvature_delta, True)), f"{v} {max_curvature} {max_curvature_delta}")
self.assertTrue(self.safety.get_controls_allowed())
self.assertTrue(self._tx(self._curvature_cmd_msg(max_curvature, True)), f"{v} {max_curvature} {max_curvature_delta}")
self.assertTrue(self.safety.get_controls_allowed())
self.assertFalse(self._tx(self._curvature_cmd_msg(max_curvature + max_curvature_delta, True)), f"{v} {max_curvature} {max_curvature_delta}")
def test_iso_jerk_limit(self):
speeds = [2., 5., 10., 15., 50.]
for v in speeds:
max_curvature_rate = MAX_LATERAL_JERK / (v - 1)**2
max_curvature_delta = max_curvature_rate * self.SEND_RATE
self._reset_speed_measurement(v)
self.safety.set_controls_allowed(True)
self._set_prev_desired_curvature(max_curvature_delta)
self.assertTrue(self._tx(self._curvature_cmd_msg(max_curvature_delta, True)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertTrue(self._tx(self._curvature_cmd_msg(0, True)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertTrue(self._tx(self._curvature_cmd_msg(-max_curvature_delta, True)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertFalse(self._tx(self._curvature_cmd_msg(max_curvature_delta, True)))
# after violation, prev is reset to 0, going past the jerk limit must fail
self.safety.set_controls_allowed(True)
self.assertFalse(self._tx(self._curvature_cmd_msg(2 * max_curvature_delta, True)))
class SafetyTest(SafetyTestBase):
TX_MSGS: list[list[int]] | None = None
SCANNED_ADDRS = [*range(0x800), # Entire 11-bit CAN address space
@@ -997,7 +1105,7 @@ class SafetyTest(SafetyTestBase):
'TestHyundaiCanCanfdBlendedLongitudinalSafety',
'TestHyundaiLegacyLongitudinalSafetyHEV'}):
continue
volkswagen_shared = ('TestVolkswagenMqb', 'TestVolkswagenMlb')
volkswagen_shared = ('TestVolkswagenMqb', 'TestVolkswagenMlb', 'TestVolkswagenMeb')
if attr.startswith(volkswagen_shared) and current_test.startswith(volkswagen_shared):
continue
opendbc_repo/opendbc/safety/tests/libsafety/libsafety_py.py
+5
View File
@@ -64,6 +64,11 @@ int get_desired_angle_last();
void set_angle_meas(int min, int max);
int get_angle_meas_min(void);
int get_angle_meas_max(void);
void set_desired_curvature_last(int t);
int get_desired_curvature_last(void);
void set_curvature_meas(int min, int max);
int get_curvature_meas_min(void);
int get_curvature_meas_max(void);
bool get_cruise_engaged_prev(void);
void set_cruise_engaged_prev(bool engaged);
opendbc_repo/opendbc/safety/tests/libsafety/safety.c
+21
View File
@@ -186,6 +186,27 @@ int get_angle_meas_max(void){
return angle_meas.max;
}
void set_desired_curvature_last(int t){
curvature_state.desired_last = t;
}
int get_desired_curvature_last(void){
return curvature_state.desired_last;
}
void set_curvature_meas(int min, int max){
curvature_state.meas.min = min;
curvature_state.meas.max = max;
}
int get_curvature_meas_min(void){
return curvature_state.meas.min;
}
int get_curvature_meas_max(void){
return curvature_state.meas.max;
}
// ***** car specific helpers *****
opendbc_repo/opendbc/safety/tests/test_volkswagen_meb.py
+342
View File
@@ -0,0 +1,342 @@
#!/usr/bin/env python3
import numpy as np
from opendbc.car.structs import CarParams
from opendbc.car.volkswagen.values import VolkswagenSafetyFlags
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerSafety
MAX_ACCEL = 2.0
MIN_ACCEL = -3.5
# MEB message IDs
MSG_LH_EPS_03 = 0x9F
MSG_ESC_51 = 0xFC
MSG_Motor_51 = 0x10B
MSG_GRA_ACC_01 = 0x12B
MSG_QFK_01 = 0x13D
MSG_ACC_18 = 0x14D
MSG_KLR_01 = 0x25D
MSG_TA_01 = 0x26B
MSG_ACC_19 = 0x300
MSG_HCA_03 = 0x303
MSG_LDW_02 = 0x397
MSG_MOTOR_14 = 0x3BE
class TestVolkswagenMebSafetyBase(common.CarSafetyTest, common.CurvatureSteeringSafetyTest):
STANDSTILL_THRESHOLD = 0
RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_03, MSG_LDW_02), 2: (MSG_KLR_01,)}
MAX_CURVATURE = 29105
MAX_CURVATURE_TEST = 0.195
CURVATURE_TO_CAN = 149253.7313
MAX_POWER = 125
MAX_POWER_TEST = 50
SEND_RATE = 0.02
LATERAL_FREQUENCY = 50 # Hz
cnt_curvature_cmd = 0
def _set_prev_desired_power(self, power: int):
# init with local tx sequence
prev_allowed = self.safety.get_controls_allowed()
self.safety.set_controls_allowed(True)
self._tx(self._curvature_cmd_msg(0, steer_req=True, power=power))
self.safety.set_controls_allowed(prev_allowed)
def test_power_limit(self):
max_power_can = self.MAX_POWER
max_power = self.MAX_POWER_TEST
self._set_prev_desired_power(max_power_can)
self.safety.set_controls_allowed(True)
self.assertTrue(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power + 1)))
self.safety.set_controls_allowed(True)
self.assertTrue(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power - 1)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=False, power=max_power)))
self.safety.set_controls_allowed(True)
self.assertTrue(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power)))
self.assertTrue(self.safety.get_controls_allowed())
self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=False, power=-max_power)))
def test_power_without_control(self):
max_power_can = self.MAX_POWER
max_power = self.MAX_POWER_TEST
self._set_prev_desired_power(max_power_can)
self.safety.set_controls_allowed(False)
self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=False, power=max_power)))
self.assertTrue(self._tx(self._curvature_cmd_msg(0, steer_req=False, power=0)))
self._set_prev_desired_power(max_power - 1)
self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=False, power=max_power)))
# When controls are not allowed, steer_req=True is gated only by the power check:
# steady or rising power is disallowed, but a strictly decreasing power (steering
# wind-down at disengagement) is permitted so the EPS torque authority ramps to zero
self._set_prev_desired_power(max_power - 1)
self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power))) # rising power
self.assertFalse(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power))) # steady power
self.assertTrue(self._tx(self._curvature_cmd_msg(0, steer_req=True, power=max_power - 1))) # decreasing power
def _speed_msg(self, speed_mps: float):
spd_kph = speed_mps * 3.6
values = {f"{s}_Radgeschw": spd_kph for s in ("VL", "VR", "HL", "HR")}
return self.packer.make_can_msg_safety("ESC_51", 0, values)
def _speed_msg_2(self, speed_mps: float):
values = {"ESP_v_Signal": speed_mps * 3.6}
return self.packer.make_can_msg_safety("ESP_21", 0, values)
def _motor_14_msg(self, brake):
values = {"MO_Fahrer_bremst": brake}
return self.packer.make_can_msg_safety("Motor_14", 0, values)
def _user_brake_msg(self, brake):
return self._motor_14_msg(brake)
def _user_gas_msg(self, gas):
values = {"Accel_Pedal_Pressure": 1 if gas else 0, "TSK_Status": 3}
return self.packer.make_can_msg_safety("Motor_51", 0, values)
def _vehicle_moving_msg(self, speed_mps: float):
return self._speed_msg(speed_mps)
def _curvature_meas_msg(self, curvature):
values = {"Curvature": abs(curvature), "Curvature_VZ": curvature > 0}
return self.packer.make_can_msg_safety("QFK_01", 0, values)
def _curvature_cmd_msg(self, curvature, steer_req=1, power=50, increment_timer=True):
if increment_timer:
self.safety.set_timer(self.cnt_curvature_cmd * int(1e6 / self.LATERAL_FREQUENCY))
self.__class__.cnt_curvature_cmd += 1
values = {
"Curvature": abs(curvature),
"Curvature_VZ": curvature > 0,
"RequestStatus": 4 if steer_req else 0,
"Power": power,
}
return self.packer.make_can_msg_safety("HCA_03", 0, values)
def _accel_msg(self, accel):
values = {"ACC_Sollbeschleunigung_02": accel}
return self.packer.make_can_msg_safety("ACC_18", 0, values)
def _tsk_status_msg(self, enable, main_switch=True):
if main_switch:
tsk_status = 3 if enable else 2
else:
tsk_status = 0
values = {"TSK_Status": tsk_status}
return self.packer.make_can_msg_safety("Motor_51", 0, values)
def _pcm_status_msg(self, enable):
return self._tsk_status_msg(enable)
def _torque_driver_msg(self, torque):
values = {"EPS_Lenkmoment": abs(torque), "EPS_VZ_Lenkmoment": torque < 0}
return self.packer.make_can_msg_safety("LH_EPS_03", 0, values)
def _button_msg(self, cancel=0, resume=0, _set=0, bus=2):
values = {"GRA_Abbrechen": cancel, "GRA_Tip_Setzen": _set, "GRA_Tip_Wiederaufnahme": resume}
return self.packer.make_can_msg_safety("GRA_ACC_01", bus, values)
def test_curvature_measurements(self):
self._rx(self._curvature_meas_msg(0.15))
self._rx(self._curvature_meas_msg(-0.1))
self._rx(self._curvature_meas_msg(0))
self._rx(self._curvature_meas_msg(0))
self._rx(self._curvature_meas_msg(0))
self._rx(self._curvature_meas_msg(0))
self.assertEqual(int(-0.1 * self.CURVATURE_TO_CAN), self.safety.get_curvature_meas_min())
self.assertEqual(int(0.15 * self.CURVATURE_TO_CAN), self.safety.get_curvature_meas_max())
self._rx(self._curvature_meas_msg(0))
self.assertEqual(0, self.safety.get_curvature_meas_max())
self.assertEqual(int(-0.1 * self.CURVATURE_TO_CAN), self.safety.get_curvature_meas_min())
self._rx(self._curvature_meas_msg(0))
self.assertEqual(0, self.safety.get_curvature_meas_max())
self.assertEqual(0, self.safety.get_curvature_meas_min())
def test_brake_signal(self):
self._rx(self._user_brake_msg(False))
self.assertFalse(self.safety.get_brake_pressed_prev())
self._rx(self._user_brake_msg(True))
self.assertTrue(self.safety.get_brake_pressed_prev())
def test_torque_driver_measurements(self):
for t in (0, 100, -100, 250, -250):
self._rx(self._torque_driver_msg(t))
def test_main_switch_off_disables_controls(self):
self.safety.set_controls_allowed(True)
self._rx(self._tsk_status_msg(False, main_switch=False))
self.assertFalse(self.safety.get_controls_allowed())
def test_cancel_button_rising_edge(self):
self.safety.set_controls_allowed(True)
self._rx(self._button_msg(cancel=1, bus=0))
self.assertFalse(self.safety.get_controls_allowed())
def test_rx_hook_speed_mismatch(self):
for speed in np.arange(0, 40, 0.5):
for speed_delta in np.arange(-5, 5, 0.1):
speed_2 = round(max(speed + speed_delta, 0), 1)
self._rx(self._speed_msg(speed))
self._rx(self._speed_msg_2(speed_2))
self.safety.set_controls_allowed(True)
self._tx(self._curvature_cmd_msg(0, steer_req=True))
within_delta = abs(speed - speed_2) <= common.MAX_SPEED_DELTA
self.assertEqual(self.safety.get_controls_allowed(), within_delta)
def test_curvature_violation(self):
# if violation occurs, desired_curvature_last is reset to 0
meas = self.MAX_CURVATURE_TEST / 4
self.safety.set_controls_allowed(True)
self._reset_curvature_measurement(meas)
self._set_prev_desired_curvature(0)
# cause a violation by sending a command far from prev=0
self.assertFalse(self._tx(self._curvature_cmd_msg(self.MAX_CURVATURE_TEST, steer_req=True, power=50)))
# prev should be reset to 0
self.assertEqual(0, self.safety.get_desired_curvature_last())
def test_curvature_cmd_when_not_steering(self):
# Tests that only a zero curvature is allowed while the steer
# actuation bit is 0, regardless of controls allowed or meas
for controls_allowed in (True, False):
self.safety.set_controls_allowed(controls_allowed)
for steer_req in (True, False):
for curvature_meas in np.arange(-self.MAX_CURVATURE_TEST, self.MAX_CURVATURE_TEST, self.MAX_CURVATURE_TEST / 5):
self._reset_curvature_measurement(curvature_meas)
for curvature_cmd in np.arange(-self.MAX_CURVATURE_TEST, self.MAX_CURVATURE_TEST, self.MAX_CURVATURE_TEST / 5):
self._set_prev_desired_curvature(curvature_cmd)
# controls_allowed is checked if actuation bit is 1, else the curvature must be zero (inactive)
should_tx = controls_allowed if steer_req else round(curvature_cmd * self.CURVATURE_TO_CAN) == 0
self.assertEqual(should_tx, self._tx(self._curvature_cmd_msg(curvature_cmd, steer_req=steer_req, power=50 if steer_req else 0)))
class TestVolkswagenMebStockSafety(TestVolkswagenMebSafetyBase):
TX_MSGS = [[MSG_HCA_03, 0], [MSG_LDW_02, 0], [MSG_GRA_ACC_01, 0], [MSG_GRA_ACC_01, 2],
[MSG_KLR_01, 0], [MSG_KLR_01, 2]]
FWD_BLACKLISTED_ADDRS = {0: [MSG_KLR_01], 2: [MSG_HCA_03, MSG_LDW_02]}
def setUp(self):
self.packer = CANPackerSafety("vw_meb_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenMeb, 0)
self.safety.init_tests()
def test_spam_cancel_safety_check(self):
self.safety.set_controls_allowed(0)
self.assertTrue(self._tx(self._button_msg(cancel=1)))
self.assertFalse(self._tx(self._button_msg(resume=1)))
self.assertFalse(self._tx(self._button_msg(_set=1)))
self.safety.set_controls_allowed(1)
self.assertTrue(self._tx(self._button_msg(resume=1)))
class TestVolkswagenMebGen2StockSafety(TestVolkswagenMebStockSafety):
def setUp(self):
self.packer = CANPackerSafety("vw_meb_2024_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenMeb, VolkswagenSafetyFlags.MEB_ALT_CRC)
self.safety.init_tests()
class TestVolkswagenMebLongSafety(TestVolkswagenMebSafetyBase):
TX_MSGS = [[MSG_HCA_03, 0], [MSG_LDW_02, 0], [MSG_ACC_19, 0], [MSG_ACC_18, 0],
[MSG_TA_01, 0], [MSG_KLR_01, 0], [MSG_KLR_01, 2]]
FWD_BLACKLISTED_ADDRS = {0: [MSG_KLR_01],
2: [MSG_HCA_03, MSG_LDW_02, MSG_ACC_19, MSG_ACC_18, MSG_TA_01]}
RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_03, MSG_LDW_02, MSG_ACC_19, MSG_ACC_18, MSG_TA_01),
2: (MSG_KLR_01,)}
ACCEL_OVERRIDE = 0
INACTIVE_ACCEL = 3.01
def setUp(self):
self.packer = CANPackerSafety("vw_meb_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenMeb, VolkswagenSafetyFlags.LONG_CONTROL)
self.safety.init_tests()
# stock cruise controls are entirely bypassed under openpilot longitudinal control
def test_disable_control_allowed_from_cruise(self):
pass
def test_enable_control_allowed_from_cruise(self):
pass
def test_cruise_engaged_prev(self):
pass
def test_set_and_resume_buttons(self):
for button in ["set", "resume"]:
# ACC main switch must be on, engage on falling edge
self.safety.set_controls_allowed(0)
self._rx(self._tsk_status_msg(False, main_switch=False))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0))
self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} with main switch off")
self._rx(self._tsk_status_msg(False, main_switch=True))
self._rx(self._button_msg(_set=(button == "set"), resume=(button == "resume"), bus=0))
self.assertFalse(self.safety.get_controls_allowed(), f"controls allowed on {button} rising edge")
self._rx(self._button_msg(bus=0))
self.assertTrue(self.safety.get_controls_allowed(), f"controls not allowed on {button} falling edge")
def test_cancel_button(self):
# Disable on rising edge of cancel button
self._rx(self._tsk_status_msg(False, main_switch=True))
self.safety.set_controls_allowed(1)
self._rx(self._button_msg(cancel=True, bus=0))
self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after cancel")
def test_main_switch(self):
# Disable as soon as main switch turns off
self._rx(self._tsk_status_msg(False, main_switch=True))
self.safety.set_controls_allowed(1)
self._rx(self._tsk_status_msg(False, main_switch=False))
self.assertFalse(self.safety.get_controls_allowed(), "controls allowed after ACC main switch off")
def test_accel_safety_check(self):
for controls_allowed in [True, False]:
for accel in np.concatenate((np.arange(MIN_ACCEL - 2, MAX_ACCEL + 2, 0.03), [0, self.INACTIVE_ACCEL])):
accel = round(accel, 2)
is_inactive_accel = accel == self.INACTIVE_ACCEL
send = (controls_allowed and MIN_ACCEL <= accel <= MAX_ACCEL) or is_inactive_accel
self.safety.set_controls_allowed(controls_allowed)
self.assertEqual(send, self._tx(self._accel_msg(accel)), (controls_allowed, accel))
def test_accel_override_with_gas(self):
self.safety.set_controls_allowed(True)
self.safety.set_gas_pressed_prev(True)
self.assertTrue(self._tx(self._accel_msg(self.ACCEL_OVERRIDE)))
self.assertFalse(self._tx(self._accel_msg(MAX_ACCEL)))
class TestVolkswagenMebGen2LongSafety(TestVolkswagenMebLongSafety):
def setUp(self):
self.packer = CANPackerSafety("vw_meb_2024_generated")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.volkswagenMeb,
VolkswagenSafetyFlags.LONG_CONTROL | VolkswagenSafetyFlags.MEB_ALT_CRC)
self.safety.init_tests()
selfdrive/car/redneck_cruise.py
+4 -4
View File
@@ -16,7 +16,7 @@ DECREASE_INACTIVE_TIMER = 0.05
LEAD_INCREASE_INACTIVE_TIMER = 0.05
MANUAL_BUTTON_INACTIVE_TIMER = 0.5
LEAD_RECOVERY_LOOKAHEAD_POINTS = 4
LEAD_RECOVERY_HOLD_BUFFER_MS = 0.5 * CV.MPH_TO_MS
LEAD_RECOVERY_HOLD_BUFFER_MS = 1.5 * CV.MPH_TO_MS
LEAD_COAST_BUFFER_MS = 1.0 * CV.MPH_TO_MS
LEAD_EXTRA_COAST_BUFFER_FACTOR = 0.6
LEAD_EXTRA_COAST_BUFFER_MAX_MS = 3.0 * CV.MPH_TO_MS
@@ -27,9 +27,9 @@ LEAD_PROACTIVE_COAST_HEADWAY_MAX_S = 4.0
LEAD_DEPARTURE_REL_SPEED_MIN_MS = 1.0 * CV.MPH_TO_MS
LEAD_DEPARTURE_HEADWAY_MIN_S = 1.8
LEAD_DEPARTURE_HEADWAY_MAX_S = 4.5
LEAD_DEPARTURE_BOOST_MIN_MS = 0.75 * CV.MPH_TO_MS
LEAD_DEPARTURE_BOOST_MAX_MS = 1.5 * CV.MPH_TO_MS
LEAD_DEPARTURE_BOOST_FACTOR = 0.35
LEAD_DEPARTURE_BOOST_MIN_MS = 1.25 * CV.MPH_TO_MS
LEAD_DEPARTURE_BOOST_MAX_MS = 3.0 * CV.MPH_TO_MS
LEAD_DEPARTURE_BOOST_FACTOR = 0.50
LEAD_DEPARTURE_PLAN_POINTS = 3
CRUISE_BUTTON_TIMERS = {
selfdrive/car/tests/test_redneck_cruise.py
+30
View File
@@ -454,6 +454,36 @@ class TestRedneckCruise(unittest.TestCase):
)
self.assertAlmostEqual(79.0 * CV.MPH_TO_MS, target_speed)
def test_target_speed_does_not_step_down_on_stale_opening_lead_plan(self):
target_speed = select_redneck_target_speed(
128.0,
79.0 * CV.MPH_TO_MS,
0.0,
[77.8 * CV.MPH_TO_MS, 77.7 * CV.MPH_TO_MS, 77.6 * CV.MPH_TO_MS, 77.5 * CV.MPH_TO_MS],
10,
allow_plan_decrease=True,
lead_present=True,
lead_distance_m=63.0,
lead_rel_speed_ms=2.0 * CV.MPH_TO_MS,
)
self.assertAlmostEqual(79.0 * CV.MPH_TO_MS, target_speed)
def test_target_speed_still_slows_for_large_opening_lead_plan_decrease(self):
target_speed = select_redneck_target_speed(
128.0,
79.0 * CV.MPH_TO_MS,
0.0,
[76.0 * CV.MPH_TO_MS, 75.5 * CV.MPH_TO_MS, 75.0 * CV.MPH_TO_MS],
10,
allow_plan_decrease=True,
lead_present=True,
lead_distance_m=63.0,
lead_rel_speed_ms=2.0 * CV.MPH_TO_MS,
)
self.assertLess(target_speed, 79.0 * CV.MPH_TO_MS)
def test_target_speed_does_not_use_recovery_branch_when_cluster_is_above_internal_max(self):
target_speed = select_redneck_target_speed(
45.0,
selfdrive/controls/controlsd.py
+15 -7
View File
@@ -20,6 +20,7 @@ from openpilot.selfdrive.controls.lib.lane_centering import LaneCenteringControl
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature
from openpilot.selfdrive.controls.lib.latcontrol_torque import (
BOLT_2018_2021_STEER_RATIO_TEST_SCALE,
LatControlTorque,
@@ -348,6 +349,8 @@ class Controls:
self.LaC: LatControl
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP, self.CI, DT_CTRL)
elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED:
self.LaC = LatControlCurvature(self.CP, self.CI, DT_CTRL)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL)
elif self.CP.lateralTuning.which() == 'torque':
@@ -677,14 +680,17 @@ class Controls:
lat_delay = self.sm["liveDelay"].lateralDelay + lat_smooth_seconds
actuators.curvature = self.desired_curvature
steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.steer_limited_by_safety, self.desired_curvature,
curvature_limited, lat_delay,
self.calibrated_pose,
self.sm['modelV2'],
self.starpilot_toggles)
steer, lateral_output, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.steer_limited_by_safety, self.desired_curvature,
curvature_limited, lat_delay,
self.calibrated_pose,
self.sm['modelV2'],
self.starpilot_toggles)
actuators.torque = float(steer)
actuators.steeringAngleDeg = float(steeringAngleDeg)
if self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED:
actuators.curvature = float(lateral_output)
else:
actuators.steeringAngleDeg = float(lateral_output)
if len(long_plan.speeds):
actuators.speed = long_plan.speeds[-1]
@@ -791,6 +797,8 @@ class Controls:
lat_tuning = self.CP.lateralTuning.which()
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
cs.lateralControlState.angleState = lac_log
elif self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED:
cs.lateralControlState.curvatureStateDEPRECATED = lac_log
elif lat_tuning == 'pid':
cs.lateralControlState.pidState = lac_log
elif lat_tuning == 'torque':
selfdrive/controls/lib/latcontrol_curvature.py
+64
View File
@@ -0,0 +1,64 @@
import math
from types import SimpleNamespace
import capnp
from cereal import log
from openpilot.common.pid import PIDController
from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
from openpilot.selfdrive.locationd.helpers import Pose
CURVATURE_SATURATION_THRESHOLD = 1e-3 # 1/m
class LatControlCurvature(LatControl):
def __init__(self, CP, CI, dt):
super().__init__(CP, CI, dt)
self.sat_check_min_speed = 5.
if CP.lateralTuning.which() == "pid":
tuning = CP.lateralTuning.pid
self.pid = PIDController((tuning.kpBP, tuning.kpV), (tuning.kiBP, tuning.kiV),
pos_limit=MAX_CURVATURE, neg_limit=-MAX_CURVATURE, rate=1 / dt)
self.kf = tuning.kf
else:
self.pid = None
self.kf = 1.
def reset(self):
super().reset()
if self.pid is not None:
self.pid.reset()
def update(self, active: bool, CS, VM, params, steer_limited_by_safety: bool, desired_curvature: float,
curvature_limited: bool, lat_delay: float, calibrated_pose: Pose,
model_data: capnp._DynamicStructReader, starpilot_toggles: SimpleNamespace):
curvature_log = log.ControlsState.LateralCurvatureState.new_message()
actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
error = desired_curvature - actual_curvature
if not active:
output_curvature = 0.0
curvature_log.active = False
if self.pid is not None:
self.pid.reset()
elif self.pid is None or CS.steeringPressed:
if self.pid is not None:
self.pid.reset()
output_curvature = self.kf * desired_curvature
curvature_log.active = True
else:
output_curvature = self.pid.update(error, speed=CS.vEgo, feedforward=self.kf * desired_curvature)
curvature_log.p = float(self.pid.p)
curvature_log.i = float(self.pid.i)
curvature_log.f = float(self.pid.f)
curvature_log.active = True
curvature_log.error = float(error)
curvature_log.actualCurvature = float(actual_curvature)
curvature_log.desiredCurvature = float(desired_curvature)
curvature_log.output = float(output_curvature)
curvature_log.saturated = bool(self._check_saturation(abs(error) > CURVATURE_SATURATION_THRESHOLD, CS,
False, curvature_limited))
return 0.0, float(output_curvature), curvature_log
selfdrive/controls/lib/latcontrol_torque.py
+8 -2
View File
@@ -129,6 +129,7 @@ class LatControlTorque(LatControl):
self.is_kia_carnival = CP.carFingerprint in KIA_CARNIVAL_CARS
self.is_tucson_4th_gen = CP.carFingerprint in TUCSON_4TH_GEN_CARS
self.is_civic_bosch_modified = CP.carFingerprint == HONDA_CAR.HONDA_CIVIC_BOSCH and bool(CP.flags & HondaFlags.EPS_MODIFIED)
self.is_honda_accord = CP.carFingerprint == HONDA_CAR.HONDA_ACCORD
self.is_silverado = CP.carFingerprint in SILVERADO_CARS
self.is_gmc_yukon_cc = CP.carFingerprint in GMC_YUKON_CC_CARS
self.is_ram_1500 = CP.carFingerprint in RAM_1500_CARS
@@ -143,6 +144,9 @@ class LatControlTorque(LatControl):
self.torque_ff_scale_neg = 1.0
self.torque_deadzone_boost = float(getattr(self.torque_params, "kfDEPRECATED", 0.0))
self.torque_ki_mult = 1.0
if self.is_honda_accord:
self.pid._k_p = [self.pid._k_p[0], [*self.pid._k_p[1][:-1], HONDA_ACCORD_TORQUE_KP]]
self.pid._k_i = [self.pid._k_i[0], [HONDA_ACCORD_TORQUE_KI] * len(self.pid._k_i[1])]
if self.is_palisade:
self.torque_params.latAccelFactor *= PALISADE_BASE_LAT_ACCEL_FACTOR_MULT
if self.is_ioniq_5:
@@ -566,8 +570,10 @@ class LatControlTorque(LatControl):
-low_speed_output_limit,
low_speed_output_limit,
))
elif self.is_ram_1500 and output_torque * setpoint > 0.0:
output_torque *= get_ram_1500_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif self.is_ram_1500:
output_torque *= get_ram_1500_center_output_scale(setpoint, CS.vEgo)
if output_torque * setpoint > 0.0:
output_torque *= get_ram_1500_transition_output_scale(setpoint, desired_lateral_jerk, CS.vEgo)
elif self.is_kona_non_scc:
output_torque *= get_kona_non_scc_center_taper_scale(setpoint, CS.vEgo)
rapid_reversal = setpoint * desired_lateral_jerk < 0.0
selfdrive/controls/lib/latcontrol_vehicle_tunes.py
+23 -3
View File
@@ -74,6 +74,8 @@ BOLT_2017_CARS = (
)
BOLT_CARS = BOLT_2022_2023_CARS + BOLT_2018_2021_CARS + BOLT_2017_CARS
HONDA_ACCORD_STEER_RATIO_SCALE = 14.0 / 16.33
HONDA_ACCORD_TORQUE_KP = 0.8
HONDA_ACCORD_TORQUE_KI = 0.15
VOLT_STANDARD_CARS = (
GM_CAR.CHEVROLET_VOLT,
GM_CAR.CHEVROLET_VOLT_2019,
@@ -197,7 +199,7 @@ RAM_1500_CARS = (
CHRYSLER_CAR.RAM_1500_5TH_GEN,
)
RAM_1500_BASE_LAT_ACCEL_FACTOR_MULT = 1.20
RAM_1500_BASE_LAT_ACCEL_FACTOR_MULT = 1.0
GENESIS_GV70_FRICTION_THRESHOLD_GAIN = 0.12
GENESIS_GV70_FRICTION_SPEED_ONSET = 8.0 * CV.MPH_TO_MS
@@ -237,7 +239,7 @@ GENESIS_G70_FRICTION_JERK_DEADZONE_LAT = 0.30
GENESIS_G70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08
GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED = 12.0
GENESIS_G70_FRICTION_JERK_DEADZONE_SPEED_WIDTH = 3.5
GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.12
GENESIS_G70_CENTER_OUTPUT_TAPER_MAX = 0.14
GENESIS_G70_CENTER_OUTPUT_TAPER_LAT = 0.30
GENESIS_G70_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.10
GENESIS_G70_CENTER_OUTPUT_TAPER_SPEED = 18.0
@@ -260,7 +262,7 @@ GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT = 0.14
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_LAT_WIDTH = 0.05
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED = 6.0
GENESIS_G70_LOW_SPEED_OUTPUT_LIMIT_SPEED_WIDTH = 1.5
GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST = 0.04
GENESIS_G70_CURVE_UNWIND_OUTPUT_BOOST = 0.02
GENESIS_G70_CURVE_UNWIND_SPEED = 18.0
GENESIS_G70_CURVE_UNWIND_SPEED_WIDTH = 3.0
GENESIS_G70_CURVE_UNWIND_LAT = 0.25
@@ -1152,6 +1154,13 @@ RAM_1500_PHASE_LAT_ONSET = 0.25
RAM_1500_PHASE_LAT_WIDTH = 0.12
RAM_1500_TURN_IN_FF_BOOST = 0.06
RAM_1500_UNWIND_FF_REDUCTION = 0.05
RAM_1500_CENTER_OUTPUT_TAPER_MAX = 0.12
RAM_1500_CENTER_OUTPUT_TAPER_LAT = 0.24
RAM_1500_CENTER_OUTPUT_TAPER_LAT_WIDTH = 0.08
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_ONSET = 5.5
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_ONSET_WIDTH = 1.5
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_MAX = 16.0
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_MAX_WIDTH = 2.0
# The Kona route is exceptionally accurate below highway speed, but Pop V2
# reverses the requested lateral acceleration roughly once per second at
@@ -1704,6 +1713,17 @@ def get_ram_1500_transition_output_scale(desired_lateral_accel: float, desired_l
return 1.0 - (RAM_1500_TRANSITION_TAPER_MAX * speed_weight * jerk_weight * lat_weight)
def get_ram_1500_center_output_scale(desired_lateral_accel: float, v_ego: float) -> float:
"""Damp only center corrections at low/mid speed, not turn authority."""
center_weight = _sigmoid((RAM_1500_CENTER_OUTPUT_TAPER_LAT - abs(desired_lateral_accel)) /
RAM_1500_CENTER_OUTPUT_TAPER_LAT_WIDTH)
speed_onset = _sigmoid((v_ego - RAM_1500_CENTER_OUTPUT_TAPER_SPEED_ONSET) /
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_ONSET_WIDTH)
speed_cutoff = _sigmoid((RAM_1500_CENTER_OUTPUT_TAPER_SPEED_MAX - v_ego) /
RAM_1500_CENTER_OUTPUT_TAPER_SPEED_MAX_WIDTH)
return 1.0 - (RAM_1500_CENTER_OUTPUT_TAPER_MAX * center_weight * speed_onset * speed_cutoff)
def get_ram_1500_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float:
phase = math.tanh((desired_lateral_accel * desired_lateral_jerk) / RAM_1500_PHASE_SCALE)
turn_in_weight = max(phase, 0.0)
selfdrive/controls/tests/test_latcontrol.py
+21
View File
@@ -36,9 +36,12 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import (
get_sonata_hybrid_center_output_scale,
get_prius_center_taper_scale,
KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT,
HONDA_ACCORD_TORQUE_KI,
HONDA_ACCORD_TORQUE_KP,
RAM_1500_BASE_LAT_ACCEL_FACTOR_MULT,
RAM_1500_MAX_LAT_JERK_UP,
get_gmc_yukon_cc_ff_scale,
get_ram_1500_center_output_scale,
get_ram_1500_transition_output_scale,
get_ram_1500_ff_scale,
get_rav4_tss2_pid_output,
@@ -1033,6 +1036,17 @@ class TestLatControl:
assert 0.6 < center_transition < medium_transition < 1.0
assert get_ram_1500_transition_output_scale(1.85, 2.5, 17.0) == pytest.approx(1.0)
def test_ram_1500_center_output_taper_is_speed_and_lat_gated(self):
center = get_ram_1500_center_output_scale(0.0, 10.0)
near_turn = get_ram_1500_center_output_scale(0.6, 10.0)
highway = get_ram_1500_center_output_scale(0.0, 25.0)
crawl = get_ram_1500_center_output_scale(0.0, 2.0)
assert center < 1.0
assert near_turn > center
assert highway > center
assert crawl > center
assert center > 0.85
def test_ram_1500_phase_feedforward_curve(self):
assert get_ram_1500_ff_scale(0.0, 1.0, 15.0) == pytest.approx(1.0)
assert get_ram_1500_ff_scale(1.2, 1.1, 17.0) > 1.0
@@ -1703,6 +1717,13 @@ class TestLatControl:
assert controller.pid._k_p[1] == pytest.approx([value * 2.0 for value in base_kp_v])
assert controller.pid._k_i[1] == pytest.approx([value * 1.25 for value in base_ki_v])
def test_honda_accord_torque_tune_uses_quick_curve_unwind(self):
controller, _, _, _, _ = self._build_torque_controller(HONDA.HONDA_ACCORD, force_torque=True)
assert controller.is_honda_accord
assert controller.pid._k_p[1][-1] == pytest.approx(HONDA_ACCORD_TORQUE_KP)
assert controller.pid._k_i[1] == pytest.approx([HONDA_ACCORD_TORQUE_KI] * len(controller.pid._k_i[1]))
def test_honda_accord_steer_ratio_calibration(self):
expected_scale = 14.0 / 16.33
assert get_honda_accord_steer_ratio_scale(0.0) == pytest.approx(expected_scale)
selfdrive/controls/tests/test_latcontrol_curvature.py
+51
View File
@@ -0,0 +1,51 @@
from types import SimpleNamespace
from cereal import car, custom, log
from opendbc.car.vehicle_model import VehicleModel
from opendbc.car.volkswagen.interface import CarInterface
from opendbc.car.volkswagen.values import CAR
from openpilot.common.realtime import DT_CTRL
from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE
from openpilot.selfdrive.controls.lib.latcontrol_curvature import LatControlCurvature
def build_controller():
cp = CarInterface.get_non_essential_params(CAR.VOLKSWAGEN_ID4_MK1)
ci = CarInterface(cp, custom.StarPilotCarParams.new_message())
controller = LatControlCurvature(cp.as_reader(), ci, DT_CTRL)
cs = car.CarState.new_message()
cs.vEgo = 15.0
cs.steeringAngleDeg = 0.0
params = log.LiveParametersData.new_message()
params.steerRatio = cp.steerRatio
params.stiffnessFactor = 1.0
params.roll = 0.0
params.angleOffsetDeg = 0.0
return controller, cs, VehicleModel(cp), params
def test_curvature_controller_output_and_reset():
controller, cs, vm, params = build_controller()
toggles = SimpleNamespace()
assert controller.pid.i_dt == DT_CTRL
_, output, state = controller.update(True, cs, vm, params, False, 0.01, False, 0.2, None, None, toggles)
assert state.active
assert 0.0 < output <= MAX_CURVATURE
_, output, state = controller.update(False, cs, vm, params, False, 0.01, False, 0.2, None, None, toggles)
assert not state.active
assert output == 0.0
def test_curvature_controller_uses_feedforward_during_driver_override():
controller, cs, vm, params = build_controller()
cs.steeringPressed = True
desired_curvature = -0.015
_, output, state = controller.update(True, cs, vm, params, False, desired_curvature, False, 0.2,
None, None, SimpleNamespace())
assert state.active
assert output == desired_curvature
selfdrive/modeld/modeld.py
+44 -98
View File
@@ -7,7 +7,6 @@ os.environ['GMMU'] = '0'
os.environ['DEV'] = 'QCOM' if TICI else 'LLVM'
from tinygrad.device import Device
from tinygrad.tensor import Tensor
import threading
import time
import pickle
import numpy as np
@@ -254,7 +253,7 @@ def _select_builtin_model(params: Params) -> None:
def _close_tinygrad_disk_cache_connection() -> None:
"""Drop a tinygrad cache connection before handing work to another thread."""
"""Drop tinygrad's process-global cache connection before loading the next model."""
import tinygrad.helpers as tinygrad_helpers
connection = getattr(tinygrad_helpers, "_db_connection", None)
@@ -437,7 +436,6 @@ class ModelState:
self.off_policy_enabled = "off_policy" in self.policy_order
self.off_policy_numpy_inputs = dict(self.numpy_inputs) if self.off_policy_enabled else {}
self.prev_desire = np.zeros(ModelConstants.DESIRE_LEN, dtype=np.float32)
self.nonfinite_count = 0
self.parser = Parser()
self.aux_parser = Parser(ignore_missing=True)
self.frame_buf_size = get_nv12_info(cam_w, cam_h)[3]
@@ -611,16 +609,9 @@ class ModelState:
)
outputs = [output.numpy().flatten() for output in output_tensors]
# A corrupted inference poisons recurrent state. Reset and retry a few
# times, then raise so modeld can fall back to the already-loaded CPU model.
if self.uses_external_gpu and any(not np.isfinite(output).all() for output in outputs):
self.nonfinite_count += 1
cloudlog.error(f"external GPU produced non-finite model output, resetting state ({self.nonfinite_count})")
self._reset_state()
if self.nonfinite_count >= 5:
raise RuntimeError("external GPU produced non-finite output after state reset")
cloudlog.error("external GPU model output not finite, dropping frame")
return None
self.nonfinite_count = 0
if self.model_type == "supercombo":
model_output = outputs[0]
@@ -661,6 +652,35 @@ def _load_model_state(cam_w: int, cam_h: int, selected_model: str, external_gpu_
return ModelState(cam_w, cam_h, False)
def _load_external_gpu_model(cam_w: int, cam_h: int, selected_model: str,
demo: bool = False) -> ModelState | None:
"""Load and warm the USB-GPU model without running another tinygrad model concurrently."""
candidate = None
try:
if not demo:
wait_for_external_gpu_power_ready()
_set_hcq_wait_timeout(BIG_MODEL_LOAD_WAIT_TIMEOUT_MS)
wait_usbgpu_link()
candidate = ModelState(
cam_w,
cam_h,
True,
model_id_override=selected_model,
write_model_version=False,
)
if not candidate.uses_external_gpu:
raise RuntimeError("external GPU model resolved to the builtin model")
candidate.warmup()
return candidate
except Exception:
cloudlog.exception("external GPU model load or warmup failed")
return None
finally:
_close_tinygrad_disk_cache_connection()
_set_hcq_wait_timeout(BIG_MODEL_RUN_WAIT_TIMEOUT_MS)
def main(demo=False):
cloudlog.warning("modeld init")
@@ -709,14 +729,14 @@ def main(demo=False):
model = None
small_model = None
big_model = None
loader = None
loader_done = threading.Event()
native_model_ready = threading.Event()
loader_result_handled = False
if external_gpu_requested:
# Never make on-road startup depend on the external GPU. The native model
# starts immediately while the GPU loader waits out the vehicle's power
# transition in the background.
big_model = _load_external_gpu_model(
vipc_client_main.width,
vipc_client_main.height,
selected_model,
demo,
)
small_model = ModelState(
vipc_client_main.width,
vipc_client_main.height,
@@ -724,63 +744,18 @@ def main(demo=False):
model_id_override=BUILTIN_MODEL_KEY,
write_model_version=False,
)
model = small_model
def load_big_model() -> None:
nonlocal big_model
candidate = None
try:
if not demo:
wait_for_external_gpu_power_ready()
# Let the native model complete one real frame first. Besides ensuring
# model output is available, this lets the main thread release
# tinygrad's thread-bound SQLite cache before this worker uses it.
native_model_ready.wait()
# Loading the large artifact streams weights into VRAM. Use a longer
# queue watchdog only for that phase; normal inference restores the
# short watchdog before activation.
_set_hcq_wait_timeout(BIG_MODEL_LOAD_WAIT_TIMEOUT_MS)
from tinygrad.helpers import DEV
device_config = tinygrad_dev_config(True, TICI)
DEV.value = device_config
os.environ["DEV"] = device_config
wait_usbgpu_link()
candidate = ModelState(
vipc_client_main.width,
vipc_client_main.height,
True,
model_id_override=selected_model,
write_model_version=False,
)
if not candidate.uses_external_gpu:
raise RuntimeError("external GPU model resolved to the builtin model")
candidate.warmup()
except Exception:
cloudlog.exception("external GPU model load or warmup failed")
candidate = None
finally:
# tinygrad's global SQLite cache connection is thread-bound. Loading and
# warming here can create it in this worker, so close it here before the
# model (or native fallback) runs on modeld's main thread.
_close_tinygrad_disk_cache_connection()
big_model = candidate
loader_done.set()
loader = threading.Thread(target=load_big_model, name="big_model_loader", daemon=True)
loader.start()
model = big_model if big_model is not None else small_model
if big_model is not None:
params.put("ModelVersion", model.policy_generation)
params.put("DrivingModelVersion", model.policy_generation)
else:
model = _load_model_state(vipc_client_main.width, vipc_client_main.height, selected_model, False, params)
external_gpu_active = model.uses_external_gpu
params.put_bool("UsbGpuCompiled", external_model_selected and file_chunked_exists(external_artifact))
params.put_bool("UsbGpuActive", external_gpu_active)
params.put_bool("UsbGpuLoading", external_gpu_requested)
if external_gpu_requested:
cloudlog.warning(f"native model loaded in {time.monotonic() - start_time:.1f}s; external GPU load scheduled")
else:
cloudlog.warning(f"model loaded in {time.monotonic() - start_time:.1f}s, modeld starting")
params.put_bool("UsbGpuLoading", False)
cloudlog.warning(f"models loaded in {time.monotonic() - start_time:.1f}s, modeld starting")
# messaging
publish_services = ["modelV2", "drivingModelData", "cameraOdometry", "starpilotModelV2"]
@@ -857,29 +832,6 @@ def main(demo=False):
sm.update(0)
if external_gpu_requested and loader_done.is_set() and not loader_result_handled:
loader.join()
_set_hcq_wait_timeout(BIG_MODEL_RUN_WAIT_TIMEOUT_MS)
loader_result_handled = True
if big_model is None:
params.put_bool("UsbGpuLoading", False)
cloudlog.error("external GPU model unavailable; continuing with builtin model")
# A model swap resets recurrent state, so only activate the external model
# while controls are known to be disengaged. The native model keeps
# publishing normally until this condition is met.
if big_model is not None and not external_gpu_active and sm.seen["carControl"] and not sm["carControl"].enabled:
model = big_model
external_gpu_active = True
params.put("ModelVersion", model.policy_generation)
params.put("DrivingModelVersion", model.policy_generation)
params.put_bool("UsbGpuActive", True)
params.put_bool("UsbGpuLoading", False)
if chestnut_state is not None:
chestnut_state.big = True
run_count = 0
cloudlog.warning(f"external GPU model {selected_model} activated while controls disengaged")
long_smooth_seconds = _model_smooth_seconds(params, "LongSmoothSeconds", LONG_SMOOTH_SECONDS)
long_delay = CP.longitudinalActuatorDelay + long_smooth_seconds
desire = DH.desire
@@ -978,12 +930,6 @@ def main(demo=False):
run_count = 0
model_output = None
if external_gpu_requested and not native_model_ready.is_set() and model_output is not None:
# The cache connection was created on this thread while preparing the
# native model. Close it here before permitting the loader thread to use
# tinygrad's process-global connection.
_close_tinygrad_disk_cache_connection()
native_model_ready.set()
mt2 = time.perf_counter()
model_execution_time = mt2 - mt1
selfdrive/modeld/tests/test_usbgpu_helpers.py
+93 -1
View File
@@ -115,7 +115,7 @@ def test_chestnut_telemetry_is_bounded_when_amd_is_unavailable(monkeypatch):
assert not message.valid
def test_tinygrad_disk_cache_connection_is_closed_before_thread_handoff(monkeypatch):
def test_tinygrad_disk_cache_connection_is_closed_between_models(monkeypatch):
import tinygrad.helpers as tinygrad_helpers
class FakeConnection:
@@ -134,6 +134,98 @@ def test_tinygrad_disk_cache_connection_is_closed_before_thread_handoff(monkeypa
assert tinygrad_helpers._db_connection is None
def test_external_gpu_load_finishes_before_native_model_can_start(monkeypatch):
calls = []
class FakeModelState:
uses_external_gpu = True
def __init__(self, cam_w, cam_h, external_gpu_active, model_id_override, write_model_version):
calls.append(("model", cam_w, cam_h, external_gpu_active, model_id_override, write_model_version))
def warmup(self):
calls.append("warmup")
monkeypatch.setattr(modeld, "wait_for_external_gpu_power_ready", lambda: calls.append("power"))
monkeypatch.setattr(modeld, "wait_usbgpu_link", lambda: calls.append("link"))
monkeypatch.setattr(modeld, "_set_hcq_wait_timeout", lambda timeout: calls.append(("timeout", timeout)))
monkeypatch.setattr(modeld, "_close_tinygrad_disk_cache_connection", lambda: calls.append("close_cache"))
monkeypatch.setattr(modeld, "ModelState", FakeModelState)
monkeypatch.setattr(
modeld,
"tinygrad_dev_config",
lambda *_args: (_ for _ in ()).throw(AssertionError("runtime must not change tinygrad's process-global DEV")),
)
loaded = modeld._load_external_gpu_model(1928, 1208, "big-model")
assert isinstance(loaded, FakeModelState)
assert calls == [
"power",
("timeout", modeld.BIG_MODEL_LOAD_WAIT_TIMEOUT_MS),
"link",
("model", 1928, 1208, True, "big-model", False),
"warmup",
"close_cache",
("timeout", modeld.BIG_MODEL_RUN_WAIT_TIMEOUT_MS),
]
def test_external_gpu_nonfinite_outputs_are_dropped_without_escalating(monkeypatch):
class FakeTensor:
@staticmethod
def from_blob(*_args, **_kwargs):
return FakeTensor()
class FakeOutput:
def numpy(self):
return np.array([np.nan], dtype=np.float32)
state = modeld.ModelState.__new__(modeld.ModelState)
state.uses_external_gpu = True
state.frame_buf_size = 4
state.vision_input_names = ["img", "big_img"]
state.road_key = "img"
state.wide_key = "big_img"
state._blob_cache = {}
state._warp_dev = "CPU"
state._queue_dev = "CPU"
state.desire_key = "desire_pulse"
state.prev_desired_curv_key = None
state.numpy_inputs = {"desire_pulse": np.zeros(8, dtype=np.float32)}
state.npy = {
"desire": np.zeros(8, dtype=np.float32),
"tfm": np.zeros((3, 3), dtype=np.float32),
"big_tfm": np.zeros((3, 3), dtype=np.float32),
}
state.prev_desire = np.zeros(8, dtype=np.float32)
state.warp_input_keys = ()
state.policy_input_keys = ()
state.input_queues = {}
state.image_history_pipeline = modeld.IMAGE_HISTORY_IN_POLICY
state.warp_enqueue = lambda **_kwargs: object()
state.run_policy = lambda **_kwargs: (FakeOutput(),)
state._reset_state = MethodType(
lambda self: (_ for _ in ()).throw(AssertionError("upstream does not reset or escalate transient non-finite output")),
state,
)
monkeypatch.setattr(modeld, "Tensor", FakeTensor)
monkeypatch.setattr(modeld.cloudlog, "error", lambda *_args, **_kwargs: None)
buffers = {
"img": SimpleNamespace(data=bytearray(4)),
"big_img": SimpleNamespace(data=bytearray(4)),
}
transforms = {
"img": np.eye(3, dtype=np.float32),
"big_img": np.eye(3, dtype=np.float32),
}
inputs = {"desire_pulse": np.zeros(8, dtype=np.float32)}
for _ in range(10):
assert state.run(buffers, transforms, inputs, False) is None
def test_out_of_band_artifact_round_trip():
artifact = {"weights": np.arange(32, dtype=np.float32), "metadata": {"version": 1}}
stream = io.BytesIO()
selfdrive/selfdrived/selfdrived.py
+22 -3
View File
@@ -56,6 +56,18 @@ MonitoringPolicy = log.DriverMonitoringState.MonitoringPolicy
StarPilotEventName = custom.StarPilotOnroadEvent.EventName
IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
VALID_ONLY_COMM_ISSUE_GRACE_FRAMES = max(1, round(0.25 / DT_CTRL))
def evaluate_comm_issue(all_checks: bool, all_alive: bool, all_freq_ok: bool,
valid_only_frames: int) -> tuple[bool, int]:
if all_checks:
return False, 0
if not all_alive or not all_freq_ok:
return True, 0
valid_only_frames += 1
return valid_only_frames >= VALID_ONLY_COMM_ISSUE_GRACE_FRAMES, valid_only_frames
def commanded_torque_at_max_for_saturation(CP, output: float) -> bool:
@@ -222,6 +234,7 @@ class SelfdriveD:
self.last_functional_fan_frame = 0
self.events_prev = []
self.logged_comm_issue = None
self.valid_only_comm_issue_frames = 0
self.not_running_prev = None
self.big_model_loading = False
self.big_model_attempted = False
@@ -651,10 +664,16 @@ class SelfdriveD:
contains_event_type(self.events, self.starpilot_events, ET.IMMEDIATE_DISABLE))
no_system_errors = (not has_disable_events) or (len(self.events) == num_events)
big_model_settling = self.big_model_loading or time.monotonic() < self.big_model_ready_t + 5.
if not self.sm.all_checks() and no_system_errors and not big_model_settling:
if not self.sm.all_alive():
all_checks = self.sm.all_checks()
all_alive = self.sm.all_alive() if not all_checks else True
all_freq_ok = self.sm.all_freq_ok() if not all_checks else True
report_comm_issue, self.valid_only_comm_issue_frames = evaluate_comm_issue(
all_checks, all_alive, all_freq_ok, self.valid_only_comm_issue_frames,
)
if not all_checks and report_comm_issue and no_system_errors and not big_model_settling:
if not all_alive:
self.events.add(EventName.commIssue)
elif not self.sm.all_freq_ok():
elif not all_freq_ok:
self.events.add(EventName.commIssueAvgFreq)
else:
self.events.add(EventName.commIssue)
selfdrive/selfdrived/tests/test_selfdrived.py
+26 -1
View File
@@ -4,7 +4,32 @@ from cereal import car, custom, log
from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR
from opendbc.car.nissan.values import CAR as NISSAN_CAR
from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD, commanded_torque_at_max_for_saturation
from openpilot.selfdrive.selfdrived.selfdrived import (
VALID_ONLY_COMM_ISSUE_GRACE_FRAMES,
SelfdriveD,
commanded_torque_at_max_for_saturation,
evaluate_comm_issue,
)
def test_valid_only_comm_issue_is_debounced():
frames = 0
for _ in range(VALID_ONLY_COMM_ISSUE_GRACE_FRAMES - 1):
should_alert, frames = evaluate_comm_issue(False, True, True, frames)
assert not should_alert
should_alert, frames = evaluate_comm_issue(False, True, True, frames)
assert should_alert
assert frames == VALID_ONLY_COMM_ISSUE_GRACE_FRAMES
should_alert, frames = evaluate_comm_issue(True, True, True, frames)
assert not should_alert
assert frames == 0
def test_dead_or_slow_comm_issue_is_immediate():
assert evaluate_comm_issue(False, False, True, 0) == (True, 0)
assert evaluate_comm_issue(False, True, False, 0) == (True, 0)
class FakeFallbackParams:
selfdrive/ui/layouts/settings/starpilot/longitudinal.py
+30 -10
View File
@@ -111,7 +111,7 @@ class AdaptiveSpeedView(CardHubManagerView):
# ═══════════════════════════════════════════════════════════════
# LongitudinalManagerView — 6-card category hub
# LongitudinalManagerView — 7-card category hub
# ═══════════════════════════════════════════════════════════════
class LongitudinalManagerView(CardHubManagerView):
@@ -138,6 +138,12 @@ class LongitudinalManagerView(CardHubManagerView):
"icon": "navigate",
"on_click": lambda: self._controller._navigate_to("slc"),
},
{
"title": tr("Vision Speed Limits"),
"desc": tr("Detect and display speed-limit signs without enabling Speed Limit Controller."),
"icon": "road",
"on_click": lambda: self._controller._navigate_to("vision_speed_limits"),
},
{
"title": tr("Adaptive Speed Controls"),
"desc": tr("Configure Curve Speed Controller and Conditional Experimental Mode triggers."),
@@ -633,10 +639,6 @@ class StarPilotLongitudinalLayout(_SettingsPage):
get_state=lambda: self._params.get_bool("SLCMapboxFiller"),
set_state=lambda s: self._params.put_bool("SLCMapboxFiller", s),
visible=self._mapbox_available),
SettingRow("VisionSpeedLimit", "toggle", tr_noop("Vision Detection"),
subtitle=tr_noop("Use the road camera to detect speed limit signs for SLC."),
get_state=lambda: self._params.get_bool("VisionSpeedLimitDetection"),
set_state=lambda s: self._params.put_bool("VisionSpeedLimitDetection", s)),
SettingRow("ShowSLCOffset", "toggle", tr_noop("Show SLC Offset"),
subtitle="",
get_state=lambda: self._params.get_bool("ShowSLCOffset"),
@@ -661,6 +663,14 @@ class StarPilotLongitudinalLayout(_SettingsPage):
on_click=self._show_slc_offsets_category),
]
# ── 4. Vision Speed Limits Rows ──
self._vision_speed_limit_rows = [
SettingRow("VisionSpeedLimit", "toggle", tr_noop("Vision Detection"),
subtitle=tr_noop("Use the road camera to detect and display speed-limit signs, with optional use by Speed Limit Controller."),
get_state=lambda: self._params.get_bool("VisionSpeedLimitDetection"),
set_state=lambda s: self._params.put_bool("VisionSpeedLimitDetection", s)),
]
# Initialize SLC Offsets rows
self._slc_offset_rows = []
for i in range(1, 8):
@@ -672,7 +682,7 @@ class StarPilotLongitudinalLayout(_SettingsPage):
on_click=lambda k=key: self._show_slider(k, *self._speed_range(), unit=self._speed_unit()),
))
# ── 4. Adaptive Speed Controls Rows (CES + CSC + CCM) ──
# ── 5. Adaptive Speed Controls Rows (CES + CSC + CCM) ──
self._curve_speed_controller_rows = [
SettingRow("CalibratedLatAccel", "value", tr_noop("Calibrated Lateral Accel"),
subtitle=tr_noop("The learned lateral acceleration from collected driving data. Higher values allow faster cornering."),
@@ -692,7 +702,7 @@ class StarPilotLongitudinalLayout(_SettingsPage):
visible=csc_on),
]
# ── 5. Driving Personalities Rows ──
# ── 6. Driving Personalities Rows ──
self._personality_rows = [
SettingRow("Traffic", "value", tr_noop("Traffic"),
subtitle=tr_noop("Configure follow distance, smoothness, and response for traffic conditions."),
@@ -712,7 +722,7 @@ class StarPilotLongitudinalLayout(_SettingsPage):
on_click=lambda: self._show_personality_profile_category("Relaxed")),
]
# ── 6. Daily QOL & Weather Rows ──
# ── 7. Daily QOL & Weather Rows ──
self._daily_rows = [
SettingRow("CustomCruise", "value", tr_noop("Cruise Interval"),
subtitle="",
@@ -761,10 +771,10 @@ class StarPilotLongitudinalLayout(_SettingsPage):
unit=self._speed_unit()),
visible=lambda: self._params.get_bool("QOLLongitudinal")),
SettingRow("PulseGlideSpeedDelta", "value", tr_noop("Pulse and Glide Delta"),
subtitle=tr_noop("Coast this far below the current cruise target before accelerating back up."),
subtitle=tr_noop("Developer-only: coast this far below the current cruise target before accelerating back up."),
get_value=lambda: f"{self._params.get_float('PulseGlideSpeedDelta'):.1f}{self._speed_unit()}",
on_click=lambda: self._show_slider("PulseGlideSpeedDelta"),
visible=lambda: self._developer_feature_access()),
visible=lambda: self._params.get_bool("QOLLongitudinal") and self._developer_feature_access()),
SettingRow("MapGears", "toggle", tr_noop("Map Gears"),
subtitle="",
get_state=lambda: self._params.get_bool("MapGears"),
@@ -827,6 +837,8 @@ class StarPilotLongitudinalLayout(_SettingsPage):
pt_daily = self._make_parent("QOLLongitudinal", "Quality of Life")
pt_slc = self._make_parent("SpeedLimitController", "Speed Limit Controller",
"Limit the car's maximum speed to the current speed limit.")
pt_vision_speed_limits = self._make_parent("VisionSpeedLimitDetection", "Vision Speed Limits",
"Detect and display speed-limit signs without enabling the Speed Limit Controller.")
pt_csc = self._make_parent("CurveSpeedController", "Curve Speed Controller",
"Configure speed control on curves and reset collected calibration data.")
@@ -869,6 +881,14 @@ class StarPilotLongitudinalLayout(_SettingsPage):
parent_toggle=pt_slc,
panel_style=PANEL_STYLE,
)
self._sub_panels["vision_speed_limits"] = AetherSettingsView(
self,
[SettingSection(title="", rows=self._vision_speed_limit_rows)],
header_title=tr_noop("Vision Speed Limits"),
header_subtitle=tr_noop("Detect and display speed-limit signs without enabling the Speed Limit Controller."),
parent_toggle=pt_vision_speed_limits,
panel_style=PANEL_STYLE,
)
self._sub_panels["personality"] = AetherSettingsView(
self,
[SettingSection(title="", rows=self._personality_rows)],
selfdrive/ui/mici/onroad/hud_renderer.py
+3 -5
View File
@@ -204,8 +204,8 @@ class HudRenderer(Widget):
if (engaged and not self._engaged and not ui_state.usbgpu_loading and ui_state.usbgpu_active is not True and
sm.recv_frame['modelV2'] > ui_state.started_frame):
self._small_model_engaged = True
if engaged and not self._engaged:
self._egpu_fade_time = rl.get_time()
if engaged != self._engaged:
self._egpu_fade_time = rl.get_time() if engaged else 0
if (set_speed != self.set_speed and engaged) or (engaged and not self._engaged):
self._set_speed_changed_time = rl.get_time()
self._engaged = engaged
@@ -341,9 +341,7 @@ class HudRenderer(Widget):
if icon is not self._egpu_icon:
self._egpu_fade_time = rl.get_time()
self._egpu_icon = icon
alpha = self._egpu_alpha_filter.update(
loading or (0 < rl.get_time() - self._egpu_fade_time < SET_SPEED_PERSISTENCE and self._engaged)
)
alpha = self._egpu_alpha_filter.update(loading or 0 < rl.get_time() - self._egpu_fade_time < SET_SPEED_PERSISTENCE)
if alpha < 1e-2:
return
selfdrive/ui/onroad/starpilot/developer_sidebar.py
+6 -8
View File
@@ -159,20 +159,18 @@ class DeveloperSidebar:
metric_rect = rl.Rectangle(card_x, y, METRIC_WIDTH, METRIC_HEIGHT)
edge_rect = rl.Rectangle(metric_rect.x + METRIC_WIDTH - 4 - 100, metric_rect.y + 4, 100, 118)
rl.begin_scissor_mode(
int(metric_rect.x + METRIC_WIDTH - 4 - 18),
int(metric_rect.y),
18,
int(metric_rect.height)
)
rl.draw_rectangle_rounded(edge_rect, 0.3, 10, color)
rl.end_scissor_mode()
accent_x = metric_rect.x + METRIC_WIDTH - 4 - 18
rl.draw_rectangle_rec(
rl.Rectangle(edge_rect.x, metric_rect.y, accent_x - edge_rect.x, metric_rect.height),
rl.BLACK,
)
rl.draw_rectangle_rounded_lines_ex(metric_rect, 0.3, 10, 2, _WHITE_DIM)
max_w = metric_rect.width - 22
labels = wrap_text(self._font_bold, label_first, max_w, FONT_SIZE, max_lines=2) if label_second == "" else [label_first, label_second]
if len(labels) == 1:
text = labels[0]
text_size = measure_text_cached(self._font_bold, text, FONT_SIZE)
selfdrive/ui/tests/test_ui_state_performance.py
+10 -21
View File
@@ -1,4 +1,3 @@
import threading
import time
from types import SimpleNamespace
@@ -11,30 +10,20 @@ def test_raylib_ui_uses_read_through_param_cache():
assert ui_state_module.ui_state.ui_params is not ui_state_module.ui_state.params
def test_usbgpu_poll_does_not_block_ui_thread(monkeypatch):
started = threading.Event()
release = threading.Event()
def poll():
started.set()
release.wait(timeout=1.0)
return True
monkeypatch.setattr(ui_state_module, "chestnut_present", poll)
def test_usbgpu_presence_comes_from_device_state_and_persists_onroad():
state = object.__new__(ui_state_module.UIState)
state.usbgpu = False
state._usbgpu_update_time = 0.0
state._usbgpu_poll_thread = None
state.started = True
state._schedule_usbgpu_poll(now=1.0, force=True)
assert started.wait(timeout=0.2)
polling_thread = state._usbgpu_poll_thread
state._schedule_usbgpu_poll(now=2.0, force=True)
assert state._usbgpu_poll_thread is polling_thread
state._update_usbgpu_presence(True)
assert state.usbgpu
release.set()
polling_thread.join(timeout=1.0)
assert state.usbgpu is True
state._update_usbgpu_presence(False)
assert state.usbgpu
state.started = False
state._update_usbgpu_presence(False)
assert not state.usbgpu
def test_ui_update_reports_subphases(monkeypatch):
selfdrive/ui/ui_state.py
+5 -24
View File
@@ -11,12 +11,10 @@ from openpilot.common.params import Params
from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.ui.lib.prime_state import PrimeState
from openpilot.selfdrive.ui.lib.ui_param_cache import shared_ui_params
from openpilot.system.hardware.usb import chestnut_present
from openpilot.system.ui.lib.application import gui_app
from openpilot.system.hardware import HARDWARE, PC
BACKLIGHT_OFFROAD = 65 if HARDWARE.get_device_type() == "mici" else 50
USBGPU_POLL_INTERVAL = 1.0
def _noop_progress(_phase: str) -> None:
@@ -93,8 +91,6 @@ class UIState:
self.usbgpu_compiled: bool = self.params.get_bool("UsbGpuCompiled")
self.usbgpu_active: bool = self.params.get_bool("UsbGpuActive")
self.usbgpu_loading: bool = self.params.get_bool("UsbGpuLoading")
self._usbgpu_update_time: float = 0.0
self._usbgpu_poll_thread: threading.Thread | None = None
self.started: bool = False
self.ignition: bool = False
self.recording_audio: bool = False
@@ -129,26 +125,8 @@ class UIState:
self._offroad_transition_callbacks: list[Callable[[], None]] = []
self._engaged_transition_callbacks: list[Callable[[], None]] = []
self._schedule_usbgpu_poll(force=True)
self.update_params()
def _poll_usbgpu_presence(self) -> None:
try:
self.usbgpu = chestnut_present()
except Exception:
cloudlog.exception("USB GPU presence poll failed")
def _schedule_usbgpu_poll(self, now: float | None = None, force: bool = False) -> None:
now = time.monotonic() if now is None else now
if not force and now - self._usbgpu_update_time < USBGPU_POLL_INTERVAL:
return
if self._usbgpu_poll_thread is not None and self._usbgpu_poll_thread.is_alive():
return
self._usbgpu_update_time = now
self._usbgpu_poll_thread = threading.Thread(target=self._poll_usbgpu_presence, name="ui_usbgpu_poll", daemon=True)
self._usbgpu_poll_thread.start()
def add_offroad_transition_callback(self, callback: Callable[[], None]):
self._offroad_transition_callbacks.append(callback)
@@ -161,6 +139,10 @@ class UIState:
def add_engaged_transition_callback(self, callback: Callable[[], None]):
self._engaged_transition_callbacks.append(callback)
def _update_usbgpu_presence(self, present: bool) -> None:
# Keep the eGPU UI active until the offroad transition if the dock drops out onroad.
self.usbgpu = present or (self.usbgpu and self.started)
@property
def engaged(self) -> bool:
return self.started and self.sm["selfdriveState"].enabled
@@ -221,14 +203,13 @@ class UIState:
started |= force_onroad
started &= not force_offroad
self.started = started
self._update_usbgpu_presence(self.sm["deviceState"].chestnutPresent)
# Update recording audio state
self.recording_audio = params.get_bool("RecordAudio") and self.started
self.is_metric = params.get_bool("IsMetric")
self.always_on_dm = params.get_bool("AlwaysOnDM")
now = time.monotonic()
self._schedule_usbgpu_poll(now)
self.usbgpu_compiled = params.get_bool("UsbGpuCompiled")
self.usbgpu_active = params.get_bool("UsbGpuActive")
self.usbgpu_loading = params.get_bool("UsbGpuLoading")
starpilot/system/adj_spot_monitor_vision.py
+1 -1
View File
@@ -20,7 +20,7 @@ from openpilot.starpilot.system.adj_spot_monitor_vision_inference import VASMInf
V_ASM_AFFINITY_CORES = [2]
V_ASM_SOLO_AFFINITY_CORES = [0, 1, 2]
BASE_INTERVAL = 0.500
BASE_INTERVAL = 0.750
FOLLOWUP_INTERVAL = 0.200
FOLLOWUP_WINDOW = 1.5
starpilot/system/speed_limit_vision.py
+17 -8
View File
@@ -19,8 +19,9 @@ from openpilot.common.swaglog import cloudlog
from openpilot.starpilot.common.cpu_throttle import device_cpu_throttle_factor
from openpilot.system.hardware import PC
RUNTIME_LOOP_HZ = 20
INFERENCE_INTERVAL = 0.15
RUNTIME_LOOP_HZ = 30
# Cap steady detector work at 6 Hz while retaining the faster confirmation cadence.
INFERENCE_INTERVAL = 1.0 / 6.0
FOLLOWUP_INFERENCE_INTERVAL = 0.10
FOLLOWUP_WINDOW_SECONDS = 2.0
TEMPORAL_TRACKING_ENABLED = False
@@ -408,6 +409,7 @@ class SpeedLimitVisionDaemon:
self.debug_session_started_at = 0.0
self.debug_session_unavailable = False
self.last_logged_status = ""
self.last_published_stream = None
self.last_logged_candidate = None
self.last_runtime_telemetry_at = 0.0
self.last_debug_heartbeat_at = 0.0
@@ -2450,13 +2452,20 @@ class SpeedLimitVisionDaemon:
self._clear_detection()
if self.params_memory is None:
return
self.params_memory.put("VisionSpeedLimitStatus", status)
if self.stream_name:
self.params_memory.put("VisionSpeedLimitStream", self.stream_name)
else:
self.params_memory.remove("VisionSpeedLimitStream")
if status != self.last_logged_status:
status_changed = status != self.last_logged_status
if status_changed:
self.params_memory.put("VisionSpeedLimitStatus", status)
self.last_logged_status = status
if self.stream_name != self.last_published_stream:
if self.stream_name:
self.params_memory.put("VisionSpeedLimitStream", self.stream_name)
else:
self.params_memory.remove("VisionSpeedLimitStream")
self.last_published_stream = self.stream_name
if status_changed:
self._write_debug_event("status", statusText=status)
def _publish_runtime_telemetry(self, now, phase, force=False, **fields):
starpilot/system/tests/test_speed_limit_vision.py
+34
View File
@@ -12,17 +12,22 @@ from starpilot.system.speed_limit_vision import DetectorProposal, HistoryEntry,
class MemoryParams:
def __init__(self):
self.values = {}
self.write_count = 0
def put_float(self, key, value):
self.write_count += 1
self.values[key] = value
def put_int(self, key, value):
self.write_count += 1
self.values[key] = value
def put(self, key, value):
self.write_count += 1
self.values[key] = value
def remove(self, key):
self.write_count += 1
self.values.pop(key, None)
@@ -129,6 +134,11 @@ def test_inference_interval_backs_off_after_expensive_inference():
assert daemon.last_inference_interval_reason == "processing_cost"
def test_runtime_loop_represents_exact_normal_cadences():
assert slv.RUNTIME_LOOP_HZ * slv.INFERENCE_INTERVAL == pytest.approx(5.0)
assert slv.RUNTIME_LOOP_HZ * slv.FOLLOWUP_INFERENCE_INTERVAL == pytest.approx(3.0)
def test_disconnect_camera_releases_client_state():
daemon = SpeedLimitVisionDaemon.__new__(SpeedLimitVisionDaemon)
daemon.client = object()
@@ -228,6 +238,30 @@ def test_receive_frame_does_not_retain_vision_buffer(monkeypatch):
assert buffer_refs[0]() is None
def test_publish_status_only_writes_changed_values():
daemon = SpeedLimitVisionDaemon.__new__(SpeedLimitVisionDaemon)
daemon.params_memory = MemoryParams()
daemon.stream_name = "road camera"
daemon.last_logged_status = ""
daemon.last_published_stream = None
daemon._write_debug_event = lambda *_args, **_kwargs: None
daemon._publish_status("Scanning road camera")
assert daemon.params_memory.write_count == 2
daemon._publish_status("Scanning road camera")
assert daemon.params_memory.write_count == 2
daemon.stream_name = "wide camera"
daemon._publish_status("Scanning road camera")
assert daemon.params_memory.write_count == 3
assert daemon.params_memory.values["VisionSpeedLimitStream"] == "wide camera"
daemon._publish_status("Holding 45 mph")
assert daemon.params_memory.write_count == 4
assert daemon.params_memory.values["VisionSpeedLimitStatus"] == "Holding 45 mph"
def test_published_sign_value_uses_configured_units():
imperial_daemon = publishing_daemon(False)
metric_daemon = publishing_daemon(True)
starpilot/system/the_galaxy/assets/components/tools/device_settings_layout.json
+79 -71
View File
@@ -1356,6 +1356,19 @@
"parent_key": "QOLLongitudinal",
"settings_tier": "simple"
},
{
"key": "PulseGlideSpeedDelta",
"label": "Pulse and Glide Speed Delta",
"description": "Developer-only: when Pulse and Glide is assigned to a wheel button, coast this far below the current cruise target before accelerating back up.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.5,
"max": 30.0,
"step": 0.5,
"precision": 1,
"parent_key": "QOLLongitudinal",
"settings_tier": "advanced"
},
{
"key": "WeatherPresets",
"label": "Weather Condition Offsets",
@@ -1592,69 +1605,12 @@
{
"key": "SpeedLimitController",
"label": "Speed Limit Controller",
"description": "Limit openpilot's maximum driving speed to the current speed limit obtained from downloaded maps, Mapbox, the dashboard, or vision-detected signs.",
"description": "Limit openpilot's maximum driving speed to the current speed limit from configured map, dashboard, and optional vision sources.",
"data_type": "bool",
"ui_type": "toggle",
"is_parent_toggle": true,
"settings_tier": "advanced"
},
{
"key": "VisionSpeedLimitDetection",
"label": "Vision Speed Limit Detection",
"description": "Use the road camera to detect speed limit signs for SLC and speed limit filling.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "SpeedLimitController",
"settings_tier": "advanced"
},
{
"key": "VisionSpeedLimitLowLimitFilter",
"label": "Ignore Low Vision Speed Limits",
"description": "Prevent SLC from acting on vision-detected speed limits at or below the configured threshold. Detection, display, debugging, and training collection remain active.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "VisionSpeedLimitDetection",
"settings_tier": "advanced"
},
{
"key": "VisionSpeedLimitLowLimitThreshold",
"label": "Ignore At or Below",
"description": "Vision-detected limits at or below this value will not control speed. The value uses your selected mph or km/h unit.",
"data_type": "int",
"ui_type": "numeric",
"min": 5,
"max": 80,
"step": 5,
"parent_key": "VisionSpeedLimitLowLimitFilter",
"settings_tier": "advanced"
},
{
"key": "VisionSpeedLimitAutoBookmark",
"label": "Auto-Bookmark Vision Signs",
"description": "Automatically save confirmed vision-detected speed limit signs into the speed-limit debug session so they can be imported into the training set later.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "VisionSpeedLimitDetection",
"settings_tier": "advanced"
},
{
"key": "VisionSpeedLimitTrainingCollector",
"label": "Collect Extra Vision Training Samples",
"description": "Save lower-threshold vision sign candidates into the debug session for later training import without showing or applying them live. Leave this on if you want to help improve the model.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "VisionSpeedLimitDetection",
"settings_tier": "advanced"
},
{
"key": "VisionSpeedLimitAutoPreserveSegment",
"label": "Preserve Auto-Bookmarked Segments",
"description": "Also send a real bookmark for confirmed auto-bookmarks so loggerd preserves the route segment. Leave this off unless you specifically want the extra storage usage.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "VisionSpeedLimitAutoBookmark",
"settings_tier": "advanced"
},
{
"key": "SLCConfirmation",
"label": "Confirm New Speed Limits",
@@ -2022,6 +1978,71 @@
}
]
},
{
"name": "Vision Speed Limits",
"icon": "bi-camera",
"params": [
{
"key": "VisionSpeedLimitDetection",
"label": "Vision Speed Limit Detection",
"description": "Use the road camera to detect and display speed-limit signs, with optional use by Speed Limit Controller.",
"data_type": "bool",
"ui_type": "toggle",
"is_parent_toggle": true,
"settings_tier": "advanced"
},
{
"key": "VisionSpeedLimitLowLimitFilter",
"label": "Ignore Low Vision Speed Limits",
"description": "When SLC is enabled, prevent it from acting on vision-detected speed limits at or below the configured threshold. Detection, display, debugging, and training collection remain active.",
"data_type": "bool",
"ui_type": "toggle",
"is_parent_toggle": true,
"parent_key": "VisionSpeedLimitDetection",
"settings_tier": "advanced"
},
{
"key": "VisionSpeedLimitLowLimitThreshold",
"label": "Ignore At or Below",
"description": "Vision-detected limits at or below this value will not control speed. The value uses your selected mph or km/h unit.",
"data_type": "int",
"ui_type": "numeric",
"min": 5,
"max": 80,
"step": 5,
"parent_key": "VisionSpeedLimitLowLimitFilter",
"settings_tier": "advanced"
},
{
"key": "VisionSpeedLimitAutoBookmark",
"label": "Auto-Bookmark Vision Signs",
"description": "Automatically save confirmed vision-detected speed limit signs into the speed-limit debug session so they can be imported into the training set later.",
"data_type": "bool",
"ui_type": "toggle",
"is_parent_toggle": true,
"parent_key": "VisionSpeedLimitDetection",
"settings_tier": "advanced"
},
{
"key": "VisionSpeedLimitTrainingCollector",
"label": "Collect Extra Vision Training Samples",
"description": "Save lower-threshold vision sign candidates into the debug session for later training import without showing or applying them live. Leave this on if you want to help improve the model.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "VisionSpeedLimitDetection",
"settings_tier": "advanced"
},
{
"key": "VisionSpeedLimitAutoPreserveSegment",
"label": "Preserve Auto-Bookmarked Segments",
"description": "Also send a real bookmark for confirmed auto-bookmarks so loggerd preserves the route segment. Leave this off unless you specifically want the extra storage usage.",
"data_type": "bool",
"ui_type": "toggle",
"parent_key": "VisionSpeedLimitAutoBookmark",
"settings_tier": "advanced"
}
]
},
{
"name": "Visual (Display & UI)",
"icon": "bi-eye",
@@ -4206,19 +4227,6 @@
"parent_key": "GalaxyDeveloperMode",
"settings_tier": "advanced"
},
{
"key": "PulseGlideSpeedDelta",
"label": "Pulse and Glide Speed Delta",
"description": "When Pulse and Glide is assigned to a wheel button, coast this far below the current cruise target before accelerating back up.",
"data_type": "float",
"ui_type": "numeric",
"min": 0.5,
"max": 30.0,
"step": 0.5,
"precision": 1,
"parent_key": "GalaxyDeveloperMode",
"settings_tier": "advanced"
},
{
"key": "CameraOffset",
"label": "Camera Offset",
starpilot/system/the_galaxy/tests/test_device_settings_layout.py
+19 -3
View File
@@ -56,8 +56,13 @@ def test_galaxy_layout_contains_basic_mode_controls():
"HumanLaneChanges",
"QOLLongitudinal",
} <= sections["Longitudinal (Speed & Following)"].keys()
assert "Vision Speed Limits" in sections
assert "VisionSpeedLimitDetection" not in sections["Longitudinal (Speed & Following)"]
assert "RedneckCruise" not in sections["Longitudinal (Speed & Following)"].keys()
assert sections["Developer"]["RedneckCruise"]["parent_key"] == "GalaxyDeveloperMode"
assert sections["Longitudinal (Speed & Following)"]["PulseGlideSpeedDelta"]["parent_key"] == "QOLLongitudinal"
assert sections["Longitudinal (Speed & Following)"]["PulseGlideSpeedDelta"]["settings_tier"] == "advanced"
assert "PulseGlideSpeedDelta" not in sections["Developer"]
assert {"AlphaLongitudinalEnabled", "ForceOffroad", "GalaxyDeveloperMode", "TestModelLeadTrajectory"} <= sections["Developer"].keys()
@@ -94,6 +99,7 @@ def test_requested_simple_and_advanced_settings_tiers():
sections = _params_by_section(_layout())
lateral = sections["Lateral (Steering)"]
longitudinal = sections["Longitudinal (Speed & Following)"]
vision = sections["Vision Speed Limits"]
developer = sections["Developer"]
for section_name in (
@@ -138,6 +144,11 @@ def test_requested_simple_and_advanced_settings_tiers():
"ConditionalChill",
):
assert longitudinal[key]["settings_tier"] == "advanced"
assert longitudinal["PulseGlideSpeedDelta"]["settings_tier"] == "advanced"
assert vision["VisionSpeedLimitDetection"]["settings_tier"] == "advanced"
assert vision["VisionSpeedLimitLowLimitFilter"]["settings_tier"] == "advanced"
assert vision["VisionSpeedLimitLowLimitThreshold"]["settings_tier"] == "advanced"
assert developer["GalaxyDeveloperMode"]["settings_tier"] == "simple"
assert developer["TestModelLeadTrajectory"]["parent_key"] == "GalaxyDeveloperMode"
@@ -203,10 +214,14 @@ def test_vasm_is_default_off_and_configured_only_in_galaxy():
def test_low_vision_limit_filter_is_default_off_and_configured_only_in_galaxy():
sections = _params_by_section(_layout())
longitudinal = sections["Longitudinal (Speed & Following)"]
toggle = longitudinal["VisionSpeedLimitLowLimitFilter"]
threshold = longitudinal["VisionSpeedLimitLowLimitThreshold"]
vision = sections["Vision Speed Limits"]
toggle = vision["VisionSpeedLimitLowLimitFilter"]
threshold = vision["VisionSpeedLimitLowLimitThreshold"]
assert vision["VisionSpeedLimitDetection"]["is_parent_toggle"] is True
assert vision["VisionSpeedLimitAutoBookmark"]["is_parent_toggle"] is True
assert "VisionSpeedLimitDetection" not in sections["Longitudinal (Speed & Following)"]
assert toggle["is_parent_toggle"] is True
assert toggle["parent_key"] == "VisionSpeedLimitDetection"
assert threshold["parent_key"] == "VisionSpeedLimitLowLimitFilter"
assert threshold["min"] == 5
@@ -214,6 +229,7 @@ def test_low_vision_limit_filter_is_default_off_and_configured_only_in_galaxy():
assert threshold["step"] == 5
assert _declared_default("VisionSpeedLimitLowLimitFilter") == "0"
assert _declared_default("VisionSpeedLimitLowLimitThreshold") == "25"
assert _declared_default("VisionSpeedLimitDetection") == "1"
physical_settings = (
REPO_ROOT / "selfdrive/ui/layouts/settings/starpilot/longitudinal.py",
system/hardware/hardwared.py
+23 -20
View File
@@ -29,12 +29,11 @@ from openpilot.system.hardware.power_monitoring import PowerMonitoring
from openpilot.system.hardware.fan_controller import TiciFanController
from openpilot.system.hardware.usb import (
CHESTNUT_FW_VERSION,
CHESTNUT_PRODUCT_ID,
CHESTNUT_ROM_USB_IDS,
CHESTNUT_VENDOR_IDS,
read_int,
read_text,
usb_devices,
CHESTNUT_USB_IDS,
get_usb_state,
get_usb_topology,
set_usb_state,
)
from openpilot.system.version import terms_version, training_version
from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID
@@ -94,14 +93,10 @@ class Chestnut:
self.last_attempt = 0.0
self.flashed = False
def _firmware_mismatch(self) -> bool:
def _firmware_mismatch(self, usb_state: list[dict]) -> bool:
expected = f"custom {CHESTNUT_FW_VERSION}-CLEAN"
ids = tuple((vendor, CHESTNUT_PRODUCT_ID) for vendor in CHESTNUT_VENDOR_IDS) + CHESTNUT_ROM_USB_IDS
for device in usb_devices():
usb_id = (read_int(device / "idVendor", 16), read_int(device / "idProduct", 16))
if usb_id in ids and read_text(device / "product") != expected:
return True
return False
ids = CHESTNUT_USB_IDS + CHESTNUT_ROM_USB_IDS
return any((device["vendorId"], device["productId"]) in ids and device["product"] != expected for device in usb_state)
def _flash(self) -> None:
script = os.path.join(os.path.dirname(__file__), "chestnut", "flash.py")
@@ -115,8 +110,8 @@ class Chestnut:
cloudlog.event("chestnut flash done", returncode=result.returncode, output=result.stdout[-1000:], error=result.returncode != 0)
self.flashed = result.returncode == 0
def update(self, offroad: bool) -> None:
if not self._firmware_mismatch():
def update(self, offroad: bool, usb_state: list[dict]) -> None:
if not self._firmware_mismatch(usb_state):
self.flashed = False
return
if not offroad or self.flashed or self.attempts >= self.MAX_ATTEMPTS:
@@ -134,7 +129,7 @@ class Chestnut:
ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp'])
HardwareState = namedtuple("HardwareState", ['network_type', 'network_info', 'network_strength', 'network_stats',
'network_metered', 'modem_temps'])
'network_metered', 'modem_temps', 'usb_state'])
# List of thermal bands. We will stay within this region as long as we are within the bounds.
# When exiting the bounds, we'll jump to the lower or higher band. Bands are ordered in the dict.
@@ -197,6 +192,7 @@ def hw_state_thread(end_event, hw_queue):
"""Handles non critical hardware state, and sends over queue"""
count = 0
prev_hw_state = None
prev_usb_topology = set()
modem_version = None
modem_configured = False
@@ -204,8 +200,12 @@ def hw_state_thread(end_event, hw_queue):
modem_restart_count = 0
while not end_event.is_set():
# these are expensive calls. update every 10s
if (count % int(10. / DT_HW)) == 0:
usb_topology = get_usb_topology()
usb_changed = usb_topology != prev_usb_topology
# these are expensive calls. update every 10s or when USB devices change
if (count % int(10. / DT_HW)) == 0 or usb_changed:
prev_usb_topology = usb_topology
try:
network_type = HARDWARE.get_network_type()
modem_temps = HARDWARE.get_modem_temperatures()
@@ -240,6 +240,7 @@ def hw_state_thread(end_event, hw_queue):
network_stats={'wwanTx': tx, 'wwanRx': rx},
network_metered=HARDWARE.get_network_metered(network_type),
modem_temps=modem_temps,
usb_state=get_usb_state(),
)
try:
@@ -287,6 +288,7 @@ def hardware_thread(end_event, hw_queue) -> None:
network_strength=NetworkStrength.unknown,
network_stats={'wwanTx': -1, 'wwanRx': -1},
modem_temps=[],
usb_state=[],
)
all_temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_HW, initialized=False)
@@ -320,9 +322,6 @@ def hardware_thread(end_event, hw_queue) -> None:
while not end_event.is_set():
sm.update(PANDA_STATES_TIMEOUT)
if chestnut is not None:
chestnut.update(started_ts is None)
pandaStates = sm['pandaStates']
peripheralState = sm['peripheralState']
peripheral_panda_present = peripheralState.pandaType != log.PandaState.PandaType.unknown
@@ -384,6 +383,10 @@ def hardware_thread(end_event, hw_queue) -> None:
msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness()
set_usb_state(msg.deviceState, last_hw_state.usb_state)
if chestnut is not None:
chestnut.update(started_ts is None, last_hw_state.usb_state)
# this subset is only used for offroad
temp_sources = [
msg.deviceState.memoryTempC,
system/hardware/tests/test_usb.py
+72
View File
@@ -1,3 +1,4 @@
from cereal import log
from openpilot.system.hardware import usb
@@ -31,3 +32,74 @@ def test_chestnut_absent_for_other_usb_device(tmp_path, monkeypatch):
(device / "idProduct").write_text("4ee7\n")
assert not usb.chestnut_present()
def test_get_usb_topology(tmp_path, monkeypatch):
monkeypatch.setattr(usb, "USB_DEVICES_PATH", tmp_path)
(tmp_path / "1-0:1.0").mkdir()
(tmp_path / "usb1").mkdir()
assert usb.get_usb_topology() == {"1-0:1.0", "usb1"}
def test_get_usb_state(tmp_path, monkeypatch):
devices_path = tmp_path / "devices"
devices_path.mkdir()
device = devices_path / "4-3"
device.mkdir()
values = {
"busnum": "4\n",
"devnum": "2\n",
"idVendor": "3801\n",
"idProduct": "0001\n",
"speed": "5000\n",
"manufacturer": "tiny\n",
"product": "custom ed4e39b7-CLEAN\n",
}
for name, value in values.items():
(device / name).write_text(value)
ctrl = tmp_path / usb.PRIMARY_USB_CONTROLLER
ctrl.mkdir()
(ctrl / "portli").write_text("0x12345\n")
orientation = tmp_path / "typec_cc_orientation"
orientation.write_text("2\n")
monkeypatch.setattr(usb, "USB_DEVICES_PATH", devices_path)
monkeypatch.setattr(usb, "TYPEC_CC_ORIENTATION_PATH", orientation)
monkeypatch.setattr(usb, "controller", lambda _: ctrl)
assert usb.get_usb_state() == [{
"busnum": 4,
"devnum": 2,
"vendorId": 0x3801,
"productId": 0x0001,
"speedMbps": 5000,
"manufacturer": "tiny",
"product": "custom ed4e39b7-CLEAN",
"linkErrorCount": 0x2345,
"usb3Lane": "b",
}]
def test_set_usb_state():
msg = log.DeviceState.new_message()
devices = [{
"busnum": 4,
"devnum": 2,
"vendorId": 0x3801,
"productId": 0x0001,
"speedMbps": 5000,
"manufacturer": "tiny",
"product": "custom ed4e39b7-CLEAN",
"linkErrorCount": 7,
"usb3Lane": "a",
}]
usb.set_usb_state(msg, devices)
assert msg.chestnutPresent
assert len(msg.usbState.devices) == 1
assert msg.usbState.devices[0].speedMbps == 5000
assert msg.usbState.devices[0].linkErrorCount == 7
assert msg.usbState.devices[0].usb3Lane == "a"
system/hardware/usb.py
+63 -9
View File
@@ -1,25 +1,40 @@
import os
from pathlib import Path
CHESTNUT_VENDOR_ID = 0xADD1
CHESTNUT_VENDOR_IDS = (CHESTNUT_VENDOR_ID, 0x3801)
CHESTNUT_PRODUCT_ID = 0x0001
CHESTNUT_USB_IDS = tuple((vendor_id, CHESTNUT_PRODUCT_ID) for vendor_id in CHESTNUT_VENDOR_IDS)
CHESTNUT_FW_VERSION = "ed4e39b7"
CHESTNUT_ROM_USB_IDS = ((0x174C, 0x2464), (0x174C, 0x2463))
USB_DEVICES_PATH = Path("/sys/bus/usb/devices")
TYPEC_CC_ORIENTATION_PATH = Path("/sys/class/power_supply/usb/typec_cc_orientation")
PRIMARY_USB_CONTROLLER = "a600000.ssusb"
def get_usb_topology() -> set[str]:
try:
return set(os.listdir(USB_DEVICES_PATH))
except OSError:
return set()
def read(path: Path) -> str | None:
try:
return path.read_text().strip()
except OSError:
return None
def read_int(path: Path, base: int = 10) -> int:
try:
return int(path.read_text(), base)
except (OSError, ValueError):
except (OSError, ValueError, TypeError):
return 0
def read_text(path: Path) -> str:
try:
return path.read_text().strip()
except OSError:
return ""
return read(path) or ""
def usb_devices() -> list[Path]:
@@ -32,8 +47,7 @@ def usb_devices() -> list[Path]:
def chestnut_present() -> bool:
return any(
read_int(device / "idVendor", 16) in CHESTNUT_VENDOR_IDS and
read_int(device / "idProduct", 16) == CHESTNUT_PRODUCT_ID
(read_int(device / "idVendor", 16), read_int(device / "idProduct", 16)) in CHESTNUT_USB_IDS
for device in usb_devices()
)
@@ -41,8 +55,7 @@ def chestnut_present() -> bool:
def chestnut_firmware_ready() -> bool:
expected = f"custom {CHESTNUT_FW_VERSION}-CLEAN"
return any(
read_int(device / "idVendor", 16) in CHESTNUT_VENDOR_IDS and
read_int(device / "idProduct", 16) == CHESTNUT_PRODUCT_ID and
(read_int(device / "idVendor", 16), read_int(device / "idProduct", 16)) in CHESTNUT_USB_IDS and
read_text(device / "product") == expected
for device in usb_devices()
)
@@ -53,3 +66,44 @@ def controller(device: Path) -> Path | None:
return next((parent for parent in device.resolve().parents if parent.name.endswith(".ssusb")), None)
except OSError:
return None
def get_usb_state() -> list[dict]:
devices = []
typec_orientation = read_int(TYPEC_CC_ORIENTATION_PATH)
for device in usb_devices():
ctrl = controller(device)
devices.append({
"busnum": read_int(device / "busnum"),
"devnum": read_int(device / "devnum"),
"vendorId": read_int(device / "idVendor", 16),
"productId": read_int(device / "idProduct", 16),
"speedMbps": read_int(device / "speed"),
"manufacturer": read(device / "manufacturer") or "",
"product": read(device / "product") or "",
"linkErrorCount": read_int(ctrl / "portli", 0) & 0xFFFF if ctrl is not None else 0,
"usb3Lane": {1: "a", 2: "b"}.get(typec_orientation, "unknown")
if ctrl is not None and ctrl.name == PRIMARY_USB_CONTROLLER else "unknown",
})
return devices
def set_usb_state(device_state, devices: list[dict]) -> None:
entries = device_state.usbState.init("devices", len(devices))
chestnut_found = False
for entry, device in zip(entries, devices, strict=True):
entry.busnum = device["busnum"]
entry.devnum = device["devnum"]
entry.vendorId = device["vendorId"]
entry.productId = device["productId"]
entry.speedMbps = device["speedMbps"]
entry.manufacturer = device["manufacturer"]
entry.product = device["product"]
entry.linkErrorCount = device["linkErrorCount"]
entry.usb3Lane = device.get("usb3Lane", "unknown")
if (entry.vendorId, entry.productId) in CHESTNUT_USB_IDS:
chestnut_found = True
device_state.chestnutPresent = chestnut_found