mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-25 10:13:43 +08:00
Compare commits
29 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| af08cca37b | |||
| 73c0af39e8 | |||
| 52eb586c65 | |||
| f13350b7ec | |||
| f22bcfee36 | |||
| b7686d025d | |||
| acda0d2ae7 | |||
| f211be9357 | |||
| f006b47db3 | |||
| 67f5ede3ad | |||
| afe777fd03 | |||
| 95382eaa0a | |||
| 7a57b0d7f3 | |||
| 6fd564789d | |||
| 1114fead6c | |||
| 3511854603 | |||
| 05331a78c9 | |||
| b9cd54f407 | |||
| 70639a33b4 | |||
| 9a74319be7 | |||
| 87b9fd5b57 | |||
| 7140e96d98 | |||
| 0c4a1cde5e | |||
| 6fc7b9ceb8 | |||
| 525e57a26e | |||
| def1979aca | |||
| 5d9b55adcb | |||
| 21805a5454 | |||
| 50e1c1d376 |
@@ -224,6 +224,7 @@ struct StarPilotPlan @0xf98d843bfd7004a3 {
|
||||
cscOverridden @38 :Bool; # driver cancelled this curve with RES+
|
||||
cscLearnedLatAccel @39 :Float32; # learned comfort at the current curvature, before margin
|
||||
cscBindingDistance @40 :Float32; # distance to the horizon point setting the target, m
|
||||
approachStopLength @41 :Float32; # pre-commit distance to a detected stop, m; 0 when off
|
||||
}
|
||||
|
||||
struct StarPilotRadarState @0xb86e6369214c01c8 {
|
||||
|
||||
Binary file not shown.
+32
-5
@@ -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;
|
||||
@@ -503,7 +507,8 @@ struct DeviceState @0xa4d8b5af2aa492eb {
|
||||
pmicTempC @39 :List(Float32);
|
||||
intakeTempC @46 :Float32;
|
||||
exhaustTempC @47 :Float32;
|
||||
caseTempC @48 :Float32;
|
||||
gnssTempC @48 :Float32;
|
||||
bottomSocTempC @50 :Float32;
|
||||
maxTempC @44 :Float32; # max of other temps, used to control fan
|
||||
thermalZones @38 :List(ThermalZone);
|
||||
thermalStatus @14 :ThermalStatus;
|
||||
@@ -517,10 +522,10 @@ struct DeviceState @0xa4d8b5af2aa492eb {
|
||||
}
|
||||
|
||||
enum ThermalStatus {
|
||||
green @0;
|
||||
yellow @1;
|
||||
red @2;
|
||||
danger @3;
|
||||
ok @0;
|
||||
warmDEPRECATED @1;
|
||||
overheated @2;
|
||||
critical @3;
|
||||
}
|
||||
|
||||
enum NetworkType {
|
||||
@@ -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;
|
||||
|
||||
@@ -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)|
|
||||
@@ -218,8 +238,8 @@
|
||||
|Hyundai|Kona Non-SCC 2019|No Smart Cruise Control (Non-SCC)|[Community](community)|
|
||||
|Hyundai|Nexo 2021|All|[Upstream](#upstream)|
|
||||
|Hyundai|Palisade 2020-22|All|[Upstream](#upstream)|
|
||||
|Hyundai|Palisade 2023-24|HDA2|[Community](#community)|
|
||||
|Hyundai|Palisade (with HDA II) 2023-24|Highway Driving Assist II|[Upstream](#upstream)|
|
||||
|Hyundai|Palisade 2023-25|HDA2|[Community](#community)|
|
||||
|Hyundai|Palisade (with HDA II) 2023-25|Highway Driving Assist II|[Upstream](#upstream)|
|
||||
|Hyundai|Palisade (without HDA II) 2023-25|Highway Driving Assist|[Upstream](#upstream)|
|
||||
|Hyundai|Santa Cruz 2022-24|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|
||||
|Hyundai|Santa Cruz (without HDA II) 2025|Smart Cruise Control (SCC)|[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)|
|
||||
|
||||
@@ -55,7 +55,7 @@ class CAR(Platforms):
|
||||
|
||||
EXTRA_HYUNDAI = ExtraPlatformConfig(
|
||||
[
|
||||
CommunityCarDocs("Hyundai Palisade 2023-24", package="HDA2"),
|
||||
CommunityCarDocs("Hyundai Palisade 2023-25", package="HDA2"),
|
||||
CommunityCarDocs("Kia Telluride 2023-24", package="HDA2"),
|
||||
],
|
||||
)
|
||||
|
||||
@@ -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'
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -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]]
|
||||
|
||||
@@ -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):
|
||||
|
||||
@@ -721,15 +721,25 @@ class CarController(CarControllerBase):
|
||||
def create_can_msgs(self, apply_steer_req, apply_torque, torque_fault, set_speed_in_units, accel, stopping, hud_control, actuators, CS, CC, lka_icon, lfa_icon):
|
||||
can_sends = []
|
||||
can_canfd_blended = bool(self.CP.flags & HyundaiFlags.CAN_CANFD_BLENDED)
|
||||
blended_hda2 = can_canfd_blended and bool(self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING)
|
||||
|
||||
# HUD messages
|
||||
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
|
||||
hud_control)
|
||||
|
||||
if can_canfd_blended and self.CP.flags & HyundaiFlags.CANFD_LKA_STEERING:
|
||||
if blended_hda2:
|
||||
can_sends.extend(hyundaicanfd.create_steering_messages(
|
||||
self.packer, self.CP, self.CAN, CC.enabled, apply_steer_req, apply_torque, 0.0,
|
||||
))
|
||||
if self.long_active_ecu:
|
||||
can_sends.extend(hyundaican.create_lkas11_can_canfd_blended(
|
||||
self.packer, self.frame, self.CP, apply_torque, apply_steer_req,
|
||||
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
|
||||
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
|
||||
left_lane_warning, right_lane_warning, CS.msg_364,
|
||||
include_alerts=False,
|
||||
counter_mod=0xF,
|
||||
))
|
||||
if self.frame % 5 == 0:
|
||||
can_sends.append(hyundaicanfd.create_suppress_lfa(
|
||||
self.packer, self.CAN, CS.lfa_block_msg, False,
|
||||
@@ -760,13 +770,22 @@ class CarController(CarControllerBase):
|
||||
can_sends.extend(self._create_can_redneck_button_messages(CS))
|
||||
|
||||
if self.long_active_ecu and can_canfd_blended:
|
||||
can_sends.extend(hyundaican.create_radar_aux_messages(self.packer, self.CAN, self.frame))
|
||||
if blended_hda2:
|
||||
can_sends.extend(hyundaicanfd.create_adrv_messages(self.packer, self.CAN, self.frame, blended_hda2=True))
|
||||
can_sends.extend(hyundaican.create_radar_aux_messages(self.packer, self.CAN, self.frame, hda2=blended_hda2))
|
||||
|
||||
if self.frame % 2 == 0 and self.long_active_ecu:
|
||||
# TODO: unclear if this is needed
|
||||
jerk = 3.0 if actuators.longControlState == LongCtrlState.pid else 1.0
|
||||
use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
|
||||
if can_canfd_blended:
|
||||
if blended_hda2:
|
||||
stopping = stopping and CS.out.vEgoRaw < 0.1
|
||||
can_sends.extend(hyundaican.create_acc_commands_can_canfd_blended_hda2(
|
||||
self.packer, CC.enabled, accel, self.accel_last, jerk, int(self.frame / 2), hud_control,
|
||||
set_speed_in_units, stopping, CC.cruiseControl.override, use_fca, self.CP,
|
||||
))
|
||||
self.accel_last = accel
|
||||
elif can_canfd_blended:
|
||||
can_sends.extend(hyundaican.create_acc_commands_can_canfd_blended(self.packer, CC.enabled, accel, jerk,
|
||||
int(self.frame / 2), hud_control,
|
||||
set_speed_in_units, stopping,
|
||||
@@ -777,7 +796,7 @@ class CarController(CarControllerBase):
|
||||
CC.cruiseControl.override, use_fca, self.CP))
|
||||
|
||||
# 20 Hz LFA MFA message
|
||||
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
|
||||
if self.frame % 5 == 0 and (self.CP.flags & HyundaiFlags.SEND_LFA.value or (self.long_active_ecu and blended_hda2)):
|
||||
can_sends.append(hyundaican.create_lfahda_mfc(self.packer, CC.enabled, self.frame, self.CP, lfa_icon))
|
||||
|
||||
# 5 Hz ACC options
|
||||
|
||||
@@ -28,6 +28,11 @@ IONIQ_6_BLINDSPOT_LEFT_MASK = 0x10
|
||||
CANFD_CAMERA_LEAD_MIN_DISTANCE = 0.1
|
||||
ALT_BUS_LDA_BUTTON_BURST_DEBOUNCE_NS = int(1.3e9)
|
||||
|
||||
CLASSIC_MEDIA_BUTTON_CARS = frozenset({
|
||||
CAR.HYUNDAI_ELANTRA_2024,
|
||||
CAR.HYUNDAI_ELANTRA_HEV_2024,
|
||||
})
|
||||
|
||||
|
||||
def get_non_scc_cruise_signals(CP) -> tuple[str, str, str, str, str, str]:
|
||||
if CP.flags & HyundaiFlags.EV:
|
||||
@@ -437,6 +442,9 @@ class CarState(CarStateBase):
|
||||
ret.lowSpeedAlert = self.low_speed_alert
|
||||
|
||||
fp_ret = custom.StarPilotCarState.new_message()
|
||||
if self.CP.carFingerprint in CLASSIC_MEDIA_BUTTON_CARS:
|
||||
fp_ret.modePressed = bool(cp.vl["GW_SWRC_PE"]["C_ModeSW"])
|
||||
fp_ret.customPressed = bool(cp.vl["GW_SWRC_PE"]["C_MTSSW"])
|
||||
|
||||
return ret, fp_ret
|
||||
|
||||
@@ -667,7 +675,7 @@ class CarState(CarStateBase):
|
||||
("CGW2", 5),
|
||||
("WHL_SPD11", 50),
|
||||
("SAS11", 100),
|
||||
("SCC12", 50),
|
||||
("SCC12", 0 if CP.openpilotLongitudinalControl and CP.flags & HyundaiFlags.CANFD_LKA_STEERING else 50),
|
||||
("EMS12", 100),
|
||||
("EMS16", 100),
|
||||
("LVR12", 100),
|
||||
@@ -686,6 +694,9 @@ class CarState(CarStateBase):
|
||||
("BCM_PO_11", 0),
|
||||
("CLU13", 0),
|
||||
]
|
||||
if CP.carFingerprint in CLASSIC_MEDIA_BUTTON_CARS:
|
||||
# Steering-wheel media switches are event-driven on the refresh Elantra.
|
||||
msgs.append(("GW_SWRC_PE", 0))
|
||||
if CP.flags & HyundaiFlags.NON_SCC and not (CP.flags & HyundaiFlags.NON_SCC_NO_FCA):
|
||||
msgs.append(("FCA11", 0)) # Non-SCC trims can stop publishing FCA11; don't let it poison canValid
|
||||
|
||||
|
||||
@@ -106,7 +106,8 @@ def create_checksum_can_canfd_blended(packer, bus, addr, values):
|
||||
def create_lkas11_can_canfd_blended(packer, frame, CP, apply_steer, steer_req,
|
||||
torque_fault, lkas11, sys_warning, sys_state, enabled,
|
||||
left_lane, right_lane,
|
||||
left_lane_depart, right_lane_depart, msg_364):
|
||||
left_lane_depart, right_lane_depart, msg_364,
|
||||
include_alerts=True, counter_mod=0x10):
|
||||
bus = CanBus(CP).ECAN
|
||||
values = {
|
||||
"CF_Lkas_LdwsActivemode": int(left_lane) + (int(right_lane) << 1),
|
||||
@@ -116,7 +117,7 @@ def create_lkas11_can_canfd_blended(packer, frame, CP, apply_steer, steer_req,
|
||||
"CR_Lkas_StrToqReq": apply_steer,
|
||||
"CF_Lkas_ActToi": steer_req,
|
||||
"CF_Lkas_ToiFlt": torque_fault,
|
||||
"CF_Lkas_MsgCount": frame % 0x10,
|
||||
"CF_Lkas_MsgCount": frame % counter_mod,
|
||||
"NEW_SIGNAL_1": 0,
|
||||
"NEW_SIGNAL_5": 100,
|
||||
}
|
||||
@@ -130,13 +131,13 @@ def create_lkas11_can_canfd_blended(packer, frame, CP, apply_steer, steer_req,
|
||||
alerts_364.setdefault("BYTE5", 0)
|
||||
alerts_364.setdefault("BYTE6", 0)
|
||||
alerts_364.setdefault("BYTE7", 0)
|
||||
alerts_364["COUNTER"] = frame % 0x10
|
||||
alerts_364["COUNTER"] = frame % counter_mod
|
||||
alerts_364["CHECKSUM"] = create_checksum_can_canfd_blended(packer, bus, "ALERTS_364", alerts_364)
|
||||
|
||||
return [
|
||||
packer.make_can_msg("LKAS11", bus, values),
|
||||
packer.make_can_msg("ALERTS_364", bus, alerts_364),
|
||||
]
|
||||
ret = [packer.make_can_msg("LKAS11", bus, values)]
|
||||
if include_alerts:
|
||||
ret.append(packer.make_can_msg("ALERTS_364", bus, alerts_364))
|
||||
return ret
|
||||
|
||||
|
||||
def create_clu11(packer, frame, clu11, button, CP):
|
||||
@@ -233,6 +234,56 @@ def create_acc_commands_can_canfd_blended(packer, enabled, accel, upper_jerk, id
|
||||
return commands
|
||||
|
||||
|
||||
def create_acc_commands_can_canfd_blended_hda2(packer, enabled, accel, accel_last, upper_jerk, idx,
|
||||
hud_control, set_speed, stopping, long_override, use_fca, CP):
|
||||
commands = []
|
||||
bus = CanBus(CP).ECAN
|
||||
jerk = 5.0
|
||||
|
||||
if not enabled or long_override:
|
||||
accel_raw, accel_value = 0.0, 0.0
|
||||
else:
|
||||
accel_raw = accel
|
||||
accel_value = max(accel_last - jerk / 50.0, min(accel, accel_last + jerk / 50.0))
|
||||
|
||||
message_values = [
|
||||
("SCC11", {
|
||||
"aReqRaw": accel_raw,
|
||||
"aReqValue": accel_value,
|
||||
"JerkUpperLimit": upper_jerk,
|
||||
"JerkLowerLimit": jerk if enabled else 1.0,
|
||||
}),
|
||||
("SCC12", {
|
||||
"MainMode_ACC": 1,
|
||||
"ACCMode_Inactive": 0 if enabled else 1,
|
||||
"TauGapSet": hud_control.leadDistanceBars,
|
||||
"VSetDis": set_speed,
|
||||
"ACC_ObjDist": 1,
|
||||
"ACCMode": 2 if enabled and long_override else 1 if enabled else 0,
|
||||
"StopReq": 1 if stopping else 0,
|
||||
}),
|
||||
("SCC14", {
|
||||
"ACC_ObjRelSpd": 0,
|
||||
"ObjValid": 0,
|
||||
"ObjStatus": 2 if hud_control.leadVisible and enabled else 1 if hud_control.leadVisible else 0,
|
||||
}),
|
||||
]
|
||||
|
||||
if use_fca and not (CP.flags & HyundaiFlags.CAMERA_SCC):
|
||||
# These values reproduce the stock status bytes without requesting AEB/FCA actuation.
|
||||
message_values.append(("FCA11", {
|
||||
"cr_vsm_deccmd": 255,
|
||||
"cf_vsm_deccmdact": 0,
|
||||
}))
|
||||
|
||||
for name, values in message_values:
|
||||
values["COUNTER"] = idx % 0xF
|
||||
values["CHECKSUM"] = create_checksum_can_canfd_blended(packer, bus, name, values)
|
||||
commands.append(packer.make_can_msg(name, bus, values))
|
||||
|
||||
return commands
|
||||
|
||||
|
||||
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CP):
|
||||
commands = []
|
||||
|
||||
@@ -324,17 +375,27 @@ def create_frt_radar_opt(packer):
|
||||
return packer.make_can_msg("FRT_RADAR11", 0, frt_radar11_values)
|
||||
|
||||
|
||||
def create_radar_aux_messages(packer, CAN, frame):
|
||||
def create_radar_aux_messages(packer, CAN, frame, hda2=False):
|
||||
commands = []
|
||||
|
||||
for addr, freq, values in (
|
||||
message_specs = (
|
||||
("RADAR_0x363", 2, {"FCA_ESA": 1}),
|
||||
("RADAR_0x398", 5, {"BYTE4": 0x80, "BYTE5": 0x5D}),
|
||||
("RADAR_0x399", 5, {"BYTE2": 0x02}),
|
||||
("RADAR_0x39a", 5, {"BYTE7": 0xFF}),
|
||||
("RADAR_0x39b", 5, {}),
|
||||
("RADAR_0x39c", 5, {"BYTE5": 0xE0, "BYTE6": 0x79}),
|
||||
("RADAR_0x43a", 20, {"BYTE2": 0x07}),
|
||||
) if hda2 else (
|
||||
("RADAR_0x363", 2, {"FCA_ESA": 1}),
|
||||
("RADAR_0x398", 5, {"BYTE4": 0x80, "BYTE5": 0x10}),
|
||||
):
|
||||
)
|
||||
|
||||
for addr, freq, values in message_specs:
|
||||
if frame % freq != 0:
|
||||
continue
|
||||
|
||||
msg_values = values | {"COUNTER": frame % 0x10}
|
||||
msg_values = values | {"COUNTER": frame % (0xF if hda2 else 0x10)}
|
||||
msg_values["CHECKSUM"] = create_checksum_can_canfd_blended(packer, CAN.ECAN, addr, msg_values)
|
||||
commands.append(packer.make_can_msg(addr, CAN.ECAN, msg_values))
|
||||
|
||||
|
||||
@@ -258,7 +258,7 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque,
|
||||
ret = []
|
||||
if CP.flags & HyundaiFlags.CANFD_LKA_STEERING:
|
||||
lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT else "LKAS"
|
||||
if CP.openpilotLongitudinalControl or send_lfa_status:
|
||||
if (CP.openpilotLongitudinalControl and not CP.flags & HyundaiFlags.CAN_CANFD_BLENDED) or send_lfa_status:
|
||||
ret.append(packer.make_can_msg("LFA", CAN.ECAN, lfa_values))
|
||||
if lfa_only:
|
||||
return ret
|
||||
@@ -820,7 +820,7 @@ def create_fca_warning_light(packer, CAN, frame):
|
||||
return ret
|
||||
|
||||
|
||||
def create_adrv_messages(packer, CAN, frame):
|
||||
def create_adrv_messages(packer, CAN, frame, blended_hda2=False):
|
||||
# messages needed to car happy after disabling
|
||||
# the ADAS Driving ECU to do longitudinal control
|
||||
|
||||
@@ -830,6 +830,9 @@ def create_adrv_messages(packer, CAN, frame):
|
||||
}
|
||||
ret.append(packer.make_can_msg("ADRV_0x51", CAN.ACAN, values))
|
||||
|
||||
if blended_hda2:
|
||||
return ret
|
||||
|
||||
ret.extend(create_fca_warning_light(packer, CAN, frame))
|
||||
|
||||
if frame % 5 == 0:
|
||||
|
||||
@@ -9,6 +9,7 @@ from opendbc.car.hyundai.values import HyundaiFlags, CAR, CarControllerParams, \
|
||||
RADAR_LIVE_LONGITUDINAL_CAR, \
|
||||
UNSUPPORTED_LONGITUDINAL_CAR, HyundaiSafetyFlags, \
|
||||
LEGACY_LONGITUDINAL_CAR, \
|
||||
CAN_CANFD_BLENDED_HDA2_LONGITUDINAL_CAR, \
|
||||
HyundaiStarPilotSafetyFlags, \
|
||||
hyundai_cancel_button_enables_cruise, \
|
||||
kia_ev6_gt_line_longitudinal_tuning
|
||||
@@ -199,7 +200,8 @@ class CarInterface(CarInterfaceBase):
|
||||
else:
|
||||
# Shared configuration for non CAN-FD cars
|
||||
ret.alphaLongitudinalAvailable = candidate not in UNSUPPORTED_LONGITUDINAL_CAR or candidate in LEGACY_LONGITUDINAL_CAR
|
||||
if ret.flags & HyundaiFlags.CAN_CANFD_BLENDED and ret.flags & HyundaiFlags.CANFD_LKA_STEERING:
|
||||
if ret.flags & HyundaiFlags.CAN_CANFD_BLENDED and ret.flags & HyundaiFlags.CANFD_LKA_STEERING and \
|
||||
candidate not in CAN_CANFD_BLENDED_HDA2_LONGITUDINAL_CAR:
|
||||
ret.alphaLongitudinalAvailable = False
|
||||
ret.enableBsm = 0x58b in fingerprint[CAN.ECAN]
|
||||
|
||||
|
||||
@@ -549,14 +549,17 @@ class TestHyundaiFingerprint:
|
||||
|
||||
assert CP.flags & HyundaiFlags.CAN_CANFD_BLENDED
|
||||
assert CP.flags & HyundaiFlags.CANFD_LKA_STEERING
|
||||
assert not CP.alphaLongitudinalAvailable
|
||||
assert not CP.openpilotLongitudinalControl
|
||||
assert CP.alphaLongitudinalAvailable
|
||||
assert CP.openpilotLongitudinalControl
|
||||
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_CANFD_BLENDED
|
||||
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANFD_LKA_STEERING
|
||||
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.LONG
|
||||
assert can_bus.ACAN == 0
|
||||
assert can_bus.ECAN == 1
|
||||
assert parsers[Bus.pt].bus == 1
|
||||
assert parsers[Bus.cam].bus == 2
|
||||
scc12_state = next(state for state in parsers[Bus.pt].message_states.values() if state.name == "SCC12")
|
||||
assert scc12_state.frequency == 0
|
||||
assert CarControllerParams(CP).STEER_MAX == 384
|
||||
|
||||
def test_palisade_telluride_hda2_sends_lkas_and_camera_suppression(self):
|
||||
@@ -587,6 +590,41 @@ class TestHyundaiFingerprint:
|
||||
assert (0x2A4, 0) in msg_addrs_buses
|
||||
assert not ({0x340, 0x364} & {addr for addr, _, _ in msgs})
|
||||
|
||||
def test_palisade_telluride_hda2_long_sends_complete_support_set(self):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
fingerprint[2][0x50] = 16
|
||||
fingerprint[1][0x38D] = 8
|
||||
car_fw = [CarParams.CarFw(ecu=Ecu.adas, fwVersion=b"", address=0x730, brand="hyundai")]
|
||||
CP = CarInterface.get_params(CAR.HYUNDAI_PALISADE_2023, fingerprint, car_fw, True, False, False, None)
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
controller.frame = 0
|
||||
|
||||
hud_control = SimpleNamespace(
|
||||
visualAlert=CarControl.HUDControl.VisualAlert.none,
|
||||
leftLaneVisible=True,
|
||||
rightLaneVisible=True,
|
||||
leftLaneDepart=False,
|
||||
rightLaneDepart=False,
|
||||
leadDistanceBars=3,
|
||||
leadVisible=True,
|
||||
)
|
||||
lfa_block_msg = {f"BYTE{i}": 0 for i in range(3, 24) if i != 7}
|
||||
lfa_block_msg["COUNTER"] = 0
|
||||
CS = SimpleNamespace(lfa_block_msg=lfa_block_msg, redneck_send_button=Buttons.NONE, lkas11={}, msg_364={},
|
||||
out=SimpleNamespace(vEgoRaw=5.0))
|
||||
CC = SimpleNamespace(enabled=True, cruiseControl=SimpleNamespace(cancel=False, resume=False, override=False))
|
||||
actuators = SimpleNamespace(longControlState=LongCtrlState.pid)
|
||||
|
||||
msgs = controller.create_can_msgs(True, 0, False, 42.0, 0.0, False, hud_control, actuators, CS, CC, 2, 2)
|
||||
msg_addrs_buses = {(addr, bus) for addr, _, bus in msgs}
|
||||
|
||||
assert {
|
||||
(0x50, 0), (0x2A4, 0), (0x51, 0),
|
||||
(0x340, 1), (0x485, 1), (0x420, 1), (0x421, 1), (0x389, 1), (0x38D, 1),
|
||||
(0x363, 1), (0x398, 1), (0x399, 1), (0x39A, 1), (0x39B, 1), (0x39C, 1), (0x43A, 1),
|
||||
} <= msg_addrs_buses
|
||||
assert (0x364, 1) not in msg_addrs_buses
|
||||
|
||||
def test_g70_aol_uses_active_lkas_icon(self):
|
||||
CP = CarInterface.get_params(CAR.GENESIS_G70_2020, gen_empty_fingerprint(), [], False, False, False, None)
|
||||
controller = CarController(DBC[CP.carFingerprint], CP)
|
||||
@@ -616,6 +654,22 @@ class TestHyundaiFingerprint:
|
||||
assert DBC[CP.carFingerprint][Bus.pt] == "hyundai_can_refresh_generated"
|
||||
assert CP.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_REFRESH_MSGS
|
||||
|
||||
def test_elantra_refresh_decodes_classic_media_buttons(self):
|
||||
toggles = get_test_toggles()
|
||||
CP = CarInterface.get_params(CAR.HYUNDAI_ELANTRA_HEV_2024, gen_empty_fingerprint(), [], True, False, False, toggles)
|
||||
FPCP = CarInterface.get_starpilot_params(CAR.HYUNDAI_ELANTRA_HEV_2024, gen_empty_fingerprint(), [], CP, toggles)
|
||||
|
||||
car_state = CarState(CP, FPCP)
|
||||
can_parsers = car_state.get_can_parsers(CP)
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
media_msg = packer.make_can_msg("GW_SWRC_PE", 0, {"C_ModeSW": 1, "C_MTSSW": 1})
|
||||
|
||||
can_parsers[Bus.pt].update([(1_000_000_000, [media_msg])])
|
||||
_, fp_ret = car_state.update(can_parsers, toggles)
|
||||
|
||||
assert fp_ret.modePressed
|
||||
assert fp_ret.customPressed
|
||||
|
||||
def test_hyundai_lkas_button_sets_starpilot_safety_flag(self):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
fingerprint[0][0x391] = 8
|
||||
@@ -2418,6 +2472,46 @@ class TestHyundaiFingerprint:
|
||||
assert parser.vl["SCC14"]["ObjStatus"] == 1
|
||||
assert parser.vl["RADAR_0x363"]["FCA_ESA"] == 1
|
||||
|
||||
def test_can_canfd_blended_hda2_acc_commands_use_hda2_layout(self):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.HYUNDAI_PALISADE_2023
|
||||
CP.flags = int(HyundaiFlags.CAN_CANFD_BLENDED | HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.USE_FCA)
|
||||
|
||||
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
|
||||
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [
|
||||
("SCC11", 0),
|
||||
("SCC12", 0),
|
||||
("SCC14", 0),
|
||||
("FCA11", 0),
|
||||
], 1)
|
||||
|
||||
msgs = hyundaican.create_acc_commands_can_canfd_blended_hda2(
|
||||
packer,
|
||||
enabled=True,
|
||||
accel=-1.0,
|
||||
accel_last=0.0,
|
||||
upper_jerk=2.5,
|
||||
idx=15,
|
||||
hud_control=SimpleNamespace(leadDistanceBars=3, leadVisible=True),
|
||||
set_speed=42,
|
||||
stopping=False,
|
||||
long_override=False,
|
||||
use_fca=True,
|
||||
CP=CP,
|
||||
)
|
||||
parser.update([(1, msgs)])
|
||||
|
||||
assert parser.can_valid
|
||||
assert parser.vl["SCC11"]["aReqRaw"] == pytest.approx(-1.0)
|
||||
assert parser.vl["SCC11"]["aReqValue"] == pytest.approx(-0.1)
|
||||
assert parser.vl["SCC11"]["COUNTER"] == 0
|
||||
assert parser.vl["SCC12"]["VSetDis"] == 42
|
||||
assert parser.vl["SCC14"]["ObjValid"] == 0
|
||||
assert parser.vl["SCC14"]["ObjStatus"] == 2
|
||||
assert parser.vl["FCA11"]["aeb_cmd_act"] == 0
|
||||
assert parser.vl["FCA11"]["fca_cmd_act"] == 0
|
||||
assert next(dat for addr, dat, _ in msgs if addr == 0x38D)[4:7] == b"\xC0\x3F\x7F"
|
||||
|
||||
def test_can_acc_optional_messages_use_enabled_fca_usm(self):
|
||||
CP = CarParams.new_message()
|
||||
CP.carFingerprint = CAR.GENESIS_G90
|
||||
|
||||
@@ -481,7 +481,7 @@ class CAR(Platforms):
|
||||
[
|
||||
HyundaiCarDocs("Hyundai Palisade (without HDA II) 2023-25", "Highway Driving Assist",
|
||||
car_parts=CarParts.common([CarHarness.hyundai_a])),
|
||||
HyundaiCarDocs("Hyundai Palisade (with HDA II) 2023-24", "Highway Driving Assist II",
|
||||
HyundaiCarDocs("Hyundai Palisade (with HDA II) 2023-25", "Highway Driving Assist II",
|
||||
car_parts=CarParts.common([CarHarness.hyundai_r])),
|
||||
HyundaiCarDocs("Kia Telluride (without HDA II) 2023-25", "Highway Driving Assist",
|
||||
car_parts=CarParts.common([CarHarness.hyundai_l])),
|
||||
@@ -968,6 +968,10 @@ CANCEL_BUTTON_ENABLE_CARS = frozenset({
|
||||
CAR.HYUNDAI_PALISADE_2023,
|
||||
})
|
||||
|
||||
CAN_CANFD_BLENDED_HDA2_LONGITUDINAL_CAR = frozenset({
|
||||
CAR.HYUNDAI_PALISADE_2023,
|
||||
})
|
||||
|
||||
KIA_EV6_GT_LINE_LONG_TUNING_VDS_PREFIXES = frozenset({
|
||||
"C4DLC",
|
||||
})
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
import numpy as np
|
||||
|
||||
from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, structs
|
||||
from opendbc.car import Bus, DT_CTRL, structs
|
||||
from opendbc.car.common.filter_simple import FirstOrderFilter
|
||||
from opendbc.car.lateral import apply_std_steer_angle_limits
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
@@ -55,7 +55,7 @@ class CarController(CarControllerBase):
|
||||
CC.longActive and brake_pressure > 0, brake_mode))
|
||||
|
||||
if self.frame % 100 == 0:
|
||||
can_sends.append(make_tester_present_msg(0x707, 0, suppress_response=True))
|
||||
can_sends.append(nissancan.create_leaf_adas_tester_present())
|
||||
|
||||
### STEER ###
|
||||
steer_hud_alert = 1 if hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0
|
||||
|
||||
@@ -1,6 +1,9 @@
|
||||
from opendbc.car import get_safety_config, structs, uds
|
||||
import time
|
||||
|
||||
from opendbc.car import get_safety_config, structs
|
||||
from opendbc.car.disable_ecu import disable_ecu, ecu_log
|
||||
from opendbc.car.interfaces import CarInterfaceBase
|
||||
from opendbc.car.isotp_parallel_query import IsoTpParallelQuery
|
||||
from opendbc.car.nissan.carcontroller import CarController
|
||||
from opendbc.car.nissan.carstate import CarState
|
||||
from opendbc.car.nissan.values import CAR, CarControllerParams, NissanSafetyFlags, \
|
||||
@@ -9,8 +12,19 @@ from opendbc.car.nissan.values import CAR, CarControllerParams, NissanSafetyFlag
|
||||
|
||||
LEAF_ADAS_ECU_ADDR = 0x707
|
||||
LEAF_ADAS_ECU_BUS = 0
|
||||
LEAF_ADAS_COMMAND_BUS = 1
|
||||
LEAF_ADAS_COMMAND_ADDRS = frozenset((0x1C3, 0x2B0))
|
||||
LEAF_2025_SV_PLUS_CAMERA_FW = b'6WK2CDB\x04\x18\x00\x00\x00\x00\x00R=1\x18\x99\x10\x00\x00\x00\x80'
|
||||
|
||||
# This Leaf camera uses KWP2000 rather than UDS for session management.
|
||||
LEAF_KWP_DATA_MONITOR_REQUEST = b"\x10\xF0"
|
||||
LEAF_KWP_DATA_MONITOR_RESPONSE = b"\x50\xF0"
|
||||
LEAF_KWP_DISABLE_NORMAL_TX = b"\x28\x01"
|
||||
|
||||
LEAF_KWP_TAKEOVER_SESSIONS = (
|
||||
(LEAF_KWP_DATA_MONITOR_REQUEST, LEAF_KWP_DATA_MONITOR_RESPONSE),
|
||||
)
|
||||
|
||||
|
||||
def is_leaf_2025_sv_plus_longitudinal(candidate, car_fw):
|
||||
return candidate == CAR.NISSAN_LEAF and any(
|
||||
@@ -19,6 +33,82 @@ def is_leaf_2025_sv_plus_longitudinal(candidate, car_fw):
|
||||
)
|
||||
|
||||
|
||||
def leaf_adas_commands_silent(can_recv, settle_time=0.05, observe_time=0.15):
|
||||
"""Confirm the stock ADAS command sender stopped while bus 1 is still observable."""
|
||||
if can_recv is None:
|
||||
return False
|
||||
|
||||
try:
|
||||
# Let already-published CAN batches age out, then discard everything queued
|
||||
# before the communication-control response.
|
||||
time.sleep(settle_time)
|
||||
can_recv()
|
||||
|
||||
saw_adas_bus_traffic = False
|
||||
deadline = time.monotonic() + observe_time
|
||||
while time.monotonic() < deadline:
|
||||
packets = can_recv(wait_for_one=True)
|
||||
for packet in packets:
|
||||
for msg in packet:
|
||||
if msg.src != LEAF_ADAS_COMMAND_BUS:
|
||||
continue
|
||||
saw_adas_bus_traffic = True
|
||||
if msg.address in LEAF_ADAS_COMMAND_ADDRS:
|
||||
ecu_log(f"Nissan Leaf ADAS TX still active: {hex(msg.address)} on bus {msg.src}")
|
||||
return False
|
||||
except Exception as e:
|
||||
ecu_log(f"Nissan Leaf ADAS TX silence verification exception: {e}")
|
||||
return False
|
||||
|
||||
if not saw_adas_bus_traffic:
|
||||
ecu_log("Nissan Leaf ADAS TX silence could not be verified: no bus 1 traffic observed")
|
||||
return saw_adas_bus_traffic
|
||||
|
||||
|
||||
def leaf_adas_commands_present(can_recv, settle_time=0.05, observe_time=0.15):
|
||||
"""Confirm the stock ADAS command sender resumed on bus 1."""
|
||||
if can_recv is None:
|
||||
return False
|
||||
|
||||
try:
|
||||
time.sleep(settle_time)
|
||||
can_recv()
|
||||
|
||||
deadline = time.monotonic() + observe_time
|
||||
while time.monotonic() < deadline:
|
||||
for packet in can_recv(wait_for_one=True):
|
||||
if any(msg.src == LEAF_ADAS_COMMAND_BUS and msg.address in LEAF_ADAS_COMMAND_ADDRS for msg in packet):
|
||||
return True
|
||||
except Exception as e:
|
||||
ecu_log(f"Nissan Leaf ADAS TX recovery verification exception: {e}")
|
||||
return False
|
||||
|
||||
ecu_log("Nissan Leaf ADAS normal TX recovery could not be verified")
|
||||
return False
|
||||
|
||||
|
||||
def restore_leaf_adas_tx(can_recv, can_send):
|
||||
"""Return to the confirmed KWP default session and verify normal TX resumes."""
|
||||
if can_recv is None or can_send is None:
|
||||
return False
|
||||
|
||||
try:
|
||||
ecu_log("Nissan Leaf ADAS TX restore using KWP default session 1081")
|
||||
query = IsoTpParallelQuery(
|
||||
can_send, can_recv, LEAF_ADAS_ECU_BUS, [LEAF_ADAS_ECU_ADDR],
|
||||
[NISSAN_DIAGNOSTIC_REQUEST_KWP], [NISSAN_DIAGNOSTIC_RESPONSE_KWP],
|
||||
response_offset=NISSAN_RX_OFFSET,
|
||||
)
|
||||
if query.get_data(0.2) and leaf_adas_commands_present(can_recv):
|
||||
ecu_log("Nissan Leaf ADAS normal TX restored and command traffic confirmed")
|
||||
return True
|
||||
except Exception as e:
|
||||
ecu_log(f"Nissan Leaf ADAS TX restore exception: {e}")
|
||||
|
||||
ecu_log("Nissan Leaf ADAS normal TX restore was not confirmed")
|
||||
return False
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
CarState = CarState
|
||||
CarController = CarController
|
||||
@@ -62,39 +152,36 @@ class CarInterface(CarInterfaceBase):
|
||||
|
||||
@staticmethod
|
||||
def init(CP, can_recv, can_send):
|
||||
if not (CP.openpilotLongitudinalControl and CP.carFingerprint == CAR.NISSAN_LEAF):
|
||||
if not (CP.openpilotLongitudinalControl and CP.alphaLongitudinalAvailable and CP.carFingerprint == CAR.NISSAN_LEAF):
|
||||
return
|
||||
|
||||
from openpilot.common.params import Params
|
||||
params = Params()
|
||||
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL,
|
||||
uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX,
|
||||
uds.MESSAGE_TYPE.NORMAL])
|
||||
ecu_disabled = disable_ecu(can_recv, can_send, bus=LEAF_ADAS_ECU_BUS, addr=LEAF_ADAS_ECU_ADDR,
|
||||
com_cont_req=communication_control, require_response=True, response_offset=NISSAN_RX_OFFSET)
|
||||
if not ecu_disabled:
|
||||
# Nissan firmware queries use the KWP-style default session. Try it after
|
||||
# standard UDS extended-session control, but still require a positive 0x68 response.
|
||||
ecu_disabled = False
|
||||
for diag_request, diag_response in LEAF_KWP_TAKEOVER_SESSIONS:
|
||||
ecu_log(f"Nissan Leaf ADAS takeover using KWP session {diag_request.hex()}")
|
||||
ecu_disabled = disable_ecu(can_recv, can_send, bus=LEAF_ADAS_ECU_BUS, addr=LEAF_ADAS_ECU_ADDR,
|
||||
com_cont_req=communication_control, require_response=True,
|
||||
diag_request=NISSAN_DIAGNOSTIC_REQUEST_KWP, diag_response=NISSAN_DIAGNOSTIC_RESPONSE_KWP,
|
||||
response_offset=NISSAN_RX_OFFSET)
|
||||
params.put_bool("EcuDisableFailed", not ecu_disabled)
|
||||
if ecu_disabled:
|
||||
ecu_log("Nissan Leaf ADAS TX disabled; experimental longitudinal control enabled")
|
||||
com_cont_req=LEAF_KWP_DISABLE_NORMAL_TX, require_response=True, retry=1,
|
||||
diag_request=diag_request, diag_response=diag_response, response_offset=NISSAN_RX_OFFSET)
|
||||
if ecu_disabled:
|
||||
break
|
||||
|
||||
takeover_confirmed = ecu_disabled and leaf_adas_commands_silent(can_recv)
|
||||
params.put_bool("EcuDisableFailed", not takeover_confirmed)
|
||||
if takeover_confirmed:
|
||||
ecu_log("Nissan Leaf ADAS TX disable and command silence confirmed; experimental longitudinal control enabled")
|
||||
else:
|
||||
# A response can be lost after the ECU accepts 0x28. Always attempt to
|
||||
# restore stock transmission before falling back to stock longitudinal.
|
||||
restore_leaf_adas_tx(can_recv, can_send)
|
||||
CP.safetyConfigs[-1].safetyParam &= ~NissanSafetyFlags.LONG_CONTROL.value
|
||||
CP.openpilotLongitudinalControl = False
|
||||
CP.pcmCruise = True
|
||||
ecu_log("Nissan Leaf ADAS TX disable failed; falling back to stock longitudinal control")
|
||||
ecu_log("Nissan Leaf ADAS takeover was not confirmed; falling back to stock longitudinal control")
|
||||
|
||||
@staticmethod
|
||||
def deinit(CP, can_recv, can_send):
|
||||
if not (CP.openpilotLongitudinalControl and CP.carFingerprint == CAR.NISSAN_LEAF):
|
||||
if not (CP.openpilotLongitudinalControl and CP.alphaLongitudinalAvailable and CP.carFingerprint == CAR.NISSAN_LEAF):
|
||||
return
|
||||
|
||||
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL,
|
||||
0x80 | uds.CONTROL_TYPE.ENABLE_RX_ENABLE_TX,
|
||||
uds.MESSAGE_TYPE.NORMAL])
|
||||
disable_ecu(can_recv, can_send, bus=LEAF_ADAS_ECU_BUS, addr=LEAF_ADAS_ECU_ADDR,
|
||||
com_cont_req=communication_control, response_offset=NISSAN_RX_OFFSET)
|
||||
restore_leaf_adas_tx(can_recv, can_send)
|
||||
|
||||
@@ -6,6 +6,11 @@ from opendbc.car.nissan.values import CAR
|
||||
nissan_checksum = crcmod.mkCrcFun(0x11d, initCrc=0x00, rev=False, xorOut=0xff)
|
||||
|
||||
|
||||
def create_leaf_adas_tester_present():
|
||||
"""KWP2000 tester-present with a response requested, for the 2025 Leaf ADAS ECU."""
|
||||
return CanData(0x707, b"\x02\x3E\x01\x00\x00\x00\x00\x00", 0)
|
||||
|
||||
|
||||
def create_accel_command(raw_command, frame, active):
|
||||
"""Build the Leaf ADAS propulsion/regen request (0x2B0)."""
|
||||
raw_command = int(raw_command)
|
||||
|
||||
@@ -2,9 +2,11 @@ from types import SimpleNamespace
|
||||
|
||||
import pytest
|
||||
|
||||
from opendbc.car import Bus, ButtonType, gen_empty_fingerprint, structs, uds
|
||||
from opendbc.car import Bus, ButtonType, gen_empty_fingerprint, structs
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.car.nissan.carstate import CarState
|
||||
from opendbc.car.nissan.interface import CarInterface, LEAF_2025_SV_PLUS_CAMERA_FW
|
||||
from opendbc.car.nissan.interface import CarInterface, LEAF_2025_SV_PLUS_CAMERA_FW, leaf_adas_commands_present, \
|
||||
leaf_adas_commands_silent, restore_leaf_adas_tx
|
||||
from opendbc.car.nissan.values import CAR, CarControllerParams, NissanSafetyFlags
|
||||
|
||||
|
||||
@@ -82,7 +84,7 @@ def test_alpha_long_controller_sends_stock_shaped_commands_and_keepalive():
|
||||
|
||||
assert can_sends[0x2B0][1].hex() == "ff6090ac5b000e03"
|
||||
assert can_sends[0x1C3][1].hex() == "000000006400ff27"
|
||||
assert can_sends[0x707][1].hex() == "023e800000000000"
|
||||
assert can_sends[0x707][1].hex() == "023e010000000000"
|
||||
assert all(can_sends[addr][2] == 1 for addr in (0x2B0, 0x1C3))
|
||||
assert can_sends[0x707][2] == 0
|
||||
|
||||
@@ -137,33 +139,92 @@ def test_leaf_ecu_disable_is_strict_and_falls_back(monkeypatch, ecu_disabled):
|
||||
return ecu_disabled
|
||||
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.disable_ecu", fake_disable_ecu)
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.leaf_adas_commands_silent", lambda *_: ecu_disabled)
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.restore_leaf_adas_tx", lambda *_: True)
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.ecu_log", lambda *_: None)
|
||||
CarInterface.init(CP, None, None)
|
||||
|
||||
assert len(calls) == (1 if ecu_disabled else 2)
|
||||
assert len(calls) == 1
|
||||
assert calls[0]["addr"] == 0x707
|
||||
assert calls[0]["bus"] == 0
|
||||
assert calls[0]["response_offset"] == 0x20
|
||||
assert calls[0]["require_response"] is True
|
||||
assert calls[0]["com_cont_req"] == bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL,
|
||||
uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX,
|
||||
uds.MESSAGE_TYPE.NORMAL])
|
||||
if not ecu_disabled:
|
||||
assert calls[1]["diag_request"] == b"\x10\x81"
|
||||
assert calls[1]["diag_response"] == b"\x50\x81"
|
||||
assert calls[0]["diag_request"] == b"\x10\xf0"
|
||||
assert calls[0]["diag_response"] == b"\x50\xf0"
|
||||
assert calls[0]["com_cont_req"] == b"\x28\x01"
|
||||
assert calls[0]["retry"] == 1
|
||||
assert CP.openpilotLongitudinalControl is ecu_disabled
|
||||
assert CP.pcmCruise is not ecu_disabled
|
||||
assert bool(CP.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL) is ecu_disabled
|
||||
|
||||
|
||||
def test_leaf_kwp_session_can_confirm_ecu_disable(monkeypatch):
|
||||
def test_leaf_kwp_data_monitor_session_can_confirm_ecu_disable(monkeypatch):
|
||||
CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None)
|
||||
results = iter((False, True))
|
||||
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.disable_ecu", lambda *args, **kwargs: next(results))
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.disable_ecu", lambda *args, **kwargs: True)
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.leaf_adas_commands_silent", lambda *_: True)
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.ecu_log", lambda *_: None)
|
||||
CarInterface.init(CP, None, None)
|
||||
|
||||
assert CP.openpilotLongitudinalControl
|
||||
assert not CP.pcmCruise
|
||||
assert CP.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL
|
||||
|
||||
|
||||
def test_leaf_positive_disable_response_without_command_silence_falls_back(monkeypatch):
|
||||
CP = CarInterface.get_params(CAR.NISSAN_LEAF, gen_empty_fingerprint(), SUPPORTED_LEAF_FW, True, False, False, None)
|
||||
restore_calls = []
|
||||
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.disable_ecu", lambda *args, **kwargs: True)
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.leaf_adas_commands_silent", lambda *_: False)
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.restore_leaf_adas_tx", lambda *args: restore_calls.append(args) or True)
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.ecu_log", lambda *_: None)
|
||||
CarInterface.init(CP, None, None)
|
||||
|
||||
assert len(restore_calls) == 1
|
||||
assert not CP.openpilotLongitudinalControl
|
||||
assert CP.pcmCruise
|
||||
assert not (CP.safetyConfigs[-1].safetyParam & NissanSafetyFlags.LONG_CONTROL)
|
||||
|
||||
|
||||
def test_leaf_adas_command_silence_requires_live_bus_without_stock_commands(monkeypatch):
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.ecu_log", lambda *_: None)
|
||||
|
||||
def unrelated_bus_traffic(wait_for_one=False):
|
||||
return [] if not wait_for_one else [[CanData(0x123, b"\x00", 1)]]
|
||||
|
||||
assert leaf_adas_commands_silent(unrelated_bus_traffic, settle_time=0, observe_time=0.001)
|
||||
|
||||
def stock_command_traffic(wait_for_one=False):
|
||||
return [] if not wait_for_one else [[CanData(0x2B0, b"\x00" * 8, 1)]]
|
||||
|
||||
assert not leaf_adas_commands_silent(stock_command_traffic, settle_time=0, observe_time=0.001)
|
||||
assert not leaf_adas_commands_silent(lambda wait_for_one=False: [], settle_time=0, observe_time=0.001)
|
||||
|
||||
|
||||
def test_leaf_adas_command_recovery_requires_stock_command(monkeypatch):
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.ecu_log", lambda *_: None)
|
||||
|
||||
def stock_command_traffic(wait_for_one=False):
|
||||
return [] if not wait_for_one else [[CanData(0x1C3, b"\x00" * 8, 1)]]
|
||||
|
||||
assert leaf_adas_commands_present(stock_command_traffic, settle_time=0, observe_time=0.001)
|
||||
assert not leaf_adas_commands_present(lambda wait_for_one=False: [], settle_time=0, observe_time=0.001)
|
||||
|
||||
|
||||
def test_leaf_adas_restore_returns_to_kwp_default_session(monkeypatch):
|
||||
queries = []
|
||||
|
||||
class FakeQuery:
|
||||
def __init__(self, can_send, can_recv, bus, addrs, request, response, response_offset):
|
||||
queries.append((bus, addrs, request, response, response_offset))
|
||||
|
||||
def get_data(self, timeout):
|
||||
return {(0x707, None): b""}
|
||||
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.IsoTpParallelQuery", FakeQuery)
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.leaf_adas_commands_present", lambda *_: True)
|
||||
monkeypatch.setattr("opendbc.car.nissan.interface.ecu_log", lambda *_: None)
|
||||
|
||||
assert restore_leaf_adas_tx(lambda **kwargs: [], lambda msgs: None)
|
||||
assert queries == [(0, [0x707], [b"\x10\x81"], [b"\x50\x81"], 0x20)]
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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 = []
|
||||
|
||||
@@ -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]
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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),
|
||||
}
|
||||
|
||||
@@ -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',
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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)
|
||||
@@ -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
|
||||
@@ -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"
|
||||
|
||||
@@ -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()
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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";
|
||||
@@ -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
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -92,13 +92,16 @@ static bool hyundai_legacy = false;
|
||||
static bool hyundai_can_canfd_blended_hda2 = false;
|
||||
static bool hyundai_acc_main_on_rx_prev = false;
|
||||
|
||||
#define HYUNDAI_CAN_CANFD_BLENDED_HDA2_RX_CHECKS() \
|
||||
#define HYUNDAI_CAN_CANFD_BLENDED_HDA2_COMMON_RX_CHECKS() \
|
||||
{.msg = {{0x260, 1, 8, 100U, .max_counter = 3U, .ignore_quality_flag = true}, \
|
||||
{0x371, 1, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }}}, \
|
||||
{.msg = {{0x386, 1, 8, 50U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x394, 1, 8, 50U, .max_counter = 7U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x251, 1, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x4F1, 1, 4, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, \
|
||||
{.msg = {{0x4F1, 1, 4, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
|
||||
#define HYUNDAI_CAN_CANFD_BLENDED_HDA2_RX_CHECKS() \
|
||||
HYUNDAI_CAN_CANFD_BLENDED_HDA2_COMMON_RX_CHECKS() \
|
||||
HYUNDAI_SCC11_ADDR_CHECK(1) \
|
||||
HYUNDAI_SCC12_ADDR_CHECK(1, true)
|
||||
|
||||
@@ -281,14 +284,21 @@ static bool hyundai_tx_hook(const CANPacket_t *msg) {
|
||||
|
||||
bool tx = true;
|
||||
|
||||
// FCA11: Block any potential actuation
|
||||
// FCA11: Block any potential actuation. The blended HDA II layout uses
|
||||
// different static fields, but its explicit AEB/FCA request bits stay zero.
|
||||
if (msg->addr == 0x38DU) {
|
||||
int CR_VSM_DecCmd = msg->data[1];
|
||||
bool FCA_CmdAct = GET_BIT(msg, 20U);
|
||||
bool CF_VSM_DecCmdAct = GET_BIT(msg, 31U);
|
||||
if (hyundai_can_canfd_blended_hda2) {
|
||||
if (GET_BIT(msg, 16U) || GET_BIT(msg, 19U)) {
|
||||
tx = false;
|
||||
}
|
||||
} else {
|
||||
int CR_VSM_DecCmd = msg->data[1];
|
||||
bool FCA_CmdAct = GET_BIT(msg, 20U);
|
||||
bool CF_VSM_DecCmdAct = GET_BIT(msg, 31U);
|
||||
|
||||
if ((CR_VSM_DecCmd != 0) || FCA_CmdAct || CF_VSM_DecCmdAct) {
|
||||
tx = false;
|
||||
if ((CR_VSM_DecCmd != 0) || FCA_CmdAct || CF_VSM_DecCmdAct) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -348,7 +358,7 @@ static bool hyundai_tx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
|
||||
// UDS: Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
|
||||
if (msg->addr == 0x7D0U) {
|
||||
if ((msg->addr == 0x7D0U) || (msg->addr == 0x730U)) {
|
||||
if ((GET_BYTES(msg, 0, 4) != 0x00803E02U) || (GET_BYTES(msg, 4, 4) != 0x0U)) {
|
||||
tx = false;
|
||||
}
|
||||
@@ -399,6 +409,27 @@ static safety_config hyundai_init(uint16_t param) {
|
||||
{0x2A4, 0, 24, .check_relay = true},
|
||||
};
|
||||
|
||||
static const CanMsg HYUNDAI_CAN_CANFD_BLENDED_HDA2_LONG_TX_MSGS[] = {
|
||||
{0x50, 0, 16, .check_relay = true},
|
||||
{0x4F1, 1, 4, .check_relay = false},
|
||||
{0x2A4, 0, 24, .check_relay = true},
|
||||
{0x51, 0, 32, .check_relay = false},
|
||||
{0x730, 1, 8, .check_relay = false},
|
||||
{0x340, 1, 8, .check_relay = true},
|
||||
{0x485, 1, 8, .check_relay = true},
|
||||
{0x420, 1, 8, .check_relay = true},
|
||||
{0x421, 1, 8, .check_relay = true},
|
||||
{0x389, 1, 8, .check_relay = true},
|
||||
{0x38D, 1, 8, .check_relay = false},
|
||||
{0x363, 1, 8, .check_relay = false},
|
||||
{0x398, 1, 8, .check_relay = false},
|
||||
{0x399, 1, 8, .check_relay = false},
|
||||
{0x39A, 1, 8, .check_relay = false},
|
||||
{0x39B, 1, 8, .check_relay = false},
|
||||
{0x39C, 1, 8, .check_relay = false},
|
||||
{0x43A, 1, 8, .check_relay = false},
|
||||
};
|
||||
|
||||
static const CanMsg HYUNDAI_CAN_CANFD_BLENDED_LONG_TX_MSGS[] = {
|
||||
{0x340, 0, 8, .check_relay = true},
|
||||
{0x4F1, 0, 4, .check_relay = false},
|
||||
@@ -462,10 +493,10 @@ static safety_config hyundai_init(uint16_t param) {
|
||||
}
|
||||
if (hyundai_can_canfd_blended_hda2) {
|
||||
static RxCheck hyundai_can_canfd_blended_hda2_long_rx_checks[] = {
|
||||
HYUNDAI_CAN_CANFD_BLENDED_HDA2_RX_CHECKS()
|
||||
HYUNDAI_CAN_CANFD_BLENDED_HDA2_COMMON_RX_CHECKS()
|
||||
};
|
||||
SET_RX_CHECKS(hyundai_can_canfd_blended_hda2_long_rx_checks, ret);
|
||||
SET_TX_MSGS(HYUNDAI_CAN_CANFD_BLENDED_HDA2_TX_MSGS, ret);
|
||||
SET_TX_MSGS(HYUNDAI_CAN_CANFD_BLENDED_HDA2_LONG_TX_MSGS, ret);
|
||||
} else if (hyundai_camera_scc) {
|
||||
if (hyundai_can_refresh_msgs) {
|
||||
SET_TX_MSGS(HYUNDAI_CAMERA_SCC_LONG_REFRESH_TX_MSGS, ret);
|
||||
|
||||
@@ -174,7 +174,8 @@ static bool nissan_tx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
|
||||
if (nissan_longitudinal && (msg->addr == 0x707U) && (msg->bus == 0U)) {
|
||||
violation |= (msg->data[0] != 0x02U) || (msg->data[1] != 0x3EU) || (msg->data[2] != 0x80U);
|
||||
// KWP2000 TesterPresent, response required. The Leaf camera rejects UDS 0x3E80.
|
||||
violation |= (msg->data[0] != 0x02U) || (msg->data[1] != 0x3EU) || (msg->data[2] != 0x01U);
|
||||
for (int i = 3; i < 8; i++) {
|
||||
violation |= msg->data[i] != 0U;
|
||||
}
|
||||
|
||||
@@ -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,
|
||||
};
|
||||
@@ -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();
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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 *****
|
||||
|
||||
|
||||
@@ -254,6 +254,10 @@ class TestHyundaiCanCanfdBlendedSafety(TestHyundaiSafety):
|
||||
|
||||
class TestHyundaiCanCanfdBlendedHda2Safety(unittest.TestCase):
|
||||
TX_MSGS = [[0x50, 0], [0x4F1, 1], [0x2A4, 0]]
|
||||
LONG_TX_MSGS = TX_MSGS + [
|
||||
[0x51, 0], [0x730, 1], [0x340, 1], [0x485, 1], [0x420, 1], [0x421, 1], [0x389, 1], [0x38D, 1],
|
||||
[0x363, 1], [0x398, 1], [0x399, 1], [0x39A, 1], [0x39B, 1], [0x39C, 1], [0x43A, 1],
|
||||
]
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerSafety("hyundai_palisade_2023_generated")
|
||||
@@ -291,6 +295,52 @@ class TestHyundaiCanCanfdBlendedHda2Safety(unittest.TestCase):
|
||||
self.assertEqual(-1, self.safety.safety_fwd_hook(2, 0x50))
|
||||
self.assertEqual(-1, self.safety.safety_fwd_hook(2, 0x2A4))
|
||||
|
||||
def test_hda2_longitudinal_support_messages_require_long_flag(self):
|
||||
flags = HyundaiSafetyFlags.CAN_CANFD_BLENDED | HyundaiSafetyFlags.CANFD_LKA_STEERING | HyundaiSafetyFlags.LONG
|
||||
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, flags)
|
||||
self.safety.init_tests()
|
||||
|
||||
for addr, bus in self.LONG_TX_MSGS:
|
||||
if addr == 0x50:
|
||||
msg = self._lkas_msg()
|
||||
elif addr == 0x420:
|
||||
msg = self.packer.make_can_msg_panda("SCC11", bus, {"aReqRaw": 0.0, "aReqValue": 0.0})
|
||||
elif addr == 0x730:
|
||||
msg = libsafety_py.make_CANPacket(addr, bus, b"\x02\x3E\x80\x00\x00\x00\x00\x00")
|
||||
else:
|
||||
length = 32 if addr == 0x51 else 24 if addr == 0x2A4 else 4 if addr == 0x4F1 else 8
|
||||
msg = common.make_msg(bus, addr, length)
|
||||
self.assertTrue(self.safety.safety_tx_hook(msg), hex(addr))
|
||||
|
||||
self.safety.set_safety_hooks(
|
||||
CarParams.SafetyModel.hyundai,
|
||||
HyundaiSafetyFlags.CAN_CANFD_BLENDED | HyundaiSafetyFlags.CANFD_LKA_STEERING,
|
||||
)
|
||||
self.safety.init_tests()
|
||||
for addr, bus in self.LONG_TX_MSGS[len(self.TX_MSGS):]:
|
||||
length = 32 if addr == 0x51 else 8
|
||||
self.assertFalse(self.safety.safety_tx_hook(common.make_msg(bus, addr, length)), hex(addr))
|
||||
|
||||
def test_hda2_longitudinal_acceleration_and_diagnostics_are_checked(self):
|
||||
flags = HyundaiSafetyFlags.CAN_CANFD_BLENDED | HyundaiSafetyFlags.CANFD_LKA_STEERING | HyundaiSafetyFlags.LONG
|
||||
self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, flags)
|
||||
self.safety.init_tests()
|
||||
self.safety.set_controls_allowed(True)
|
||||
|
||||
for accel, allowed in ((-3.5, True), (3.5, True), (-3.51, False), (3.51, False)):
|
||||
msg = self.packer.make_can_msg_panda("SCC11", 1, {"aReqRaw": accel, "aReqValue": accel})
|
||||
self.assertEqual(allowed, self.safety.safety_tx_hook(msg))
|
||||
|
||||
valid_tester = libsafety_py.make_CANPacket(0x730, 1, b"\x02\x3E\x80\x00\x00\x00\x00\x00")
|
||||
invalid_tester = libsafety_py.make_CANPacket(0x730, 1, b"\x03\x28\x83\x01\x00\x00\x00\x00")
|
||||
self.assertTrue(self.safety.safety_tx_hook(valid_tester))
|
||||
self.assertFalse(self.safety.safety_tx_hook(invalid_tester))
|
||||
|
||||
fca_status = self.packer.make_can_msg_panda("FCA11", 1, {"cr_vsm_deccmd": 255, "cf_vsm_deccmdact": 0})
|
||||
fca_request = self.packer.make_can_msg_panda("FCA11", 1, {"aeb_cmd_act": 1})
|
||||
self.assertTrue(self.safety.safety_tx_hook(fca_status))
|
||||
self.assertFalse(self.safety.safety_tx_hook(fca_request))
|
||||
|
||||
|
||||
class TestHyundaiSafetyFCEV(TestHyundaiSafety):
|
||||
def setUp(self):
|
||||
|
||||
@@ -1,7 +1,6 @@
|
||||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
|
||||
from opendbc.car import make_tester_present_msg
|
||||
from opendbc.car.nissan import nissancan
|
||||
from opendbc.car.nissan.values import NissanSafetyFlags
|
||||
from opendbc.car.structs import CarParams
|
||||
@@ -261,7 +260,7 @@ class TestNissanLeafLongSafety(TestNissanLeafSafety):
|
||||
self.assertTrue(self._tx(self._brake_msg(0, active=False, brake_mode=False)))
|
||||
|
||||
def test_tester_present(self):
|
||||
tester_present = make_tester_present_msg(0x707, 0, suppress_response=True)
|
||||
tester_present = nissancan.create_leaf_adas_tester_present()
|
||||
self.assertTrue(self._tx(self._make_msg(tester_present)))
|
||||
|
||||
for index in range(8):
|
||||
|
||||
@@ -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()
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,2 +1,2 @@
|
||||
extern const uint8_t gitversion[19];
|
||||
const uint8_t gitversion[19] = "DEV-9b9878ed-DEBUG";
|
||||
const uint8_t gitversion[19] = "DEV-f22bcfee-DEBUG";
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user