mirror of
https://github.com/infiniteCable2/opendbc.git
synced 2026-02-18 13:03:52 +08:00
Merge branch 'upstream/opendbc/master' into sync-20251114
# Conflicts: # SConstruct # opendbc/car/tests/routes.py # opendbc/car/toyota/carcontroller.py # opendbc/car/toyota/carstate.py # opendbc/safety/lateral.h # opendbc/safety/modes/subaru.h # opendbc/safety/modes/subaru_preglobal.h # opendbc/safety/modes/toyota.h # opendbc/safety/tests/common.py # opendbc/safety/tests/libsafety/safety_helpers.h # opendbc/safety/tests/libsafety/safety_helpers.py # opendbc/safety/tests/misra/main.c # opendbc/safety/tests/test_chrysler.py # opendbc/safety/tests/test_honda.py # opendbc/safety/tests/test_hyundai.py # opendbc/safety/tests/test_hyundai_canfd.py # opendbc/safety/tests/test_toyota.py
This commit is contained in:
@@ -67,7 +67,7 @@ At its most basic, a car port will control the steering on a car. A "complete" c
|
||||
|
||||
### Connect to the Car
|
||||
|
||||
The first step is to get connected to the car with a comma 3X and a car harness.
|
||||
The first step is to get connected to the car with a comma four and a car harness.
|
||||
The car harness gets you connected to two different CAN buses and splits one of those buses to send our own actuation messages.
|
||||
|
||||
If you're lucky, a harness compatible with your car will already be designed and sold on comma.ai/shop.
|
||||
@@ -151,7 +151,7 @@ In addition to the standard bounties, we also offer higher value bounties for mo
|
||||
|
||||
## FAQ
|
||||
|
||||
***How do I use this?*** A [comma 3X](https://comma.ai/shop/comma-3x) is custom-designed to be the best way to run and develop opendbc and openpilot.
|
||||
***How do I use this?*** A [comma four](https://comma.ai/shop/comma-four) is custom-designed to be the best way to run and develop opendbc and openpilot.
|
||||
|
||||
***Which cars are supported?*** See the [supported cars list](docs/CARS.md).
|
||||
|
||||
@@ -179,7 +179,7 @@ In addition to the standard bounties, we also offer higher value bounties for mo
|
||||
* **[DBC file](https://en.wikipedia.org/wiki/CAN_bus#DBC)**: contains definitions for messages on a CAN bus
|
||||
* **[openpilot](https://github.com/commaai/openpilot)**: an ADAS system for cars supported by opendbc
|
||||
* **[comma](https://github.com/commaai)**: the company behind opendbc
|
||||
* **[comma 3X](https://comma.ai/shop/comma-3x)**: the hardware used to run openpilot
|
||||
* **[comma four](https://comma.ai/shop/comma-four)**: the hardware used to run openpilot
|
||||
|
||||
### More resources
|
||||
|
||||
|
||||
@@ -1,6 +1,4 @@
|
||||
Import("env")
|
||||
|
||||
SConscript(['opendbc/dbc/SConscript'], exports={'env': env})
|
||||
SConscript(['opendbc/dbc/SConscript'])
|
||||
|
||||
# test files
|
||||
if GetOption('extras'):
|
||||
|
||||
53
SConstruct
53
SConstruct
@@ -1,27 +1,9 @@
|
||||
import os
|
||||
import subprocess
|
||||
import platform
|
||||
|
||||
arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
|
||||
if platform.system() == "Darwin":
|
||||
arch = "Darwin"
|
||||
|
||||
cpppath = [
|
||||
'#',
|
||||
'/usr/lib/include',
|
||||
]
|
||||
|
||||
AddOption('--minimal',
|
||||
action='store_false',
|
||||
dest='extras',
|
||||
default=True,
|
||||
help='the minimum build. no tests, tools, etc.')
|
||||
|
||||
AddOption('--asan',
|
||||
action='store_true',
|
||||
help='turn on ASAN')
|
||||
|
||||
# safety options
|
||||
AddOption('--ubsan',
|
||||
action='store_true',
|
||||
help='turn on UBSan')
|
||||
@@ -30,39 +12,4 @@ AddOption('--coverage',
|
||||
action='store_true',
|
||||
help='build with test coverage options')
|
||||
|
||||
AddOption('--mutation',
|
||||
action='store_true',
|
||||
help='generate mutation-ready code')
|
||||
|
||||
ccflags_asan = ["-fsanitize=address", "-fno-omit-frame-pointer"] if GetOption('asan') else []
|
||||
ldflags_asan = ["-fsanitize=address"] if GetOption('asan') else []
|
||||
|
||||
env = Environment(
|
||||
ENV=os.environ,
|
||||
CC='gcc',
|
||||
CXX='g++',
|
||||
CCFLAGS=[
|
||||
"-g",
|
||||
"-fPIC",
|
||||
"-O2",
|
||||
"-Wunused",
|
||||
"-Werror",
|
||||
"-Wshadow",
|
||||
"-Wno-vla-cxx-extension",
|
||||
"-Wno-unknown-warning-option", # for compatibility across compiler versions
|
||||
] + ccflags_asan,
|
||||
LDFLAGS=ldflags_asan,
|
||||
LINKFLAGS=ldflags_asan,
|
||||
CFLAGS="-std=gnu11",
|
||||
CXXFLAGS=["-std=c++1z"],
|
||||
CPPPATH=cpppath,
|
||||
tools=["default", "compilation_db"]
|
||||
)
|
||||
if arch != "Darwin":
|
||||
env.Append(CCFLAGS=["-fmax-errors=1", ])
|
||||
|
||||
env.CompilationDatabase('compile_commands.json')
|
||||
|
||||
Export('env', 'arch')
|
||||
|
||||
SConscript(['SConscript'])
|
||||
|
||||
18
docs/CARS.md
18
docs/CARS.md
@@ -1,6 +1,6 @@
|
||||
<!--- AUTOGENERATED FROM selfdrive/car/CARS_template.md, DO NOT EDIT. --->
|
||||
|
||||
# Support Information for 379 Known Cars
|
||||
# Support Information for 384 Known Cars
|
||||
|
||||
|Make|Model|Package|Support Level|
|
||||
|---|---|---|:---:|
|
||||
@@ -9,6 +9,7 @@
|
||||
|Acura|Integra 2023-25|All|[Community](#community)|
|
||||
|Acura|MDX 2015-16|Advance Package|[Community](#community)|
|
||||
|Acura|MDX 2017-20|All|[Community](#community)|
|
||||
|Acura|MDX 2022-24|All|[Community](#community)|
|
||||
|Acura|MDX 2025|All except Type S|[Upstream](#upstream)|
|
||||
|Acura|RDX 2016-18|AcuraWatch Plus or Advance Package|[Upstream](#upstream)|
|
||||
|Acura|RDX 2019-21|All|[Upstream](#upstream)|
|
||||
@@ -16,6 +17,8 @@
|
||||
|Acura|RLX 2017|Advance Package or Technology Package|[Community](#community)|
|
||||
|Acura|TLX 2015-17|Advance Package|[Community](#community)|
|
||||
|Acura|TLX 2018-20|All|[Community](#community)|
|
||||
|Acura|TLX 2021|All|[Upstream](#upstream)|
|
||||
|Acura|TLX 2022-23|All|[Community](#community)|
|
||||
|Acura|ZDX 2024|All|[Not compatible](#can-bus-security)|
|
||||
|Audi|A3 2014-19|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|
||||
|Audi|A3 Sportback e-tron 2017-18|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|
||||
@@ -94,11 +97,11 @@
|
||||
|Honda|Civic Hatchback 2022-24|All|[Upstream](#upstream)|
|
||||
|Honda|Civic Hatchback Hybrid 2025-26|All|[Upstream](#upstream)|
|
||||
|Honda|Civic Hatchback Hybrid (Europe only) 2023|All|[Upstream](#upstream)|
|
||||
|Honda|Civic Hybrid 2025|All|[Upstream](#upstream)|
|
||||
|Honda|Civic Hybrid 2025-26|All|[Upstream](#upstream)|
|
||||
|Honda|Clarity 2018-21|All|[Community](#community)|
|
||||
|Honda|CR-V 2015-16|Touring Trim|[Upstream](#upstream)|
|
||||
|Honda|CR-V 2017-22|Honda Sensing|[Upstream](#upstream)|
|
||||
|Honda|CR-V 2023-25|All|[Upstream](#upstream)|
|
||||
|Honda|CR-V 2023-26|All|[Upstream](#upstream)|
|
||||
|Honda|CR-V Hybrid 2017-22|Honda Sensing|[Upstream](#upstream)|
|
||||
|Honda|CR-V Hybrid 2023-25|All|[Upstream](#upstream)|
|
||||
|Honda|e 2020|All|[Upstream](#upstream)|
|
||||
@@ -108,10 +111,11 @@
|
||||
|Honda|HR-V 2023-25|All|[Upstream](#upstream)|
|
||||
|Honda|Insight 2019-22|All|[Upstream](#upstream)|
|
||||
|Honda|Inspire 2018|All|[Upstream](#upstream)|
|
||||
|Honda|N-Box 2018|All|[Upstream](#upstream)|
|
||||
|Honda|Odyssey 2018-20|Honda Sensing|[Upstream](#upstream)|
|
||||
|Honda|Odyssey 2021-25|All|[Community](#community)|
|
||||
|Honda|Odyssey 2021-26|All|[Upstream](#upstream)|
|
||||
|Honda|Passport 2019-25|All|[Upstream](#upstream)|
|
||||
|Honda|Passport 2026|All|[Community](#community)|
|
||||
|Honda|Passport 2026|All|[Upstream](#upstream)|
|
||||
|Honda|Pilot 2016-22|Honda Sensing|[Upstream](#upstream)|
|
||||
|Honda|Pilot 2023-25|All|[Upstream](#upstream)|
|
||||
|Honda|Prologue 2024-25|All|[Not compatible](#can-bus-security)|
|
||||
@@ -238,11 +242,12 @@
|
||||
|Mazda|CX-5 2022-25|All|[Upstream](#upstream)|
|
||||
|Mazda|CX-9 2016-20|All|[Dashcam mode](#dashcam)|
|
||||
|Mazda|CX-9 2021-23|All|[Upstream](#upstream)|
|
||||
|Nissan|Altima 2019-20|ProPILOT Assist|[Upstream](#upstream)|
|
||||
|Nissan|Altima 2019-20, 2024|ProPILOT Assist|[Upstream](#upstream)|
|
||||
|Nissan|Leaf 2018-23|ProPILOT Assist|[Upstream](#upstream)|
|
||||
|Nissan|Rogue 2018-20|ProPILOT Assist|[Upstream](#upstream)|
|
||||
|Nissan|X-Trail 2017|ProPILOT Assist|[Upstream](#upstream)|
|
||||
|Peugeot|208 2019-25|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|
||||
|Porsche|Macan 2017-24|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|
||||
|Ram|1500 2019-24|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|
||||
|Ram|2500 2020-24|Adaptive Cruise Control (ACC)|[Dashcam mode](#dashcam)|
|
||||
|Ram|3500 2019-22|Adaptive Cruise Control (ACC)|[Dashcam mode](#dashcam)|
|
||||
@@ -423,7 +428,6 @@ Supported Models' section of each make [on our wiki](https://wiki.comma.ai/).
|
||||
Some notable works-in-progress:
|
||||
* Honda
|
||||
* 2022-24 Acura RDX, commaai/opendbc#1967
|
||||
* 2021-25 Honda Odyssey, commaai/opendbc#2488
|
||||
* Camera ACC stability improvements, commaai/opendbc#2192
|
||||
* Alpha longitudinal stability improvements, commaai/opendbc#2347 and commaai/opendbc#2165
|
||||
|
||||
|
||||
@@ -11,6 +11,7 @@ from opendbc.car.toyota.toyotacan import toyota_checksum
|
||||
from opendbc.car.subaru.subarucan import subaru_checksum
|
||||
from opendbc.car.chrysler.chryslercan import chrysler_checksum, fca_giorgio_checksum
|
||||
from opendbc.car.hyundai.hyundaicanfd import hkg_can_fd_checksum
|
||||
from opendbc.car.volkswagen.mlbcan import volkswagen_mlb_checksum
|
||||
from opendbc.car.volkswagen.mqbcan import volkswagen_mqb_meb_checksum, xor_checksum
|
||||
from opendbc.car.tesla.teslacan import tesla_checksum
|
||||
from opendbc.car.body.bodycan import body_checksum
|
||||
@@ -31,6 +32,7 @@ class SignalType:
|
||||
FCA_GIORGIO_CHECKSUM = 10
|
||||
TESLA_CHECKSUM = 11
|
||||
PSA_CHECKSUM = 12
|
||||
VOLKSWAGEN_MLB_CHECKSUM = 13
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -186,6 +188,8 @@ def get_checksum_state(dbc_name: str) -> ChecksumState | None:
|
||||
return ChecksumState(16, -1, 0, -1, True, SignalType.HKG_CAN_FD_CHECKSUM, hkg_can_fd_checksum)
|
||||
elif dbc_name.startswith(("vw_mqb", "vw_mqbevo", "vw_meb")):
|
||||
return ChecksumState(8, 4, 0, 0, True, SignalType.VOLKSWAGEN_MQB_MEB_CHECKSUM, volkswagen_mqb_meb_checksum)
|
||||
elif dbc_name.startswith("vw_mlb"):
|
||||
return ChecksumState(8, 4, 0, 0, True, SignalType.VOLKSWAGEN_MLB_CHECKSUM, volkswagen_mlb_checksum)
|
||||
elif dbc_name.startswith("vw_pq"):
|
||||
return ChecksumState(8, 4, 0, -1, True, SignalType.XOR_CHECKSUM, xor_checksum)
|
||||
elif dbc_name.startswith("subaru_global_"):
|
||||
|
||||
@@ -549,14 +549,15 @@ struct CarParams {
|
||||
}
|
||||
|
||||
struct LateralTorqueTuning {
|
||||
kp @1 :Float32;
|
||||
ki @2 :Float32;
|
||||
friction @3 :Float32;
|
||||
kf @4 :Float32;
|
||||
steeringAngleDeadzoneDeg @5 :Float32;
|
||||
latAccelFactor @6 :Float32;
|
||||
latAccelOffset @7 :Float32;
|
||||
useSteeringAngleDEPRECATED @0 :Bool;
|
||||
kpDEPRECATED @1 :Float32;
|
||||
kiDEPRECATED @2 :Float32;
|
||||
kfDEPRECATED @4 :Float32;
|
||||
kdDEPRECATED @8 : Float32;
|
||||
}
|
||||
|
||||
struct LongitudinalPIDTuning {
|
||||
@@ -564,7 +565,7 @@ struct CarParams {
|
||||
kpV @1 :List(Float32);
|
||||
kiBP @2 :List(Float32);
|
||||
kiV @3 :List(Float32);
|
||||
kf @6 :Float32;
|
||||
kfDEPRECATED @6 :Float32;
|
||||
deadzoneBPDEPRECATED @4 :List(Float32);
|
||||
deadzoneVDEPRECATED @5 :List(Float32);
|
||||
}
|
||||
|
||||
@@ -354,6 +354,7 @@ FW_VERSIONS = {
|
||||
b'68402707AB',
|
||||
b'68402708AB',
|
||||
b'68402714AB',
|
||||
b'68402736AB',
|
||||
b'68402971AD',
|
||||
b'68454144AD',
|
||||
b'68454145AB',
|
||||
@@ -382,6 +383,7 @@ FW_VERSIONS = {
|
||||
b'68417279AA',
|
||||
b'68417280AA',
|
||||
b'68417281AA',
|
||||
b'68417283AA',
|
||||
b'68453431AA',
|
||||
b'68453433AA',
|
||||
b'68453435AA',
|
||||
@@ -394,6 +396,7 @@ FW_VERSIONS = {
|
||||
b'05035674AB ',
|
||||
b'68412635AE ',
|
||||
b'68412635AG ',
|
||||
b'68412635AH ',
|
||||
b'68412660AD ',
|
||||
b'68412660AF ',
|
||||
b'68422860AB',
|
||||
@@ -407,6 +410,7 @@ FW_VERSIONS = {
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'05035707AA',
|
||||
b'68419672AC',
|
||||
b'68419675AC',
|
||||
b'68419678AB',
|
||||
b'68423905AB',
|
||||
b'68449258AC',
|
||||
|
||||
@@ -2,17 +2,43 @@ class FirstOrderFilter:
|
||||
# first order filter
|
||||
def __init__(self, x0, rc, dt, initialized=True):
|
||||
self.x = x0
|
||||
self.dt = dt
|
||||
self._dt = dt
|
||||
self.update_alpha(rc)
|
||||
self.initialized = initialized
|
||||
|
||||
def update_dt(self, dt):
|
||||
self._dt = dt
|
||||
self.update_alpha(self._rc)
|
||||
|
||||
def update_alpha(self, rc):
|
||||
self.alpha = self.dt / (rc + self.dt)
|
||||
self._rc = rc
|
||||
self._alpha = self._dt / (self._rc + self._dt)
|
||||
|
||||
def update(self, x):
|
||||
if self.initialized:
|
||||
self.x = (1. - self.alpha) * self.x + self.alpha * x
|
||||
self.x = (1. - self._alpha) * self.x + self._alpha * x
|
||||
else:
|
||||
self.initialized = True
|
||||
self.x = x
|
||||
return self.x
|
||||
|
||||
|
||||
class HighPassFilter:
|
||||
# technically a band-pass filter
|
||||
def __init__(self, x0, rc1, rc2, dt, initialized=True):
|
||||
self.x = x0
|
||||
self._f1 = FirstOrderFilter(x0, rc1, dt, initialized)
|
||||
self._f2 = FirstOrderFilter(x0, rc2, dt, initialized)
|
||||
assert rc2 > rc1, "rc2 must be greater than rc1"
|
||||
|
||||
def update_dt(self, dt):
|
||||
self._f1.update_dt(dt)
|
||||
self._f2.update_dt(dt)
|
||||
|
||||
def update_alpha(self, rc1, rc2):
|
||||
self._f1.update_alpha(rc1)
|
||||
self._f2.update_alpha(rc2)
|
||||
|
||||
def update(self, x):
|
||||
self.x = self._f1.update(x) - self._f2.update(x)
|
||||
return self.x
|
||||
|
||||
@@ -72,7 +72,6 @@ class EnumBase(Enum):
|
||||
|
||||
class Mount(EnumBase):
|
||||
mount = BasePart("mount")
|
||||
angled_mount_8_degrees = BasePart("angled mount (8 degrees)")
|
||||
|
||||
|
||||
class Cable(EnumBase):
|
||||
@@ -80,8 +79,7 @@ class Cable(EnumBase):
|
||||
usb_a_2_a_cable = BasePart("USB A-A cable")
|
||||
usbc_otg_cable = BasePart("USB C OTG cable")
|
||||
usbc_coupler = BasePart("USB-C coupler")
|
||||
obd_c_cable_1_5ft = BasePart("OBD-C cable (1.5 ft)")
|
||||
right_angle_obd_c_cable_1_5ft = BasePart("right angle OBD-C cable (1.5 ft)")
|
||||
obd_c_cable_2ft = BasePart("OBD-C cable (2 ft)")
|
||||
|
||||
|
||||
class Accessory(EnumBase):
|
||||
@@ -143,8 +141,8 @@ class CarHarness(EnumBase):
|
||||
ford_q3 = BaseCarHarness("Ford Q3 connector")
|
||||
ford_q4 = BaseCarHarness("Ford Q4 connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler])
|
||||
rivian = BaseCarHarness("Rivian A connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler])
|
||||
tesla_a = BaseCarHarness("Tesla A connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler])
|
||||
tesla_b = BaseCarHarness("Tesla B connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler])
|
||||
tesla_a = BaseCarHarness("Tesla A connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler])
|
||||
tesla_b = BaseCarHarness("Tesla B connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler])
|
||||
psa_a = BaseCarHarness("PSA A connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler])
|
||||
|
||||
# custom harness
|
||||
@@ -152,15 +150,7 @@ class CarHarness(EnumBase):
|
||||
|
||||
|
||||
class Device(EnumBase):
|
||||
threex = BasePart("comma 3X", parts=[Mount.mount, Cable.right_angle_obd_c_cable_1_5ft])
|
||||
# variant of comma 3X with angled mounts
|
||||
threex_angled_mount = BasePart("comma 3X", parts=[Mount.angled_mount_8_degrees, Cable.right_angle_obd_c_cable_1_5ft])
|
||||
red_panda = BasePart("red panda")
|
||||
|
||||
|
||||
class Kit(EnumBase):
|
||||
red_panda_kit = BasePart("CAN FD panda kit", parts=[Device.red_panda, Accessory.harness_box,
|
||||
Cable.usb_a_2_a_cable, Cable.usbc_otg_cable, Cable.obd_c_cable_1_5ft])
|
||||
four = BasePart("comma four", parts=[Mount.mount, Cable.obd_c_cable_2ft])
|
||||
|
||||
|
||||
class PartType(Enum):
|
||||
@@ -168,12 +158,11 @@ class PartType(Enum):
|
||||
cable = Cable
|
||||
connector = CarHarness
|
||||
device = Device
|
||||
kit = Kit
|
||||
mount = Mount
|
||||
tool = Tool
|
||||
|
||||
|
||||
DEFAULT_CAR_PARTS: list[EnumBase] = [Device.threex]
|
||||
DEFAULT_CAR_PARTS: list[EnumBase] = [Device.four]
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -273,6 +262,7 @@ class CarDocs:
|
||||
def init(self, CP: CarParams, all_footnotes=None):
|
||||
self.brand = CP.brand
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.longitudinal_control = CP.openpilotLongitudinalControl and not CP.alphaLongitudinalAvailable
|
||||
|
||||
if self.merged and CP.dashcamOnly:
|
||||
if self.support_type != SupportType.REVIEW:
|
||||
|
||||
@@ -40,14 +40,15 @@ class CAR(Platforms):
|
||||
CommunityCarDocs("Acura Integra 2023-25", "All"),
|
||||
CommunityCarDocs("Acura MDX 2015-16", "Advance Package"),
|
||||
CommunityCarDocs("Acura MDX 2017-20", "All"),
|
||||
CommunityCarDocs("Acura MDX 2022-24", "All"),
|
||||
CommunityCarDocs("Acura RDX 2022-25", "All"),
|
||||
CommunityCarDocs("Acura RLX 2017", "Advance Package or Technology Package"),
|
||||
CommunityCarDocs("Acura TLX 2015-17", "Advance Package"),
|
||||
CommunityCarDocs("Acura TLX 2018-20", "All"),
|
||||
CommunityCarDocs("Acura TLX 2022-23", "All"),
|
||||
GMSecurityCarDocs("Acura ZDX 2024", "All"),
|
||||
CommunityCarDocs("Honda Accord 2016-17", "Honda Sensing"),
|
||||
CommunityCarDocs("Honda Clarity 2018-21", "All"),
|
||||
CommunityCarDocs("Honda Passport 2026", "All"),
|
||||
GMSecurityCarDocs("Honda Prologue 2024-25", "All"),
|
||||
],
|
||||
)
|
||||
|
||||
@@ -6,8 +6,7 @@ from enum import Enum, IntFlag
|
||||
from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
|
||||
from opendbc.car.lateral import AngleSteeringLimits
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, \
|
||||
Device
|
||||
from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
|
||||
from opendbc.car.fw_query_definitions import FwQueryConfig, LiveFwVersions, OfflineFwVersions, Request, StdQueries, p16
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
@@ -75,10 +74,7 @@ class FordCarDocs(CarDocs):
|
||||
|
||||
def init_make(self, CP: CarParams):
|
||||
harness = CarHarness.ford_q4 if CP.flags & FordFlags.CANFD else CarHarness.ford_q3
|
||||
if CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_MAVERICK_MK1, CAR.FORD_F_150_MK14, CAR.FORD_F_150_LIGHTNING_MK1):
|
||||
self.car_parts = CarParts([Device.threex_angled_mount, harness])
|
||||
else:
|
||||
self.car_parts = CarParts([Device.threex, harness])
|
||||
self.car_parts = CarParts.common([harness])
|
||||
|
||||
if harness == CarHarness.ford_q4:
|
||||
self.setup_video = "https://www.youtube.com/watch?v=uUGkH6C_EQU"
|
||||
|
||||
@@ -4,7 +4,7 @@ from opendbc.can import CANPacker
|
||||
from opendbc.car import Bus, DT_CTRL, rate_limit, make_tester_present_msg, structs
|
||||
from opendbc.car.honda import hondacan
|
||||
from opendbc.car.honda.values import CAR, CruiseButtons, HONDA_BOSCH, HONDA_BOSCH_CANFD, HONDA_BOSCH_RADARLESS, \
|
||||
HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
|
||||
HONDA_BOSCH_TJA_CONTROL, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
|
||||
from opendbc.sunnypilot.car.honda.mads import MadsCarController
|
||||
@@ -101,6 +101,7 @@ class CarController(CarControllerBase, MadsCarController, GasInterceptorCarContr
|
||||
self.packer = CANPacker(dbc_names[Bus.pt])
|
||||
self.params = CarControllerParams(CP)
|
||||
self.CAN = hondacan.CanBus(CP)
|
||||
self.tja_control = CP.carFingerprint in HONDA_BOSCH_TJA_CONTROL
|
||||
|
||||
self.braking = False
|
||||
self.brake_steady = 0.
|
||||
@@ -159,7 +160,7 @@ class CarController(CarControllerBase, MadsCarController, GasInterceptorCarContr
|
||||
can_sends.append(make_tester_present_msg(0x18DAB0F1, 1, suppress_response=True))
|
||||
|
||||
# Send steering command.
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, self.CAN, apply_torque, CC.latActive))
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, self.CAN, apply_torque, CC.latActive, self.tja_control))
|
||||
|
||||
# wind brake from air resistance decel at high speed
|
||||
wind_brake = np.interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15])
|
||||
|
||||
@@ -29,7 +29,7 @@ FW_VERSIONS = {
|
||||
],
|
||||
(Ecu.fwdCamera, 0x18dab5f1, None): [
|
||||
b'38897-TTA-J010\x00\x00',
|
||||
]
|
||||
],
|
||||
},
|
||||
CAR.HONDA_ACCORD: {
|
||||
(Ecu.shiftByWire, 0x18da0bf1, None): [
|
||||
@@ -334,6 +334,7 @@ FW_VERSIONS = {
|
||||
(Ecu.vsa, 0x18da28f1, None): [
|
||||
b'57114-T1W-A230\x00\x00',
|
||||
b'57114-T1W-A240\x00\x00',
|
||||
b'57114-TFF-A930\x00\x00',
|
||||
b'57114-TFF-A940\x00\x00',
|
||||
],
|
||||
(Ecu.srs, 0x18da53f1, None): [
|
||||
@@ -723,20 +724,16 @@ FW_VERSIONS = {
|
||||
(Ecu.vsa, 0x18da28f1, None): [
|
||||
b'57114-TJB-A030\x00\x00',
|
||||
b'57114-TJB-A040\x00\x00',
|
||||
b'57114-TJB-A120\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x18dab0f1, None): [
|
||||
b'36802-TJB-A040\x00\x00',
|
||||
b'36802-TJB-A050\x00\x00',
|
||||
b'36802-TJB-A540\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x18dab5f1, None): [
|
||||
b'36161-TJB-A040\x00\x00',
|
||||
b'36161-TJB-A530\x00\x00',
|
||||
],
|
||||
(Ecu.shiftByWire, 0x18da0bf1, None): [
|
||||
b'54008-TJB-A520\x00\x00',
|
||||
b'54008-TJB-A530\x00\x00',
|
||||
],
|
||||
(Ecu.transmission, 0x18da1ef1, None): [
|
||||
b'28102-5YK-A610\x00\x00',
|
||||
@@ -744,32 +741,27 @@ FW_VERSIONS = {
|
||||
b'28102-5YK-A630\x00\x00',
|
||||
b'28102-5YK-A700\x00\x00',
|
||||
b'28102-5YK-A711\x00\x00',
|
||||
b'28102-5YK-A800\x00\x00',
|
||||
b'28102-5YL-A620\x00\x00',
|
||||
b'28102-5YL-A700\x00\x00',
|
||||
b'28102-5YL-A711\x00\x00',
|
||||
],
|
||||
(Ecu.srs, 0x18da53f1, None): [
|
||||
b'77959-TJB-A040\x00\x00',
|
||||
b'77959-TJB-A120\x00\x00',
|
||||
b'77959-TJB-A210\x00\x00',
|
||||
],
|
||||
(Ecu.electricBrakeBooster, 0x18da2bf1, None): [
|
||||
b'46114-TJB-A040\x00\x00',
|
||||
b'46114-TJB-A050\x00\x00',
|
||||
b'46114-TJB-A060\x00\x00',
|
||||
b'46114-TJB-A120\x00\x00',
|
||||
],
|
||||
(Ecu.gateway, 0x18daeff1, None): [
|
||||
b'38897-TJB-A040\x00\x00',
|
||||
b'38897-TJB-A110\x00\x00',
|
||||
b'38897-TJB-A120\x00\x00',
|
||||
b'38897-TJB-A220\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x18da30f1, None): [
|
||||
b'39990-TJB-A030\x00\x00',
|
||||
b'39990-TJB-A040\x00\x00',
|
||||
b'39990-TJB-A070\x00\x00',
|
||||
b'39990-TJB-A130\x00\x00',
|
||||
],
|
||||
},
|
||||
@@ -893,11 +885,13 @@ FW_VERSIONS = {
|
||||
b'38897-TX6-A010\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x18dab0f1, None): [
|
||||
b'36161-TV9-A040\x00\x00',
|
||||
b'36161-TV9-A140\x00\x00',
|
||||
b'36161-TV9-C140\x00\x00',
|
||||
b'36161-TX6-A030\x00\x00',
|
||||
],
|
||||
(Ecu.srs, 0x18da53f1, None): [
|
||||
b'77959-TX6-A210\x00\x00',
|
||||
b'77959-TX6-A230\x00\x00',
|
||||
b'77959-TX6-C210\x00\x00',
|
||||
],
|
||||
@@ -979,6 +973,7 @@ FW_VERSIONS = {
|
||||
b'8S102-T47-AA20\x00\x00',
|
||||
b'8S102-T50-EA10\x00\x00',
|
||||
b'8S102-T56-A060\x00\x00',
|
||||
b'8S102-T56-A070\x00\x00',
|
||||
b'8S102-T60-AA10\x00\x00',
|
||||
b'8S102-T64-A040\x00\x00',
|
||||
],
|
||||
@@ -1022,11 +1017,14 @@ FW_VERSIONS = {
|
||||
CAR.HONDA_CRV_6G: {
|
||||
(Ecu.fwdRadar, 0x18dab0f1, None): [
|
||||
b'8S302-3A0-A060\x00\x00',
|
||||
b'8S302-3A0-A220\x00\x00',
|
||||
b'8S302-3C0-Q050\x00\x00',
|
||||
b'8S302-3D4-A050\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x18dab5f1, None): [
|
||||
b'8S102-3A0-A090\x00\x00',
|
||||
b'8S102-3A0-A110\x00\x00',
|
||||
b'8S102-3A0-A230\x00\x00',
|
||||
b'8S102-3C0-Q060\x00\x00',
|
||||
b'8S102-3D4-A060\x00\x00',
|
||||
b'8S102-3D4-A070\x00\x00',
|
||||
@@ -1054,6 +1052,22 @@ FW_VERSIONS = {
|
||||
b'28101-63B-M420\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.HONDA_PASSPORT_4G: {
|
||||
(Ecu.fwdRadar, 0x18dab0f1, None): [
|
||||
b'8S302-3BM-A020\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x18dab5f1, None): [
|
||||
b'8S102-3BM-A020\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.ACURA_TLX_2G: {
|
||||
(Ecu.fwdRadar, 0x18dab0f1, None): [
|
||||
b'36802-TGV-A060\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x18dab5f1, None): [
|
||||
b'36161-TGV-A030\x00\x00',
|
||||
],
|
||||
},
|
||||
}
|
||||
|
||||
FW_VERSIONS = merge_fw_versions(FW_VERSIONS, FW_VERSIONS_EXT)
|
||||
|
||||
@@ -121,11 +121,15 @@ def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_count
|
||||
return commands
|
||||
|
||||
|
||||
def create_steering_control(packer, CAN, apply_torque, lkas_active):
|
||||
def create_steering_control(packer, CAN, apply_torque, lkas_active, tja_control):
|
||||
values = {
|
||||
"STEER_TORQUE": apply_torque if lkas_active else 0,
|
||||
"STEER_TORQUE_REQUEST": lkas_active,
|
||||
}
|
||||
|
||||
if tja_control:
|
||||
values["STEER_DOWN_TO_ZERO"] = lkas_active
|
||||
|
||||
return packer.make_can_msg("STEERING_CONTROL", CAN.lkas, values)
|
||||
|
||||
|
||||
|
||||
@@ -52,7 +52,7 @@ class CarInterface(CarInterfaceBase):
|
||||
# If Bosch radarless, this blocks ACC messages from the camera
|
||||
# TODO: get radar disable working on Bosch CANFD
|
||||
ret.alphaLongitudinalAvailable = candidate not in HONDA_BOSCH_CANFD
|
||||
ret.openpilotLongitudinalControl = alpha_long
|
||||
ret.openpilotLongitudinalControl = alpha_long and (candidate not in HONDA_BOSCH_CANFD)
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
else:
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.hondaNidec)]
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
import re
|
||||
|
||||
from opendbc.car.honda.fingerprints import FW_VERSIONS
|
||||
from opendbc.car.honda.values import HONDA_BOSCH, HONDA_BOSCH_TJA_CONTROL
|
||||
|
||||
HONDA_FW_VERSION_RE = br"[A-Z0-9]{5}(-|,)[A-Z0-9]{3}(-|,)[A-Z0-9]{4}(\x00){2}$"
|
||||
|
||||
@@ -12,3 +13,6 @@ class TestHondaFingerprint:
|
||||
for fws in fw_by_ecu.values():
|
||||
for fw in fws:
|
||||
assert re.match(HONDA_FW_VERSION_RE, fw) is not None, fw
|
||||
|
||||
def test_tja_bosch_only(self):
|
||||
assert set(HONDA_BOSCH_TJA_CONTROL).issubset(set(HONDA_BOSCH)), "Nidec car found in TJA control list"
|
||||
|
||||
@@ -3,7 +3,7 @@ from enum import Enum, IntFlag
|
||||
|
||||
from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, structs, uds
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, Device
|
||||
from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
|
||||
from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
|
||||
|
||||
Ecu = structs.CarParams.Ecu
|
||||
@@ -78,6 +78,7 @@ class HondaFlags(IntFlag):
|
||||
BOSCH_ALT_RADAR = 512
|
||||
ALLOW_MANUAL_TRANS = 1024
|
||||
HYBRID = 2048
|
||||
BOSCH_TJA_CONTROL = 4096
|
||||
|
||||
|
||||
# Car button codes
|
||||
@@ -108,10 +109,7 @@ class HondaCarDocs(CarDocs):
|
||||
else:
|
||||
harness = CarHarness.nidec
|
||||
|
||||
if CP.carFingerprint in (CAR.HONDA_PILOT_4G,):
|
||||
self.car_parts = CarParts([Device.threex_angled_mount, harness])
|
||||
else:
|
||||
self.car_parts = CarParts.common([harness])
|
||||
self.car_parts = CarParts.common([harness])
|
||||
|
||||
if CP.carFingerprint in (CAR.HONDA_CLARITY,):
|
||||
self.car_parts = CarParts.common([CarHarness.honda_clarity])
|
||||
@@ -173,6 +171,7 @@ class CAR(Platforms):
|
||||
# steerRatio: 11.82 is spec end-to-end
|
||||
CarSpecs(mass=3279 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=16.33, centerToFrontRatio=0.39, tireStiffnessFactor=0.8467),
|
||||
{Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated'},
|
||||
flags=HondaFlags.ALLOW_MANUAL_TRANS,
|
||||
)
|
||||
HONDA_ACCORD_11G = HondaBoschCANFDPlatformConfig(
|
||||
[
|
||||
@@ -199,7 +198,7 @@ class CAR(Platforms):
|
||||
HONDA_CIVIC_2022 = HondaBoschPlatformConfig(
|
||||
[
|
||||
HondaCarDocs("Honda Civic 2022-24", "All", video="https://youtu.be/ytiOT5lcp6Q"),
|
||||
HondaCarDocs("Honda Civic Hybrid 2025", "All"),
|
||||
HondaCarDocs("Honda Civic Hybrid 2025-26", "All"),
|
||||
HondaCarDocs("Honda Civic Hatchback 2022-24", "All", video="https://youtu.be/ytiOT5lcp6Q"),
|
||||
HondaCarDocs("Honda Civic Hatchback Hybrid (Europe only) 2023", "All"),
|
||||
# TODO: Confirm 2024
|
||||
@@ -218,7 +217,7 @@ class CAR(Platforms):
|
||||
)
|
||||
HONDA_CRV_6G = HondaBoschCANFDPlatformConfig(
|
||||
[
|
||||
HondaCarDocs("Honda CR-V 2023-25", "All"),
|
||||
HondaCarDocs("Honda CR-V 2023-26", "All"),
|
||||
HondaCarDocs("Honda CR-V Hybrid 2023-25", "All"),
|
||||
],
|
||||
CarSpecs(mass=1703, wheelbase=2.7, steerRatio=16.2, centerToFrontRatio=0.42),
|
||||
@@ -261,17 +260,27 @@ class CAR(Platforms):
|
||||
[HondaCarDocs("Honda Pilot 2023-25", "All")],
|
||||
CarSpecs(mass=4660 * CV.LB_TO_KG, wheelbase=2.89, centerToFrontRatio=0.442, steerRatio=17.5),
|
||||
)
|
||||
HONDA_PASSPORT_4G = HondaBoschCANFDPlatformConfig(
|
||||
[HondaCarDocs("Honda Passport 2026", "All")],
|
||||
CarSpecs(mass=4620 * CV.LB_TO_KG, wheelbase=2.89, centerToFrontRatio=0.442, steerRatio=18.5),
|
||||
)
|
||||
# mid-model refresh
|
||||
ACURA_MDX_4G_MMR = HondaBoschCANFDPlatformConfig(
|
||||
[HondaCarDocs("Acura MDX 2025", "All except Type S")],
|
||||
CarSpecs(mass=4544 * CV.LB_TO_KG, wheelbase=2.89, centerToFrontRatio=0.428, steerRatio=16.2),
|
||||
)
|
||||
HONDA_ODYSSEY_5G_MMR = HondaBoschPlatformConfig(
|
||||
[HondaCarDocs("Honda Odyssey 2021-25", "All", min_steer_speed=70. * CV.KPH_TO_MS)],
|
||||
[HondaCarDocs("Honda Odyssey 2021-26", "All", min_steer_speed=70. * CV.KPH_TO_MS)],
|
||||
CarSpecs(mass=4590 * CV.LB_TO_KG, wheelbase=3.00, steerRatio=19.4, centerToFrontRatio=0.41),
|
||||
{Bus.pt: 'acura_rdx_2020_can_generated'},
|
||||
flags=HondaFlags.BOSCH_ALT_BRAKE | HondaFlags.BOSCH_ALT_RADAR,
|
||||
)
|
||||
ACURA_TLX_2G = HondaBoschPlatformConfig(
|
||||
[HondaCarDocs("Acura TLX 2021", "All")],
|
||||
CarSpecs(mass=3982 * CV.LB_TO_KG, wheelbase=2.87, steerRatio=14.0, centerToFrontRatio=0.43),
|
||||
{Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated'},
|
||||
flags=HondaFlags.BOSCH_ALT_RADAR,
|
||||
)
|
||||
|
||||
# Nidec Cars
|
||||
ACURA_ILX = HondaNidecPlatformConfig(
|
||||
@@ -362,6 +371,7 @@ HONDA_BOSCH = CAR.with_flags(HondaFlags.BOSCH)
|
||||
HONDA_BOSCH_RADARLESS = CAR.with_flags(HondaFlags.BOSCH_RADARLESS)
|
||||
HONDA_BOSCH_CANFD = CAR.with_flags(HondaFlags.BOSCH_CANFD)
|
||||
HONDA_BOSCH_ALT_RADAR = CAR.with_flags(HondaFlags.BOSCH_ALT_RADAR)
|
||||
HONDA_BOSCH_TJA_CONTROL = CAR.with_flags(HondaFlags.BOSCH_TJA_CONTROL)
|
||||
|
||||
|
||||
DBC = CAR.create_dbc_map()
|
||||
@@ -373,6 +383,7 @@ STEER_THRESHOLD = {
|
||||
CAR.HONDA_CRV_EU: 400,
|
||||
CAR.HONDA_ACCORD_11G: 600,
|
||||
CAR.HONDA_PILOT_4G: 600,
|
||||
CAR.HONDA_PASSPORT_4G: 600,
|
||||
CAR.ACURA_MDX_4G_MMR: 600,
|
||||
CAR.HONDA_CRV_6G: 600,
|
||||
CAR.HONDA_CITY_7G: 600,
|
||||
|
||||
@@ -56,6 +56,7 @@ FW_VERSIONS = {
|
||||
(Ecu.eps, 0x7d4, None): [
|
||||
b'\xf1\x00AE MDPS C 1.00 1.03 56310/G2300 4AEHC103',
|
||||
b'\xf1\x00AE MDPS C 1.00 1.03 56310G2300\x00 4AEHC103',
|
||||
b'\xf1\x00AE MDPS C 1.00 1.04 56310G2550\x00 4AEHC104',
|
||||
b'\xf1\x00AE MDPS C 1.00 1.05 56310/G2500 4AEHC105',
|
||||
b'\xf1\x00AE MDPS C 1.00 1.05 56310/G2501 4AEHC105',
|
||||
b'\xf1\x00AE MDPS C 1.00 1.07 56310/G2301 4AEHC107',
|
||||
@@ -65,6 +66,7 @@ FW_VERSIONS = {
|
||||
(Ecu.fwdCamera, 0x7c4, None): [
|
||||
b'\xf1\x00AEH MFC AT EUR LHD 1.00 1.00 95740-G2400 180222',
|
||||
b'\xf1\x00AEH MFC AT EUR LHD 1.00 1.00 95740-G7200 160418',
|
||||
b'\xf1\x00AEH MFC AT EUR RHD 1.00 1.00 95740-G2200 161014',
|
||||
b'\xf1\x00AEH MFC AT EUR RHD 1.00 1.00 95740-G2400 180222',
|
||||
b'\xf1\x00AEH MFC AT USA LHD 1.00 1.00 95740-G2300 170703',
|
||||
b'\xf1\x00AEH MFC AT USA LHD 1.00 1.00 95740-G2400 180222',
|
||||
@@ -922,6 +924,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.06 99210-AA000 220111',
|
||||
b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.07 99210-AA000 220426',
|
||||
b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.08 99210-AA000 220728',
|
||||
b'\xf1\x00CN7 MFC AT USA LHD 1.00 1.09 99210-AA000 221108',
|
||||
],
|
||||
(Ecu.abs, 0x7d1, None): [
|
||||
b'\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800',
|
||||
@@ -1125,6 +1128,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x00JK1 MFC AT CAN LHD 1.00 1.04 99211-AR100 210204',
|
||||
b'\xf1\x00JK1 MFC AT USA LHD 1.00 1.01 99211-AR200 220125',
|
||||
b'\xf1\x00JK1 MFC AT USA LHD 1.00 1.01 99211-AR300 220125',
|
||||
b'\xf1\x00JK1 MFC AT USA LHD 1.00 1.02 99211-IY000 230627',
|
||||
b'\xf1\x00JK1 MFC AT USA LHD 1.00 1.04 99211-AR000 210204',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x7d0, None): [
|
||||
|
||||
@@ -1,11 +1,11 @@
|
||||
import re
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum, IntFlag
|
||||
from enum import IntFlag
|
||||
|
||||
from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
|
||||
from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.structs import CarParams
|
||||
from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, Device
|
||||
from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts
|
||||
from opendbc.car.fw_query_definitions import FwQueryConfig, Request, p16
|
||||
|
||||
from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP
|
||||
@@ -129,21 +129,10 @@ class HyundaiFlags(IntFlag):
|
||||
ALT_LIMITS_2 = 2 ** 26
|
||||
|
||||
|
||||
class Footnote(Enum):
|
||||
CANFD = CarFootnote(
|
||||
"Requires a <a href=\"https://comma.ai/shop/can-fd-panda-kit\" target=\"_blank\">CAN FD panda kit</a> if not using " +
|
||||
"comma 3X for this <a href=\"https://en.wikipedia.org/wiki/CAN_FD\" target=\"_blank\">CAN FD car</a>.",
|
||||
Column.MODEL)
|
||||
|
||||
|
||||
@dataclass
|
||||
class HyundaiCarDocs(CarDocs):
|
||||
package: str = "Smart Cruise Control (SCC)"
|
||||
|
||||
def init_make(self, CP: CarParams):
|
||||
if CP.flags & HyundaiFlags.CANFD:
|
||||
self.footnotes.insert(0, Footnote.CANFD)
|
||||
|
||||
|
||||
@dataclass
|
||||
class HyundaiPlatformConfig(PlatformConfig):
|
||||
@@ -331,7 +320,7 @@ class CAR(Platforms):
|
||||
HYUNDAI_PALISADE = HyundaiPlatformConfig(
|
||||
[
|
||||
HyundaiCarDocs("Hyundai Palisade 2020-22", "All", video="https://youtu.be/TAnDqjF4fDY?t=456", car_parts=CarParts.common([CarHarness.hyundai_h])),
|
||||
HyundaiCarDocs("Kia Telluride 2020-22", "All", car_parts=CarParts([Device.threex_angled_mount, CarHarness.hyundai_h])),
|
||||
HyundaiCarDocs("Kia Telluride 2020-22", "All", car_parts=CarParts.common([CarHarness.hyundai_h])),
|
||||
],
|
||||
CarSpecs(mass=1999, wheelbase=2.9, steerRatio=15.6 * 1.15, tireStiffnessFactor=0.63),
|
||||
flags=HyundaiFlags.MANDO_RADAR | HyundaiFlags.CHECKSUM_CRC8,
|
||||
|
||||
@@ -246,7 +246,6 @@ class CarInterfaceBase(ABC, CarInterfaceBaseSP):
|
||||
ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
|
||||
ret.vEgoStopping = 0.5
|
||||
ret.vEgoStarting = 0.5
|
||||
ret.longitudinalTuning.kf = 1.
|
||||
ret.longitudinalTuning.kpBP = [0.]
|
||||
ret.longitudinalTuning.kpV = [0.]
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
@@ -261,9 +260,6 @@ class CarInterfaceBase(ABC, CarInterfaceBaseSP):
|
||||
params = get_torque_params()[candidate]
|
||||
|
||||
tune.init('torque')
|
||||
tune.torque.kf = 1.0
|
||||
tune.torque.kp = 1.0
|
||||
tune.torque.ki = 0.3
|
||||
tune.torque.friction = params['FRICTION']
|
||||
tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR']
|
||||
tune.torque.latAccelOffset = 0.0
|
||||
|
||||
@@ -11,12 +11,15 @@ FW_VERSIONS = {
|
||||
],
|
||||
(Ecu.eps, 0x742, None): [
|
||||
b'6CA2B\xa9A\x02\x02G8A89P90D6A\x00\x00\x01\x80',
|
||||
b'6CA2C\xa9A\x02\x02G8A89P90D6A\x00\x00\x01\x80',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'237106GU3B',
|
||||
b'237109HE2B',
|
||||
],
|
||||
(Ecu.gateway, 0x18dad0f1, None): [
|
||||
b'284U29HE0A',
|
||||
b'284U29HF0A',
|
||||
],
|
||||
},
|
||||
CAR.NISSAN_LEAF: {
|
||||
@@ -56,6 +59,7 @@ FW_VERSIONS = {
|
||||
b'5SK0ADB\x04\x18\x00\x00\x00\x00\x00_(5\x07\x9aQ\x00\x00\x00\x80',
|
||||
],
|
||||
(Ecu.abs, 0x740, None): [
|
||||
b'476605SD2D',
|
||||
b'476605SD2E',
|
||||
b'476605SH1D',
|
||||
b'476605SH7D',
|
||||
@@ -66,13 +70,17 @@ FW_VERSIONS = {
|
||||
b'5SH2A\x99A\x05\x02N123F\x15\x81\x00\x00\x00\x00\x00\x00\x00\x80',
|
||||
b'5SH2A\xb7A\x05\x02N123F\x15\xa3\x00\x00\x00\x00\x00\x00\x00\x80',
|
||||
b'5SH2C\xb7A\x05\x02N123F\x15\xa3\x00\x00\x00\x00\x00\x00\x00\x80',
|
||||
b'5SH3A\x99A\x05\x02N123F\x15\x81\x00\x00\x00\x00\x00\x00\x00\x80',
|
||||
b'5SH3C\xb7A\x05\x02N123F\x15\xa3\x00\x00\x00\x00\x00\x00\x00\x80',
|
||||
b'5SK3A\x99A\x05\x02N123F\x15u\x00\x00\x00\x00\x00\x00\x00\x80',
|
||||
],
|
||||
(Ecu.gateway, 0x18dad0f1, None): [
|
||||
b'284U25SF0C',
|
||||
b'284U25SH3A',
|
||||
b'284U25SH3B',
|
||||
b'284U25SK2D',
|
||||
b'284U25SR0B',
|
||||
b'284U25SR0C',
|
||||
],
|
||||
},
|
||||
CAR.NISSAN_XTRAIL: {
|
||||
|
||||
@@ -72,7 +72,7 @@ class CAR(Platforms):
|
||||
NissanCarSpecs(mass=1610, wheelbase=2.705)
|
||||
)
|
||||
NISSAN_ALTIMA = NissanPlatformConfig(
|
||||
[NissanCarDocs("Nissan Altima 2019-20", car_parts=CarParts.common([CarHarness.nissan_b]))],
|
||||
[NissanCarDocs("Nissan Altima 2019-20, 2024", car_parts=CarParts.common([CarHarness.nissan_b]))],
|
||||
NissanCarSpecs(mass=1492, wheelbase=2.824)
|
||||
)
|
||||
|
||||
|
||||
@@ -2,7 +2,7 @@ from dataclasses import dataclass, field
|
||||
from enum import StrEnum, IntFlag
|
||||
|
||||
from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, structs, uds
|
||||
from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts, Device
|
||||
from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts
|
||||
from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
|
||||
from opendbc.car.vin import Vin
|
||||
|
||||
@@ -27,7 +27,7 @@ class ModelYear(StrEnum):
|
||||
@dataclass
|
||||
class RivianCarDocs(CarDocs):
|
||||
package: str = "All"
|
||||
car_parts: CarParts = field(default_factory=CarParts([Device.threex_angled_mount, CarHarness.rivian]))
|
||||
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.rivian]))
|
||||
setup_video: str = "https://youtu.be/uaISd1j7Z4U"
|
||||
|
||||
|
||||
|
||||
@@ -473,6 +473,7 @@ FW_VERSIONS = {
|
||||
b'\xa1 \x06\x02',
|
||||
b'\xa1 \x06\x03',
|
||||
b'\xa1 \x07\x00',
|
||||
b'\xa1 \x07\x01',
|
||||
b'\xa1 \x07\x02',
|
||||
b'\xa1 \x07\x03',
|
||||
b'\xa1 \x08\x00',
|
||||
@@ -504,6 +505,7 @@ FW_VERSIONS = {
|
||||
b'\xe2"`0\x07',
|
||||
b'\xe2"`p\x07',
|
||||
b'\xe2"`q\x07',
|
||||
b'\xe2"`t\x07',
|
||||
b'\xe2,\xa0p\x07',
|
||||
b'\xe3,\xa0@\x07',
|
||||
],
|
||||
|
||||
@@ -41,6 +41,7 @@ FW_VERSIONS = {
|
||||
CAR.TESLA_MODEL_X: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'TeM3_SP_XP002p2_0.0.0 (23),XPR003.6.0',
|
||||
b'TeM3_SP_XP002p2_0.0.0 (36),XPR003.10.0',
|
||||
],
|
||||
},
|
||||
}
|
||||
|
||||
@@ -26,7 +26,11 @@ non_tested_cars = [
|
||||
HYUNDAI.GENESIS_G90,
|
||||
VOLKSWAGEN.VOLKSWAGEN_CRAFTER_MK2, # need a route from an ACC-equipped Crafter
|
||||
SUBARU.SUBARU_FORESTER_HYBRID,
|
||||
HONDA.HONDA_CRV_6G,
|
||||
# Honda/Acura test routes below expired, replace when CI bucket sync is fixed
|
||||
HONDA.ACURA_TLX_2G,
|
||||
HONDA.HONDA_NBOX_2G,
|
||||
HONDA.ACURA_MDX_4G_MMR,
|
||||
HONDA.HONDA_CITY_7G,
|
||||
|
||||
# port extensions
|
||||
HYUNDAI.KIA_CEED_PHEV_2022_NON_SCC,
|
||||
@@ -112,7 +116,6 @@ routes = [
|
||||
CarTestRoute("320098ff6c5e4730/2023-04-13--17-47-46", HONDA.HONDA_HRV_3G),
|
||||
CarTestRoute("147613502316e718/00000001--dd141a3140", HONDA.HONDA_HRV_3G), # Brazilian model
|
||||
CarTestRoute("1e4baee1aa2687a0/00000001--74c4cc0b23", HONDA.HONDA_HRV_3G), # Thailand model use ALT_GEAR
|
||||
CarTestRoute("414af83891dbf72c/00000006--51fa6d99cd", HONDA.HONDA_NBOX_2G),
|
||||
CarTestRoute("917b074700869333/2021-05-24--20-40-20", HONDA.ACURA_ILX),
|
||||
CarTestRoute("08a3deb07573f157/2020-03-06--16-11-19", HONDA.HONDA_ACCORD), # 1.5T
|
||||
CarTestRoute("1da5847ac2488106/2021-05-24--19-31-50", HONDA.HONDA_ACCORD), # 2.0T
|
||||
@@ -135,8 +138,9 @@ routes = [
|
||||
CarTestRoute("b1c832ad56b6bc9d/00000010--debfcf5867", HONDA.HONDA_CIVIC_2022), # 2025 Civic Hatch Hybrid with new eCVT transmission
|
||||
CarTestRoute("f9c43864cf057d05/2024-01-15--23-01-20", HONDA.HONDA_PILOT_4G), # TODO: Replace with a newer route
|
||||
CarTestRoute("f39cf149898833ff/0000002b--54f3fae045", HONDA.HONDA_ACCORD_11G),
|
||||
CarTestRoute("ad9840558640c31d/0000001a--d6cd4871c2", HONDA.ACURA_MDX_4G_MMR), # 2025 MDX
|
||||
CarTestRoute("56b2cf1dacdcd033/00000017--d24ffdb376", HONDA.HONDA_CITY_7G), # Brazilian model
|
||||
# CarTestRoute("56b2cf1dacdcd033/00000017--d24ffdb376", HONDA.HONDA_CITY_7G), # Brazilian model
|
||||
CarTestRoute("2dc4489d7e1410ca/00000001--bbec3f5117", HONDA.HONDA_CRV_6G),
|
||||
CarTestRoute("a703d058f4e05aeb/00000008--f169423024", HONDA.HONDA_PASSPORT_4G),
|
||||
|
||||
CarTestRoute("87d7f06ade479c2e/2023-09-11--23-30-11", HYUNDAI.HYUNDAI_AZERA_6TH_GEN),
|
||||
CarTestRoute("66189dd8ec7b50e6/2023-09-20--07-02-12", HYUNDAI.HYUNDAI_AZERA_HEV_6TH_GEN),
|
||||
@@ -243,6 +247,7 @@ routes = [
|
||||
CarTestRoute("7e34a988419b5307/2019-12-18--19-13-30", TOYOTA.TOYOTA_RAV4_TSS2), # hybrid
|
||||
CarTestRoute("2475fb3eb2ffcc2e/2022-04-29--12-46-23", TOYOTA.TOYOTA_RAV4_TSS2_2022), # hybrid
|
||||
CarTestRoute("20ba9ade056a8c7b/2021-02-08--21-57-35", TOYOTA.TOYOTA_RAV4_PRIME), # SecOC
|
||||
CarTestRoute("41ba5b181f29435d/00000001--e3ae76382f", TOYOTA.TOYOTA_RAV4_PRIME), # SecOC longitudinal
|
||||
CarTestRoute("8bfb000e03b2a257/00000004--f9eee5f52e", TOYOTA.TOYOTA_SIENNA_4TH_GEN), # SecOC
|
||||
CarTestRoute("0b54d0594d924cd9/00000057--b6206a3205", TOYOTA.TOYOTA_YARIS), # SecOC
|
||||
CarTestRoute("7a31f030957b9c85/2023-04-01--14-12-51", TOYOTA.LEXUS_ES),
|
||||
@@ -308,6 +313,7 @@ routes = [
|
||||
CarTestRoute("66e5edc3a16459c5/2021-05-25--19-00-29", VOLKSWAGEN.SKODA_OCTAVIA_MK3),
|
||||
CarTestRoute("026b6d18fba6417f/2021-03-26--09-17-04", VOLKSWAGEN.SKODA_KAMIQ_MK1), # Scala
|
||||
CarTestRoute("b2e9858e29db492b/2021-03-26--16-58-42", VOLKSWAGEN.SKODA_SUPERB_MK3),
|
||||
CarTestRoute("e8dac44d3a3458a5/0000008d--c3e9496f1c", VOLKSWAGEN.PORSCHE_MACAN_MK1), # placeholder, replace after merge
|
||||
|
||||
CarTestRoute("3c8f0c502e119c1c/2020-06-30--12-58-02", SUBARU.SUBARU_ASCENT),
|
||||
CarTestRoute("c321c6b697c5a5ff/2020-06-23--11-04-33", SUBARU.SUBARU_FORESTER),
|
||||
|
||||
@@ -94,7 +94,7 @@ class TestCarInterfaces:
|
||||
assert len(tune.pid.kiV) > 0 and len(tune.pid.kiV) == len(tune.pid.kiBP)
|
||||
|
||||
elif tune.which() == 'torque':
|
||||
assert not math.isnan(tune.torque.kf) and tune.torque.kf > 0
|
||||
assert not math.isnan(tune.torque.latAccelFactor) and tune.torque.latAccelFactor > 0
|
||||
assert not math.isnan(tune.torque.friction) and tune.torque.friction > 0
|
||||
|
||||
# Run car interface
|
||||
|
||||
@@ -76,4 +76,4 @@ class TestCarDocs:
|
||||
assert len(car_parts) > 0, f"Need to specify car parts: {car.name}"
|
||||
assert car_part_type.count(PartType.connector) == 1, f"Need to specify one harness connector: {car.name}"
|
||||
assert car_part_type.count(PartType.mount) == 1, f"Need to specify one mount: {car.name}"
|
||||
assert Cable.right_angle_obd_c_cable_1_5ft in car_parts, f"Need to specify a right angle OBD-C cable (1.5ft): {car.name}"
|
||||
assert Cable.obd_c_cable_2ft in car_parts, f"Need to specify an OBD-C cable (2ft): {car.name}"
|
||||
|
||||
@@ -84,11 +84,14 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
|
||||
"HYUNDAI_NEXO_1ST_GEN" = [2.5, 2.5, 0.1]
|
||||
"HONDA_ACCORD_11G" = [1.35, 1.35, 0.17]
|
||||
"HONDA_PILOT_4G" = [1.25, 1.25, 0.21]
|
||||
"HONDA_PASSPORT_4G" = [1.2, 1.2, 0.16]
|
||||
"ACURA_MDX_4G_MMR" = [1.25, 1.25, 0.15]
|
||||
"HONDA_CRV_6G" = [1.3, 1.3, 0.2]
|
||||
"HONDA_CITY_7G" = [1.2, 1.2, 0.23]
|
||||
"HONDA_ODYSSEY_5G_MMR" = [0.9, 0.9, 0.2]
|
||||
"HONDA_NBOX_2G" = [1.2, 1.2, 0.2]
|
||||
"ACURA_TLX_2G" = [1.2, 1.2, 0.15]
|
||||
"PORSCHE_MACAN_MK1" = [2.0, 2.0, 0.2]
|
||||
|
||||
# Dashcam or fallback configured as ideal car
|
||||
"MOCK" = [10.0, 10, 0.0]
|
||||
|
||||
@@ -4,7 +4,7 @@ from opendbc.car import Bus, make_tester_present_msg, rate_limit, structs, ACCEL
|
||||
from opendbc.car.lateral import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance
|
||||
from opendbc.car.can_definitions import CanData
|
||||
from opendbc.car.carlog import carlog
|
||||
from opendbc.car.common.filter_simple import FirstOrderFilter
|
||||
from opendbc.car.common.filter_simple import FirstOrderFilter, HighPassFilter
|
||||
from opendbc.car.common.pid import PIDController
|
||||
from opendbc.car.secoc import add_mac, build_sync_mac
|
||||
from opendbc.car.interfaces import CarControllerBase
|
||||
@@ -70,8 +70,8 @@ class CarController(CarControllerBase, SecOCLongCarController, GasInterceptorCar
|
||||
# *** start long control state ***
|
||||
self.long_pid = get_long_tune(self.CP, self.params)
|
||||
self.aego = FirstOrderFilter(0.0, 0.25, DT_CTRL * 3)
|
||||
self.pitch = FirstOrderFilter(0, 0.25, DT_CTRL)
|
||||
self.pitch_slow = FirstOrderFilter(0, 1.5, DT_CTRL)
|
||||
self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL)
|
||||
self.pitch_hp = HighPassFilter(0.0, 0.25, 1.5, DT_CTRL)
|
||||
|
||||
self.accel = 0
|
||||
self.prev_accel = 0
|
||||
@@ -81,6 +81,7 @@ class CarController(CarControllerBase, SecOCLongCarController, GasInterceptorCar
|
||||
|
||||
self.secoc_lka_message_counter = 0
|
||||
self.secoc_lta_message_counter = 0
|
||||
self.secoc_acc_message_counter = 0
|
||||
self.secoc_prev_reset_counter = 0
|
||||
|
||||
def update(self, CC, CC_SP, CS, now_nanos):
|
||||
@@ -92,7 +93,7 @@ class CarController(CarControllerBase, SecOCLongCarController, GasInterceptorCar
|
||||
|
||||
if len(CC.orientationNED) == 3:
|
||||
self.pitch.update(CC.orientationNED[1])
|
||||
self.pitch_slow.update(CC.orientationNED[1])
|
||||
self.pitch_hp.update(CC.orientationNED[1])
|
||||
|
||||
# *** control msgs ***
|
||||
can_sends = []
|
||||
@@ -104,6 +105,7 @@ class CarController(CarControllerBase, SecOCLongCarController, GasInterceptorCar
|
||||
if CS.secoc_synchronization['RESET_CNT'] != self.secoc_prev_reset_counter:
|
||||
self.secoc_lka_message_counter = 0
|
||||
self.secoc_lta_message_counter = 0
|
||||
self.secoc_acc_message_counter = 0
|
||||
self.secoc_prev_reset_counter = CS.secoc_synchronization['RESET_CNT']
|
||||
|
||||
expected_mac = build_sync_mac(self.secoc_key, int(CS.secoc_synchronization['TRIP_CNT']), int(CS.secoc_synchronization['RESET_CNT']))
|
||||
@@ -235,8 +237,7 @@ class CarController(CarControllerBase, SecOCLongCarController, GasInterceptorCar
|
||||
if not stopping:
|
||||
# Toyota's PCM slowly responds to changes in pitch. On change, we amplify our
|
||||
# acceleration request to compensate for the undershoot and following overshoot
|
||||
high_pass_pitch = self.pitch.x - self.pitch_slow.x
|
||||
pitch_compensation = float(np.clip(math.sin(high_pass_pitch) * ACCELERATION_DUE_TO_GRAVITY,
|
||||
pitch_compensation = float(np.clip(math.sin(self.pitch_hp.x) * ACCELERATION_DUE_TO_GRAVITY,
|
||||
-MAX_PITCH_COMPENSATION, MAX_PITCH_COMPENSATION))
|
||||
pcm_accel_cmd += pitch_compensation
|
||||
|
||||
@@ -258,8 +259,19 @@ class CarController(CarControllerBase, SecOCLongCarController, GasInterceptorCar
|
||||
pcm_accel_cmd = pcm_accel_cmd if self.CP.carFingerprint in TSS2_CAR else actuators.accel
|
||||
pcm_accel_cmd = float(np.clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX))
|
||||
|
||||
can_sends.append(toyotacan.create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead,
|
||||
CS.acc_type, fcw_alert, self.distance_button, self.SECOC_LONG))
|
||||
main_accel_cmd = 0. if self.CP.flags & ToyotaFlags.SECOC.value else pcm_accel_cmd
|
||||
can_sends.append(toyotacan.create_accel_command(self.packer, main_accel_cmd, pcm_cancel_cmd, self.permit_braking, self.standstill_req, lead,
|
||||
CS.acc_type, fcw_alert, self.distance_button))
|
||||
if self.CP.flags & ToyotaFlags.SECOC.value:
|
||||
acc_cmd_2 = toyotacan.create_accel_command_2(self.packer, pcm_accel_cmd)
|
||||
acc_cmd_2 = add_mac(self.secoc_key,
|
||||
int(CS.secoc_synchronization['TRIP_CNT']),
|
||||
int(CS.secoc_synchronization['RESET_CNT']),
|
||||
self.secoc_acc_message_counter,
|
||||
acc_cmd_2)
|
||||
self.secoc_acc_message_counter += 1
|
||||
can_sends.append(acc_cmd_2)
|
||||
|
||||
self.accel = pcm_accel_cmd
|
||||
|
||||
else:
|
||||
|
||||
@@ -6,7 +6,8 @@ from opendbc.car.common.conversions import Conversions as CV
|
||||
from opendbc.car.common.filter_simple import FirstOrderFilter
|
||||
from opendbc.car.interfaces import CarStateBase
|
||||
from opendbc.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
|
||||
TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR
|
||||
TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR, \
|
||||
SECOC_CAR
|
||||
from opendbc.sunnypilot.car.toyota.carstate_ext import CarStateExt
|
||||
from opendbc.sunnypilot.car.toyota.values import ToyotaFlagsSP
|
||||
|
||||
@@ -195,7 +196,7 @@ class CarState(CarStateBase, CarStateExt):
|
||||
buttonEvents.extend(create_button_events(1, 0, {1: ButtonType.lkas}) +
|
||||
create_button_events(0, 1, {1: ButtonType.lkas}))
|
||||
|
||||
if self.CP.carFingerprint not in RADAR_ACC_CAR:
|
||||
if self.CP.carFingerprint not in (RADAR_ACC_CAR | SECOC_CAR):
|
||||
# distance button is wired to the ACC module (camera or radar)
|
||||
self.distance_button = cp_acc.vl["ACC_CONTROL"]["DISTANCE"]
|
||||
|
||||
|
||||
@@ -60,6 +60,13 @@ def create_accel_command(packer, accel, pcm_cancel, permit_braking, standstill_r
|
||||
return packer.make_can_msg("ACC_CONTROL", 0, values)
|
||||
|
||||
|
||||
def create_accel_command_2(packer, accel):
|
||||
values = {
|
||||
"ACCEL_CMD": accel,
|
||||
}
|
||||
return packer.make_can_msg("ACC_CONTROL_2", 0, values)
|
||||
|
||||
|
||||
def create_pcs_commands(packer, accel, active, mass):
|
||||
values1 = {
|
||||
"COUNTER": 0,
|
||||
|
||||
@@ -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 mqbcan, pqcan
|
||||
from opendbc.car.volkswagen import mlbcan, mqbcan, pqcan
|
||||
from opendbc.car.volkswagen.values import CanBus, CarControllerParams, VolkswagenFlags
|
||||
|
||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||
@@ -16,10 +16,16 @@ class CarController(CarControllerBase):
|
||||
super().__init__(dbc_names, CP, CP_SP)
|
||||
self.CCP = CarControllerParams(CP)
|
||||
self.CAN = CanBus(CP)
|
||||
self.CCS = pqcan if CP.flags & VolkswagenFlags.PQ else mqbcan
|
||||
self.packer_pt = CANPacker(dbc_names[Bus.pt])
|
||||
self.aeb_available = not CP.flags & VolkswagenFlags.PQ
|
||||
|
||||
if CP.flags & VolkswagenFlags.PQ:
|
||||
self.CCS = pqcan
|
||||
elif CP.flags & VolkswagenFlags.MLB:
|
||||
self.CCS = mlbcan
|
||||
else:
|
||||
self.CCS = mqbcan
|
||||
|
||||
self.apply_torque_last = 0
|
||||
self.gra_acc_counter_last = None
|
||||
self.eps_timer_soft_disable_alert = False
|
||||
|
||||
@@ -18,6 +18,7 @@ class CarState(CarStateBase):
|
||||
self.esp_hold_confirmation = False
|
||||
self.upscale_lead_car_signal = False
|
||||
self.eps_stock_values = False
|
||||
self.acc_type = 0
|
||||
|
||||
def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]):
|
||||
if not self.CP.pcmCruise:
|
||||
@@ -48,6 +49,8 @@ class CarState(CarStateBase):
|
||||
|
||||
if self.CP.flags & VolkswagenFlags.PQ:
|
||||
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)
|
||||
|
||||
ret = structs.CarState()
|
||||
ret_sp = structs.CarStateSP()
|
||||
@@ -74,11 +77,9 @@ class CarState(CarStateBase):
|
||||
pt_cp.vl["ESP_19"]["ESP_HR_Radgeschw_02"],
|
||||
)
|
||||
|
||||
hca_status = self.CCP.hca_status_values.get(pt_cp.vl["LH_EPS_03"]["EPS_HCA_Status"])
|
||||
if self.CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT:
|
||||
ret.carFaultedNonCritical = bool(cam_cp.vl["HCA_01"]["EA_Ruckfreigabe"]) or cam_cp.vl["HCA_01"]["EA_ACC_Sollstatus"] > 0 # EA
|
||||
|
||||
drive_mode = True
|
||||
ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects
|
||||
brake_pedal_pressed = bool(pt_cp.vl["Motor_14"]["MO_Fahrer_bremst"])
|
||||
brake_pressure_detected = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"])
|
||||
@@ -115,11 +116,7 @@ class CarState(CarStateBase):
|
||||
# Shared logic
|
||||
ret.vEgoCluster = pt_cp.vl["Kombi_01"]["KBI_angez_Geschw"] * CV.KPH_TO_MS
|
||||
|
||||
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 = abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE
|
||||
ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status, drive_mode)
|
||||
self.parse_mlb_mqb_steering_state(ret, pt_cp)
|
||||
|
||||
ret.gasPressed = pt_cp.vl["Motor_20"]["MO_Fahrpedalrohwert_01"] > 0
|
||||
ret.espActive = bool(pt_cp.vl["ESP_21"]["ESP_Eingriff"])
|
||||
@@ -161,14 +158,14 @@ class CarState(CarStateBase):
|
||||
ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status)
|
||||
|
||||
# Update gas, brakes, and gearshift.
|
||||
ret.gasPressed = pt_cp.vl["Motor_3"]["Fahrpedal_Rohsignal"] > 0
|
||||
ret.gasPressed = pt_cp.vl["Motor_3"]["MO3_Pedalwert"] > 0
|
||||
ret.brake = pt_cp.vl["Bremse_5"]["BR5_Bremsdruck"] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects
|
||||
ret.brakePressed = bool(pt_cp.vl["Motor_2"]["MO2_BLS"])
|
||||
ret.parkingBrake = bool(pt_cp.vl["Kombi_1"]["Bremsinfo"])
|
||||
|
||||
# Update gear and/or clutch position data.
|
||||
if self.CP.transmissionType == TransmissionType.automatic:
|
||||
ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Getriebe_1"]["Waehlhebelposition__Getriebe_1_"], None))
|
||||
ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Getriebe_1"]["GE1_Wahl_Pos"], None))
|
||||
elif self.CP.transmissionType == TransmissionType.manual:
|
||||
reverse_light = bool(pt_cp.vl["Gate_Komf_1"]["GK1_Rueckfahr"])
|
||||
if reverse_light:
|
||||
@@ -208,7 +205,7 @@ class CarState(CarStateBase):
|
||||
|
||||
# Update ACC radar status.
|
||||
self.acc_type = ext_cp.vl["ACC_System"]["ACS_Typ_ACC"]
|
||||
ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["GRA_Hauptschalter"])
|
||||
ret.cruiseState.available = bool(pt_cp.vl["Motor_5"]["MO5_GRA_Hauptsch"])
|
||||
ret.cruiseState.enabled = pt_cp.vl["Motor_2"]["MO2_Sta_GRA"] in (1, 2)
|
||||
if self.CP.pcmCruise:
|
||||
ret.accFaulted = ext_cp.vl["ACC_GRA_Anzeige"]["ACA_StaACC"] in (6, 7)
|
||||
@@ -235,6 +232,62 @@ class CarState(CarStateBase):
|
||||
self.frame += 1
|
||||
return ret, ret_sp
|
||||
|
||||
def update_mlb(self, pt_cp, cam_cp, ext_cp) -> structs.CarState:
|
||||
ret = structs.CarState()
|
||||
ret_sp = structs.CarStateSP()
|
||||
|
||||
self.parse_wheel_speeds(ret,
|
||||
pt_cp.vl["ESP_03"]["ESP_VL_Radgeschw"],
|
||||
pt_cp.vl["ESP_03"]["ESP_VR_Radgeschw"],
|
||||
pt_cp.vl["ESP_03"]["ESP_HL_Radgeschw"],
|
||||
pt_cp.vl["ESP_03"]["ESP_HR_Radgeschw"],
|
||||
)
|
||||
|
||||
ret.gasPressed = pt_cp.vl["Motor_03"]["MO_Fahrpedalrohwert_01"] > 0
|
||||
ret.gearShifter = self.parse_gear_shifter(self.CCP.shifter_values.get(pt_cp.vl["Getriebe_03"]["GE_Waehlhebel"], None))
|
||||
|
||||
# ACC okay but disabled (1), ACC ready (2), a radar visibility or other fault/disruption (6 or 7)
|
||||
# currently regulating speed (3), driver accel override (4), brake only (5)
|
||||
# TODO: get this from the drivetrain side instead, for openpilot long support later
|
||||
ret.cruiseState.available = ext_cp.vl["ACC_05"]["ACC_Status_ACC"] in (2, 3, 4, 5)
|
||||
ret.cruiseState.enabled = ext_cp.vl["ACC_05"]["ACC_Status_ACC"] in (3, 4, 5)
|
||||
ret.accFaulted = ext_cp.vl["ACC_05"]["ACC_Status_ACC"] in (6, 7)
|
||||
|
||||
self.parse_mlb_mqb_steering_state(ret, pt_cp)
|
||||
|
||||
ret.brake = pt_cp.vl["ESP_05"]["ESP_Bremsdruck"] / 250.0
|
||||
brake_pedal_pressed = bool(pt_cp.vl["Motor_03"]["MO_Fahrer_bremst"])
|
||||
brake_pressure_detected = bool(pt_cp.vl["ESP_05"]["ESP_Fahrer_bremst"])
|
||||
ret.brakePressed = brake_pedal_pressed or brake_pressure_detected
|
||||
ret.parkingBrake = bool(pt_cp.vl["Kombi_01"]["KBI_Handbremse"])
|
||||
ret.espDisabled = pt_cp.vl["ESP_01"]["ESP_Tastung_passiv"] != 0
|
||||
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(300, pt_cp.vl["Gateway_11"]["BH_Blinker_li"],
|
||||
pt_cp.vl["Gateway_11"]["BH_Blinker_re"])
|
||||
|
||||
ret.seatbeltUnlatched = pt_cp.vl["Gateway_06"]["AB_Gurtschloss_FA"] != 3
|
||||
ret.doorOpen = any([pt_cp.vl["Gateway_05"]["FT_Tuer_geoeffnet"],
|
||||
pt_cp.vl["Gateway_05"]["BT_Tuer_geoeffnet"],
|
||||
pt_cp.vl["Gateway_05"]["HL_Tuer_geoeffnet"],
|
||||
pt_cp.vl["Gateway_05"]["HR_Tuer_geoeffnet"]])
|
||||
|
||||
# Consume blind-spot monitoring info/warning LED states, if available.
|
||||
# Infostufe: BSM LED on, Warnung: BSM LED flashing
|
||||
if self.CP.enableBsm:
|
||||
ret.leftBlindspot = bool(ext_cp.vl["SWA_01"]["SWA_Infostufe_SWA_li"]) or bool(ext_cp.vl["SWA_01"]["SWA_Warnung_SWA_li"])
|
||||
ret.rightBlindspot = bool(ext_cp.vl["SWA_01"]["SWA_Infostufe_SWA_re"]) or bool(ext_cp.vl["SWA_01"]["SWA_Warnung_SWA_re"])
|
||||
|
||||
self.ldw_stock_values = cam_cp.vl["LDW_02"] if self.CP.networkLocation == NetworkLocation.fwdCamera else {}
|
||||
self.gra_stock_values = pt_cp.vl["LS_01"]
|
||||
|
||||
ret.buttonEvents = self.create_button_events(pt_cp, self.CCP.BUTTONS)
|
||||
|
||||
ret.cruiseState.standstill = self.CP.pcmCruise and self.esp_hold_confirmation
|
||||
ret.standstill = ret.vEgoRaw == 0
|
||||
|
||||
self.frame += 1
|
||||
return ret, ret_sp
|
||||
|
||||
def update_low_speed_alert(self, v_ego: float) -> bool:
|
||||
# Low speed steer alert hysteresis logic
|
||||
if (self.CP.minSteerSpeed - 1e-3) > CarControllerParams.DEFAULT_MIN_STEER_SPEED and v_ego < (self.CP.minSteerSpeed + 1.):
|
||||
@@ -243,6 +296,16 @@ class CarState(CarStateBase):
|
||||
self.low_speed_alert = False
|
||||
return self.low_speed_alert
|
||||
|
||||
def parse_mlb_mqb_steering_state(self, ret, pt_cp, drive_mode=True):
|
||||
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 = abs(ret.steeringTorque) > self.CCP.STEER_DRIVER_ALLOWANCE
|
||||
|
||||
hca_status = self.CCP.hca_status_values.get(pt_cp.vl["LH_EPS_03"]["EPS_HCA_Status"])
|
||||
ret.steerFaultTemporary, ret.steerFaultPermanent = self.update_hca_state(hca_status, drive_mode)
|
||||
return
|
||||
|
||||
def update_hca_state(self, hca_status, drive_mode=True):
|
||||
# Treat FAULT as temporary for worst likely EPS recovery time, for cars without factory Lane Assist
|
||||
# DISABLED means the EPS hasn't been configured to support Lane Assist
|
||||
@@ -256,18 +319,20 @@ class CarState(CarStateBase):
|
||||
if CP.flags & VolkswagenFlags.PQ:
|
||||
return CarState.get_can_parsers_pq(CP)
|
||||
|
||||
# another case of the 1-50Hz
|
||||
cam_messages = []
|
||||
# manually configure some optional and variable-rate/edge-triggered messages
|
||||
pt_messages, cam_messages = [], []
|
||||
|
||||
if not CP.flags & VolkswagenFlags.MLB:
|
||||
pt_messages += [
|
||||
("Blinkmodi_02", 1) # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active)
|
||||
]
|
||||
if CP.flags & VolkswagenFlags.STOCK_HCA_PRESENT:
|
||||
cam_messages += [
|
||||
("HCA_01", 1), # From R242 Driver assistance camera, 50Hz if steering/1Hz if not
|
||||
]
|
||||
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [
|
||||
# the 50->1Hz is currently too much for the CANParser to figure out
|
||||
("Blinkmodi_02", 1), # From J519 BCM (sent at 1Hz when no lights active, 50Hz when active)
|
||||
], CanBus(CP).pt),
|
||||
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),
|
||||
}
|
||||
|
||||
|
||||
@@ -837,6 +837,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x878V0906259H \xf1\x890002',
|
||||
b'\xf1\x878V0906259J \xf1\x890002',
|
||||
b'\xf1\x878V0906259K \xf1\x890001',
|
||||
b'\xf1\x878V0906259K \xf1\x890002',
|
||||
b'\xf1\x878V0906264B \xf1\x890003',
|
||||
b'\xf1\x878V0907115B \xf1\x890007',
|
||||
b'\xf1\x878V0907404A \xf1\x890005',
|
||||
@@ -852,6 +853,7 @@ FW_VERSIONS = {
|
||||
b'\xf1\x870D9300012L \xf1\x894521',
|
||||
b'\xf1\x870D9300013B \xf1\x894902',
|
||||
b'\xf1\x870D9300013B \xf1\x894931',
|
||||
b'\xf1\x870D9300014Q \xf1\x895006',
|
||||
b'\xf1\x870D9300041N \xf1\x894512',
|
||||
b'\xf1\x870D9300043T \xf1\x899699',
|
||||
b'\xf1\x870DD300046 \xf1\x891604',
|
||||
@@ -966,6 +968,35 @@ FW_VERSIONS = {
|
||||
b'\xf1\x872Q0907572T \xf1\x890383',
|
||||
],
|
||||
},
|
||||
CAR.PORSCHE_MACAN_MK1: {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xf1\x8795B906259BJ\xf1\x890001',
|
||||
b'\xf1\x8795B90652013\xf1\x893485',
|
||||
b'\xf1\x8795B90654002\xf1\x893495',
|
||||
b'\xf1\x8795B907551D \xf1\x890006',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'\xf1\x8795B927156CE\xf1\x890022',
|
||||
b'\xf1\x8795B927156JH\xf1\x890001',
|
||||
b'\xf1\x8795B927156KK\xf1\x890001',
|
||||
b'\xf1\x8795B927156R \xf1\x890021',
|
||||
],
|
||||
(Ecu.srs, 0x715, None): [
|
||||
b'\xf1\x8795B959655F \xf1\x890130\xf1\x82\x05065Q033513',
|
||||
b'\xf1\x8795B959655F \xf1\x890130\xf1\x82\x050682033514',
|
||||
b'\xf1\x8795B959655G \xf1\x890150\xf1\x82\x0506B1033514',
|
||||
b'\xf1\x8795B959655G \xf1\x890150\xf1\x82\x0506CJ02D417',
|
||||
],
|
||||
(Ecu.eps, 0x712, None): [
|
||||
b'\xf1\x8795B909144K \xf1\x891902\xf1\x82\x02\x04#\x04#',
|
||||
b'\xf1\x8795B909144K \xf1\x891902\xf1\x82\x02\x04/\x04/',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x757, None): [
|
||||
b'\xf1\x8795B907567B\x00\xf1\x890800\xf1\x82108',
|
||||
b'\xf1\x8795B907567G\x00\xf1\x890410\xf1\x82104',
|
||||
b'\xf1\x8795B907567J \xf1\x890440\xf1\x82104',
|
||||
],
|
||||
},
|
||||
CAR.SEAT_ATECA_MK1: {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'\xf1\x8704E906027KA\xf1\x893749',
|
||||
|
||||
@@ -37,6 +37,13 @@ class CarInterface(CarInterfaceBase):
|
||||
# Panda ALLOW_DEBUG firmware required.
|
||||
ret.dashcamOnly = True
|
||||
|
||||
elif ret.flags & VolkswagenFlags.MLB:
|
||||
# Set global MLB parameters
|
||||
safety_configs = [get_safety_config(structs.CarParams.SafetyModel.volkswagenMlb)]
|
||||
ret.enableBsm = 0x30F in fingerprint[0] # SWA_01
|
||||
ret.networkLocation = NetworkLocation.gateway
|
||||
ret.dashcamOnly = True # Pending HCA timeout fix, safety validation, harness termination, install procedure
|
||||
|
||||
else:
|
||||
# Set global MQB parameters
|
||||
safety_configs = [get_safety_config(structs.CarParams.SafetyModel.volkswagen)]
|
||||
@@ -62,7 +69,7 @@ class CarInterface(CarInterfaceBase):
|
||||
# Global lateral tuning defaults, can be overridden per-vehicle
|
||||
|
||||
ret.steerLimitTimer = 0.4
|
||||
if ret.flags & VolkswagenFlags.PQ:
|
||||
if ret.flags & VolkswagenFlags.PQ or ret.flags & VolkswagenFlags.MLB:
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
else:
|
||||
@@ -83,6 +90,11 @@ class CarInterface(CarInterfaceBase):
|
||||
if ret.transmissionType == TransmissionType.manual:
|
||||
ret.minEnableSpeed = 4.5
|
||||
|
||||
# Per-vehicle overrides
|
||||
|
||||
if candidate == CAR.PORSCHE_MACAN_MK1:
|
||||
ret.steerActuatorDelay = 0.07
|
||||
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
ret.stopAccel = -0.55
|
||||
ret.vEgoStarting = 0.1
|
||||
|
||||
71
opendbc/car/volkswagen/mlbcan.py
Normal file
71
opendbc/car/volkswagen/mlbcan.py
Normal file
@@ -0,0 +1,71 @@
|
||||
from opendbc.car.volkswagen.mqbcan import (volkswagen_mqb_meb_checksum, xor_checksum,
|
||||
create_lka_hud_control as mqb_create_lka_hud_control)
|
||||
|
||||
# TODO: Parameterize the hca control type (5 vs 7) and consolidate with MQB (and PQ?)
|
||||
def create_steering_control(packer, bus, apply_steer, lkas_enabled):
|
||||
values = {
|
||||
"HCA_01_Status_HCA": 7 if lkas_enabled else 3,
|
||||
"HCA_01_LM_Offset": abs(apply_steer),
|
||||
"HCA_01_LM_OffSign": 1 if apply_steer < 0 else 0,
|
||||
"HCA_01_Vib_Freq": 18,
|
||||
"HCA_01_Sendestatus": 1 if lkas_enabled else 0,
|
||||
"EA_ACC_Wunschgeschwindigkeit": 327.36,
|
||||
}
|
||||
return packer.make_can_msg("HCA_01", bus, values)
|
||||
|
||||
|
||||
def create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control):
|
||||
return mqb_create_lka_hud_control(packer, bus, ldw_stock_values, enabled, steering_pressed, hud_alert, hud_control)
|
||||
|
||||
|
||||
def create_acc_buttons_control(packer, bus, gra_stock_values, cancel=False, resume=False):
|
||||
values = {s: gra_stock_values[s] for s in [
|
||||
"LS_Hauptschalter",
|
||||
"LS_Typ_Hauptschalter",
|
||||
"LS_Codierung",
|
||||
"LS_Tip_Stufe_2",
|
||||
]}
|
||||
|
||||
values.update({
|
||||
"COUNTER": (gra_stock_values["COUNTER"] + 1) % 16,
|
||||
"LS_Abbrechen": cancel,
|
||||
"LS_Tip_Wiederaufnahme": resume,
|
||||
})
|
||||
|
||||
return packer.make_can_msg("LS_01", bus, values)
|
||||
|
||||
|
||||
def acc_control_value(main_switch_on, acc_faulted, long_active):
|
||||
return 0
|
||||
|
||||
|
||||
def acc_hud_status_value(main_switch_on, acc_faulted, long_active):
|
||||
return 0
|
||||
|
||||
|
||||
def create_acc_accel_control(packer, bus, acc_type, acc_enabled, accel, acc_control, stopping, starting, esp_hold):
|
||||
values = {}
|
||||
return packer.make_can_msg("ACC_05", bus, values)
|
||||
|
||||
|
||||
def create_acc_hud_control(packer, bus, acc_hud_status, set_speed, lead_distance, distance):
|
||||
values = {}
|
||||
return packer.make_can_msg("ACC_02", bus, values)
|
||||
|
||||
def volkswagen_mlb_checksum(address: int, sig, d: bytearray) -> int:
|
||||
xor_starting_value = {
|
||||
0x109: 0x08, # ACC_01
|
||||
0x111: 0x10, # TSK_05
|
||||
0x30C: 0x0F, # ACC_02
|
||||
0x324: 0x27, # ACC_04
|
||||
0x10B: 0xA, # LS_01
|
||||
0x10D: 0x0C, # ACC_05
|
||||
0x10F: 0x0E, # ACC_0x10F
|
||||
0x311: 0x12, # ACC_0x311
|
||||
0x397: 0x94, # LDW_02
|
||||
0x10C: 0x0D, # TSK_02
|
||||
}
|
||||
if address in xor_starting_value:
|
||||
return xor_checksum(address, sig, d, xor_starting_value[address])
|
||||
else:
|
||||
return volkswagen_mqb_meb_checksum(address, sig, d)
|
||||
@@ -185,8 +185,8 @@ def volkswagen_mqb_meb_checksum(address: int, sig, d: bytearray) -> int:
|
||||
return crc ^ 0xFF
|
||||
|
||||
|
||||
def xor_checksum(address: int, sig, d: bytearray) -> int:
|
||||
checksum = 0
|
||||
def xor_checksum(address: int, sig, d: bytearray, initial_value: int = 0) -> int:
|
||||
checksum = initial_value
|
||||
checksum_byte = sig.start_bit // 8
|
||||
for i in range(len(d)):
|
||||
if i != checksum_byte:
|
||||
|
||||
@@ -5,8 +5,7 @@ from enum import Enum, IntFlag, StrEnum
|
||||
from opendbc.car import Bus, CanBusBase, CarSpecs, DbcDict, PlatformConfig, Platforms, structs, uds
|
||||
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, \
|
||||
Device
|
||||
from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
|
||||
from opendbc.car.fw_query_definitions import EcuAddrSubAddr, FwQueryConfig, Request, p16
|
||||
from opendbc.car.vin import Vin
|
||||
|
||||
@@ -81,7 +80,7 @@ class CarControllerParams:
|
||||
self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60))
|
||||
|
||||
if CP.transmissionType == TransmissionType.automatic:
|
||||
self.shifter_values = can_define.dv["Getriebe_1"]["Waehlhebelposition__Getriebe_1_"]
|
||||
self.shifter_values = can_define.dv["Getriebe_1"]["GE1_Wahl_Pos"]
|
||||
self.hca_status_values = can_define.dv["Lenkhilfe_2"]["LH2_Sta_HCA"]
|
||||
|
||||
self.BUTTONS = [
|
||||
@@ -90,7 +89,7 @@ class CarControllerParams:
|
||||
Button(structs.CarState.ButtonEvent.Type.accelCruise, "GRA_Neu", "GRA_Up_kurz", [1]),
|
||||
Button(structs.CarState.ButtonEvent.Type.decelCruise, "GRA_Neu", "GRA_Down_kurz", [1]),
|
||||
Button(structs.CarState.ButtonEvent.Type.cancel, "GRA_Neu", "GRA_Abbrechen", [1]),
|
||||
Button(structs.CarState.ButtonEvent.Type.gapAdjustCruise, "GRA_Neu", "GRA_Zeitluecke", [1]),
|
||||
Button(structs.CarState.ButtonEvent.Type.gapAdjustCruise, "GRA_Neu", "GRA_Zeitluecke", [3]),
|
||||
]
|
||||
|
||||
self.LDW_MESSAGES = {
|
||||
@@ -105,24 +104,43 @@ class CarControllerParams:
|
||||
else:
|
||||
self.LDW_STEP = 10 # LDW_02 message frequency 10Hz
|
||||
self.ACC_HUD_STEP = 6 # ACC_02 message frequency 16Hz
|
||||
self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm
|
||||
self.STEER_DELTA_UP = 4 # Max HCA reached in 1.50s (STEER_MAX / (50Hz * 1.50))
|
||||
self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60))
|
||||
|
||||
if CP.transmissionType == TransmissionType.automatic:
|
||||
self.shifter_values = can_define.dv["Gateway_73"]["GE_Fahrstufe"]
|
||||
elif CP.transmissionType == TransmissionType.direct:
|
||||
self.shifter_values = can_define.dv["Motor_EV_01"]["MO_Waehlpos"]
|
||||
self.hca_status_values = can_define.dv["LH_EPS_03"]["EPS_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", [1]),
|
||||
]
|
||||
if CP.flags & VolkswagenFlags.MLB:
|
||||
self.STEER_DRIVER_ALLOWANCE = 60 # Driver intervention threshold 0.6 Nm
|
||||
self.STEER_DELTA_UP = 9 # Max HCA reached in 0.66s (STEER_MAX / (50Hz * 0.66))
|
||||
self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60))
|
||||
|
||||
self.shifter_values = can_define.dv["Getriebe_03"]["GE_Waehlhebel"]
|
||||
|
||||
self.BUTTONS = [
|
||||
Button(structs.CarState.ButtonEvent.Type.setCruise, "LS_01", "LS_Tip_Setzen", [1]),
|
||||
Button(structs.CarState.ButtonEvent.Type.resumeCruise, "LS_01", "LS_Tip_Wiederaufnahme", [1]),
|
||||
Button(structs.CarState.ButtonEvent.Type.accelCruise, "LS_01", "LS_Tip_Hoch", [1]),
|
||||
Button(structs.CarState.ButtonEvent.Type.decelCruise, "LS_01", "LS_Tip_Runter", [1]),
|
||||
Button(structs.CarState.ButtonEvent.Type.cancel, "LS_01", "LS_Abbrechen", [1]),
|
||||
Button(structs.CarState.ButtonEvent.Type.gapAdjustCruise, "LS_01", "LS_Verstellung_Zeitluecke", [1]),
|
||||
]
|
||||
|
||||
else:
|
||||
self.STEER_DRIVER_ALLOWANCE = 80 # Driver intervention threshold 0.8 Nm
|
||||
self.STEER_DELTA_UP = 4 # Max HCA reached in 1.50s (STEER_MAX / (50Hz * 1.50))
|
||||
self.STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60))
|
||||
|
||||
if CP.transmissionType == TransmissionType.automatic:
|
||||
self.shifter_values = can_define.dv["Gateway_73"]["GE_Fahrstufe"]
|
||||
elif CP.transmissionType == TransmissionType.direct:
|
||||
self.shifter_values = can_define.dv["Motor_EV_01"]["MO_Waehlpos"]
|
||||
|
||||
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", [1]),
|
||||
]
|
||||
|
||||
self.LDW_MESSAGES = {
|
||||
"none": 0, # Nothing to display
|
||||
@@ -150,6 +168,7 @@ class WMI(StrEnum):
|
||||
AUDI_EUROPE_MPV = "WA1"
|
||||
AUDI_GERMANY_CAR = "WAU"
|
||||
MAN = "WMA"
|
||||
PORSCHE_SUV = "WP1"
|
||||
AUDI_SPORT = "WUA"
|
||||
VOLKSWAGEN_COMMERCIAL = "WV1"
|
||||
VOLKSWAGEN_COMMERCIAL_BUS_VAN = "WV2"
|
||||
@@ -169,6 +188,17 @@ class VolkswagenFlags(IntFlag):
|
||||
|
||||
# Static flags
|
||||
PQ = 2
|
||||
MLB = 8
|
||||
|
||||
|
||||
@dataclass
|
||||
class VolkswagenMLBPlatformConfig(PlatformConfig):
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'vw_mlb'})
|
||||
chassis_codes: set[str] = field(default_factory=set)
|
||||
wmis: set[WMI] = field(default_factory=set)
|
||||
|
||||
def init(self):
|
||||
self.flags |= VolkswagenFlags.MLB
|
||||
|
||||
|
||||
@dataclass
|
||||
@@ -204,7 +234,7 @@ class Footnote(Enum):
|
||||
Column.MODEL)
|
||||
SKODA_HEATED_WINDSHIELD = CarFootnote(
|
||||
"Some Škoda vehicles are equipped with heated windshields, which are known " +
|
||||
"to block GPS signal needed for some comma 3X functionality.",
|
||||
"to block GPS signal needed for some comma four functionality.",
|
||||
Column.MODEL)
|
||||
VW_EXP_LONG = CarFootnote(
|
||||
"Only available for vehicles using a gateway (J533) harness. At this time, vehicles using a camera harness " +
|
||||
@@ -226,9 +256,6 @@ class VWCarDocs(CarDocs):
|
||||
if "SKODA" in CP.carFingerprint:
|
||||
self.footnotes.append(Footnote.SKODA_HEATED_WINDSHIELD)
|
||||
|
||||
if CP.carFingerprint in (CAR.VOLKSWAGEN_CRAFTER_MK2, CAR.VOLKSWAGEN_TRANSPORTER_T61):
|
||||
self.car_parts = CarParts([Device.threex_angled_mount, CarHarness.vw_j533])
|
||||
|
||||
if abs(CP.minSteerSpeed - CarControllerParams.DEFAULT_MIN_STEER_SPEED) < 1e-3:
|
||||
self.min_steer_speed = 0
|
||||
|
||||
@@ -414,6 +441,12 @@ class CAR(Platforms):
|
||||
chassis_codes={"8U", "F3", "FS"},
|
||||
wmis={WMI.AUDI_EUROPE_MPV, WMI.AUDI_GERMANY_CAR},
|
||||
)
|
||||
PORSCHE_MACAN_MK1 = VolkswagenMLBPlatformConfig(
|
||||
[VWCarDocs("Porsche Macan 2017-24")],
|
||||
VolkswagenCarSpecs(mass=1895, wheelbase=2.81, steerRatio=16.2),
|
||||
chassis_codes={"95", "A5"},
|
||||
wmis={WMI.PORSCHE_SUV},
|
||||
)
|
||||
SEAT_ATECA_MK1 = VolkswagenMQBPlatformConfig(
|
||||
[
|
||||
VWCarDocs("CUPRA Ateca 2018-23"),
|
||||
|
||||
@@ -1,7 +1,8 @@
|
||||
Import("env")
|
||||
|
||||
import os
|
||||
from pathlib import Path
|
||||
|
||||
env = Environment(ENV=os.environ)
|
||||
|
||||
generator = File("generator/generator.py")
|
||||
|
||||
source_files = [
|
||||
|
||||
@@ -1,9 +1,3 @@
|
||||
BO_ 148 KINEMATICS: 8 XXX
|
||||
SG_ LAT_ACCEL : 7|10@0+ (-0.035,17.92) [-20|20] "m/s2" EON
|
||||
SG_ LONG_ACCEL : 25|10@0+ (-0.035,17.92) [-20|20] "m/s2" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 228 STEERING_CONTROL: 5 EON
|
||||
SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS
|
||||
SG_ DRIVER_OVERRIDE : 17|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
@@ -1,3 +1,14 @@
|
||||
BO_ 145 KINEMATICS_ALT: 8 XXX
|
||||
SG_ LAT_ACCEL : 7|10@0+ (0.02,-512) [-20|20] "m/s2" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 148 KINEMATICS: 8 XXX
|
||||
SG_ LAT_ACCEL : 7|10@0+ (0.02,-512) [-20|20] "m/s2" EON
|
||||
SG_ LONG_ACCEL : 24|9@0- (-0.02,0) [-20|20] "m/s2" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 304 GAS_PEDAL_2: 8 PCM
|
||||
SG_ ENGINE_TORQUE_ESTIMATE : 7|16@0- (1,0) [-1000|1000] "Nm" EON
|
||||
SG_ ENGINE_TORQUE_REQUEST : 23|16@0- (1,0) [-1000|1000] "Nm" EON
|
||||
|
||||
@@ -1,14 +1,3 @@
|
||||
BO_ 145 KINEMATICS: 8 XXX
|
||||
SG_ LAT_ACCEL : 7|10@0+ (0.02,-512) [-20|20] "m/s2" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 148 KINEMATICS_ALT: 8 XXX
|
||||
SG_ LAT_ACCEL : 7|10@0+ (0.02,-512) [-20|20] "m/s2" EON
|
||||
SG_ LONG_ACCEL : 24|9@0- (-0.02,0) [-20|20] "m/s2" EON
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|3] "" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 432 STANDSTILL: 7 VSA
|
||||
SG_ CONTROLLED_STANDSTILL : 0|1@0+ (1,0) [0|1] "" EON
|
||||
SG_ WHEELS_MOVING : 12|1@0+ (1,0) [0|1] "" EON
|
||||
@@ -71,8 +60,8 @@ BO_ 927 RADAR_HUD: 8 ADAS
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" BDY
|
||||
|
||||
CM_ SG_ 506 AEB_REQ_1 "set for duration of suspected AEB event";
|
||||
CM_ SG_ 506 COMPUTER_BRAKE_ALT "Used by dual-can Nidec";
|
||||
CM_ SG_ 506 BRAKE_PUMP_REQUEST_ALT "Used by dual-can Nidec";
|
||||
CM_ SG_ 506 COMPUTER_BRAKE_HYBRID "Used by Hybrid Nidec";
|
||||
CM_ SG_ 506 BRAKE_PUMP_REQUEST_HYBRID "Used by Hybrid Nidec";
|
||||
|
||||
VAL_ 506 FCW 3 "fcw" 2 "fcw" 1 "fcw" 0 "no_fcw";
|
||||
VAL_ 506 CHIME 4 "double_chime" 3 "single_chime" 2 "continuous_chime" 1 "repeating_chime" 0 "no_chime";
|
||||
|
||||
2749
opendbc/dbc/vw_mlb.dbc
Normal file
2749
opendbc/dbc/vw_mlb.dbc
Normal file
File diff suppressed because it is too large
Load Diff
@@ -58,6 +58,7 @@ BO_ 290 ACC_06: 8 XXX
|
||||
BO_ 279 ACC_10: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ AWV_AWA_Status_EPS : 12|4@1+ (1,0) [0|7] "" XXX
|
||||
SG_ AWV1_Anf_Prefill : 16|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ ANB_CM_Info : 17|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ AWV2_Freigabe : 18|1@1+ (1,0) [0|1] "" XXX
|
||||
@@ -1329,6 +1330,7 @@ BO_ 159 LH_EPS_03: 8 XXX
|
||||
SG_ EPS_BLW_QBit : 30|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ EPS_VZ_BLW : 31|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ EPS_HCA_Status : 32|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ EPS_AWA_Status : 36|4@1+ (1,0) [0|14] "" XXX
|
||||
SG_ EPS_Lenkmoment : 40|10@1+ (1,0) [0|8] "Unit_centiNewtoMeter" XXX
|
||||
SG_ EPS_Lenkmoment_QBit : 54|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ EPS_VZ_Lenkmoment : 55|1@1+ (1,0) [0|1] "" XXX
|
||||
@@ -1673,7 +1675,9 @@ CM_ SG_ 1720 KBI_Variante_USA "In diesem Signal wird die HW-Variante des Kombis
|
||||
|
||||
|
||||
VAL_ 159 EPS_HCA_Status 0 "disabled" 1 "initializing" 2 "fault" 3 "ready" 4 "rejected" 5 "active" 8 "preempted" ;
|
||||
VAL_ 159 EPS_AWA_Status 0 "AWA_deaktiviert" 1 "AWA_Anf_nicht_ausfuehrbar" 2 "Kom_AWA_gestoert" 3 "EPS_bzgl_AWA_funktionsbereit" 4 "AWA_Anf_abgewiesen" 5 "AWA_Momeingriff_1_aktiv" 7 "AWA_Momeingriff_2_aktiv" 8 "AWA_abgewiesen_Momentenpriorisierung" 9 "Anf_abgewiesen_Bedienueblich" 14 "Init" ;
|
||||
VAL_ 173 GE_Fahrstufe 5 "P" 6 "R" 7 "N" 8 "D" 9 "S" 10 "E" 13 "T" 14 "T" ;
|
||||
VAL_ 279 AWV_AWA_Status_EPS 0 "AWA_deactivated" 1 "AWA_request_not_executable" 2 "EPS_communication_disturbed" 3 "AWA_ready" 5 "AWA_torque_intervention_1" 7 "AWA_torque_intervention_2" ;
|
||||
VAL_ 288 TSK_Status 0 "init" 1 "disabled" 2 "enabled" 3 "regulating" 4 "accel_pedal_override" 5 "brake_only" 6 "temp_fault" 7 "perm_fault" ;
|
||||
VAL_ 288 TSK_v_Begrenzung_aktiv 0 "inaktiv" 1 "aktiv" ;
|
||||
VAL_ 288 TSK_Standby_Anf_ESP 0 "keine_Standby_Anforderung" 1 "Standby_Anforderung" ;
|
||||
|
||||
@@ -22,6 +22,7 @@ NS_ :
|
||||
SIG_GROUP_
|
||||
SIG_VALTYPE_
|
||||
SIGTYPE_VALTYPE_
|
||||
SG_MUL_VAL_
|
||||
|
||||
BS_:
|
||||
|
||||
@@ -330,50 +331,64 @@ BO_ 1160 Motor_6: 8 XXX
|
||||
SG_ Sollmoment_f_r_Getriebe : 8|8@1+ (0.39,0) [0|99] "MDI" XXX
|
||||
SG_ Checksumme_Motor_6 : 0|8@1+ (1,0) [0|0] "" XXX
|
||||
|
||||
BO_ 1152 Motor_5: 8 XXX
|
||||
SG_ CHECKSUM : 56|8@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Motortext_Bits__4_1_ : 52|4@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Doppelte_Momente : 51|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ GRA_Hauptschalter : 50|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Anlasser_Ausspuren : 49|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Anlasser_Freigabe : 48|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Klimadrucksignal__Motor_5_ : 40|8@1+ (0.2,0) [0|50.8] "bar" XXX
|
||||
SG_ K_hlerluefteransteuerung : 32|8@1+ (0.4,0) [0|101.6] "%" XXX
|
||||
SG_ Verbrauch_Ueberlauf : 31|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Kraftstoffverbrauchssignal : 16|15@1+ (1,0) [0|32767] "ul" XXX
|
||||
SG_ Klimakompressor_Leistungsreduzi : 15|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Kennfeldkuehlung : 14|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Klimakompressor_aus__Motor_5_ : 13|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ CAT_Warnung : 12|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ OBD_2_Lampe : 11|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ E_Gas_Lampe : 10|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Vorgluehlampe__Motor_5_ : 9|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Ladekontroll_Lampe : 8|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Multiplex_Code M : 6|2@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Multiplex_Info_norm__Verbrauch m3 : 0|6@1+ (20,0) [0|1260] "l/Zyl" XXX
|
||||
SG_ Multiplex_Info_Motortyp m2 : 0|6@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Multiplex_Info_Drehzahl_MD_Max m1 : 0|6@1+ (100,0) [0|6300] "U/min" XXX
|
||||
SG_ Multiplex_Info_Max_Drehmoment m0 : 0|6@1+ (10,0) [0|630] "Nm" XXX
|
||||
BO_ 1152 Motor_5: 8 Motor
|
||||
SG_ MO5_Mp_Code M : 6|2@1+ (1,0) [0|3] "" Gateway
|
||||
SG_ MO5_max_Moment m0 : 0|6@1+ (10,0) [0|630] "Nm" Gateway
|
||||
SG_ MO5_Drehzahl m1 : 0|6@1+ (100,0) [0|6300] "U/min" Gateway
|
||||
SG_ MO5_Motortyp m2 : 0|6@1+ (1,0) [0|63] "" Gateway
|
||||
SG_ MO5_Abgastyp m3 : 0|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO5_Abgastyp2 m3 : 1|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO5_Abgastyp3 m3 : 2|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ OBD_Kaltstart_Denominator m3 : 3|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ OBD_Minimum_Trip m3 : 4|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO5_DPF_reg m3 : 5|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO5_Vorgluehen : 9|1@1+ (1,0) [0|1] "" KOMBI
|
||||
SG_ MO5_E_Gas : 10|1@1+ (1,0) [0|1] "" KOMBI
|
||||
SG_ MO5_OBD_2 : 11|1@1+ (1,0) [0|1] "" KOMBI
|
||||
SG_ MO5_Heissl : 12|1@1+ (1,0) [0|1] "" KOMBI
|
||||
SG_ MO5_KlimaKompr : 13|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO5_Feld_kuehl : 14|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO5_KliKo_Red : 15|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO5_Verbrauch : 16|15@1+ (1,0) [0|32767] "µl" KOMBI
|
||||
SG_ MO5_UeberlVerb : 31|1@1+ (1,0) [0|1] "" KOMBI
|
||||
SG_ MO5_Luefter : 32|8@1+ (0.4,0) [0|101.6] "%" Gateway
|
||||
SG_ MO5_HLeuchte : 40|1@1+ (1,0) [0|1] "" KOMBI
|
||||
SG_ MO5_PartikelLamp : 41|1@1+ (1,0) [0|1] "" KOMBI
|
||||
SG_ MO5_Sta_BKU : 42|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO5_TypStartSteu : 43|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO5_TDE_Lampe : 44|1@1+ (1,0) [0|1] "" KOMBI
|
||||
SG_ MO5_TDE_Text : 45|1@1+ (1,0) [0|1] "" KOMBI
|
||||
SG_ MO5_DZM_Daempf : 46|1@1+ (1,0) [0|1] "" KOMBI
|
||||
SG_ MO5_Interlock : 47|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO5_Start : 48|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO5_Anlasser : 49|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO5_GRA_Hauptsch : 50|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO5_Momente : 51|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO5_Motortext1 : 52|1@1+ (1,0) [0|1] "" KOMBI
|
||||
SG_ MO5_Motortext2 : 53|1@1+ (1,0) [0|1] "" KOMBI
|
||||
SG_ MO5_Motortext3 : 54|1@1+ (1,0) [0|1] "" KOMBI
|
||||
SG_ MO5_Motortext4 : 55|1@1+ (1,0) [0|1] "" KOMBI
|
||||
SG_ CHECKSUM : 56|8@1+ (1,0) [0|255] "" Gateway
|
||||
|
||||
BO_ 896 Motor_3: 8 XXX
|
||||
SG_ Drosselklappenpoti : 56|8@1+ (0.4,0) [0|101.6] "%" XXX
|
||||
SG_ Motor_Wunschdrehzahl : 48|8@1+ (25,0) [0|6350] "U/min" XXX
|
||||
SG_ Motordrehzahlbeeinflussung : 40|8@1+ (0.392,0) [0|100] "%" XXX
|
||||
SG_ Kein_Start_Stop : 39|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Kein_E_Gas : 38|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Reserviert_Motor_3_1 : 37|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Vorzeichen_Rad_Wunschmoment : 36|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Rad_Wunschmoment : 24|12@1+ (0.39,0) [0|1597] "MDI" XXX
|
||||
SG_ Fahrpedal_Rohsignal : 16|8@1+ (0.4,0) [0|101.6] "%" XXX
|
||||
SG_ Ansauglufttemperatur : 8|8@1+ (0.75,-48) [-48|142.5] "" XXX
|
||||
SG_ Fehlerstatus_Ansauglufttemperat : 7|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Motorsteuerger_t_gesperrt : 6|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Drosselklappenwinkel_ungenau : 5|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Fahrpedalwert_ungenau__Motor_3_ : 4|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Frei_Motor_3_1 : 3|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Motor_Wunschdrehzahl_Priorit_t : 2|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Uebertemperaturschutz__Motor_3_ : 1|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Vorgluehmeldung : 0|1@1+ (1,0) [0|0] "" XXX
|
||||
BO_ 896 Motor_3: 8 Motor
|
||||
SG_ MO3_Vorgluehen : 0|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO3_Prio_Dz : 1|1@1+ (1,0) [0|1] "" Transmission
|
||||
SG_ MO_Schalter_StartStopp : 2|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO3_Winterprg : 3|1@1+ (1,0) [0|1] "" Transmission
|
||||
SG_ MO3_Sta_Pedal : 4|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO3_Sta_FPG M : 5|1@1+ (1,0) [0|1] "" ESP
|
||||
SG_ MO3_Sign_FPG m1 : 6|1@1+ (1,0) [0|1] "" ESP
|
||||
SG_ MO3_Sta_Temp : 7|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO3_Offsentemp : 8|8@1+ (0.75,-48) [-48|142.5] "°C" Gateway
|
||||
SG_ MO3_Pedalwert : 16|8@1+ (0.4,0) [0|101.6] "%" Gateway
|
||||
SG_ MO3_Rad_Wu_Mo : 24|12@1+ (0.39,0) [0|1596.66] "%" Transmission
|
||||
SG_ MO3_Vorz_RadWu : 36|1@1+ (1,0) [0|1] "" Transmission
|
||||
SG_ MO3_Freigabe_Segeln : 37|1@1+ (1,0) [0|1] "" Transmission
|
||||
SG_ MO_StartStopp_StoppVorbereitung : 38|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ MO3_Dz_Beeinfl : 40|8@1+ (0.39,0) [0|99.45] "%" Transmission
|
||||
SG_ MO3_WunschDz : 48|8@1+ (25,0) [0|6350] "U/min" Transmission
|
||||
SG_ MO3_DKW m0 : 56|8@1+ (0.4,0) [0|101.6] "%" Gateway
|
||||
SG_ MO3_FPGradient m1 : 56|8@1+ (25,0) [0|6350] "%/s" ESP
|
||||
|
||||
BO_ 648 Motor_2: 8 Motor
|
||||
SG_ MO2_Mp_Code m : 6|2@1+ (1,0) [0|3] "" Gateway
|
||||
@@ -705,28 +720,27 @@ BO_ 1344 Getriebe_2: 8 XXX
|
||||
SG_ Schubabschaltunterstuetzung : 1|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ LFR_Adaption_Freigabeflag : 0|1@1+ (1,0) [0|0] "" XXX
|
||||
|
||||
BO_ 1088 Getriebe_1: 8 XXX
|
||||
SG_ Wandlerverlustmoment : 56|8@1+ (0.39,0) [0|99.06] "MDI" XXX
|
||||
SG_ Fehlerspeichereintrag__Getriebe : 55|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ COUNTER : 51|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ Gang_eingelegt : 50|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Schaltabsicht : 49|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Motor_aus : 48|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ OBD_Status__Getriebe_1___4_1_ : 46|2@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Kuehlleistung : 44|2@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Getriebe_Notlauf : 40|4@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Fahrwiderstandsindex : 32|8@1+ (0.249,-31.6) [-31.6|31.6] "" XXX
|
||||
SG_ inneres_Soll_Motormoment : 24|8@1+ (0.39,0) [0|99.06] "MDI" XXX
|
||||
SG_ Uebertragungsfunktion : 16|8@1+ (0.1,0) [0|25.4] "" XXX
|
||||
SG_ Waehlhebelposition__Getriebe_1_ : 12|4@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Zielgang_oder_eingelegter_Gang : 8|4@1+ (1,0) [0|0] "" XXX
|
||||
SG_ EGS_Anforderung : 7|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Kodierung_im_MSG : 6|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Leerlaufsolldrehzahlanhebung : 5|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Wandlerkupplung : 3|2@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Klimakompressor_aus__Getriebe_1 : 2|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Status_Getriebe_und_Wandlerschu : 1|1@1+ (1,0) [0|0] "" XXX
|
||||
SG_ Schaltung_aktiv__Getriebe_1_ : 0|1@1+ (1,0) [0|0] "" XXX
|
||||
BO_ 1088 Getriebe_1: 8 Transmission
|
||||
SG_ GE1_Schaltung : 0|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ GE1_Sta_Schutz : 1|1@1+ (1,0) [0|1] "" Motor
|
||||
SG_ GE1_Klimakompr : 2|1@1+ (1,0) [0|1] "" Motor
|
||||
SG_ GE1_WK : 3|2@1+ (1,0) [0|3] "" Gateway
|
||||
SG_ GE1_StSt_Info : 5|2@1+ (1,0) [0|3] "" Gateway
|
||||
SG_ GE1_EGS_Anf : 7|1@1+ (1,0) [0|1] "" Motor
|
||||
SG_ GE1_Zielgang : 8|4@1+ (1,0) [0|15] "" Gateway
|
||||
SG_ GE1_Wahl_Pos : 12|4@1+ (1,0) [0|15] "" Gateway
|
||||
SG_ GE1_Mrad_Mkurb : 16|8@1+ (0.1,0) [0|25.4] "" Gateway
|
||||
SG_ GE1_Soll_Mo : 24|8@1+ (0.39,0) [0|99.06] "%" Motor
|
||||
SG_ GE1_Fahrwistd : 32|8@1+ (0.249,-31.623) [-31.623|31.623] "" Gateway
|
||||
SG_ GE1_Notlauf : 40|4@1+ (1,0) [0|15] "" Gateway
|
||||
SG_ GE1_Kuehlung : 44|2@1+ (1,0) [0|3] "" Gateway
|
||||
SG_ GE1_Sta_OBD : 47|1@1+ (1,0) [0|1] "" Motor
|
||||
SG_ GE1_LaunchControl : 48|1@1+ (1,0) [0|1] "" ESP
|
||||
SG_ GE1_Infobit : 49|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ GE1_SleepInd : 50|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ COUNTER : 51|4@1+ (1,0) [0|15] "" Gateway
|
||||
SG_ GE1_Fehlereintr : 55|1@1+ (1,0) [0|1] "" Gateway
|
||||
SG_ GE1_WaVerl_Mo : 56|8@1+ (0.39,0) [0|99.06] "%" Motor
|
||||
|
||||
BO_ 912 Gate_Komf_1: 8 XXX
|
||||
SG_ GK1_Sta_RDK_Warn : 0|1@1+ (1,0) [0|1] "" XXX
|
||||
@@ -1406,6 +1420,8 @@ BO_ 1175 Parkhilfe_01: 8 XXX
|
||||
SG_ PH_KD_Fehler : 63|1@1+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 1463 Bremse_11: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ B11_HydHalten : 13|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ B11_Br_StSt_Info : 14|2@1+ (1,0) [0|3] "" XXX
|
||||
SG_ B11_OBD_Nib_VL : 16|4@1+ (1,0) [0|15] "" XXX
|
||||
@@ -1600,14 +1616,24 @@ CM_ SG_ 648 MO2_LL_Solldz "Idle target speed";
|
||||
CM_ SG_ 648 MO2_Begr_Mo "Limiting torque - max possible at speed";
|
||||
CM_ SG_ 648 MO2_Mo_ZWR "Minimum engine torque with ignition angle retardation";
|
||||
|
||||
CM_ SG_ 896 Drosselklappenpoti "Throttle Position";
|
||||
CM_ SG_ 896 Motor_Wunschdrehzahl "Desired engine speed";
|
||||
CM_ SG_ 896 Motordrehzahlbeeinflussung "Shift Target Influence";
|
||||
CM_ SG_ 896 Fahrpedal_Rohsignal "Accelerator Pedal Position";
|
||||
CM_ SG_ 896 Ansauglufttemperatur "Intake Air Temperature";
|
||||
CM_ SG_ 896 Kein_E_Gas "ETB flag";
|
||||
CM_ SG_ 896 Kein_Start_Stop "Start/stop flag";
|
||||
CM_ SG_ 896 Rad_Wunschmoment "Desired wheel torque";
|
||||
CM_ SG_ 896 MO3_Vorgluehen "Pre-glow active (diesel engines)";
|
||||
CM_ SG_ 896 MO3_Prio_Dz "Engine speed priority: 0=request, 1=mandatory";
|
||||
CM_ SG_ 896 MO_Schalter_StartStopp "Start/stop deactivated by driver (LED on when 0)";
|
||||
CM_ SG_ 896 MO3_Winterprg "Winter driving program active";
|
||||
CM_ SG_ 896 MO3_Sta_Pedal "Accelerator pedal status: 0=OK, 1=substitute value";
|
||||
CM_ SG_ 896 MO3_Sta_FPG "Accelerator pedal gradient control bit (multiplexer for byte 8)";
|
||||
CM_ SG_ 896 MO3_Sign_FPG "Accelerator pedal gradient sign (only when MO3_Sta_FPG=1)";
|
||||
CM_ SG_ 896 MO3_Sta_Temp "Intake air temperature status: 0=OK, 1=substitute value";
|
||||
CM_ SG_ 896 MO3_Offsentemp "Intake air temperature";
|
||||
CM_ SG_ 896 MO3_Pedalwert "Accelerator pedal raw signal (0-101.6%)";
|
||||
CM_ SG_ 896 MO3_Rad_Wu_Mo "Wheel torque request (12-bit value, 0-1596.66% MDI)";
|
||||
CM_ SG_ 896 MO3_Vorz_RadWu "Wheel torque sign: 0=positive, 1=negative";
|
||||
CM_ SG_ 896 MO3_Freigabe_Segeln "Sailing operation enable for transmission";
|
||||
CM_ SG_ 896 MO_StartStopp_StoppVorbereitung "Engine stop in preparation";
|
||||
CM_ SG_ 896 MO3_Dz_Beeinfl "Engine speed influence factor (0-100% interpolation)";
|
||||
CM_ SG_ 896 MO3_WunschDz "Desired engine speed (when MO3_Prio_Dz=1)";
|
||||
CM_ SG_ 896 MO3_DKW "Throttle valve position when MO3_Sta_FPG=0 (0-101.6%)";
|
||||
CM_ SG_ 896 MO3_FPGradient "Accelerator pedal gradient when MO3_Sta_FPG=1 (0-6350%/s)";
|
||||
|
||||
CM_ SG_ 912 GK1_Fa_Tuerkont "Status of the driver's door rotary latch";
|
||||
CM_ SG_ 912 BSK_HL_geoeffnet "Status of the rear left door rotary latch";
|
||||
@@ -1615,18 +1641,28 @@ CM_ SG_ 912 BSK_HR_geoeffnet "Status of the rear right door rotary latch";
|
||||
CM_ SG_ 912 BSK_BT_geoeffnet "Status of the passenger door rotary latch";
|
||||
CM_ SG_ 912 BSK_HD_Hauptraste "Status of trunk lid main detent";
|
||||
|
||||
CM_ SG_ 1088 Zaehler_Getriebe_1 "Counter Getriebe_1";
|
||||
CM_ SG_ 1088 Waehlhebelposition__Getriebe_1_ "Gear Selector Position";
|
||||
CM_ SG_ 1088 inneres_Soll_Motormoment "Desired Inner Torque";
|
||||
CM_ SG_ 1088 Gang_eingelegt "Gear Engaged";
|
||||
CM_ SG_ 1088 Schaltabsicht "Shift Intent";
|
||||
CM_ SG_ 1088 Kuehlleistung "Cooling Power";
|
||||
CM_ SG_ 1088 Wandlerverlustmoment "Converter Torque Loss";
|
||||
CM_ SG_ 1088 Getriebe_Notlauf "Transmission_Notlauf";
|
||||
CM_ SG_ 1088 Zielgang_oder_eingelegter_Gang "target_gear_or_gear_in_engagement";
|
||||
CM_ SG_ 1088 Uebertragungsfunktion "transfer function";
|
||||
CM_ SG_ 1088 EGS_Anforderung "EGS Requirement";
|
||||
CM_ SG_ 1088 Schaltung_aktiv__Getriebe_1_ "Shift Activity";
|
||||
CM_ BO_ 1088 "Getriebe 1 - Contains gear status, torque requests, and control signals";
|
||||
|
||||
CM_ SG_ 1088 GE1_Schaltung "Gear shift active: 0=no shift, 1=shift in progress";
|
||||
CM_ SG_ 1088 GE1_Sta_Schutz "Transmission/converter protection request (power reduction or ignition angle)";
|
||||
CM_ SG_ 1088 GE1_Klimakompr "AC compressor off request";
|
||||
CM_ SG_ 1088 GE1_WK "Torque converter clutch status";
|
||||
CM_ SG_ 1088 GE1_StSt_Info "Start/stop coordinator info";
|
||||
CM_ SG_ 1088 GE1_EGS_Anf "EGS request - byte 4 becomes valid when set";
|
||||
CM_ SG_ 1088 GE1_Zielgang "Target gear when shifting or current gear";
|
||||
CM_ SG_ 1088 GE1_Wahl_Pos "Selector lever position / driving program";
|
||||
CM_ SG_ 1088 GE1_Mrad_Mkurb "Wheel torque to crankshaft torque ratio (Mrad/Mcrank)";
|
||||
CM_ SG_ 1088 GE1_Soll_Mo "Internal target engine torque request (0-99.06%)";
|
||||
CM_ SG_ 1088 GE1_Fahrwistd "Driving resistance index (normalized % slope)";
|
||||
CM_ SG_ 1088 GE1_Notlauf "Emergency mode status";
|
||||
CM_ SG_ 1088 GE1_Kuehlung "Cooling request level";
|
||||
CM_ SG_ 1088 GE1_Sta_OBD "OBD status - MIL lamp control";
|
||||
CM_ SG_ 1088 GE1_LaunchControl "Launch control active";
|
||||
CM_ SG_ 1088 GE1_Infobit "Transmission type: 0=stepped automatic, 1=CVT";
|
||||
CM_ SG_ 1088 GE1_SleepInd "Sleep readiness after terminal 15 off";
|
||||
CM_ SG_ 1088 GE1_Zaehler "Message counter (synchronized with Getriebe_2)";
|
||||
CM_ SG_ 1088 GE1_Fehlereintr "Fault memory entry present";
|
||||
CM_ SG_ 1088 GE1_WaVerl_Mo "Transmission torque loss / clutch torque (0-99.06%)";
|
||||
|
||||
CM_ SG_ 1056 Fehlerstatus_Aussentemp__4_1 "ambient temp error";
|
||||
CM_ SG_ 1056 Fehlerstatus_Oeltemperatur_4_1 "oil temp error";
|
||||
@@ -1637,19 +1673,45 @@ CM_ SG_ 1056 Kuehlmitteltemp__4_1__Kombi_2_ "kombi coolant temperature";
|
||||
|
||||
CM_ SG_ 1096 Zaehler_Waehlhebel_1 "Counter Waehlhebel_1";
|
||||
|
||||
CM_ SG_ 1152 CHECKSUM "Checksum Motor_5";
|
||||
CM_ SG_ 1152 Anlasser_Ausspuren "Starter Disable";
|
||||
CM_ SG_ 1152 Anlasser_Freigabe "Starter Release";
|
||||
CM_ SG_ 1152 Klimadrucksignal__Motor_5_ "Air conditioning pressure signal";
|
||||
CM_ SG_ 1152 Kraftstoffverbrauchssignal "Fuel consumption signal";
|
||||
CM_ SG_ 1152 K_hlerluefteransteuerung "Cooling fan control signal";
|
||||
CM_ SG_ 1152 Klimakompressor_Leistungsreduzi "Air conditioning compressor power reduction flag";
|
||||
CM_ SG_ 1152 Klimakompressor_aus__Motor_5_ "Air conditioning compressor";
|
||||
CM_ SG_ 1152 Anlasser_Freigabe "Starter release";
|
||||
CM_ SG_ 1152 OBD_2_Lampe "OBD light";
|
||||
CM_ SG_ 1152 E_Gas_Lampe "ETB light";
|
||||
CM_ SG_ 1152 Ladekontroll_Lampe "Charge light";
|
||||
CM_ SG_ 1152 Vorgluehlampe__Motor_5_ "Glow light";
|
||||
CM_ BO_ 1152 "Motor message 5 - Contains engine type, consumption, lamps, and diagnostic info";
|
||||
|
||||
CM_ SG_ 1152 MO5_Mp_Code "Multiplex selector - switches every 4 transmissions";
|
||||
CM_ SG_ 1152 MO5_max_Moment "Maximum torque when Mp_Code=0 (0-630 Nm)";
|
||||
CM_ SG_ 1152 MO5_Drehzahl "Engine speed at max torque when Mp_Code=1 (0-6300 RPM)";
|
||||
CM_ SG_ 1152 MO5_Motortyp "Engine type info when Mp_Code=2 (cylinder count, turbo, fuel type)";
|
||||
CM_ SG_ 1152 MO5_Abgastyp "Exhaust type EOBD when Mp_Code=3";
|
||||
CM_ SG_ 1152 MO5_Abgastyp2 "Exhaust type OBD when Mp_Code=3";
|
||||
CM_ SG_ 1152 MO5_Abgastyp3 "DPF installed when Mp_Code=3";
|
||||
CM_ SG_ 1152 OBD_Kaltstart_Denominator "Cold start denominator increment when Mp_Code=3";
|
||||
CM_ SG_ 1152 OBD_Minimum_Trip "Minimum normalized trip when Mp_Code=3";
|
||||
CM_ SG_ 1152 MO5_DPF_reg "DPF regenerating when Mp_Code=3";
|
||||
CM_ SG_ 1152 MO5_Vorgluehen "Diesel pre-glow lamp";
|
||||
CM_ SG_ 1152 MO5_E_Gas "E-Gas system lamp (petrol only)";
|
||||
CM_ SG_ 1152 MO5_OBD_2 "OBD/MIL lamp control";
|
||||
CM_ SG_ 1152 MO5_Heissl "Coolant overheat warning lamp";
|
||||
CM_ SG_ 1152 MO5_KlimaKompr "AC compressor off request";
|
||||
CM_ SG_ 1152 MO5_Feld_kuehl "Map cooling installed and OK";
|
||||
CM_ SG_ 1152 MO5_KliKo_Red "AC compressor power reduction";
|
||||
CM_ SG_ 1152 MO5_Verbrauch "Fuel consumption counter (15-bit, µl)";
|
||||
CM_ SG_ 1152 MO5_UeberlVerb "Consumption counter overflow";
|
||||
CM_ SG_ 1152 MO5_Luefter "Cooling fan PWM control (0-101.6%)";
|
||||
CM_ SG_ 1152 MO5_HLeuchte "Overheat pre-warning";
|
||||
CM_ SG_ 1152 MO5_PartikelLamp "Particle filter lamp";
|
||||
CM_ SG_ 1152 MO5_Sta_BKU "Brake vacuum pump status (Porsche only)";
|
||||
CM_ SG_ 1152 MO5_TypStartSteu "Starter control type: 0=BSG/BCM, 1=MSG";
|
||||
CM_ SG_ 1152 MO5_TDE_Lampe "Fuel cap warning lamp (petrol only)";
|
||||
CM_ SG_ 1152 MO5_TDE_Text "Fuel cap text display (petrol only)";
|
||||
CM_ SG_ 1152 MO5_DZM_Daempf "RPM gauge damping control";
|
||||
CM_ SG_ 1152 MO5_Interlock "Interlock switch for manual transmission start";
|
||||
CM_ SG_ 1152 MO5_Start "Automatic start permission";
|
||||
CM_ SG_ 1152 MO5_Anlasser "Starter control permission";
|
||||
CM_ SG_ 1152 MO5_GRA_Hauptsch "GRA main switch / readiness";
|
||||
CM_ SG_ 1152 MO5_Momente "MDI torque values doubled";
|
||||
CM_ SG_ 1152 MO5_Motortext1 "Text: Engine workshop";
|
||||
CM_ SG_ 1152 MO5_Motortext2 "Text: Exhaust workshop (deprecated)";
|
||||
CM_ SG_ 1152 MO5_Motortext3 "Text: Fuel system fault";
|
||||
CM_ SG_ 1152 MO5_Motortext4 "Text: Particle filter needs regeneration";
|
||||
CM_ SG_ 1152 CHECKSUM "Checksum with final value 0x0";
|
||||
|
||||
CM_ SG_ 1160 Zaehler_Motor_6 "Counter Motor_6";
|
||||
CM_ SG_ 1160 Hoeheninfo__Motor_6_ "Altitude Correction";
|
||||
@@ -1688,6 +1750,10 @@ CM_ SG_ 1550 MFA_v_Signal_02 "0=km/h, 1=mph";
|
||||
SG_MUL_VAL_ 416 BR1_ASRMo_fa BR1_MSR_Anf 0-0;
|
||||
SG_MUL_VAL_ 416 BR1_MSR_Mo_inv BR1_MSR_Anf 1-1;
|
||||
|
||||
SG_MUL_VAL_ 896 MO3_DKW MO3_Sta_FPG 0-0;
|
||||
SG_MUL_VAL_ 896 MO3_FPGradient MO3_Sta_FPG 1-1;
|
||||
SG_MUL_VAL_ 896 MO3_Sign_FPG MO3_Sta_FPG 1-1;
|
||||
|
||||
VAL_ 194 LW1_LRW_Sign 0 "positives_Vorzeichen" 1 "negatives_Vorzeichen";
|
||||
VAL_ 194 LW1_Gesch_Sign 0 "positives_Vorzeichen" 1 "negatives_Vorzeichen";
|
||||
VAL_ 194 LW1_ID 0 "noch_not_kalibriert" 128 "kalibriert_nur_bei_PQ35_46_VW32x_VW411";
|
||||
@@ -1721,6 +1787,16 @@ SG_MUL_VAL_ 648 MO2_max_Mo MO2_Mp_Code 3-3;
|
||||
SG_MUL_VAL_ 648 MO2_CAN_Vers MO2_Mp_Code 0-0;
|
||||
SG_MUL_VAL_ 648 MO2_Motor_Code MO2_Mp_Code 1-1;
|
||||
|
||||
SG_MUL_VAL_ 1152 MO5_max_Moment MO5_Mp_Code 0-0;
|
||||
SG_MUL_VAL_ 1152 MO5_Drehzahl MO5_Mp_Code 1-1;
|
||||
SG_MUL_VAL_ 1152 MO5_Motortyp MO5_Mp_Code 2-2;
|
||||
SG_MUL_VAL_ 1152 MO5_Abgastyp MO5_Mp_Code 3-3;
|
||||
SG_MUL_VAL_ 1152 MO5_Abgastyp2 MO5_Mp_Code 3-3;
|
||||
SG_MUL_VAL_ 1152 MO5_Abgastyp3 MO5_Mp_Code 3-3;
|
||||
SG_MUL_VAL_ 1152 OBD_Kaltstart_Denominator MO5_Mp_Code 3-3;
|
||||
SG_MUL_VAL_ 1152 OBD_Minimum_Trip MO5_Mp_Code 3-3;
|
||||
SG_MUL_VAL_ 1152 MO5_DPF_reg MO5_Mp_Code 3-3;
|
||||
|
||||
VAL_ 648 MO2_Mp_Code 0 "CAN_Stand" 1 "Motor_Kod" 2 "Getriebe_Kod" 3 "MDI_Max";
|
||||
|
||||
VAL_ 648 MO2_Getr_Code 0 "_5HP19" 1 "_5HP24" 2 "AG4" 3 "VL300" 4 "VQ250" 5 "VQ35" 6 "AG4_yestco" 7 "AG5_yestco" 8 "autom_Kupplung" 9 "autom_Kupplung_mit_ASG" 10 "AG6_yestco_ZF_6HP_AISIN" 11 "DQ500_DQ250_DQ200" 12 "SQ100_SQ200" 14 "AL1000_AL551_AL951_ZF8HPx" 15 "Handschalter_konv" 16 "DL800" 17 "E_Fahrzeug__Single_Gear";
|
||||
@@ -1779,8 +1855,68 @@ VAL_ 872 ACS_Fehler 1 "Fehlerspeichereintrag" 0 "kein_Fehlerspeichereintrag" ;
|
||||
VAL_ 872 ACS_zul_Regelabw 254 "ADR_nicht_aktiv" 255 "Fehler" ;
|
||||
VAL_ 872 ACS_max_AendGrad 254 "Neutralwert" 0 "Neutralwert" 255 "Fehler" ;
|
||||
|
||||
VAL_ 896 MO3_Vorgluehen 0 "no_Vorgluehen" 1 "Vorgluehen_active";
|
||||
VAL_ 896 MO3_Prio_Dz 0 "Wunsch" 1 "Zwang";
|
||||
VAL_ 896 MO_Schalter_StartStopp 0 "StartStopp_ueber_Hauptschalter_deactivated_LED_an" 1 "StartStopp_ueber_Hauptschalter_activated_LED_aus";
|
||||
VAL_ 896 MO3_Winterprg 0 "not_active" 1 "active";
|
||||
VAL_ 896 MO3_Sta_Pedal 0 "Fahrpedal_iO" 1 "Ersatzwert";
|
||||
VAL_ 896 MO3_Sta_FPG 0 "Gradient_not_bed" 1 "Gradient_bed";
|
||||
VAL_ 896 MO3_Sign_FPG 0 "positives_Vorzeichen" 1 "negatives_Vorzeichen";
|
||||
VAL_ 896 MO3_Sta_Temp 0 "Geber_iO" 1 "Ersatzwert";
|
||||
VAL_ 896 MO3_Vorz_RadWu 0 "positives_Vorzeichen" 1 "negatives_Vorzeichen";
|
||||
VAL_ 896 MO3_Freigabe_Segeln 0 "Segelbetrieb_not_freigegeben" 1 "Segelbetrieb_freigegeben";
|
||||
VAL_ 896 MO_StartStopp_StoppVorbereitung 0 "Motorstopp_not_in_Vorbereitung" 1 "Motorstopp_in_Vorbereitung";
|
||||
CM_ BO_ 896 "Motor message 3 - Contains accelerator pedal, temperatures, and control signals";
|
||||
|
||||
VAL_ 978 LH2_Sta_HCA 0 "disabled" 1 "initializing" 2 "fault" 3 "ready" 4 "rejected" 5 "active" 7 "active";
|
||||
VAL_ 1088 Waehlhebelposition__Getriebe_1_ 8 "P" 7 "R" 6 "N" 5 "D" 9 "U" 12 "S" 14 "T" 10 "T" 11 "T";
|
||||
|
||||
VAL_ 1088 GE1_Schaltung 0 "no_circuit" 1 "circuit_is_running";
|
||||
VAL_ 1088 GE1_Sta_Schutz 0 "no_protection" 1 "protection_requested";
|
||||
VAL_ 1088 GE1_Klimakompr 0 "no_Anforderung" 1 "Kompressor_aus";
|
||||
VAL_ 1088 GE1_WK 0 "WK_open" 1 "WK_regulated" 2 "WK_closed" 3 "Failure";
|
||||
VAL_ 1088 GE1_StSt_Info 0 "Engine_running_not_necessary_stop_release" 1 "Motor_start_not_mandatory_necessary_stop_prohibition" 2 "Motor_start_absolute_necessary_start_request" 3 "System_error";
|
||||
VAL_ 1088 GE1_EGS_Anf 0 "no_requirement" 1 "EGS_requirement";
|
||||
VAL_ 1088 GE1_Zielgang 0 "P (Park) | Disengaged" 1 "Gear 1 | Engaged" 2 "Gear 2 | Engaged" 3 "Gear 3 | Engaged" 4 "Gear 4 | Engaged" 5 "Gear 5 | Engaged" 6 "1m Gear" 7 "Gang_R" 8 "Gear 6 | Engaged" 9 "Gear 7 | Engaged" 10 "Gear 8 | Engaged" 14 "Speed Not Defined" 15 "Failure";
|
||||
VAL_ 1088 GE1_Wahl_Pos 0 "Intermediate Position" 1 "Pos_1" 2 "Pos_2" 3 "Pos_3" 4 "Pos_4" 5 "D (Drive)" 6 "Pos_N" 7 "R (Reverse)" 8 "P (Park)" 9 "Tiptronic Override" 10 "Pos_Z1" 11 "Pos_Z2" 12 "S (Sport)" 13 "Pos_L" 14 "Gas Pedal Force Downshift" 15 "Failure";
|
||||
VAL_ 1088 GE1_Notlauf 0 "No Emergency" 1 "No Switching" 2 "Switch to Neutral" 3 "50% Shifting Capacity" 4 "Driving Without UK" 5 "No Emeregency" 6 "No Emergency" 7 "No Emergency, Target Shifting Reached" 15 "Failure";
|
||||
VAL_ 1088 GE1_Kuehlung 0 "No Additional Cooling Requested" 1 "20% Additional Fan Cooling Requested" 2 "40% Additional Fan Cooling Requested" 3 "Maximum Additional Fan Cooling Requested";
|
||||
VAL_ 1088 GE1_Sta_OBD 0 "MIL Off" 1 "MIL On";
|
||||
VAL_ 1088 GE1_LaunchControl 0 "Launch Control Not Active" 1 "Launch Control Program Active";
|
||||
VAL_ 1088 GE1_Infobit 0 "Standard Automatic" 1 "CVT";
|
||||
VAL_ 1088 GE1_SleepInd 0 "CAN is Required" 1 "Sleep Ready";
|
||||
VAL_ 1088 GE1_Fehlereintr 0 "No Failure Memory Entry" 1 "Failure Memory Entry";
|
||||
|
||||
VAL_ 1152 MO5_Mp_Code 0 "Max_Moment" 1 "Drehzahl" 2 "Motor_Typ" 3 "Abgastyp_od_norm_Verbrauch";
|
||||
VAL_ 1152 MO5_Abgastyp 0 "no_EOBD" 1 "EOBD";
|
||||
VAL_ 1152 MO5_Abgastyp2 0 "no_OBD" 1 "OBD";
|
||||
VAL_ 1152 MO5_Abgastyp3 0 "no_DPF_verbaut" 1 "DPF_verbaut";
|
||||
VAL_ 1152 OBD_Kaltstart_Denominator 0 "Denominator_not_hochzaehlen" 1 "Bedingungen_zum_Inkr_des_Kaltstart_Denom_erf";
|
||||
VAL_ 1152 OBD_Minimum_Trip 0 "no_Min_Normed_Trip" 1 "Min_Normed_Trip";
|
||||
VAL_ 1152 MO5_DPF_reg 0 "DPF_regeneriert_nicht" 1 "DPF_regeneriert";
|
||||
VAL_ 1152 MO5_Vorgluehen 0 "Lamp_off" 1 "Lamp_on";
|
||||
VAL_ 1152 MO5_E_Gas 0 "Lamp_off" 1 "Lamp_on";
|
||||
VAL_ 1152 MO5_OBD_2 0 "Lamp_off" 1 "Lamp_on";
|
||||
VAL_ 1152 MO5_Heissl 0 "Lamp_off" 1 "Lamp_on";
|
||||
VAL_ 1152 MO5_KlimaKompr 0 "no_Anforderung" 1 "Klimakompr_aus";
|
||||
VAL_ 1152 MO5_Feld_kuehl 0 "no" 1 "yes";
|
||||
VAL_ 1152 MO5_KliKo_Red 0 "no" 1 "yes";
|
||||
VAL_ 1152 MO5_UeberlVerb 0 "no_Ueberlauf" 1 "mindestens_einmal_uebergelaufen";
|
||||
VAL_ 1152 MO5_HLeuchte 0 "no_Warnung" 1 "Vorwarnung";
|
||||
VAL_ 1152 MO5_PartikelLamp 0 "Lamp_off" 1 "Lamp_on";
|
||||
VAL_ 1152 MO5_Sta_BKU 0 "iO_oder_not_verbaut" 1 "n_iO";
|
||||
VAL_ 1152 MO5_TypStartSteu 0 "Startersteuerung_BSG_BCM_KessyD1_ZAS" 1 "Startersteuerung_durch_MSG";
|
||||
VAL_ 1152 MO5_TDE_Lampe 0 "Lamp_off" 1 "Lamp_on";
|
||||
VAL_ 1152 MO5_TDE_Text 0 "no_Text" 1 "Textanzeige_nach_ISO";
|
||||
VAL_ 1152 MO5_DZM_Daempf 0 "normale_Daempfung" 1 "dynamische_Daempfung";
|
||||
VAL_ 1152 MO5_Interlock 0 "Interlock_not_betaetigt" 1 "Interlock_betaetigt";
|
||||
VAL_ 1152 MO5_Start 0 "Start_not_zulaessig" 1 "Startfreigabe";
|
||||
VAL_ 1152 MO5_Anlasser 0 "Anlasser_darf_angesteuert_werden" 1 "Anlasser_ausspuren_Ansteuerung_not_moeglich";
|
||||
VAL_ 1152 MO5_GRA_Hauptsch 0 "Off" 1 "On";
|
||||
VAL_ 1152 MO5_Momente 0 "Einfach" 1 "Doppelt";
|
||||
VAL_ 1152 MO5_Motortext1 0 "no_Text" 1 "Text_1";
|
||||
VAL_ 1152 MO5_Motortext2 0 "no_Text" 1 "Text_2";
|
||||
VAL_ 1152 MO5_Motortext3 0 "no_Text" 1 "Text_3";
|
||||
VAL_ 1152 MO5_Motortext4 0 "no_Text" 1 "Text_4";
|
||||
|
||||
VAL_ 1386 ACA_StaACC 6 "ACC_rev_aus" 0 "Hauptschalter_aus" 4 "ACC_im_Hintergrund" 3 "ACC_aktiv" 1 "Reserve" 2 "ACC_passiv" 7 "ACC_irrev_aus" 5 "frei" ;
|
||||
VAL_ 1386 ACA_ID_StaACC 0 "keine_Anzeige" ;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
# constants from panda/python/__init__.py
|
||||
# constants from can.h
|
||||
DLC_TO_LEN = [0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64]
|
||||
LEN_TO_DLC = {length: dlc for (dlc, length) in enumerate(DLC_TO_LEN)}
|
||||
|
||||
|
||||
@@ -1,10 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
// TODO: clean this up. it's for interop with the panda version
|
||||
#ifndef CANPACKET_HEAD_SIZE
|
||||
|
||||
#include "opendbc/safety/board/can_declarations.h"
|
||||
|
||||
static const unsigned char dlc_to_len[] = {0U, 1U, 2U, 3U, 4U, 5U, 6U, 7U, 8U, 12U, 16U, 20U, 24U, 32U, 48U, 64U};
|
||||
|
||||
#endif
|
||||
@@ -1,19 +0,0 @@
|
||||
#pragma once
|
||||
#include "opendbc/safety/board/drivers/can_common_declarations.h"
|
||||
|
||||
uint8_t calculate_checksum(const uint8_t *dat, uint32_t len) {
|
||||
uint8_t checksum = 0U;
|
||||
for (uint32_t i = 0U; i < len; i++) {
|
||||
checksum ^= dat[i];
|
||||
}
|
||||
return checksum;
|
||||
}
|
||||
|
||||
void can_set_checksum(CANPacket_t *packet) {
|
||||
packet->checksum = 0U;
|
||||
packet->checksum = calculate_checksum((uint8_t *) packet, CANPACKET_HEAD_SIZE + GET_LEN(packet));
|
||||
}
|
||||
|
||||
bool can_check_checksum(CANPacket_t *packet) {
|
||||
return (calculate_checksum((uint8_t *) packet, CANPACKET_HEAD_SIZE + GET_LEN(packet)) == 0U);
|
||||
}
|
||||
@@ -1,4 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
uint8_t calculate_checksum(const uint8_t *dat, uint32_t len);
|
||||
void can_set_checksum(CANPacket_t *packet);
|
||||
@@ -1,28 +0,0 @@
|
||||
// minimal code to fake a panda for tests
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "opendbc/safety/board/utils.h"
|
||||
|
||||
#define ALLOW_DEBUG
|
||||
|
||||
void print(const char *a) {
|
||||
printf("%s", a);
|
||||
}
|
||||
|
||||
void puth(unsigned int i) {
|
||||
printf("%u", i);
|
||||
}
|
||||
|
||||
typedef struct {
|
||||
uint32_t CNT;
|
||||
} TIM_TypeDef;
|
||||
|
||||
TIM_TypeDef timer;
|
||||
TIM_TypeDef *MICROSECOND_TIMER = &timer;
|
||||
uint32_t microsecond_timer_get(void);
|
||||
|
||||
uint32_t microsecond_timer_get(void) {
|
||||
return MICROSECOND_TIMER->CNT;
|
||||
}
|
||||
@@ -1,25 +0,0 @@
|
||||
#include "opendbc/safety/board/faults_declarations.h"
|
||||
|
||||
uint8_t fault_status = FAULT_STATUS_NONE;
|
||||
uint32_t faults = 0U;
|
||||
|
||||
void fault_occurred(uint32_t fault) {
|
||||
if ((faults & fault) == 0U) {
|
||||
if ((PERMANENT_FAULTS & fault) != 0U) {
|
||||
print("Permanent fault occurred: 0x"); puth(fault); print("\n");
|
||||
fault_status = FAULT_STATUS_PERMANENT;
|
||||
} else {
|
||||
print("Temporary fault occurred: 0x"); puth(fault); print("\n");
|
||||
fault_status = FAULT_STATUS_TEMPORARY;
|
||||
}
|
||||
}
|
||||
faults |= fault;
|
||||
}
|
||||
|
||||
void fault_recovered(uint32_t fault) {
|
||||
if ((PERMANENT_FAULTS & fault) == 0U) {
|
||||
faults &= ~fault;
|
||||
} else {
|
||||
print("Cannot recover from a permanent fault!\n");
|
||||
}
|
||||
}
|
||||
@@ -1,18 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#define FAULT_STATUS_NONE 0U
|
||||
#define FAULT_STATUS_TEMPORARY 1U
|
||||
#define FAULT_STATUS_PERMANENT 2U
|
||||
|
||||
// Fault types, excerpt from cereal.log.PandaState.FaultType for safety tests
|
||||
#define FAULT_RELAY_MALFUNCTION (1UL << 0)
|
||||
// ...
|
||||
|
||||
// Permanent faults
|
||||
#define PERMANENT_FAULTS 0U
|
||||
|
||||
extern uint8_t fault_status;
|
||||
extern uint32_t faults;
|
||||
|
||||
void fault_occurred(uint32_t fault);
|
||||
void fault_recovered(uint32_t fault);
|
||||
@@ -1,47 +0,0 @@
|
||||
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
|
||||
#define MIN(a, b) ({ \
|
||||
__typeof__(a) _a = (a); \
|
||||
__typeof__(b) _b = (b); \
|
||||
(_a < _b) ? _a : _b; \
|
||||
})
|
||||
|
||||
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
|
||||
#define MAX(a, b) ({ \
|
||||
__typeof__(a) _a = (a); \
|
||||
__typeof__(b) _b = (b); \
|
||||
(_a > _b) ? _a : _b; \
|
||||
})
|
||||
|
||||
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
|
||||
#define CLAMP(x, low, high) ({ \
|
||||
__typeof__(x) __x = (x); \
|
||||
__typeof__(low) __low = (low);\
|
||||
__typeof__(high) __high = (high);\
|
||||
(__x > __high) ? __high : ((__x < __low) ? __low : __x); \
|
||||
})
|
||||
|
||||
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
|
||||
#define ABS(a) ({ \
|
||||
__typeof__(a) _a = (a); \
|
||||
(_a > 0) ? _a : (-_a); \
|
||||
})
|
||||
|
||||
#ifndef NULL
|
||||
// this just provides a standard implementation of NULL
|
||||
// in lieu of including libc in the panda build
|
||||
// cppcheck-suppress [misra-c2012-21.1]
|
||||
#define NULL ((void*)0)
|
||||
#endif
|
||||
|
||||
// STM32 HAL defines this
|
||||
#ifndef UNUSED
|
||||
#define UNUSED(x) ((void)(x))
|
||||
#endif
|
||||
|
||||
#define COMPILE_TIME_ASSERT(pred) ((void)sizeof(char[1 - (2 * (!(pred) ? 1 : 0))]))
|
||||
|
||||
// compute the time elapsed (in microseconds) from 2 counter samples
|
||||
// case where ts < ts_last is ok: overflow is properly re-casted into uint32_t
|
||||
static inline uint32_t get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
|
||||
return ts - ts_last;
|
||||
}
|
||||
@@ -1,8 +1,12 @@
|
||||
#pragma once
|
||||
|
||||
#define CANPACKET_HEAD_SIZE 6U
|
||||
static const unsigned char dlc_to_len[] = {0U, 1U, 2U, 3U, 4U, 5U, 6U, 7U, 8U, 12U, 16U, 20U, 24U, 32U, 48U, 64U};
|
||||
|
||||
#define CANPACKET_HEAD_SIZE 6U // non-data portion of CANPacket_t
|
||||
#define CANPACKET_DATA_SIZE_MAX 64U
|
||||
|
||||
// bump this when changing the CAN packet
|
||||
#define CAN_PACKET_VERSION 4
|
||||
typedef struct {
|
||||
unsigned char fd : 1;
|
||||
unsigned char bus : 3;
|
||||
@@ -26,7 +26,7 @@
|
||||
#define SAFETY_SUBARU_PREGLOBAL 22U
|
||||
#define SAFETY_HYUNDAI_LEGACY 23U
|
||||
#define SAFETY_HYUNDAI_COMMUNITY 24U
|
||||
#define SAFETY_STELLANTIS 25U
|
||||
#define SAFETY_VOLKSWAGEN_MLB 25U
|
||||
#define SAFETY_FAW 26U
|
||||
#define SAFETY_BODY 27U
|
||||
#define SAFETY_HYUNDAI_CANFD 28U
|
||||
@@ -340,6 +340,7 @@ extern const safety_hooks subaru_hooks;
|
||||
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_mqb_hooks;
|
||||
extern const safety_hooks volkswagen_pq_hooks;
|
||||
extern const safety_hooks rivian_hooks;
|
||||
@@ -1,12 +1,47 @@
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
static bool max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) {
|
||||
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
|
||||
#define SAFETY_MIN(a, b) ({ \
|
||||
__typeof__(a) _a = (a); \
|
||||
__typeof__(b) _b = (b); \
|
||||
(_a < _b) ? _a : _b; \
|
||||
})
|
||||
|
||||
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
|
||||
#define SAFETY_MAX(a, b) ({ \
|
||||
__typeof__(a) _a = (a); \
|
||||
__typeof__(b) _b = (b); \
|
||||
(_a > _b) ? _a : _b; \
|
||||
})
|
||||
|
||||
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
|
||||
#define SAFETY_CLAMP(x, low, high) ({ \
|
||||
__typeof__(x) __x = (x); \
|
||||
__typeof__(low) __low = (low);\
|
||||
__typeof__(high) __high = (high);\
|
||||
(__x > __high) ? __high : ((__x < __low) ? __low : __x); \
|
||||
})
|
||||
|
||||
// cppcheck-suppress-macro misra-c2012-1.2; allow __typeof__ extension
|
||||
#define SAFETY_ABS(a) ({ \
|
||||
__typeof__(a) _a = (a); \
|
||||
(_a > 0) ? _a : (-_a); \
|
||||
})
|
||||
|
||||
#define SAFETY_UNUSED(x) ((void)(x))
|
||||
|
||||
// compute the time elapsed (in microseconds) from 2 counter samples
|
||||
// case where ts < ts_last is ok: overflow is properly re-casted into uint32_t
|
||||
static inline uint32_t safety_get_ts_elapsed(uint32_t ts, uint32_t ts_last) {
|
||||
return ts - ts_last;
|
||||
}
|
||||
|
||||
static bool safety_max_limit_check(int val, const int MAX_VAL, const int MIN_VAL) {
|
||||
return (val > MAX_VAL) || (val < MIN_VAL);
|
||||
}
|
||||
|
||||
// interp function that holds extreme values
|
||||
static float interpolate(struct lookup_t xy, float x) {
|
||||
|
||||
static float safety_interpolate(struct lookup_t xy, float x) {
|
||||
int size = sizeof(xy.x) / sizeof(xy.x[0]);
|
||||
float ret = xy.y[size - 1]; // default output is last point
|
||||
|
||||
@@ -23,7 +58,7 @@ static float interpolate(struct lookup_t xy, float x) {
|
||||
float dx = xy.x[i+1] - x0;
|
||||
float dy = xy.y[i+1] - y0;
|
||||
// dx should not be zero as xy.x is supposed to be monotonic
|
||||
dx = MAX(dx, 0.0001);
|
||||
dx = SAFETY_MAX(dx, 0.0001);
|
||||
ret = (dy * (x - x0) / dx) + y0;
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
#include "opendbc/safety/sunnypilot/safety_mads.h"
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/sunnypilot/mads.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// ISO 11270
|
||||
static const float ISO_LATERAL_ACCEL = 3.0; // m/s^2
|
||||
@@ -16,15 +16,15 @@ static bool dist_to_meas_check(int val, int val_last, struct sample_t *val_meas,
|
||||
const int MAX_RATE_UP, const int MAX_RATE_DOWN, const int MAX_ERROR) {
|
||||
|
||||
// *** val rate limit check ***
|
||||
int highest_allowed_rl = MAX(val_last, 0) + MAX_RATE_UP;
|
||||
int lowest_allowed_rl = MIN(val_last, 0) - MAX_RATE_UP;
|
||||
int highest_allowed_rl = SAFETY_MAX(val_last, 0) + MAX_RATE_UP;
|
||||
int lowest_allowed_rl = SAFETY_MIN(val_last, 0) - MAX_RATE_UP;
|
||||
|
||||
// if we've exceeded the meas val, we must start moving toward 0
|
||||
int highest_allowed = MIN(highest_allowed_rl, MAX(val_last - MAX_RATE_DOWN, MAX(val_meas->max, 0) + MAX_ERROR));
|
||||
int lowest_allowed = MAX(lowest_allowed_rl, MIN(val_last + MAX_RATE_DOWN, MIN(val_meas->min, 0) - MAX_ERROR));
|
||||
int highest_allowed = SAFETY_MIN(highest_allowed_rl, SAFETY_MAX(val_last - MAX_RATE_DOWN, SAFETY_MAX(val_meas->max, 0) + MAX_ERROR));
|
||||
int lowest_allowed = SAFETY_MAX(lowest_allowed_rl, SAFETY_MIN(val_last + MAX_RATE_DOWN, SAFETY_MIN(val_meas->min, 0) - MAX_ERROR));
|
||||
|
||||
// check for violation
|
||||
return max_limit_check(val, highest_allowed, lowest_allowed);
|
||||
return safety_max_limit_check(val, highest_allowed, lowest_allowed);
|
||||
}
|
||||
|
||||
// check that commanded value isn't fighting against driver
|
||||
@@ -33,32 +33,32 @@ static bool driver_limit_check(int val, int val_last, const struct sample_t *val
|
||||
const int MAX_ALLOWANCE, const int DRIVER_FACTOR) {
|
||||
|
||||
// torque delta/rate limits
|
||||
int highest_allowed_rl = MAX(val_last, 0) + MAX_RATE_UP;
|
||||
int lowest_allowed_rl = MIN(val_last, 0) - MAX_RATE_UP;
|
||||
int highest_allowed_rl = SAFETY_MAX(val_last, 0) + MAX_RATE_UP;
|
||||
int lowest_allowed_rl = SAFETY_MIN(val_last, 0) - MAX_RATE_UP;
|
||||
|
||||
// driver
|
||||
int driver_max_limit = MAX_VAL + (MAX_ALLOWANCE + val_driver->max) * DRIVER_FACTOR;
|
||||
int driver_min_limit = -MAX_VAL + (-MAX_ALLOWANCE + val_driver->min) * DRIVER_FACTOR;
|
||||
|
||||
// if we've exceeded the applied torque, we must start moving toward 0
|
||||
int highest_allowed = MIN(highest_allowed_rl, MAX(val_last - MAX_RATE_DOWN,
|
||||
MAX(driver_max_limit, 0)));
|
||||
int lowest_allowed = MAX(lowest_allowed_rl, MIN(val_last + MAX_RATE_DOWN,
|
||||
MIN(driver_min_limit, 0)));
|
||||
int highest_allowed = SAFETY_MIN(highest_allowed_rl, SAFETY_MAX(val_last - MAX_RATE_DOWN,
|
||||
SAFETY_MAX(driver_max_limit, 0)));
|
||||
int lowest_allowed = SAFETY_MAX(lowest_allowed_rl, SAFETY_MIN(val_last + MAX_RATE_DOWN,
|
||||
SAFETY_MIN(driver_min_limit, 0)));
|
||||
|
||||
// check for violation
|
||||
return max_limit_check(val, highest_allowed, lowest_allowed);
|
||||
return safety_max_limit_check(val, highest_allowed, lowest_allowed);
|
||||
}
|
||||
|
||||
// real time check, mainly used for steer torque rate limiter
|
||||
static bool rt_torque_rate_limit_check(int val, int val_last, const int MAX_RT_DELTA) {
|
||||
|
||||
// *** torque real time rate limit check ***
|
||||
int highest_val = MAX(val_last, 0) + MAX_RT_DELTA;
|
||||
int lowest_val = MIN(val_last, 0) - MAX_RT_DELTA;
|
||||
int highest_val = SAFETY_MAX(val_last, 0) + MAX_RT_DELTA;
|
||||
int lowest_val = SAFETY_MIN(val_last, 0) - MAX_RT_DELTA;
|
||||
|
||||
// check for violation
|
||||
return max_limit_check(val, highest_val, lowest_val);
|
||||
return safety_max_limit_check(val, highest_val, lowest_val);
|
||||
}
|
||||
|
||||
// Safety checks for torque-based steering commands
|
||||
@@ -71,12 +71,12 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee
|
||||
int max_torque = limits.max_torque;
|
||||
if (limits.dynamic_max_torque) {
|
||||
const float fudged_speed = (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.;
|
||||
max_torque = interpolate(limits.max_torque_lookup, fudged_speed) + 1;
|
||||
max_torque = CLAMP(max_torque, -limits.max_torque, limits.max_torque);
|
||||
max_torque = safety_interpolate(limits.max_torque_lookup, fudged_speed) + 1;
|
||||
max_torque = SAFETY_CLAMP(max_torque, -limits.max_torque, limits.max_torque);
|
||||
}
|
||||
|
||||
// *** global torque limit check ***
|
||||
violation |= max_limit_check(desired_torque, max_torque, -max_torque);
|
||||
violation |= safety_max_limit_check(desired_torque, max_torque, -max_torque);
|
||||
|
||||
// *** torque rate limit check ***
|
||||
if (limits.type == TorqueDriverLimited) {
|
||||
@@ -93,7 +93,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee
|
||||
violation |= rt_torque_rate_limit_check(desired_torque, rt_torque_last, limits.max_rt_delta);
|
||||
|
||||
// every RT_INTERVAL set the new limits
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_torque_check_last);
|
||||
uint32_t ts_elapsed = safety_get_ts_elapsed(ts, ts_torque_check_last);
|
||||
if (ts_elapsed > MAX_RT_INTERVAL) {
|
||||
rt_torque_last = desired_torque;
|
||||
ts_torque_check_last = ts;
|
||||
@@ -122,7 +122,7 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee
|
||||
}
|
||||
|
||||
// or we've cut torque too recently in time
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_steer_req_mismatch_last);
|
||||
uint32_t ts_elapsed = safety_get_ts_elapsed(ts, ts_steer_req_mismatch_last);
|
||||
if (ts_elapsed < limits.min_valid_request_rt_interval) {
|
||||
violation = true;
|
||||
}
|
||||
@@ -135,9 +135,9 @@ bool steer_torque_cmd_checks(int desired_torque, int steer_req, const TorqueStee
|
||||
|
||||
valid_steer_req_count = 0;
|
||||
ts_steer_req_mismatch_last = ts;
|
||||
invalid_steer_req_count = MIN(invalid_steer_req_count + 1, limits.max_invalid_request_frames);
|
||||
invalid_steer_req_count = SAFETY_MIN(invalid_steer_req_count + 1, limits.max_invalid_request_frames);
|
||||
} else {
|
||||
valid_steer_req_count = MIN(valid_steer_req_count + 1, limits.min_valid_request_frames);
|
||||
valid_steer_req_count = SAFETY_MIN(valid_steer_req_count + 1, limits.min_valid_request_frames);
|
||||
invalid_steer_req_count = 0;
|
||||
}
|
||||
}
|
||||
@@ -168,7 +168,7 @@ static bool rt_angle_rate_limit_check(AngleSteeringLimits limits) {
|
||||
rt_angle_msgs += 1U;
|
||||
|
||||
// every RT_INTERVAL reset message counter
|
||||
uint32_t ts_elapsed = get_ts_elapsed(ts, ts_angle_check_last);
|
||||
uint32_t ts_elapsed = safety_get_ts_elapsed(ts, ts_angle_check_last);
|
||||
if (ts_elapsed >= MAX_RT_INTERVAL) {
|
||||
rt_angle_msgs = 0;
|
||||
ts_angle_check_last = ts;
|
||||
@@ -187,8 +187,8 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
|
||||
// always slightly above openpilot's in case we read an updated speed in between angle commands
|
||||
// TODO: this speed fudge can be much lower, look at data to determine the lowest reasonable offset
|
||||
const float fudged_speed = (vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.;
|
||||
int delta_angle_up = (interpolate(limits.angle_rate_up_lookup, fudged_speed) * limits.angle_deg_to_can) + 1.;
|
||||
int delta_angle_down = (interpolate(limits.angle_rate_down_lookup, fudged_speed) * limits.angle_deg_to_can) + 1.;
|
||||
int delta_angle_up = (safety_interpolate(limits.angle_rate_up_lookup, fudged_speed) * limits.angle_deg_to_can) + 1.;
|
||||
int delta_angle_down = (safety_interpolate(limits.angle_rate_down_lookup, fudged_speed) * limits.angle_deg_to_can) + 1.;
|
||||
|
||||
// allow down limits at zero since small floats from openpilot will be rounded to 0
|
||||
// TODO: openpilot should be cognizant of this and not send small floats
|
||||
@@ -200,8 +200,8 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
|
||||
if (limits.enforce_angle_error && ((vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR) > limits.angle_error_min_speed)) {
|
||||
// flipped fudge to avoid false positives
|
||||
const float fudged_speed_error = (vehicle_speed.max / VEHICLE_SPEED_FACTOR) + 1.;
|
||||
const int delta_angle_up_relaxed = (interpolate(limits.angle_rate_up_lookup, fudged_speed_error) * limits.angle_deg_to_can) - 1.;
|
||||
const int delta_angle_down_relaxed = (interpolate(limits.angle_rate_down_lookup, fudged_speed_error) * limits.angle_deg_to_can) - 1.;
|
||||
const int delta_angle_up_relaxed = (safety_interpolate(limits.angle_rate_up_lookup, fudged_speed_error) * limits.angle_deg_to_can) - 1.;
|
||||
const int delta_angle_down_relaxed = (safety_interpolate(limits.angle_rate_down_lookup, fudged_speed_error) * limits.angle_deg_to_can) - 1.;
|
||||
|
||||
// the minimum and maximum angle allowed based on the measured angle
|
||||
const int lowest_desired_angle_error = angle_meas.min - limits.max_angle_error - 1;
|
||||
@@ -210,22 +210,22 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
|
||||
// the MAX is to allow the desired angle to hit the edge of the bounds and not require going under it
|
||||
if (desired_angle_last > highest_desired_angle_error) {
|
||||
const int delta = (desired_angle_last >= 0) ? delta_angle_down_relaxed : delta_angle_up_relaxed;
|
||||
highest_desired_angle = MAX(desired_angle_last - delta, highest_desired_angle_error);
|
||||
highest_desired_angle = SAFETY_MAX(desired_angle_last - delta, highest_desired_angle_error);
|
||||
|
||||
} else if (desired_angle_last < lowest_desired_angle_error) {
|
||||
const int delta = (desired_angle_last <= 0) ? delta_angle_down_relaxed : delta_angle_up_relaxed;
|
||||
lowest_desired_angle = MIN(desired_angle_last + delta, lowest_desired_angle_error);
|
||||
lowest_desired_angle = SAFETY_MIN(desired_angle_last + delta, lowest_desired_angle_error);
|
||||
|
||||
} else {
|
||||
// already inside error boundary, don't allow commanding outside it
|
||||
highest_desired_angle = MIN(highest_desired_angle, highest_desired_angle_error);
|
||||
lowest_desired_angle = MAX(lowest_desired_angle, lowest_desired_angle_error);
|
||||
highest_desired_angle = SAFETY_MIN(highest_desired_angle, highest_desired_angle_error);
|
||||
lowest_desired_angle = SAFETY_MAX(lowest_desired_angle, lowest_desired_angle_error);
|
||||
}
|
||||
|
||||
// don't enforce above the max steer
|
||||
// TODO: this should always be done
|
||||
lowest_desired_angle = CLAMP(lowest_desired_angle, -limits.max_angle, limits.max_angle);
|
||||
highest_desired_angle = CLAMP(highest_desired_angle, -limits.max_angle, limits.max_angle);
|
||||
lowest_desired_angle = SAFETY_CLAMP(lowest_desired_angle, -limits.max_angle, limits.max_angle);
|
||||
highest_desired_angle = SAFETY_CLAMP(highest_desired_angle, -limits.max_angle, limits.max_angle);
|
||||
}
|
||||
|
||||
// check not above ISO 11270 lateral accel assuming worst case road roll
|
||||
@@ -235,23 +235,23 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
|
||||
static const float MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - (EARTH_G * AVERAGE_ROAD_ROLL); // ~2.4 m/s^2
|
||||
|
||||
// Allow small tolerance by using minimum speed and rounding curvature up
|
||||
const float speed_lower = MAX(vehicle_speed.min / VEHICLE_SPEED_FACTOR, 1.0);
|
||||
const float speed_upper = MAX(vehicle_speed.max / VEHICLE_SPEED_FACTOR, 1.0);
|
||||
const float speed_lower = SAFETY_MAX(vehicle_speed.min / VEHICLE_SPEED_FACTOR, 1.0);
|
||||
const float speed_upper = SAFETY_MAX(vehicle_speed.max / VEHICLE_SPEED_FACTOR, 1.0);
|
||||
const int max_curvature_upper = (MAX_LATERAL_ACCEL / (speed_lower * speed_lower) * limits.angle_deg_to_can) + 1.;
|
||||
const int max_curvature_lower = (MAX_LATERAL_ACCEL / (speed_upper * speed_upper) * limits.angle_deg_to_can) - 1.;
|
||||
|
||||
// ensure that the curvature error doesn't try to enforce above this limit
|
||||
if (desired_angle_last > 0) {
|
||||
lowest_desired_angle = CLAMP(lowest_desired_angle, -max_curvature_lower, max_curvature_lower);
|
||||
highest_desired_angle = CLAMP(highest_desired_angle, -max_curvature_upper, max_curvature_upper);
|
||||
lowest_desired_angle = SAFETY_CLAMP(lowest_desired_angle, -max_curvature_lower, max_curvature_lower);
|
||||
highest_desired_angle = SAFETY_CLAMP(highest_desired_angle, -max_curvature_upper, max_curvature_upper);
|
||||
} else {
|
||||
lowest_desired_angle = CLAMP(lowest_desired_angle, -max_curvature_upper, max_curvature_upper);
|
||||
highest_desired_angle = CLAMP(highest_desired_angle, -max_curvature_lower, max_curvature_lower);
|
||||
lowest_desired_angle = SAFETY_CLAMP(lowest_desired_angle, -max_curvature_upper, max_curvature_upper);
|
||||
highest_desired_angle = SAFETY_CLAMP(highest_desired_angle, -max_curvature_lower, max_curvature_lower);
|
||||
}
|
||||
}
|
||||
|
||||
// check for violation;
|
||||
violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
|
||||
violation |= safety_max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
|
||||
}
|
||||
desired_angle_last = desired_angle;
|
||||
|
||||
@@ -260,9 +260,9 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
|
||||
if (limits.inactive_angle_is_zero) {
|
||||
violation |= desired_angle != 0;
|
||||
} else {
|
||||
const int max_inactive_angle = CLAMP(angle_meas.max, -limits.max_angle, limits.max_angle) + 1;
|
||||
const int min_inactive_angle = CLAMP(angle_meas.min, -limits.max_angle, limits.max_angle) - 1;
|
||||
violation |= max_limit_check(desired_angle, max_inactive_angle, min_inactive_angle);
|
||||
const int max_inactive_angle = SAFETY_CLAMP(angle_meas.max, -limits.max_angle, limits.max_angle) + 1;
|
||||
const int min_inactive_angle = SAFETY_CLAMP(angle_meas.min, -limits.max_angle, limits.max_angle) - 1;
|
||||
violation |= safety_max_limit_check(desired_angle, max_inactive_angle, min_inactive_angle);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -276,7 +276,7 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
|
||||
if (limits.inactive_angle_is_zero) {
|
||||
desired_angle_last = 0;
|
||||
} else {
|
||||
desired_angle_last = CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
|
||||
desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -304,7 +304,7 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co
|
||||
// Lower than ISO 11270 lateral jerk limit, which is 5.0 m/s^3
|
||||
static const float MAX_LATERAL_JERK = 3.0 + (EARTH_G * AVERAGE_ROAD_ROLL); // ~3.6 m/s^3
|
||||
|
||||
const float fudged_speed = MAX((vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0, 1.0);
|
||||
const float fudged_speed = SAFETY_MAX((vehicle_speed.min / VEHICLE_SPEED_FACTOR) - 1.0, 1.0);
|
||||
const float curvature_factor = get_curvature_factor(fudged_speed, params);
|
||||
|
||||
bool violation = false;
|
||||
@@ -323,14 +323,14 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co
|
||||
const int highest_desired_angle = desired_angle_last + max_angle_delta_can;
|
||||
const int lowest_desired_angle = desired_angle_last - max_angle_delta_can;
|
||||
|
||||
violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
|
||||
violation |= safety_max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
|
||||
|
||||
// *** ISO lateral accel limit ***
|
||||
const float max_curvature = MAX_LATERAL_ACCEL / (fudged_speed * fudged_speed);
|
||||
const float max_angle = get_angle_from_curvature(max_curvature, curvature_factor, params);
|
||||
const int max_angle_can = (max_angle * limits.angle_deg_to_can) + 1.;
|
||||
|
||||
violation |= max_limit_check(desired_angle, max_angle_can, -max_angle_can);
|
||||
violation |= safety_max_limit_check(desired_angle, max_angle_can, -max_angle_can);
|
||||
|
||||
// *** angle real time rate limit check ***
|
||||
violation |= rt_angle_rate_limit_check(limits);
|
||||
@@ -339,9 +339,9 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co
|
||||
|
||||
// Angle should either be 0 or same as current angle while not steering
|
||||
if (!steer_control_enabled) {
|
||||
const int max_inactive_angle = CLAMP(angle_meas.max, -limits.max_angle, limits.max_angle) + 1;
|
||||
const int min_inactive_angle = CLAMP(angle_meas.min, -limits.max_angle, limits.max_angle) - 1;
|
||||
violation |= max_limit_check(desired_angle, max_inactive_angle, min_inactive_angle);
|
||||
const int max_inactive_angle = SAFETY_CLAMP(angle_meas.max, -limits.max_angle, limits.max_angle) + 1;
|
||||
const int min_inactive_angle = SAFETY_CLAMP(angle_meas.min, -limits.max_angle, limits.max_angle) - 1;
|
||||
violation |= safety_max_limit_check(desired_angle, max_inactive_angle, min_inactive_angle);
|
||||
}
|
||||
|
||||
// No angle control allowed when controls are not allowed
|
||||
@@ -351,7 +351,7 @@ bool steer_angle_cmd_checks_vm(int desired_angle, bool steer_control_enabled, co
|
||||
|
||||
// reset to current angle if either controls is not allowed or there's a violation
|
||||
if (violation || !is_lat_active()) {
|
||||
desired_angle_last = CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
|
||||
desired_angle_last = SAFETY_CLAMP(angle_meas.values[0], -limits.max_angle, limits.max_angle);
|
||||
}
|
||||
|
||||
return violation;
|
||||
|
||||
@@ -1,4 +1,4 @@
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
bool get_longitudinal_allowed(void) {
|
||||
return controls_allowed && !gas_pressed_prev;
|
||||
@@ -6,7 +6,7 @@ bool get_longitudinal_allowed(void) {
|
||||
|
||||
// Safety checks for longitudinal actuation
|
||||
bool longitudinal_accel_checks(int desired_accel, const LongitudinalLimits limits) {
|
||||
bool accel_valid = get_longitudinal_allowed() && !max_limit_check(desired_accel, limits.max_accel, limits.min_accel);
|
||||
bool accel_valid = get_longitudinal_allowed() && !safety_max_limit_check(desired_accel, limits.max_accel, limits.min_accel);
|
||||
bool accel_inactive = desired_accel == limits.inactive_accel;
|
||||
return !(accel_valid || accel_inactive);
|
||||
}
|
||||
@@ -16,13 +16,13 @@ bool longitudinal_speed_checks(int desired_speed, const LongitudinalLimits limit
|
||||
}
|
||||
|
||||
bool longitudinal_transmission_rpm_checks(int desired_transmission_rpm, const LongitudinalLimits limits) {
|
||||
bool transmission_rpm_valid = get_longitudinal_allowed() && !max_limit_check(desired_transmission_rpm, limits.max_transmission_rpm, limits.min_transmission_rpm);
|
||||
bool transmission_rpm_valid = get_longitudinal_allowed() && !safety_max_limit_check(desired_transmission_rpm, limits.max_transmission_rpm, limits.min_transmission_rpm);
|
||||
bool transmission_rpm_inactive = desired_transmission_rpm == limits.inactive_transmission_rpm;
|
||||
return !(transmission_rpm_valid || transmission_rpm_inactive);
|
||||
}
|
||||
|
||||
bool longitudinal_gas_checks(int desired_gas, const LongitudinalLimits limits) {
|
||||
bool gas_valid = get_longitudinal_allowed() && !max_limit_check(desired_gas, limits.max_gas, limits.min_gas);
|
||||
bool gas_valid = get_longitudinal_allowed() && !safety_max_limit_check(desired_gas, limits.max_gas, limits.min_gas);
|
||||
bool gas_inactive = desired_gas == limits.inactive_gas;
|
||||
return !(gas_valid || gas_inactive);
|
||||
}
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
static void body_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->addr == 0x201U) {
|
||||
@@ -32,7 +32,7 @@ static safety_config body_init(uint16_t param) {
|
||||
static const CanMsg BODY_TX_MSGS[] = {{0x250, 0, 8, .check_relay = false}, {0x250, 0, 6, .check_relay = false}, {0x251, 0, 5, .check_relay = false}, // body
|
||||
{0x1, 0, 8, .check_relay = false}}; // CAN flasher
|
||||
|
||||
UNUSED(param);
|
||||
SAFETY_UNUSED(param);
|
||||
safety_config ret = BUILD_SAFETY_CFG(body_rx_checks, BODY_TX_MSGS);
|
||||
ret.disable_forwarding = true;
|
||||
return ret;
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
typedef struct {
|
||||
const unsigned int EPS_2;
|
||||
|
||||
@@ -1,25 +1,25 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// GCOV_EXCL_START
|
||||
// Unreachable by design (doesn't define any rx msgs)
|
||||
void default_rx_hook(const CANPacket_t *msg) {
|
||||
UNUSED(msg);
|
||||
SAFETY_UNUSED(msg);
|
||||
}
|
||||
// GCOV_EXCL_STOP
|
||||
|
||||
// *** no output safety mode ***
|
||||
|
||||
static safety_config nooutput_init(uint16_t param) {
|
||||
UNUSED(param);
|
||||
SAFETY_UNUSED(param);
|
||||
return (safety_config){NULL, 0, NULL, 0, true}; // NOLINT(readability/braces)
|
||||
}
|
||||
|
||||
// GCOV_EXCL_START
|
||||
// Unreachable by design (doesn't define any tx msgs)
|
||||
static bool nooutput_tx_hook(const CANPacket_t *msg) {
|
||||
UNUSED(msg);
|
||||
SAFETY_UNUSED(msg);
|
||||
return false;
|
||||
}
|
||||
// GCOV_EXCL_STOP
|
||||
@@ -40,7 +40,7 @@ static safety_config alloutput_init(uint16_t param) {
|
||||
}
|
||||
|
||||
static bool alloutput_tx_hook(const CANPacket_t *msg) {
|
||||
UNUSED(msg);
|
||||
SAFETY_UNUSED(msg);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/defaults.h"
|
||||
|
||||
static bool elm327_tx_hook(const CANPacket_t *msg) {
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// Safety-relevant CAN messages for Ford vehicles.
|
||||
#define FORD_EngBrakeData 0x165U // RX from PCM, for driver brake pedal and cruise state
|
||||
@@ -137,7 +137,7 @@ static void ford_rx_hook(const CANPacket_t *msg) {
|
||||
// Signal: VehYaw_W_Actl
|
||||
// TODO: we should use the speed which results in the closest angle measurement to the desired angle
|
||||
float ford_yaw_rate = (((msg->data[2] << 8U) | msg->data[3]) * 0.0002) - 6.5;
|
||||
float current_curvature = ford_yaw_rate / MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1);
|
||||
float current_curvature = ford_yaw_rate / SAFETY_MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1);
|
||||
// convert current curvature into units on CAN for comparison with desired curvature
|
||||
update_sample(&angle_meas, ROUND(current_curvature * FORD_STEERING_LIMITS.angle_deg_to_can));
|
||||
}
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// TODO: do checksum and counter checks. Add correct timestep, 0.1s for now.
|
||||
#define GM_COMMON_RX_CHECKS \
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// All common address checks except SCM_BUTTONS which isn't on one Nidec safety configuration
|
||||
#define HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(pt_bus) \
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/hyundai_common.h"
|
||||
|
||||
#define HYUNDAI_LIMITS(steer, rate_up, rate_down) { \
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/hyundai_common.h"
|
||||
|
||||
#define HYUNDAI_CANFD_CRUISE_BUTTON_TX_MSGS(bus) \
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
extern uint16_t hyundai_canfd_crc_lut[256];
|
||||
uint16_t hyundai_canfd_crc_lut[256];
|
||||
@@ -127,7 +127,7 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai
|
||||
if ((cruise_button == HYUNDAI_BTN_RESUME) || (cruise_button == HYUNDAI_BTN_SET) || (cruise_button == HYUNDAI_BTN_CANCEL) || main_button) {
|
||||
hyundai_last_button_interaction = 0U;
|
||||
} else {
|
||||
hyundai_last_button_interaction = MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES);
|
||||
hyundai_last_button_interaction = SAFETY_MIN(hyundai_last_button_interaction + 1U, HYUNDAI_PREV_BUTTON_SAMPLES);
|
||||
}
|
||||
|
||||
if (hyundai_longitudinal) {
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// CAN msgs we care about
|
||||
#define MAZDA_LKAS 0x243U
|
||||
@@ -95,7 +95,7 @@ static safety_config mazda_init(uint16_t param) {
|
||||
{.msg = {{MAZDA_PEDALS, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
UNUSED(param);
|
||||
SAFETY_UNUSED(param);
|
||||
return BUILD_SAFETY_CFG(mazda_rx_checks, MAZDA_TX_MSGS);
|
||||
}
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
#define NISSAN_COMMON_RX_CHECKS \
|
||||
{.msg = {{0x2, 0, 5, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, \
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
#define PSA_STEERING 757U // RX from XXX, driver torque
|
||||
#define PSA_STEERING_ALT 773U // RX from EPS, steering angle
|
||||
@@ -124,7 +124,7 @@ static bool psa_tx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
|
||||
static safety_config psa_init(uint16_t param) {
|
||||
UNUSED(param);
|
||||
SAFETY_UNUSED(param);
|
||||
static const CanMsg PSA_TX_MSGS[] = {
|
||||
{PSA_LANE_KEEP_ASSIST, PSA_MAIN_BUS, 8, .check_relay = true}, // EPS steering
|
||||
};
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
static uint8_t rivian_get_counter(const CANPacket_t *msg) {
|
||||
uint8_t cnt = 0;
|
||||
@@ -167,7 +167,7 @@ static safety_config rivian_init(uint16_t param) {
|
||||
|
||||
bool rivian_longitudinal = false;
|
||||
|
||||
UNUSED(param);
|
||||
SAFETY_UNUSED(param);
|
||||
#ifdef ALLOW_DEBUG
|
||||
const int FLAG_RIVIAN_LONG_CONTROL = 1;
|
||||
rivian_longitudinal = GET_FLAG(param, FLAG_RIVIAN_LONG_CONTROL);
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/subaru_common.h"
|
||||
|
||||
#define SUBARU_STEERING_LIMITS_GENERATOR(steer_max, rate_up, rate_down) \
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/subaru_common.h"
|
||||
|
||||
// Preglobal platform
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
#define TESLA_COMMON_RX_CHECKS \
|
||||
{.msg = {{0x2b9, 2, 8, 25U, .max_counter = 7U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, /* DAS_control */ \
|
||||
@@ -354,7 +354,7 @@ static safety_config tesla_init(uint16_t param) {
|
||||
{0x27D, 0, 3, .check_relay = true, .disable_static_blocking = true}, // APS_eacMonitor
|
||||
};
|
||||
|
||||
UNUSED(param);
|
||||
SAFETY_UNUSED(param);
|
||||
#ifdef ALLOW_DEBUG
|
||||
const int TESLA_FLAG_LONGITUDINAL_CONTROL = 1;
|
||||
tesla_longitudinal = GET_FLAG(param, TESLA_FLAG_LONGITUDINAL_CONTROL);
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
|
||||
// Stock longitudinal
|
||||
#define TOYOTA_BASE_TX_MSGS \
|
||||
@@ -14,7 +14,7 @@
|
||||
#define TOYOTA_COMMON_SECOC_TX_MSGS \
|
||||
TOYOTA_BASE_TX_MSGS \
|
||||
{0x2E4, 0, 8, .check_relay = true}, {0x131, 0, 8, .check_relay = true}, \
|
||||
{0x343, 0, 8, .check_relay = false}, /* ACC cancel cmd */ \
|
||||
{0x343, 0, 8, .check_relay = false}, /* ACC cancel cmd */ \
|
||||
|
||||
#define TOYOTA_COMMON_LONG_TX_MSGS \
|
||||
TOYOTA_COMMON_TX_MSGS \
|
||||
@@ -31,6 +31,11 @@
|
||||
/* ACC */ \
|
||||
{0x343, 0, 8, .check_relay = true}, \
|
||||
|
||||
#define TOYOTA_COMMON_SECOC_LONG_TX_MSGS \
|
||||
TOYOTA_COMMON_SECOC_TX_MSGS \
|
||||
{0x343, 0, 8, .check_relay = true}, \
|
||||
{0x183, 0, 8, .check_relay = true}, /* ACC_CONTROL_2 */ \
|
||||
|
||||
#define TOYOTA_COMMON_SECOC_LONG_TX_MSGS \
|
||||
TOYOTA_COMMON_SECOC_TX_MSGS \
|
||||
{0x343, 0, 8, .check_relay = true}, /* ACC */ \
|
||||
@@ -260,6 +265,10 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
|
||||
int desired_accel = toyota_get_longitudinal_desired_accel_tx(msg);
|
||||
|
||||
bool violation = false;
|
||||
if (toyota_secoc) {
|
||||
// SecOC cars move accel to 0x183. Only allow inactive accel on 0x343 to match stock behavior
|
||||
violation = desired_accel != TOYOTA_LONG_LIMITS.inactive_accel;
|
||||
}
|
||||
violation |= longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS);
|
||||
|
||||
// only ACC messages that cancel are allowed when openpilot is not controlling longitudinal
|
||||
@@ -294,6 +303,13 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
}
|
||||
|
||||
if (msg->addr == 0x183U) {
|
||||
int desired_accel = (msg->data[0] << 8) | msg->data[1];
|
||||
desired_accel = to_signed(desired_accel, 16);
|
||||
|
||||
tx = !longitudinal_accel_checks(desired_accel, TOYOTA_LONG_LIMITS);
|
||||
}
|
||||
|
||||
// AEB: block all actuation. only used when DSU is unplugged
|
||||
if (msg->addr == 0x283U) {
|
||||
// only allow the checksum, which is the last byte
|
||||
@@ -339,12 +355,12 @@ static bool toyota_tx_hook(const CANPacket_t *msg) {
|
||||
}
|
||||
|
||||
// check if we should wind down torque
|
||||
int driver_torque = MIN(ABS(torque_driver.min), ABS(torque_driver.max));
|
||||
int driver_torque = SAFETY_MIN(SAFETY_ABS(torque_driver.min), SAFETY_ABS(torque_driver.max));
|
||||
if ((driver_torque > TOYOTA_LTA_MAX_DRIVER_TORQUE) && (torque_wind_down != 0)) {
|
||||
tx = false;
|
||||
}
|
||||
|
||||
int eps_torque = MIN(ABS(torque_meas.min), ABS(torque_meas.max));
|
||||
int eps_torque = SAFETY_MIN(SAFETY_ABS(torque_meas.min), SAFETY_ABS(torque_meas.max));
|
||||
if ((eps_torque > TOYOTA_LTA_MAX_MEAS_TORQUE) && (torque_wind_down != 0)) {
|
||||
tx = false;
|
||||
}
|
||||
@@ -424,6 +440,10 @@ static safety_config toyota_init(uint16_t param) {
|
||||
{0x200, 0, 6, .check_relay = false}, // gas interceptor
|
||||
};
|
||||
|
||||
static const CanMsg TOYOTA_SECOC_LONG_TX_MSGS[] = {
|
||||
TOYOTA_COMMON_SECOC_LONG_TX_MSGS
|
||||
};
|
||||
|
||||
// safety param flags
|
||||
// first byte is for EPS factor, second is for flags
|
||||
const uint32_t TOYOTA_PARAM_OFFSET = 8U;
|
||||
|
||||
@@ -14,6 +14,10 @@ bool volkswagen_set_button_prev = false;
|
||||
extern bool volkswagen_resume_button_prev;
|
||||
bool volkswagen_resume_button_prev = false;
|
||||
|
||||
extern bool volkswagen_brake_pedal_switch;
|
||||
extern bool volkswagen_brake_pressure_detected;
|
||||
bool volkswagen_brake_pedal_switch = false;
|
||||
bool volkswagen_brake_pressure_detected = false;
|
||||
|
||||
#define MSG_LH_EPS_03 0x09FU // RX from EPS, for driver steering torque
|
||||
#define MSG_ESP_19 0x0B2U // RX from ABS, for wheel speeds
|
||||
@@ -28,6 +32,21 @@ bool volkswagen_resume_button_prev = false;
|
||||
#define MSG_LDW_02 0x397U // TX by OP, Lane line recognition and text alerts
|
||||
#define MSG_MOTOR_14 0x3BEU // RX from ECU, for brake switch status
|
||||
|
||||
// MLB only messages
|
||||
#define MSG_ESP_03 0x103U // RX from ABS, for wheel speeds
|
||||
#define MSG_LS_01 0x10BU // TX by OP, ACC control buttons for cancel/resume
|
||||
#define MSG_MOTOR_03 0x105U // RX from ECU, for driver throttle input and brake switch status
|
||||
#define MSG_TSK_02 0x10CU // RX from ECU, for ACC status from drivetrain coordinator
|
||||
#define MSG_ACC_05 0x10DU // RX from radar, for ACC status
|
||||
|
||||
static void volkswagen_common_init(void) {
|
||||
volkswagen_set_button_prev = false;
|
||||
volkswagen_resume_button_prev = false;
|
||||
volkswagen_brake_pedal_switch = false;
|
||||
volkswagen_brake_pressure_detected = false;
|
||||
gen_crc_lookup_table_8(0x2F, volkswagen_crc8_lut_8h2f);
|
||||
return;
|
||||
}
|
||||
|
||||
static uint32_t volkswagen_mqb_meb_get_checksum(const CANPacket_t *msg) {
|
||||
return (uint8_t)msg->data[0];
|
||||
@@ -68,3 +87,25 @@ static uint32_t volkswagen_mqb_meb_compute_crc(const CANPacket_t *msg) {
|
||||
|
||||
return (uint8_t)(crc ^ 0xFFU);
|
||||
}
|
||||
|
||||
static int volkswagen_mlb_mqb_driver_input_torque(const CANPacket_t *msg) {
|
||||
// Signal: LH_EPS_03.EPS_Lenkmoment (absolute torque)
|
||||
// Signal: LH_EPS_03.EPS_VZ_Lenkmoment (direction)
|
||||
int torque_driver_new = msg->data[5] | ((msg->data[6] & 0x1FU) << 8);
|
||||
bool sign = GET_BIT(msg, 55U);
|
||||
if (sign) {
|
||||
torque_driver_new *= -1;
|
||||
}
|
||||
return torque_driver_new;
|
||||
}
|
||||
|
||||
static int volkswagen_mlb_mqb_steering_control_torque(const CANPacket_t *msg) {
|
||||
// Signal: HCA_01.HCA_01_LM_Offset (absolute torque)
|
||||
// Signal: HCA_01.HCA_01_LM_OffSign (direction)
|
||||
int desired_torque = msg->data[2] | ((msg->data[3] & 0x1U) << 8);
|
||||
bool sign = GET_BIT(msg, 31U);
|
||||
if (sign) {
|
||||
desired_torque *= -1;
|
||||
}
|
||||
return desired_torque;
|
||||
}
|
||||
|
||||
134
opendbc/safety/modes/volkswagen_mlb.h
Normal file
134
opendbc/safety/modes/volkswagen_mlb.h
Normal file
@@ -0,0 +1,134 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/volkswagen_common.h"
|
||||
|
||||
|
||||
static safety_config volkswagen_mlb_init(uint16_t param) {
|
||||
// Transmit of LS_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
|
||||
static const CanMsg VOLKSWAGEN_MLB_STOCK_TX_MSGS[] = {{MSG_HCA_01, 0, 8, .check_relay = true}, {MSG_LDW_02, 0, 8, .check_relay = true},
|
||||
{MSG_LS_01, 0, 4, .check_relay = false}, {MSG_LS_01, 2, 4, .check_relay = false}};
|
||||
|
||||
static RxCheck volkswagen_mlb_rx_checks[] = {
|
||||
// TODO: implement checksum validation
|
||||
{.msg = {{MSG_ESP_03, 0, 8, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_LH_EPS_03, 0, 8, 100U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_ESP_05, 0, 8, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_ACC_05, 2, 8, 50U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_MOTOR_03, 0, 8, 100U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
{.msg = {{MSG_LS_01, 0, 4, 10U, .ignore_checksum = true, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
SAFETY_UNUSED(param);
|
||||
volkswagen_common_init();
|
||||
|
||||
return BUILD_SAFETY_CFG(volkswagen_mlb_rx_checks, VOLKSWAGEN_MLB_STOCK_TX_MSGS);
|
||||
}
|
||||
|
||||
static void volkswagen_mlb_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->bus == 0U) {
|
||||
// Check all wheel speeds for any movement
|
||||
// Signals: ESP_03.ESP_[VL|VR|HL|HR]_Radgeschw
|
||||
if (msg->addr == MSG_ESP_03) {
|
||||
uint32_t speed = 0;
|
||||
speed += ((msg->data[3] & 0xFU) << 8) | msg->data[2]; // FL
|
||||
speed += (msg->data[4] << 4) | (msg->data[3] >> 4); // FR
|
||||
speed += ((msg->data[6] & 0xFU) << 8) | msg->data[5]; // RL
|
||||
speed += (msg->data[7] << 4) | (msg->data[6] >> 4); // RR
|
||||
vehicle_moving = speed > 0U;
|
||||
}
|
||||
|
||||
// Update driver input torque
|
||||
if (msg->addr == MSG_LH_EPS_03) {
|
||||
update_sample(&torque_driver, volkswagen_mlb_mqb_driver_input_torque(msg));
|
||||
}
|
||||
|
||||
if (msg->addr == MSG_LS_01) {
|
||||
// Always exit controls on rising edge of Cancel
|
||||
// Signal: LS_01.LS_Abbrechen
|
||||
if (GET_BIT(msg, 13U)) {
|
||||
controls_allowed = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Signal: Motor_03.MO_Fahrpedalrohwert_01
|
||||
// Signal: Motor_03.MO_Fahrer_bremst
|
||||
if (msg->addr == MSG_MOTOR_03) {
|
||||
gas_pressed = msg->data[6] != 0U;
|
||||
volkswagen_brake_pedal_switch = GET_BIT(msg, 35U);
|
||||
}
|
||||
|
||||
if (msg->addr == MSG_ESP_05) {
|
||||
volkswagen_brake_pressure_detected = GET_BIT(msg, 26U);
|
||||
}
|
||||
|
||||
brake_pressed = volkswagen_brake_pedal_switch || volkswagen_brake_pressure_detected;
|
||||
|
||||
}
|
||||
|
||||
if (msg->bus == 2U) {
|
||||
// TODO: See if there's a bus-agnostic TSK message we can use instead
|
||||
if (msg->addr == MSG_ACC_05) {
|
||||
// When using stock ACC, enter controls on rising edge of stock ACC engage, exit on disengage
|
||||
// Always exit controls on main switch off
|
||||
// Signal: ACC_05.ACC_Status_ACC
|
||||
int acc_status = (msg->data[7] & 0xEU) >> 1;
|
||||
bool cruise_engaged = (acc_status == 3) || (acc_status == 4) || (acc_status == 5);
|
||||
acc_main_on = cruise_engaged || (acc_status == 2);
|
||||
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
|
||||
if (!acc_main_on) {
|
||||
controls_allowed = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool volkswagen_mlb_tx_hook(const CANPacket_t *msg) {
|
||||
// lateral limits
|
||||
const TorqueSteeringLimits VOLKSWAGEN_MLB_STEERING_LIMITS = {
|
||||
.max_torque = 300, // 3.0 Nm (EPS side max of 3.0Nm with fault if violated)
|
||||
.max_rt_delta = 169, // 10 max rate up * 50Hz send rate * 250000 RT interval / 1000000 = 112.5 ; 112.5 * 1.5 for safety pad = 168.75
|
||||
.max_rate_up = 9, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
|
||||
.max_rate_down = 10, // 5.0 Nm/s RoC limit (EPS rack has own soft-limit of 5.0 Nm/s)
|
||||
.driver_torque_allowance = 60,
|
||||
.driver_torque_multiplier = 3,
|
||||
.type = TorqueDriverLimited,
|
||||
};
|
||||
|
||||
bool tx = true;
|
||||
|
||||
// Safety check for HCA_01 Heading Control Assist torque
|
||||
if (msg->addr == MSG_HCA_01) {
|
||||
int desired_torque = volkswagen_mlb_mqb_steering_control_torque(msg);
|
||||
|
||||
int steer_status = msg->data[4] & 0xFU;
|
||||
bool steer_req = (steer_status == 5) || (steer_status == 7);
|
||||
|
||||
if (steer_torque_cmd_checks(desired_torque, steer_req, VOLKSWAGEN_MLB_STEERING_LIMITS)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// FORCE CANCEL: ensuring that only the cancel button press is sent when controls are off.
|
||||
// This avoids unintended engagements while still allowing resume spam
|
||||
if ((msg->addr == MSG_LS_01) && !controls_allowed) {
|
||||
// disallow resume and set: bits 16 and 19
|
||||
if (GET_BIT(msg, 16U) || GET_BIT(msg, 19U)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
return tx;
|
||||
}
|
||||
|
||||
// TODO: rename these functions to MXB or something
|
||||
const safety_hooks volkswagen_mlb_hooks = {
|
||||
.init = volkswagen_mlb_init,
|
||||
.rx = volkswagen_mlb_rx_hook,
|
||||
.tx = volkswagen_mlb_tx_hook,
|
||||
.get_counter = volkswagen_mqb_meb_get_counter,
|
||||
.get_checksum = volkswagen_mqb_meb_get_checksum,
|
||||
.compute_checksum = volkswagen_mqb_meb_compute_crc,
|
||||
};
|
||||
@@ -1,12 +1,8 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/volkswagen_common.h"
|
||||
|
||||
static bool volkswagen_mqb_brake_pedal_switch = false;
|
||||
static bool volkswagen_mqb_brake_pressure_detected = false;
|
||||
|
||||
|
||||
static safety_config volkswagen_mqb_init(uint16_t param) {
|
||||
// Transmit of GRA_ACC_01 is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
|
||||
// MSG_LH_EPS_03: openpilot needs to replace apparent driver steering input torque to pacify VW Emergency Assist
|
||||
@@ -26,17 +22,14 @@ static safety_config volkswagen_mqb_init(uint16_t param) {
|
||||
{.msg = {{MSG_GRA_ACC_01, 0, 8, 33U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
UNUSED(param);
|
||||
|
||||
volkswagen_set_button_prev = false;
|
||||
volkswagen_resume_button_prev = false;
|
||||
volkswagen_mqb_brake_pedal_switch = false;
|
||||
volkswagen_mqb_brake_pressure_detected = false;
|
||||
volkswagen_common_init();
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL);
|
||||
#else
|
||||
SAFETY_UNUSED(param);
|
||||
#endif
|
||||
gen_crc_lookup_table_8(0x2F, volkswagen_crc8_lut_8h2f);
|
||||
|
||||
return volkswagen_longitudinal ? BUILD_SAFETY_CFG(volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_LONG_TX_MSGS) : \
|
||||
BUILD_SAFETY_CFG(volkswagen_mqb_rx_checks, VOLKSWAGEN_MQB_STOCK_TX_MSGS);
|
||||
}
|
||||
@@ -59,12 +52,7 @@ static void volkswagen_mqb_rx_hook(const CANPacket_t *msg) {
|
||||
// Signal: LH_EPS_03.EPS_Lenkmoment (absolute torque)
|
||||
// Signal: LH_EPS_03.EPS_VZ_Lenkmoment (direction)
|
||||
if (msg->addr == MSG_LH_EPS_03) {
|
||||
int torque_driver_new = msg->data[5] | ((msg->data[6] & 0x1FU) << 8);
|
||||
int sign = (msg->data[6] & 0x80U) >> 7;
|
||||
if (sign == 1) {
|
||||
torque_driver_new *= -1;
|
||||
}
|
||||
update_sample(&torque_driver, torque_driver_new);
|
||||
update_sample(&torque_driver, volkswagen_mlb_mqb_driver_input_torque(msg));
|
||||
}
|
||||
|
||||
if (msg->addr == MSG_TSK_06) {
|
||||
@@ -111,15 +99,15 @@ static void volkswagen_mqb_rx_hook(const CANPacket_t *msg) {
|
||||
|
||||
// Signal: Motor_14.MO_Fahrer_bremst (ECU detected brake pedal switch F63)
|
||||
if (msg->addr == MSG_MOTOR_14) {
|
||||
volkswagen_mqb_brake_pedal_switch = (msg->data[3] & 0x10U) >> 4;
|
||||
volkswagen_brake_pedal_switch = GET_BIT(msg, 28U);
|
||||
}
|
||||
|
||||
// Signal: ESP_05.ESP_Fahrer_bremst (ESP detected driver brake pressure above platform specified threshold)
|
||||
if (msg->addr == MSG_ESP_05) {
|
||||
volkswagen_mqb_brake_pressure_detected = (msg->data[3] & 0x4U) >> 2;
|
||||
volkswagen_brake_pressure_detected = GET_BIT(msg, 26U);
|
||||
}
|
||||
|
||||
brake_pressed = volkswagen_mqb_brake_pedal_switch || volkswagen_mqb_brake_pressure_detected;
|
||||
brake_pressed = volkswagen_brake_pedal_switch || volkswagen_brake_pressure_detected;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -146,15 +134,8 @@ static bool volkswagen_mqb_tx_hook(const CANPacket_t *msg) {
|
||||
bool tx = true;
|
||||
|
||||
// Safety check for HCA_01 Heading Control Assist torque
|
||||
// Signal: HCA_01.HCA_01_LM_Offset (absolute torque)
|
||||
// Signal: HCA_01.HCA_01_LM_OffSign (direction)
|
||||
if (msg->addr == MSG_HCA_01) {
|
||||
int desired_torque = msg->data[2] | ((msg->data[3] & 0x1U) << 8);
|
||||
bool sign = GET_BIT(msg, 31U);
|
||||
if (sign) {
|
||||
desired_torque *= -1;
|
||||
}
|
||||
|
||||
int desired_torque = volkswagen_mlb_mqb_steering_control_torque(msg);
|
||||
bool steer_req = GET_BIT(msg, 30U);
|
||||
|
||||
if (steer_torque_cmd_checks(desired_torque, steer_req, VOLKSWAGEN_MQB_STEERING_LIMITS)) {
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/modes/volkswagen_common.h"
|
||||
|
||||
#define MSG_LENKHILFE_3 0x0D0U // RX from EPS, for steering angle and driver steering torque
|
||||
@@ -63,13 +63,12 @@ static safety_config volkswagen_pq_init(uint16_t param) {
|
||||
{.msg = {{MSG_GRA_NEU, 0, 4, 30U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}},
|
||||
};
|
||||
|
||||
UNUSED(param);
|
||||
|
||||
volkswagen_set_button_prev = false;
|
||||
volkswagen_resume_button_prev = false;
|
||||
volkswagen_common_init();
|
||||
|
||||
#ifdef ALLOW_DEBUG
|
||||
volkswagen_longitudinal = GET_FLAG(param, FLAG_VOLKSWAGEN_LONG_CONTROL);
|
||||
#else
|
||||
SAFETY_UNUSED(param);
|
||||
#endif
|
||||
return volkswagen_longitudinal ? BUILD_SAFETY_CFG(volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_LONG_TX_MSGS) : \
|
||||
BUILD_SAFETY_CFG(volkswagen_pq_rx_checks, VOLKSWAGEN_PQ_STOCK_TX_MSGS);
|
||||
@@ -78,7 +77,7 @@ static safety_config volkswagen_pq_init(uint16_t param) {
|
||||
static void volkswagen_pq_rx_hook(const CANPacket_t *msg) {
|
||||
if (msg->bus == 0U) {
|
||||
// Update in-motion state from speed value.
|
||||
// Signal: Bremse_1.Geschwindigkeit_neu__Bremse_1_
|
||||
// Signal: Bremse_1.BR1_Rad_kmh
|
||||
if (msg->addr == MSG_BREMSE_1) {
|
||||
int speed = ((msg->data[2] & 0xFEU) >> 1) | (msg->data[3] << 7);
|
||||
vehicle_moving = speed > 0;
|
||||
@@ -99,7 +98,7 @@ static void volkswagen_pq_rx_hook(const CANPacket_t *msg) {
|
||||
if (volkswagen_longitudinal) {
|
||||
if (msg->addr == MSG_MOTOR_5) {
|
||||
// ACC main switch on is a prerequisite to enter controls, exit controls immediately on main switch off
|
||||
// Signal: Motor_5.GRA_Hauptschalter
|
||||
// Signal: Motor_5.MO5_GRA_Hauptsch
|
||||
acc_main_on = GET_BIT(msg, 50U);
|
||||
if (!acc_main_on) {
|
||||
controls_allowed = false;
|
||||
@@ -126,19 +125,19 @@ static void volkswagen_pq_rx_hook(const CANPacket_t *msg) {
|
||||
} else {
|
||||
if (msg->addr == MSG_MOTOR_2) {
|
||||
// Enter controls on rising edge of stock ACC, exit controls if stock ACC disengages
|
||||
// Signal: Motor_2.GRA_Status
|
||||
// Signal: Motor_2.MO2_Sta_GRA
|
||||
int acc_status = (msg->data[2] & 0xC0U) >> 6;
|
||||
bool cruise_engaged = (acc_status == 1) || (acc_status == 2);
|
||||
pcm_cruise_check(cruise_engaged);
|
||||
}
|
||||
}
|
||||
|
||||
// Signal: Motor_3.Fahrpedal_Rohsignal
|
||||
// Signal: Motor_3.MO3_Pedalwert
|
||||
if (msg->addr == MSG_MOTOR_3) {
|
||||
gas_pressed = (msg->data[2]);
|
||||
}
|
||||
|
||||
// Signal: Motor_2.Bremslichtschalter
|
||||
// Signal: Motor_2.MO2_BLS
|
||||
if (msg->addr == MSG_MOTOR_2) {
|
||||
brake_pressed = (msg->data[2] & 0x1U);
|
||||
}
|
||||
|
||||
@@ -3,8 +3,8 @@
|
||||
#include "opendbc/safety/helpers.h"
|
||||
#include "opendbc/safety/lateral.h"
|
||||
#include "opendbc/safety/longitudinal.h"
|
||||
#include "opendbc/safety/safety_declarations.h"
|
||||
#include "opendbc/safety/board/can.h"
|
||||
#include "opendbc/safety/declarations.h"
|
||||
#include "opendbc/safety/can.h"
|
||||
|
||||
// all the safety modes
|
||||
#include "opendbc/safety/modes/defaults.h"
|
||||
@@ -20,6 +20,7 @@
|
||||
#include "opendbc/safety/modes/subaru_preglobal.h"
|
||||
#include "opendbc/safety/modes/mazda.h"
|
||||
#include "opendbc/safety/modes/nissan.h"
|
||||
#include "opendbc/safety/modes/volkswagen_mlb.h"
|
||||
#include "opendbc/safety/modes/volkswagen_mqb.h"
|
||||
#include "opendbc/safety/modes/volkswagen_pq.h"
|
||||
#include "opendbc/safety/modes/elm327.h"
|
||||
@@ -145,7 +146,7 @@ static void update_counter(RxCheck addr_list[], int index, uint8_t counter) {
|
||||
if (index != -1) {
|
||||
uint8_t expected_counter = (addr_list[index].status.last_counter + 1U) % (addr_list[index].msg[addr_list[index].status.index].max_counter + 1U);
|
||||
addr_list[index].status.wrong_counters += (expected_counter == counter) ? -1 : 1;
|
||||
addr_list[index].status.wrong_counters = CLAMP(addr_list[index].status.wrong_counters, 0, MAX_WRONG_COUNTERS);
|
||||
addr_list[index].status.wrong_counters = SAFETY_CLAMP(addr_list[index].status.wrong_counters, 0, MAX_WRONG_COUNTERS);
|
||||
addr_list[index].status.last_counter = counter;
|
||||
}
|
||||
}
|
||||
@@ -316,12 +317,12 @@ void safety_tick(const safety_config *cfg) {
|
||||
uint32_t ts = microsecond_timer_get();
|
||||
if (cfg != NULL) {
|
||||
for (int i=0; i < cfg->rx_checks_len; i++) {
|
||||
uint32_t elapsed_time = get_ts_elapsed(ts, cfg->rx_checks[i].status.last_timestamp);
|
||||
uint32_t elapsed_time = safety_get_ts_elapsed(ts, cfg->rx_checks[i].status.last_timestamp);
|
||||
// lag threshold is max of: 1s and MAX_MISSED_MSGS * expected timestep.
|
||||
// Quite conservative to not risk false triggers.
|
||||
// 2s of lag is worse case, since the function is called at 1Hz
|
||||
uint32_t timestep = 1e6 / cfg->rx_checks[i].msg[cfg->rx_checks[i].status.index].frequency;
|
||||
bool lagging = elapsed_time > MAX(timestep * MAX_MISSED_MSGS, 1e6);
|
||||
bool lagging = elapsed_time > SAFETY_MAX(timestep * MAX_MISSED_MSGS, 1e6);
|
||||
cfg->rx_checks[i].status.lagging = lagging;
|
||||
if (lagging) {
|
||||
controls_allowed = false;
|
||||
@@ -339,7 +340,6 @@ void safety_tick(const safety_config *cfg) {
|
||||
|
||||
static void relay_malfunction_set(void) {
|
||||
relay_malfunction = true;
|
||||
fault_occurred(FAULT_RELAY_MALFUNCTION);
|
||||
}
|
||||
|
||||
static void generic_rx_checks(void) {
|
||||
@@ -377,7 +377,6 @@ static void stock_ecu_check(bool stock_ecu_detected) {
|
||||
|
||||
static void relay_malfunction_reset(void) {
|
||||
relay_malfunction = false;
|
||||
fault_recovered(FAULT_RELAY_MALFUNCTION);
|
||||
}
|
||||
|
||||
// resets values and min/max for sample_t struct
|
||||
@@ -412,6 +411,7 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
|
||||
#ifdef ALLOW_DEBUG
|
||||
{SAFETY_PSA, &psa_hooks},
|
||||
{SAFETY_SUBARU_PREGLOBAL, &subaru_preglobal_hooks},
|
||||
{SAFETY_VOLKSWAGEN_MLB, &volkswagen_mlb_hooks},
|
||||
{SAFETY_VOLKSWAGEN_PQ, &volkswagen_pq_hooks},
|
||||
{SAFETY_ALLOUTPUT, &alloutput_hooks},
|
||||
#endif
|
||||
@@ -490,9 +490,9 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
|
||||
// convert a trimmed integer to signed 32 bit int
|
||||
int to_signed(int d, int bits) {
|
||||
int d_signed = d;
|
||||
int max_value = (1 << MAX((bits - 1), 0));
|
||||
int max_value = (1 << SAFETY_MAX((bits - 1), 0));
|
||||
if (d >= max_value) {
|
||||
d_signed = d - (1 << MAX(bits, 0));
|
||||
d_signed = d - (1 << SAFETY_MAX(bits, 0));
|
||||
}
|
||||
return d_signed;
|
||||
}
|
||||
@@ -536,7 +536,7 @@ void speed_mismatch_check(const float speed_2) {
|
||||
// Disable controls if speeds from two sources are too far apart.
|
||||
// For safety modes that use speed to adjust torque or angle limits
|
||||
const float MAX_SPEED_DELTA = 2.0; // m/s
|
||||
bool is_invalid_speed = ABS(speed_2 - ((float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR)) > MAX_SPEED_DELTA;
|
||||
bool is_invalid_speed = SAFETY_ABS(speed_2 - ((float)vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR)) > MAX_SPEED_DELTA;
|
||||
if (is_invalid_speed) {
|
||||
controls_allowed = false;
|
||||
}
|
||||
|
||||
@@ -7,7 +7,7 @@
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "opendbc/safety/sunnypilot/safety_mads_declarations.h"
|
||||
#include "opendbc/safety/sunnypilot/mads_declarations.h"
|
||||
|
||||
// ===============================
|
||||
// Global Variables
|
||||
@@ -42,8 +42,8 @@ def make_msg(bus, addr, length=8, dat=None):
|
||||
return libsafety_py.make_CANPacket(addr, bus, dat)
|
||||
|
||||
|
||||
class CANPackerPanda(CANPacker):
|
||||
def make_can_msg_panda(self, name_or_addr, bus, values, fix_checksum=None):
|
||||
class CANPackerSafety(CANPacker):
|
||||
def make_can_msg_safety(self, name_or_addr, bus, values, fix_checksum=None):
|
||||
msg = self.make_can_msg(name_or_addr, bus, values)
|
||||
if fix_checksum is not None:
|
||||
msg = fix_checksum(msg)
|
||||
@@ -73,12 +73,12 @@ def add_regen_tests(cls):
|
||||
return cls
|
||||
|
||||
|
||||
class PandaSafetyTestBase(unittest.TestCase):
|
||||
safety: libsafety_py.Panda
|
||||
class SafetyTestBase(unittest.TestCase):
|
||||
safety: libsafety_py.LibSafety
|
||||
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
if cls.__name__ == "PandaSafetyTestBase":
|
||||
if cls.__name__ == "SafetyTestBase":
|
||||
cls.safety = None
|
||||
raise unittest.SkipTest
|
||||
|
||||
@@ -135,7 +135,7 @@ class PandaSafetyTestBase(unittest.TestCase):
|
||||
self.assertEqual(meas_max_func(), 0)
|
||||
|
||||
|
||||
class LongitudinalAccelSafetyTest(PandaSafetyTestBase, abc.ABC):
|
||||
class LongitudinalAccelSafetyTest(SafetyTestBase, abc.ABC):
|
||||
|
||||
LONGITUDINAL = True
|
||||
MAX_ACCEL: float = 2.0
|
||||
@@ -175,7 +175,7 @@ class LongitudinalAccelSafetyTest(PandaSafetyTestBase, abc.ABC):
|
||||
self.assertEqual(should_tx, self._tx(self._accel_msg(accel)))
|
||||
|
||||
|
||||
class LongitudinalGasBrakeSafetyTest(PandaSafetyTestBase, abc.ABC):
|
||||
class LongitudinalGasBrakeSafetyTest(SafetyTestBase, abc.ABC):
|
||||
|
||||
MIN_BRAKE: int = 0
|
||||
MAX_BRAKE: int | None = None
|
||||
@@ -209,7 +209,7 @@ class LongitudinalGasBrakeSafetyTest(PandaSafetyTestBase, abc.ABC):
|
||||
self._generic_limit_safety_check(self._send_gas_msg, self.MIN_GAS, self.MAX_GAS, self.MIN_POSSIBLE_GAS, self.MAX_POSSIBLE_GAS, 1, self.INACTIVE_GAS)
|
||||
|
||||
|
||||
class TorqueSteeringSafetyTestBase(PandaSafetyTestBase, abc.ABC):
|
||||
class TorqueSteeringSafetyTestBase(SafetyTestBase, abc.ABC):
|
||||
|
||||
MAX_RATE_UP = 0
|
||||
MAX_RATE_DOWN = 0
|
||||
@@ -642,7 +642,7 @@ class MotorTorqueSteeringSafetyTest(TorqueSteeringSafetyTestBase, abc.ABC):
|
||||
self.assertEqual(self.safety.get_torque_meas_max(), 0)
|
||||
|
||||
|
||||
class VehicleSpeedSafetyTest(PandaSafetyTestBase):
|
||||
class VehicleSpeedSafetyTest(SafetyTestBase):
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
if cls.__name__ == "VehicleSpeedSafetyTest":
|
||||
@@ -809,7 +809,7 @@ class AngleSteeringSafetyTest(VehicleSpeedSafetyTest):
|
||||
self.assertTrue(self._tx(self._angle_cmd_msg(0, True, increment_timer=False)))
|
||||
|
||||
|
||||
class PandaSafetyTest(PandaSafetyTestBase):
|
||||
class SafetyTest(SafetyTestBase):
|
||||
TX_MSGS: list[list[int]] | None = None
|
||||
SCANNED_ADDRS = [*range(0x800), # Entire 11-bit CAN address space
|
||||
*range(0x18DA00F1, 0x18DB00F1, 0x100), # 29-bit UDS physical addressing
|
||||
@@ -820,7 +820,7 @@ class PandaSafetyTest(PandaSafetyTestBase):
|
||||
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
if cls.__name__ == "PandaSafetyTest" or cls.__name__.endswith('Base'):
|
||||
if cls.__name__ == "SafetyTest" or cls.__name__.endswith('Base'):
|
||||
cls.safety = None
|
||||
raise unittest.SkipTest
|
||||
|
||||
@@ -897,13 +897,12 @@ class PandaSafetyTest(PandaSafetyTestBase):
|
||||
continue
|
||||
if {attr, current_test}.issubset({'TestHyundaiLongitudinalSafety', 'TestHyundaiLongitudinalSafetyCameraSCC', 'TestHyundaiSafetyFCEVLong'}):
|
||||
continue
|
||||
|
||||
base_tests = {'TestHyundaiLongitudinalSafety', 'TestHyundaiLongitudinalSafetyCameraSCC', 'TestHyundaiSafetyFCEVLong',
|
||||
'TestHyundaiLongitudinalESCCSafety'}
|
||||
if any(attr.startswith(test) for test in base_tests) and any(current_test.startswith(test) for test in base_tests):
|
||||
continue
|
||||
|
||||
if {attr, current_test}.issubset({'TestVolkswagenMqbSafety', 'TestVolkswagenMqbStockSafety', 'TestVolkswagenMqbLongSafety'}):
|
||||
volkswagen_shared = ('TestVolkswagenMqb', 'TestVolkswagenMlb')
|
||||
if attr.startswith(volkswagen_shared) and current_test.startswith(volkswagen_shared):
|
||||
continue
|
||||
|
||||
# overlapping TX addrs, but they're not actuating messages for either car
|
||||
@@ -946,14 +945,14 @@ class PandaSafetyTest(PandaSafetyTestBase):
|
||||
|
||||
|
||||
@add_regen_tests
|
||||
class PandaCarSafetyTest(PandaSafetyTest, MadsSafetyTestBase):
|
||||
class CarSafetyTest(SafetyTest, MadsSafetyTestBase):
|
||||
STANDSTILL_THRESHOLD: float = 0.0
|
||||
GAS_PRESSED_THRESHOLD = 0
|
||||
RELAY_MALFUNCTION_ADDRS: dict[int, tuple[int, ...]] | None = None
|
||||
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
if cls.__name__ == "PandaCarSafetyTest" or cls.__name__.endswith('Base'):
|
||||
if cls.__name__ == "CarSafetyTest" or cls.__name__.endswith('Base'):
|
||||
cls.safety = None
|
||||
raise unittest.SkipTest
|
||||
|
||||
|
||||
@@ -8,17 +8,17 @@ See the LICENSE.md file in the root directory for more details.
|
||||
import numpy as np
|
||||
import unittest
|
||||
|
||||
from opendbc.safety.tests.common import CANPackerPanda, PandaSafetyTestBase
|
||||
from opendbc.safety.tests.common import CANPackerSafety, CarSafetyTest
|
||||
|
||||
|
||||
class GasInterceptorSafetyTest(PandaSafetyTestBase):
|
||||
class GasInterceptorSafetyTest(CarSafetyTest):
|
||||
|
||||
INTERCEPTOR_THRESHOLD = 0
|
||||
|
||||
cnt_gas_cmd = 0
|
||||
cnt_user_gas = 0
|
||||
|
||||
packer: CANPackerPanda
|
||||
packer: CANPackerSafety
|
||||
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
@@ -32,13 +32,13 @@ class GasInterceptorSafetyTest(PandaSafetyTestBase):
|
||||
values["GAS_COMMAND"] = gas * 255.
|
||||
values["GAS_COMMAND2"] = gas * 255.
|
||||
self.__class__.cnt_gas_cmd += 1
|
||||
return self.packer.make_can_msg_panda("GAS_COMMAND", 0, values)
|
||||
return self.packer.make_can_msg_safety("GAS_COMMAND", 0, values)
|
||||
|
||||
def _interceptor_user_gas(self, gas: int):
|
||||
values = {"INTERCEPTOR_GAS": gas, "INTERCEPTOR_GAS2": gas,
|
||||
"PEDAL_COUNTER": self.__class__.cnt_user_gas}
|
||||
self.__class__.cnt_user_gas += 1
|
||||
return self.packer.make_can_msg_panda("GAS_SENSOR", 0, values)
|
||||
return self.packer.make_can_msg_safety("GAS_SENSOR", 0, values)
|
||||
|
||||
# Skip non-interceptor user gas tests
|
||||
def test_prev_gas(self):
|
||||
|
||||
@@ -84,7 +84,7 @@ class HyundaiLongitudinalBase(common.LongitudinalAccelSafetyTest):
|
||||
cls.safety = None
|
||||
raise unittest.SkipTest
|
||||
|
||||
# override these tests from PandaCarSafetyTest, hyundai longitudinal uses button enable
|
||||
# override these tests from CarSafetyTest, hyundai longitudinal uses button enable
|
||||
def test_disable_control_allowed_from_cruise(self):
|
||||
pass
|
||||
|
||||
|
||||
@@ -1,11 +0,0 @@
|
||||
#!/usr/bin/env bash
|
||||
set -e
|
||||
|
||||
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
|
||||
cd $DIR
|
||||
|
||||
if ! command -v "mull-runner-17" > /dev/null 2>&1; then
|
||||
sudo apt-get update && sudo apt-get install -y curl clang-17
|
||||
curl -1sLf 'https://dl.cloudsmith.io/public/mull-project/mull-stable/setup.deb.sh' | sudo -E bash
|
||||
sudo apt-get update && sudo apt-get install -y mull-17
|
||||
fi
|
||||
@@ -11,6 +11,7 @@ mac_ver = platform.mac_ver()
|
||||
if system == 'Darwin' and mac_ver[0] and mac_ver[0] < '15':
|
||||
CC += '-13'
|
||||
|
||||
# standard env
|
||||
env = Environment(
|
||||
CC=CC,
|
||||
CFLAGS=[
|
||||
@@ -24,46 +25,45 @@ env = Environment(
|
||||
'-Wno-pointer-to-int-cast',
|
||||
'-g',
|
||||
'-O0',
|
||||
"-fno-omit-frame-pointer"
|
||||
'-fno-omit-frame-pointer',
|
||||
'-fprofile-arcs',
|
||||
'-ftest-coverage',
|
||||
'-DALLOW_DEBUG',
|
||||
],
|
||||
LINKFLAGS=[
|
||||
'-fprofile-arcs',
|
||||
'-ftest-coverage',
|
||||
],
|
||||
CPPPATH=["#", "../../board/"],
|
||||
tools=["default", "compilation_db"],
|
||||
)
|
||||
if system == "Darwin":
|
||||
env.PrependENVPath('PATH', '/opt/homebrew/bin')
|
||||
|
||||
if GetOption('mutation'):
|
||||
env['CC'] = 'clang-17'
|
||||
flags = [
|
||||
'-fprofile-instr-generate',
|
||||
'-fcoverage-mapping',
|
||||
'-fpass-plugin=/usr/lib/mull-ir-frontend-17',
|
||||
'-g',
|
||||
'-grecord-command-line',
|
||||
]
|
||||
env['CFLAGS'] += flags
|
||||
env['LINKFLAGS'] += flags
|
||||
|
||||
if GetOption('ubsan'):
|
||||
flags = [
|
||||
env.Prepend(LINKFLAGS=[
|
||||
"-fsanitize=undefined",
|
||||
"-fno-sanitize-recover=undefined",
|
||||
]
|
||||
env['CFLAGS'] += flags
|
||||
env['LINKFLAGS'] += flags
|
||||
])
|
||||
|
||||
safety = env.SharedObject("safety.os", "safety.c")
|
||||
libsafety = env.SharedLibrary("libsafety.so", [safety])
|
||||
|
||||
coverage_flags = [
|
||||
# GCC coverage flags
|
||||
'-fprofile-arcs',
|
||||
'-ftest-coverage',
|
||||
# mutation env
|
||||
menv = env.Clone()
|
||||
menv['CC'] = 'clang-17'
|
||||
flags = [
|
||||
'-fprofile-instr-generate',
|
||||
'-fcoverage-mapping',
|
||||
'-fpass-plugin=/usr/lib/mull-ir-frontend-17',
|
||||
'-g',
|
||||
'-grecord-command-line',
|
||||
]
|
||||
env.Append(
|
||||
CFLAGS=coverage_flags,
|
||||
LINKFLAGS=coverage_flags,
|
||||
)
|
||||
menv['CFLAGS'] += flags
|
||||
menv['LINKFLAGS'] += flags
|
||||
|
||||
# GCC note file is generated by compiler, allow scons to clean it up
|
||||
env.SideEffect("safety.gcno", safety)
|
||||
for build_env, suffix in ((env, ""), (menv, "_mutation")):
|
||||
# TODO: add macOS support
|
||||
if (build_env == menv) and (system == "Darwin"):
|
||||
continue
|
||||
safety = env.SharedObject(f"safety{suffix}.os", "safety.c")
|
||||
libsafety = env.SharedLibrary(f"libsafety{suffix}.so", [safety])
|
||||
|
||||
# GCC note file is generated by compiler, allow scons to clean it up
|
||||
env.SideEffect("safety.gcno", safety)
|
||||
|
||||
12
opendbc/safety/tests/libsafety/fake_stm.h
Normal file
12
opendbc/safety/tests/libsafety/fake_stm.h
Normal file
@@ -0,0 +1,12 @@
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#define ALLOW_DEBUG
|
||||
|
||||
// TODO: time should just be passed into the hooks we expose
|
||||
uint32_t timer_cnt = 0;
|
||||
uint32_t microsecond_timer_get(void);
|
||||
uint32_t microsecond_timer_get(void) {
|
||||
return timer_cnt;
|
||||
}
|
||||
@@ -1,12 +1,10 @@
|
||||
import os
|
||||
from cffi import FFI
|
||||
from typing import Protocol
|
||||
|
||||
from opendbc.safety import LEN_TO_DLC
|
||||
from opendbc.safety.tests.libsafety.safety_helpers import PandaSafety, setup_safety_helpers
|
||||
|
||||
libsafety_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
libsafety_fn = os.path.join(libsafety_dir, "libsafety.so")
|
||||
libsafety_fn = os.path.join(libsafety_dir, "libsafety_mutation.so" if "MUTATION" in os.environ else "libsafety.so")
|
||||
|
||||
ffi = FFI()
|
||||
|
||||
@@ -23,47 +21,91 @@ typedef struct {
|
||||
unsigned char data[64];
|
||||
} CANPacket_t;
|
||||
""", packed=True)
|
||||
class CANPacket:
|
||||
pass
|
||||
|
||||
ffi.cdef("""
|
||||
bool safety_rx_hook(CANPacket_t *msg);
|
||||
bool safety_tx_hook(CANPacket_t *msg);
|
||||
int safety_fwd_hook(int bus_num, int addr);
|
||||
int set_safety_hooks(uint16_t mode, uint16_t param);
|
||||
|
||||
void set_controls_allowed(bool c);
|
||||
bool get_controls_allowed(void);
|
||||
bool get_longitudinal_allowed(void);
|
||||
void set_alternative_experience(int mode);
|
||||
int get_alternative_experience(void);
|
||||
void set_relay_malfunction(bool c);
|
||||
bool get_relay_malfunction(void);
|
||||
bool get_gas_pressed_prev(void);
|
||||
void set_gas_pressed_prev(bool);
|
||||
bool get_brake_pressed_prev(void);
|
||||
bool get_regen_braking_prev(void);
|
||||
bool get_steering_disengage_prev(void);
|
||||
bool get_acc_main_on(void);
|
||||
float get_vehicle_speed_min(void);
|
||||
float get_vehicle_speed_max(void);
|
||||
int get_current_safety_mode(void);
|
||||
int get_current_safety_param(void);
|
||||
|
||||
void set_torque_meas(int min, int max);
|
||||
int get_torque_meas_min(void);
|
||||
int get_torque_meas_max(void);
|
||||
void set_torque_driver(int min, int max);
|
||||
int get_torque_driver_min(void);
|
||||
int get_torque_driver_max(void);
|
||||
void set_desired_torque_last(int t);
|
||||
void set_rt_torque_last(int t);
|
||||
void set_desired_angle_last(int t);
|
||||
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);
|
||||
|
||||
bool get_cruise_engaged_prev(void);
|
||||
void set_cruise_engaged_prev(bool engaged);
|
||||
bool get_vehicle_moving(void);
|
||||
void set_timer(uint32_t t);
|
||||
|
||||
void safety_tick_current_safety_config();
|
||||
bool safety_config_valid();
|
||||
|
||||
void init_tests(void);
|
||||
|
||||
void set_honda_fwd_brake(bool c);
|
||||
bool get_honda_fwd_brake(void);
|
||||
void set_honda_alt_brake_msg(bool c);
|
||||
void set_honda_bosch_long(bool c);
|
||||
int get_honda_hw(void);
|
||||
|
||||
bool get_lat_active(void);
|
||||
bool get_controls_allowed_lat(void);
|
||||
bool get_controls_requested_lat(void);
|
||||
void set_current_safety_param_sp(int param);
|
||||
int get_current_safety_param_sp(void);
|
||||
bool get_enable_mads(void);
|
||||
bool get_disengage_lateral_on_brake(void);
|
||||
bool get_pause_lateral_on_brake(void);
|
||||
void set_mads_button_press(int mads_button_press);
|
||||
void set_controls_allowed_lat(bool c);
|
||||
void set_controls_requested_lat(bool c);
|
||||
bool get_mads_acc_main(void);
|
||||
void set_acc_main_on(bool c);
|
||||
int get_mads_button_press(void);
|
||||
void mads_set_current_disengage_reason(int reason);
|
||||
int mads_get_current_disengage_reason(void);
|
||||
int get_temp_debug(void);
|
||||
uint32_t get_acc_main_on_mismatches(void);
|
||||
void set_mads_params(bool enable_mads, bool disengage_lateral_on_brake, bool pause_lateral_on_brake);
|
||||
void set_heartbeat_engaged_mads(bool c);
|
||||
void mads_heartbeat_engaged_check(void);
|
||||
void set_steering_disengage(bool c);
|
||||
int get_gas_interceptor_prev(void);
|
||||
""")
|
||||
|
||||
ffi.cdef("""
|
||||
void can_set_checksum(CANPacket_t *packet);
|
||||
""")
|
||||
|
||||
setup_safety_helpers(ffi)
|
||||
|
||||
|
||||
class CANPacket:
|
||||
reserved: int
|
||||
bus: int
|
||||
data_len_code: int
|
||||
rejected: int
|
||||
returned: int
|
||||
extended: int
|
||||
addr: int
|
||||
data: list[int]
|
||||
|
||||
|
||||
class Panda(PandaSafety, Protocol):
|
||||
# CAN
|
||||
def can_set_checksum(self, p: CANPacket) -> None: ...
|
||||
|
||||
# safety
|
||||
def safety_rx_hook(self, msg: CANPacket) -> int: ...
|
||||
def safety_tx_hook(self, msg: CANPacket) -> int: ...
|
||||
def safety_fwd_hook(self, bus_num: int, addr: int) -> int: ...
|
||||
def set_safety_hooks(self, mode: int, param: int) -> int: ...
|
||||
|
||||
|
||||
libsafety: Panda = ffi.dlopen(libsafety_fn)
|
||||
|
||||
|
||||
# helpers
|
||||
class LibSafety:
|
||||
pass
|
||||
libsafety: LibSafety = ffi.dlopen(libsafety_fn)
|
||||
|
||||
def make_CANPacket(addr: int, bus: int, dat):
|
||||
ret = ffi.new('CANPacket_t *')
|
||||
@@ -72,6 +114,4 @@ def make_CANPacket(addr: int, bus: int, dat):
|
||||
ret[0].data_len_code = LEN_TO_DLC[len(dat)]
|
||||
ret[0].bus = bus
|
||||
ret[0].data = bytes(dat)
|
||||
libsafety.can_set_checksum(ret)
|
||||
|
||||
return ret
|
||||
|
||||
@@ -1,13 +1,301 @@
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "opendbc/safety/board/fake_stm.h"
|
||||
#include "opendbc/safety/board/can.h"
|
||||
// TODO: time should just be passed into the hooks we expose
|
||||
uint32_t timer_cnt = 0;
|
||||
uint32_t microsecond_timer_get(void);
|
||||
uint32_t microsecond_timer_get(void) {
|
||||
return timer_cnt;
|
||||
}
|
||||
|
||||
//int safety_tx_hook(CANPacket_t *msg) { return 1; }
|
||||
|
||||
#include "opendbc/safety/board/faults.h"
|
||||
#include "opendbc/safety/can.h"
|
||||
#include "opendbc/safety/safety.h"
|
||||
#include "opendbc/safety/board/drivers/can_common.h"
|
||||
|
||||
// libsafety stuff
|
||||
#include "opendbc/safety/tests/libsafety/safety_helpers.h"
|
||||
void safety_tick_current_safety_config() {
|
||||
safety_tick(¤t_safety_config);
|
||||
}
|
||||
|
||||
bool safety_config_valid() {
|
||||
if (current_safety_config.rx_checks_len <= 0) {
|
||||
printf("missing RX checks\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int i = 0; i < current_safety_config.rx_checks_len; i++) {
|
||||
const RxCheck addr = current_safety_config.rx_checks[i];
|
||||
bool valid = addr.status.msg_seen && !addr.status.lagging && addr.status.valid_checksum && (addr.status.wrong_counters < MAX_WRONG_COUNTERS) && addr.status.valid_quality_flag;
|
||||
if (!valid) {
|
||||
// printf("i %d seen %d lagging %d valid checksum %d wrong counters %d valid quality flag %d\n", i, addr.status.msg_seen, addr.status.lagging, addr.status.valid_checksum, addr.status.wrong_counters, addr.status.valid_quality_flag);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void set_controls_allowed(bool c){
|
||||
controls_allowed = c;
|
||||
}
|
||||
|
||||
void set_alternative_experience(int mode){
|
||||
alternative_experience = mode;
|
||||
}
|
||||
|
||||
void set_relay_malfunction(bool c){
|
||||
relay_malfunction = c;
|
||||
}
|
||||
|
||||
bool get_controls_allowed(void){
|
||||
return controls_allowed;
|
||||
}
|
||||
|
||||
int get_alternative_experience(void){
|
||||
return alternative_experience;
|
||||
}
|
||||
|
||||
bool get_relay_malfunction(void){
|
||||
return relay_malfunction;
|
||||
}
|
||||
|
||||
bool get_gas_pressed_prev(void){
|
||||
return gas_pressed_prev;
|
||||
}
|
||||
|
||||
void set_gas_pressed_prev(bool c){
|
||||
gas_pressed_prev = c;
|
||||
}
|
||||
|
||||
bool get_brake_pressed_prev(void){
|
||||
return brake_pressed_prev;
|
||||
}
|
||||
|
||||
bool get_regen_braking_prev(void){
|
||||
return regen_braking_prev;
|
||||
}
|
||||
|
||||
bool get_steering_disengage_prev(void){
|
||||
return steering_disengage_prev;
|
||||
}
|
||||
|
||||
bool get_cruise_engaged_prev(void){
|
||||
return cruise_engaged_prev;
|
||||
}
|
||||
|
||||
void set_cruise_engaged_prev(bool engaged){
|
||||
cruise_engaged_prev = engaged;
|
||||
}
|
||||
|
||||
bool get_vehicle_moving(void){
|
||||
return vehicle_moving;
|
||||
}
|
||||
|
||||
bool get_acc_main_on(void){
|
||||
return acc_main_on;
|
||||
}
|
||||
|
||||
float get_vehicle_speed_min(void){
|
||||
return vehicle_speed.min / VEHICLE_SPEED_FACTOR;
|
||||
}
|
||||
|
||||
float get_vehicle_speed_max(void){
|
||||
return vehicle_speed.max / VEHICLE_SPEED_FACTOR;
|
||||
}
|
||||
|
||||
int get_current_safety_mode(void){
|
||||
return current_safety_mode;
|
||||
}
|
||||
|
||||
int get_current_safety_param(void){
|
||||
return current_safety_param;
|
||||
}
|
||||
|
||||
void set_timer(uint32_t t){
|
||||
timer_cnt = t;
|
||||
}
|
||||
|
||||
void set_torque_meas(int min, int max){
|
||||
torque_meas.min = min;
|
||||
torque_meas.max = max;
|
||||
}
|
||||
|
||||
int get_torque_meas_min(void){
|
||||
return torque_meas.min;
|
||||
}
|
||||
|
||||
int get_torque_meas_max(void){
|
||||
return torque_meas.max;
|
||||
}
|
||||
|
||||
void set_torque_driver(int min, int max){
|
||||
torque_driver.min = min;
|
||||
torque_driver.max = max;
|
||||
}
|
||||
|
||||
int get_torque_driver_min(void){
|
||||
return torque_driver.min;
|
||||
}
|
||||
|
||||
int get_torque_driver_max(void){
|
||||
return torque_driver.max;
|
||||
}
|
||||
|
||||
void set_rt_torque_last(int t){
|
||||
rt_torque_last = t;
|
||||
}
|
||||
|
||||
void set_desired_torque_last(int t){
|
||||
desired_torque_last = t;
|
||||
}
|
||||
|
||||
void set_desired_angle_last(int t){
|
||||
desired_angle_last = t;
|
||||
}
|
||||
|
||||
int get_desired_angle_last(void){
|
||||
return desired_angle_last;
|
||||
}
|
||||
|
||||
void set_angle_meas(int min, int max){
|
||||
angle_meas.min = min;
|
||||
angle_meas.max = max;
|
||||
}
|
||||
|
||||
int get_angle_meas_min(void){
|
||||
return angle_meas.min;
|
||||
}
|
||||
|
||||
int get_angle_meas_max(void){
|
||||
return angle_meas.max;
|
||||
}
|
||||
|
||||
|
||||
// ***** car specific helpers *****
|
||||
|
||||
void set_honda_alt_brake_msg(bool c){
|
||||
honda_alt_brake_msg = c;
|
||||
}
|
||||
|
||||
void set_honda_bosch_long(bool c){
|
||||
honda_bosch_long = c;
|
||||
}
|
||||
|
||||
int get_honda_hw(void) {
|
||||
return honda_hw;
|
||||
}
|
||||
|
||||
void set_honda_fwd_brake(bool c){
|
||||
honda_fwd_brake = c;
|
||||
}
|
||||
|
||||
bool get_honda_fwd_brake(void){
|
||||
return honda_fwd_brake;
|
||||
}
|
||||
|
||||
static MADSState *get_mads_state(void) {
|
||||
return &m_mads_state;
|
||||
}
|
||||
|
||||
bool get_lat_active(void){
|
||||
return is_lat_active();
|
||||
}
|
||||
|
||||
bool get_controls_allowed_lat(void){
|
||||
return mads_is_lateral_control_allowed_by_mads();
|
||||
}
|
||||
|
||||
bool get_controls_requested_lat(void){
|
||||
return get_mads_state()->controls_requested_lat;
|
||||
}
|
||||
|
||||
bool get_enable_mads(void){
|
||||
return get_mads_state()->system_enabled;
|
||||
}
|
||||
|
||||
bool get_disengage_lateral_on_brake(void){
|
||||
return get_mads_state()->disengage_lateral_on_brake;
|
||||
}
|
||||
|
||||
bool get_pause_lateral_on_brake(void){
|
||||
return get_mads_state()->pause_lateral_on_brake;
|
||||
}
|
||||
|
||||
void set_acc_main_on(bool c){
|
||||
acc_main_on = c;
|
||||
}
|
||||
|
||||
void set_current_safety_param_sp(int param){
|
||||
current_safety_param_sp = param;
|
||||
}
|
||||
|
||||
int get_current_safety_param_sp(void){
|
||||
return current_safety_param_sp;
|
||||
}
|
||||
|
||||
void set_mads_button_press(int c){
|
||||
mads_button_press = c;
|
||||
}
|
||||
|
||||
int get_mads_button_press(void){
|
||||
return mads_button_press;
|
||||
}
|
||||
|
||||
void set_controls_allowed_lat(bool c){
|
||||
m_mads_state.controls_allowed_lat = c;
|
||||
}
|
||||
|
||||
bool get_mads_acc_main(void){
|
||||
return m_mads_state.acc_main.current;
|
||||
}
|
||||
|
||||
int mads_get_current_disengage_reason(void) {
|
||||
return get_mads_state()->current_disengage.active_reason;
|
||||
}
|
||||
|
||||
void mads_set_current_disengage_reason(int reason) {
|
||||
m_mads_state.current_disengage.active_reason = reason;
|
||||
}
|
||||
|
||||
void set_controls_requested_lat(bool c){
|
||||
m_mads_state.controls_requested_lat = c;
|
||||
}
|
||||
|
||||
void set_mads_params(bool enable_mads, bool disengage_lateral_on_brake, bool pause_lateral_on_brake){
|
||||
alternative_experience = 0;
|
||||
if (enable_mads) {
|
||||
alternative_experience |= ALT_EXP_ENABLE_MADS;
|
||||
|
||||
if (disengage_lateral_on_brake) {
|
||||
alternative_experience |= ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE;
|
||||
} else if (pause_lateral_on_brake) {
|
||||
alternative_experience |= ALT_EXP_MADS_PAUSE_LATERAL_ON_BRAKE;
|
||||
} else {
|
||||
}
|
||||
}
|
||||
|
||||
mads_set_alternative_experience(&alternative_experience);
|
||||
}
|
||||
|
||||
void set_heartbeat_engaged_mads(bool c){
|
||||
heartbeat_engaged_mads = c;
|
||||
}
|
||||
|
||||
void set_steering_disengage(bool c){
|
||||
steering_disengage = c;
|
||||
}
|
||||
|
||||
int get_gas_interceptor_prev(void){
|
||||
return gas_interceptor_prev;
|
||||
}
|
||||
|
||||
void init_tests(void){
|
||||
safety_mode_cnt = 2U; // avoid ignoring relay_malfunction logic
|
||||
alternative_experience = 0;
|
||||
current_safety_param_sp = 0;
|
||||
set_timer(0);
|
||||
ts_steer_req_mismatch_last = 0;
|
||||
valid_steer_req_count = 0;
|
||||
invalid_steer_req_count = 0;
|
||||
|
||||
// assumes autopark on safety mode init to avoid a fault. get rid of that for testing
|
||||
tesla_autopark = false;
|
||||
}
|
||||
|
||||
@@ -1,294 +0,0 @@
|
||||
#include <stdio.h>
|
||||
|
||||
void safety_tick_current_safety_config() {
|
||||
safety_tick(¤t_safety_config);
|
||||
}
|
||||
|
||||
bool safety_config_valid() {
|
||||
if (current_safety_config.rx_checks_len <= 0) {
|
||||
printf("missing RX checks\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
for (int i = 0; i < current_safety_config.rx_checks_len; i++) {
|
||||
const RxCheck addr = current_safety_config.rx_checks[i];
|
||||
bool valid = addr.status.msg_seen && !addr.status.lagging && addr.status.valid_checksum && (addr.status.wrong_counters < MAX_WRONG_COUNTERS) && addr.status.valid_quality_flag;
|
||||
if (!valid) {
|
||||
// printf("i %d seen %d lagging %d valid checksum %d wrong counters %d valid quality flag %d\n", i, addr.status.msg_seen, addr.status.lagging, addr.status.valid_checksum, addr.status.wrong_counters, addr.status.valid_quality_flag);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
static MADSState *get_mads_state(void) {
|
||||
return &m_mads_state;
|
||||
}
|
||||
|
||||
void set_controls_allowed(bool c){
|
||||
controls_allowed = c;
|
||||
}
|
||||
|
||||
void set_alternative_experience(int mode){
|
||||
alternative_experience = mode;
|
||||
}
|
||||
|
||||
void set_relay_malfunction(bool c){
|
||||
relay_malfunction = c;
|
||||
}
|
||||
|
||||
bool get_controls_allowed(void){
|
||||
return controls_allowed;
|
||||
}
|
||||
|
||||
bool get_lat_active(void){
|
||||
return is_lat_active();
|
||||
}
|
||||
|
||||
bool get_controls_allowed_lat(void){
|
||||
return mads_is_lateral_control_allowed_by_mads();
|
||||
}
|
||||
|
||||
bool get_controls_requested_lat(void){
|
||||
return get_mads_state()->controls_requested_lat;
|
||||
}
|
||||
|
||||
bool get_enable_mads(void){
|
||||
return get_mads_state()->system_enabled;
|
||||
}
|
||||
|
||||
bool get_disengage_lateral_on_brake(void){
|
||||
return get_mads_state()->disengage_lateral_on_brake;
|
||||
}
|
||||
|
||||
bool get_pause_lateral_on_brake(void){
|
||||
return get_mads_state()->pause_lateral_on_brake;
|
||||
}
|
||||
|
||||
int get_alternative_experience(void){
|
||||
return alternative_experience;
|
||||
}
|
||||
|
||||
bool get_relay_malfunction(void){
|
||||
return relay_malfunction;
|
||||
}
|
||||
|
||||
bool get_gas_pressed_prev(void){
|
||||
return gas_pressed_prev;
|
||||
}
|
||||
|
||||
void set_gas_pressed_prev(bool c){
|
||||
gas_pressed_prev = c;
|
||||
}
|
||||
|
||||
bool get_brake_pressed_prev(void){
|
||||
return brake_pressed_prev;
|
||||
}
|
||||
|
||||
bool get_regen_braking_prev(void){
|
||||
return regen_braking_prev;
|
||||
}
|
||||
|
||||
bool get_steering_disengage_prev(void){
|
||||
return steering_disengage_prev;
|
||||
}
|
||||
|
||||
bool get_cruise_engaged_prev(void){
|
||||
return cruise_engaged_prev;
|
||||
}
|
||||
|
||||
void set_cruise_engaged_prev(bool engaged){
|
||||
cruise_engaged_prev = engaged;
|
||||
}
|
||||
|
||||
bool get_vehicle_moving(void){
|
||||
return vehicle_moving;
|
||||
}
|
||||
|
||||
bool get_acc_main_on(void){
|
||||
return acc_main_on;
|
||||
}
|
||||
|
||||
float get_vehicle_speed_min(void){
|
||||
return vehicle_speed.min / VEHICLE_SPEED_FACTOR;
|
||||
}
|
||||
|
||||
float get_vehicle_speed_max(void){
|
||||
return vehicle_speed.max / VEHICLE_SPEED_FACTOR;
|
||||
}
|
||||
|
||||
void set_acc_main_on(bool c){
|
||||
acc_main_on = c;
|
||||
}
|
||||
|
||||
int get_current_safety_mode(void){
|
||||
return current_safety_mode;
|
||||
}
|
||||
|
||||
int get_current_safety_param(void){
|
||||
return current_safety_param;
|
||||
}
|
||||
|
||||
void set_current_safety_param_sp(int param){
|
||||
current_safety_param_sp = param;
|
||||
}
|
||||
|
||||
int get_current_safety_param_sp(void){
|
||||
return current_safety_param_sp;
|
||||
}
|
||||
|
||||
void set_timer(uint32_t t){
|
||||
timer.CNT = t;
|
||||
}
|
||||
|
||||
void set_torque_meas(int min, int max){
|
||||
torque_meas.min = min;
|
||||
torque_meas.max = max;
|
||||
}
|
||||
|
||||
int get_torque_meas_min(void){
|
||||
return torque_meas.min;
|
||||
}
|
||||
|
||||
int get_torque_meas_max(void){
|
||||
return torque_meas.max;
|
||||
}
|
||||
|
||||
void set_torque_driver(int min, int max){
|
||||
torque_driver.min = min;
|
||||
torque_driver.max = max;
|
||||
}
|
||||
|
||||
int get_torque_driver_min(void){
|
||||
return torque_driver.min;
|
||||
}
|
||||
|
||||
int get_torque_driver_max(void){
|
||||
return torque_driver.max;
|
||||
}
|
||||
|
||||
void set_rt_torque_last(int t){
|
||||
rt_torque_last = t;
|
||||
}
|
||||
|
||||
void set_desired_torque_last(int t){
|
||||
desired_torque_last = t;
|
||||
}
|
||||
|
||||
void set_desired_angle_last(int t){
|
||||
desired_angle_last = t;
|
||||
}
|
||||
|
||||
int get_desired_angle_last(void){
|
||||
return desired_angle_last;
|
||||
}
|
||||
|
||||
void set_angle_meas(int min, int max){
|
||||
angle_meas.min = min;
|
||||
angle_meas.max = max;
|
||||
}
|
||||
|
||||
int get_angle_meas_min(void){
|
||||
return angle_meas.min;
|
||||
}
|
||||
|
||||
int get_angle_meas_max(void){
|
||||
return angle_meas.max;
|
||||
}
|
||||
|
||||
|
||||
// ***** car specific helpers *****
|
||||
|
||||
void set_honda_alt_brake_msg(bool c){
|
||||
honda_alt_brake_msg = c;
|
||||
}
|
||||
|
||||
void set_honda_bosch_long(bool c){
|
||||
honda_bosch_long = c;
|
||||
}
|
||||
|
||||
int get_honda_hw(void) {
|
||||
return honda_hw;
|
||||
}
|
||||
|
||||
void set_honda_fwd_brake(bool c){
|
||||
honda_fwd_brake = c;
|
||||
}
|
||||
|
||||
bool get_honda_fwd_brake(void){
|
||||
return honda_fwd_brake;
|
||||
}
|
||||
|
||||
void set_mads_button_press(int c){
|
||||
mads_button_press = c;
|
||||
}
|
||||
|
||||
int get_mads_button_press(void){
|
||||
return mads_button_press;
|
||||
}
|
||||
|
||||
void set_controls_allowed_lat(bool c){
|
||||
m_mads_state.controls_allowed_lat = c;
|
||||
}
|
||||
|
||||
bool get_mads_acc_main(void){
|
||||
return m_mads_state.acc_main.current;
|
||||
}
|
||||
|
||||
int mads_get_current_disengage_reason(void) {
|
||||
return get_mads_state()->current_disengage.active_reason;
|
||||
}
|
||||
|
||||
void mads_set_current_disengage_reason(int reason) {
|
||||
m_mads_state.current_disengage.active_reason = reason;
|
||||
}
|
||||
|
||||
void set_controls_requested_lat(bool c){
|
||||
m_mads_state.controls_requested_lat = c;
|
||||
}
|
||||
|
||||
void set_mads_params(bool enable_mads, bool disengage_lateral_on_brake, bool pause_lateral_on_brake){
|
||||
alternative_experience = 0;
|
||||
if (enable_mads) {
|
||||
alternative_experience |= ALT_EXP_ENABLE_MADS;
|
||||
|
||||
if (disengage_lateral_on_brake) {
|
||||
alternative_experience |= ALT_EXP_MADS_DISENGAGE_LATERAL_ON_BRAKE;
|
||||
} else if (pause_lateral_on_brake) {
|
||||
alternative_experience |= ALT_EXP_MADS_PAUSE_LATERAL_ON_BRAKE;
|
||||
} else {
|
||||
}
|
||||
}
|
||||
|
||||
mads_set_alternative_experience(&alternative_experience);
|
||||
}
|
||||
|
||||
void set_heartbeat_engaged_mads(bool c){
|
||||
heartbeat_engaged_mads = c;
|
||||
}
|
||||
|
||||
void set_steering_disengage(bool c){
|
||||
steering_disengage = c;
|
||||
}
|
||||
|
||||
int get_gas_interceptor_prev(void){
|
||||
return gas_interceptor_prev;
|
||||
}
|
||||
|
||||
//int get_temp_debug(void){
|
||||
// return temp_debug;
|
||||
//}
|
||||
|
||||
void init_tests(void){
|
||||
safety_mode_cnt = 2U; // avoid ignoring relay_malfunction logic
|
||||
alternative_experience = 0;
|
||||
current_safety_param_sp = 0;
|
||||
set_timer(0);
|
||||
ts_steer_req_mismatch_last = 0;
|
||||
valid_steer_req_count = 0;
|
||||
invalid_steer_req_count = 0;
|
||||
|
||||
// assumes autopark on safety mode init to avoid a fault. get rid of that for testing
|
||||
tesla_autopark = false;
|
||||
}
|
||||
@@ -1,155 +0,0 @@
|
||||
# panda safety helpers, from safety_helpers.c
|
||||
from typing import Protocol
|
||||
|
||||
|
||||
def setup_safety_helpers(ffi):
|
||||
ffi.cdef("""
|
||||
void set_controls_allowed(bool c);
|
||||
bool get_controls_allowed(void);
|
||||
bool get_lat_active(void);
|
||||
bool get_controls_allowed_lat(void);
|
||||
bool get_controls_requested_lat(void);
|
||||
bool get_longitudinal_allowed(void);
|
||||
void set_alternative_experience(int mode);
|
||||
int get_alternative_experience(void);
|
||||
void set_relay_malfunction(bool c);
|
||||
bool get_relay_malfunction(void);
|
||||
bool get_gas_pressed_prev(void);
|
||||
void set_gas_pressed_prev(bool);
|
||||
bool get_brake_pressed_prev(void);
|
||||
bool get_regen_braking_prev(void);
|
||||
bool get_steering_disengage_prev(void);
|
||||
bool get_acc_main_on(void);
|
||||
float get_vehicle_speed_min(void);
|
||||
float get_vehicle_speed_max(void);
|
||||
int get_current_safety_mode(void);
|
||||
int get_current_safety_param(void);
|
||||
void set_current_safety_param_sp(int param);
|
||||
int get_current_safety_param_sp(void);
|
||||
|
||||
void set_torque_meas(int min, int max);
|
||||
int get_torque_meas_min(void);
|
||||
int get_torque_meas_max(void);
|
||||
void set_torque_driver(int min, int max);
|
||||
int get_torque_driver_min(void);
|
||||
int get_torque_driver_max(void);
|
||||
void set_desired_torque_last(int t);
|
||||
void set_rt_torque_last(int t);
|
||||
void set_desired_angle_last(int t);
|
||||
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);
|
||||
|
||||
bool get_cruise_engaged_prev(void);
|
||||
void set_cruise_engaged_prev(bool engaged);
|
||||
bool get_vehicle_moving(void);
|
||||
void set_timer(uint32_t t);
|
||||
|
||||
void safety_tick_current_safety_config();
|
||||
bool safety_config_valid();
|
||||
|
||||
void init_tests(void);
|
||||
|
||||
void set_honda_fwd_brake(bool c);
|
||||
bool get_honda_fwd_brake(void);
|
||||
void set_honda_alt_brake_msg(bool c);
|
||||
void set_honda_bosch_long(bool c);
|
||||
int get_honda_hw(void);
|
||||
|
||||
bool get_enable_mads(void);
|
||||
bool get_disengage_lateral_on_brake(void);
|
||||
bool get_pause_lateral_on_brake(void);
|
||||
void set_mads_button_press(int mads_button_press);
|
||||
void set_controls_allowed_lat(bool c);
|
||||
void set_controls_requested_lat(bool c);
|
||||
|
||||
bool get_mads_acc_main(void);
|
||||
void set_acc_main_on(bool c);
|
||||
int get_mads_button_press(void);
|
||||
void mads_set_current_disengage_reason(int reason);
|
||||
int mads_get_current_disengage_reason(void);
|
||||
int get_temp_debug(void);
|
||||
uint32_t get_acc_main_on_mismatches(void);
|
||||
void set_mads_params(bool enable_mads, bool disengage_lateral_on_brake, bool pause_lateral_on_brake);
|
||||
void set_heartbeat_engaged_mads(bool c);
|
||||
void mads_heartbeat_engaged_check(void);
|
||||
void set_steering_disengage(bool c);
|
||||
int get_gas_interceptor_prev(void);
|
||||
""")
|
||||
|
||||
|
||||
class PandaSafety(Protocol):
|
||||
def set_controls_allowed(self, c: bool) -> None: ...
|
||||
def get_controls_allowed(self) -> bool: ...
|
||||
def set_controls_allowed_lat(self, c: bool) -> None: ...
|
||||
def set_controls_requested_lat(self, c: bool) -> None: ...
|
||||
def get_lat_active(self) -> bool: ...
|
||||
def get_controls_allowed_lat(self) -> bool: ...
|
||||
def get_controls_requested_lat(self) -> bool: ...
|
||||
def get_mads_acc_main(self) -> bool: ...
|
||||
def get_longitudinal_allowed(self) -> bool: ...
|
||||
def set_alternative_experience(self, mode: int) -> None: ...
|
||||
def get_alternative_experience(self) -> int: ...
|
||||
def set_relay_malfunction(self, c: bool) -> None: ...
|
||||
def get_relay_malfunction(self) -> bool: ...
|
||||
def get_gas_pressed_prev(self) -> bool: ...
|
||||
def set_gas_pressed_prev(self, c: bool) -> None: ...
|
||||
def get_brake_pressed_prev(self) -> bool: ...
|
||||
def get_regen_braking_prev(self) -> bool: ...
|
||||
def get_steering_disengage_prev(self) -> bool: ...
|
||||
def get_acc_main_on(self) -> bool: ...
|
||||
def set_acc_main_on(self, c: bool) -> None: ...
|
||||
def get_vehicle_speed_min(self) -> int: ...
|
||||
def get_vehicle_speed_max(self) -> int: ...
|
||||
def get_current_safety_mode(self) -> int: ...
|
||||
def get_current_safety_param(self) -> int: ...
|
||||
def set_current_safety_param_sp(self, param: int) -> int: ...
|
||||
def get_current_safety_param_sp(self) -> int: ...
|
||||
|
||||
def set_torque_meas(self, min: int, max: int) -> None: ... # noqa: A002
|
||||
def get_torque_meas_min(self) -> int: ...
|
||||
def get_torque_meas_max(self) -> int: ...
|
||||
def set_torque_driver(self, min: int, max: int) -> None: ... # noqa: A002
|
||||
def get_torque_driver_min(self) -> int: ...
|
||||
def get_torque_driver_max(self) -> int: ...
|
||||
def set_desired_torque_last(self, t: int) -> None: ...
|
||||
def set_rt_torque_last(self, t: int) -> None: ...
|
||||
def set_desired_angle_last(self, t: int) -> None: ...
|
||||
def get_desired_angle_last(self) -> int: ...
|
||||
def set_angle_meas(self, min: int, max: int) -> None: ... # noqa: A002
|
||||
def get_angle_meas_min(self) -> int: ...
|
||||
def get_angle_meas_max(self) -> int: ...
|
||||
|
||||
def get_cruise_engaged_prev(self) -> bool: ...
|
||||
def set_cruise_engaged_prev(self, enabled: bool) -> None: ...
|
||||
def get_vehicle_moving(self) -> bool: ...
|
||||
def set_timer(self, t: int) -> None: ...
|
||||
|
||||
def safety_tick_current_safety_config(self) -> None: ...
|
||||
def safety_config_valid(self) -> bool: ...
|
||||
|
||||
def init_tests(self) -> None: ...
|
||||
|
||||
def set_honda_fwd_brake(self, c: bool) -> None: ...
|
||||
def get_honda_fwd_brake(self) -> bool: ...
|
||||
def set_honda_alt_brake_msg(self, c: bool) -> None: ...
|
||||
def set_honda_bosch_long(self, c: bool) -> None: ...
|
||||
def get_honda_hw(self) -> int: ...
|
||||
|
||||
def set_mads_button_press(self, mads_button_press: int) -> None: ...
|
||||
def get_enable_mads(self) -> bool: ...
|
||||
def get_disengage_lateral_on_brake(self) -> bool: ...
|
||||
def get_pause_lateral_on_brake(self) -> bool: ...
|
||||
|
||||
def get_mads_button_press(self) -> int: ...
|
||||
def mads_set_current_disengage_reason(self, reason: int) -> None: ...
|
||||
def mads_get_current_disengage_reason(self) -> int: ...
|
||||
def get_acc_main_on_mismatches(self) -> int: ...
|
||||
|
||||
def set_mads_params(self, enable_mads: bool, disengage_lateral_on_brake: bool, pause_lateral_on_brake: bool) -> None: ...
|
||||
def set_heartbeat_engaged_mads(self, c: bool) -> None: ...
|
||||
def mads_heartbeat_engaged_check(self) -> None: ...
|
||||
def set_steering_disengage(self, c: bool) -> None: ...
|
||||
def get_gas_interceptor_prev(self) -> int: ...
|
||||
# def get_temp_debug(self) -> int: ...
|
||||
@@ -6,7 +6,7 @@ from opendbc.safety.tests.libsafety import libsafety_py
|
||||
|
||||
|
||||
class MadsSafetyTestBase(unittest.TestCase):
|
||||
safety: libsafety_py.Panda
|
||||
safety: libsafety_py.LibSafety
|
||||
|
||||
@abc.abstractmethod
|
||||
def _lkas_button_msg(self, enabled):
|
||||
@@ -17,6 +17,7 @@ class MadsSafetyTestBase(unittest.TestCase):
|
||||
raise NotImplementedError
|
||||
|
||||
def teardown_method(self, method):
|
||||
self.safety = libsafety_py.libsafety
|
||||
self.safety.set_mads_button_press(-1)
|
||||
self.safety.set_controls_allowed_lat(False)
|
||||
self.safety.set_controls_requested_lat(False)
|
||||
|
||||
@@ -2,14 +2,14 @@
|
||||
|
||||
// this file is checked by cppcheck
|
||||
|
||||
// Ignore misra-c2012-8.7 as these functions are only called from panda and libsafety
|
||||
UNUSED(heartbeat_engaged);
|
||||
// Ignore misra-c2012-8.7 as these functions are only called from libsafety
|
||||
SAFETY_UNUSED(heartbeat_engaged);
|
||||
|
||||
UNUSED(safety_rx_hook);
|
||||
UNUSED(safety_tx_hook);
|
||||
UNUSED(safety_fwd_hook);
|
||||
UNUSED(safety_tick);
|
||||
UNUSED(set_safety_hooks);
|
||||
UNUSED(mads_heartbeat_engaged_check);
|
||||
UNUSED(mads_set_alternative_experience);
|
||||
UNUSED(get_acc_main_on_mismatches);
|
||||
SAFETY_UNUSED(safety_rx_hook);
|
||||
SAFETY_UNUSED(safety_tx_hook);
|
||||
SAFETY_UNUSED(safety_fwd_hook);
|
||||
SAFETY_UNUSED(safety_tick);
|
||||
SAFETY_UNUSED(set_safety_hooks);
|
||||
SAFETY_UNUSED(mads_heartbeat_engaged_check);
|
||||
SAFETY_UNUSED(mads_set_alternative_experience);
|
||||
SAFETY_UNUSED(get_acc_main_on_mismatches);
|
||||
|
||||
@@ -53,10 +53,10 @@ cppcheck() {
|
||||
fi
|
||||
}
|
||||
|
||||
PANDA_OPTS=" --enable=all --enable=unusedFunction --addon=misra"
|
||||
OPTS=" --enable=all --enable=unusedFunction --addon=misra"
|
||||
|
||||
printf "\n${GREEN}** Safety **${NC}\n"
|
||||
cppcheck $PANDA_OPTS $BASEDIR/opendbc/safety/tests/misra/main.c
|
||||
cppcheck $OPTS $BASEDIR/opendbc/safety/tests/misra/main.c
|
||||
|
||||
printf "\n${GREEN}Success!${NC} took $SECONDS seconds\n"
|
||||
|
||||
|
||||
@@ -5,13 +5,13 @@ DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
|
||||
cd $DIR
|
||||
|
||||
source $DIR/../../../setup.sh
|
||||
$DIR/install_mull.sh
|
||||
|
||||
GIT_REF="${GIT_REF:-origin/master}"
|
||||
GIT_ROOT=$(git rev-parse --show-toplevel)
|
||||
MULL_OPS="mutators: [cxx_increment, cxx_decrement, cxx_comparison, cxx_boundary, cxx_bitwise_assignment, cxx_bitwise, cxx_arithmetic_assignment, cxx_arithmetic, cxx_remove_negation]"
|
||||
echo -e "$MULL_OPS" > $GIT_ROOT/mull.yml
|
||||
scons --mutation -j$(nproc) -D
|
||||
scons -j$(nproc) -D
|
||||
echo -e "timeout: 1000000\ngitDiffRef: $GIT_REF\ngitProjectRoot: $GIT_ROOT" >> $GIT_ROOT/mull.yml
|
||||
|
||||
mull-runner-17 --ld-search-path /lib/x86_64-linux-gnu/ ./libsafety/libsafety.so -test-program=pytest -- -n8 --ignore-glob=misra/*
|
||||
export MUTATION=1
|
||||
mull-runner-17 --ld-search-path /lib/x86_64-linux-gnu/ ./libsafety/libsafety_mutation.so -test-program=pytest -- -n8 --ignore-glob=misra/*
|
||||
|
||||
@@ -108,4 +108,4 @@ def init_segment(safety, msgs, mode, param):
|
||||
safety.set_controls_allowed_lat(1)
|
||||
safety.set_desired_angle_last(angle)
|
||||
safety.set_angle_meas(angle, angle)
|
||||
assert safety.safety_tx_hook(msg), "failed to initialize panda safety for segment"
|
||||
assert safety.safety_tx_hook(msg), "failed to initialize safety for segment"
|
||||
|
||||
@@ -4,31 +4,31 @@ import unittest
|
||||
from opendbc.car.structs import CarParams
|
||||
import opendbc.safety.tests.common as common
|
||||
from opendbc.safety.tests.libsafety import libsafety_py
|
||||
from opendbc.safety.tests.common import CANPackerPanda
|
||||
from opendbc.safety.tests.common import CANPackerSafety
|
||||
|
||||
|
||||
class TestBody(common.PandaSafetyTest):
|
||||
class TestBody(common.SafetyTest):
|
||||
TX_MSGS = [[0x250, 0], [0x251, 0],
|
||||
[0x1, 0], [0x1, 1], [0x1, 2], [0x1, 3]]
|
||||
FWD_BUS_LOOKUP = {}
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerPanda("comma_body")
|
||||
self.packer = CANPackerSafety("comma_body")
|
||||
self.safety = libsafety_py.libsafety
|
||||
self.safety.set_safety_hooks(CarParams.SafetyModel.body, 0)
|
||||
self.safety.init_tests()
|
||||
|
||||
def _motors_data_msg(self, speed_l, speed_r):
|
||||
values = {"SPEED_L": speed_l, "SPEED_R": speed_r}
|
||||
return self.packer.make_can_msg_panda("MOTORS_DATA", 0, values)
|
||||
return self.packer.make_can_msg_safety("MOTORS_DATA", 0, values)
|
||||
|
||||
def _torque_cmd_msg(self, torque_l, torque_r):
|
||||
values = {"TORQUE_L": torque_l, "TORQUE_R": torque_r}
|
||||
return self.packer.make_can_msg_panda("TORQUE_CMD", 0, values)
|
||||
return self.packer.make_can_msg_safety("TORQUE_CMD", 0, values)
|
||||
|
||||
def _max_motor_rpm_cmd_msg(self, max_rpm_l, max_rpm_r):
|
||||
values = {"MAX_RPM_L": max_rpm_l, "MAX_RPM_R": max_rpm_r}
|
||||
return self.packer.make_can_msg_panda("MAX_MOTOR_RPM_CMD", 0, values)
|
||||
return self.packer.make_can_msg_safety("MAX_MOTOR_RPM_CMD", 0, values)
|
||||
|
||||
def test_rx_hook(self):
|
||||
self.assertFalse(self.safety.get_controls_allowed())
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user