diff --git a/.github/workflows/test_models_trigger.yaml b/.github/workflows/test_models_trigger.yaml
index ac5ad8351b..c93469a6f6 100644
--- a/.github/workflows/test_models_trigger.yaml
+++ b/.github/workflows/test_models_trigger.yaml
@@ -37,6 +37,7 @@ jobs:
submodules: 'true'
- name: bump opendbc
+ if: steps.check_comment.outputs.result == 'true'
run: |
cd opendbc_repo
git fetch origin pull/${{ github.event.issue.number }}/head
diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml
index ba8fc68532..c99b2fc0a2 100644
--- a/.github/workflows/tests.yml
+++ b/.github/workflows/tests.yml
@@ -93,7 +93,7 @@ jobs:
mutation:
name: Safety mutation tests
runs-on: ${{ github.repository == 'commaai/opendbc' && 'namespace-profile-amd64-8x16' || 'ubuntu-latest' }}
- timeout-minutes: 20
+ timeout-minutes: 60
env:
GIT_REF: ${{ github.event_name == 'push' && github.ref == format('refs/heads/{0}', github.event.repository.default_branch) && github.event.before || format('origin/{0}', github.event.repository.default_branch) }}
steps:
@@ -101,7 +101,7 @@ jobs:
with:
fetch-depth: 0 # need master to get diff
- name: Run mutation tests
- timeout-minutes: 10
+ timeout-minutes: 60
run: |
source setup.sh
scons -j8
diff --git a/.gitignore b/.gitignore
index 8abe8b1bae..cb96f08721 100644
--- a/.gitignore
+++ b/.gitignore
@@ -24,4 +24,7 @@ opendbc/can/packer_pyx.cpp
opendbc/can/parser_pyx.cpp
opendbc/can/packer_pyx.html
opendbc/can/parser_pyx.html
-opendbc/dbc/*_generated.dbc
\ No newline at end of file
+opendbc/dbc/*_generated.dbc
+
+cppcheck-addon-ctu-file-list
+opendbc/safety/tests/coverage-out
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
index 89fb6db918..5ac1c0b79e 100644
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
@@ -37,10 +37,3 @@ repos:
- --counting=detailed
- --linelength=240
- --filter=-build,-legal,-readability,-runtime,-whitespace,+build/include_subdir,+build/forward_decl,+build/include_what_you_use,+build/deprecated,+whitespace/comma,+whitespace/line_length,+whitespace/empty_if_body,+whitespace/empty_loop_body,+whitespace/empty_conditional_body,+whitespace/forcolon,+whitespace/parens,+whitespace/semicolon,+whitespace/tab,+readability/braces
-- repo: local
- hooks:
- - id: generator
- name: dbc generator
- entry: opendbc/dbc/generator/test_generator.py
- language: script
- pass_filenames: false
diff --git a/docs/CARS.md b/docs/CARS.md
index 24c6b81543..d264c5ea95 100644
--- a/docs/CARS.md
+++ b/docs/CARS.md
@@ -36,8 +36,8 @@
|Ford|Escape Plug-in Hybrid 2020-22|Co-Pilot360 Assist+|[Upstream](#upstream)|
|Ford|Explorer 2020-24|Co-Pilot360 Assist+|[Upstream](#upstream)|
|Ford|Explorer Hybrid 2020-24|Co-Pilot360 Assist+|[Upstream](#upstream)|
-|Ford|F-150 2021-23|Co-Pilot360 Assist 2.0|[Under review](#upstream)|
-|Ford|F-150 Hybrid 2021-23|Co-Pilot360 Assist 2.0|[Under review](#upstream)|
+|Ford|F-150 2021-23|Co-Pilot360 Assist 2.0|[Upstream](#upstream)|
+|Ford|F-150 Hybrid 2021-23|Co-Pilot360 Assist 2.0|[Upstream](#upstream)|
|Ford|Focus 2018|Adaptive Cruise Control with Lane Centering|[Upstream](#upstream)|
|Ford|Focus Hybrid 2018|Adaptive Cruise Control with Lane Centering|[Upstream](#upstream)|
|Ford|Kuga 2020-22|Adaptive Cruise Control with Lane Centering|[Upstream](#upstream)|
@@ -47,8 +47,8 @@
|Ford|Maverick 2023-24|Co-Pilot360 Assist|[Upstream](#upstream)|
|Ford|Maverick Hybrid 2022|LARIAT Luxury|[Upstream](#upstream)|
|Ford|Maverick Hybrid 2023-24|Co-Pilot360 Assist|[Upstream](#upstream)|
-|Ford|Mustang Mach-E 2021-23|All|[Under review](#upstream)|
-|Ford|Ranger 2024|Adaptive Cruise Control with Lane Centering|[Under review](#upstream)|
+|Ford|Mustang Mach-E 2021-23|All|[Upstream](#upstream)|
+|Ford|Ranger 2024|Adaptive Cruise Control with Lane Centering|[Upstream](#upstream)|
|Genesis|G70 2018|All|[Upstream](#upstream)|
|Genesis|G70 2019-21|All|[Upstream](#upstream)|
|Genesis|G70 2022-23|All|[Upstream](#upstream)|
diff --git a/opendbc/car/CARS_template.md b/opendbc/car/CARS_template.md
index d13f3a4b76..c08b191f84 100644
--- a/opendbc/car/CARS_template.md
+++ b/opendbc/car/CARS_template.md
@@ -1,10 +1,10 @@
-# Support Information for {{car_docs_with_extras | length}} Known Cars
+# Support Information for {{all_car_docs | length}} Known Cars
|{{ExtraCarsColumn | map(attribute='value') | join('|') | replace(hardware_col_name, wide_hardware_col_name)}}|
|---|---|---|{% for _ in range((ExtraCarsColumn | length) - 3) %}{{':---:|'}}{% endfor +%}
-{% for car_docs in car_docs_with_extras %}
+{% for car_docs in all_car_docs %}
|{% for column in ExtraCarsColumn %}{{car_docs.get_extra_cars_column(column)}}|{% endfor %}
{% endfor %}
diff --git a/opendbc/car/__init__.py b/opendbc/car/__init__.py
index 038b8d60eb..3b33a1dcb1 100644
--- a/opendbc/car/__init__.py
+++ b/opendbc/car/__init__.py
@@ -1,6 +1,5 @@
# functions common among cars
import numpy as np
-from collections import namedtuple
from dataclasses import dataclass, field
from enum import IntFlag, ReprEnum, StrEnum, EnumType, auto
from dataclasses import replace
@@ -17,7 +16,13 @@
ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2
ButtonType = structs.CarState.ButtonEvent.Type
-AngleRateLimit = namedtuple('AngleRateLimit', ['speed_bp', 'angle_v'])
+
+
+@dataclass
+class AngleSteeringLimits:
+ STEER_ANGLE_MAX: float
+ ANGLE_RATE_LIMIT_UP: tuple[list[float], list[float]]
+ ANGLE_RATE_LIMIT_DOWN: tuple[list[float], list[float]]
def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float:
@@ -141,19 +146,20 @@ def apply_meas_steer_torque_limits(apply_torque, apply_torque_last, motor_torque
LIMITS.STEER_ERROR_MAX, LIMITS.STEER_MAX)))
-def apply_std_steer_angle_limits(apply_angle, apply_angle_last, v_ego, steering_angle, lat_active, LIMITS):
+def apply_std_steer_angle_limits(apply_angle: float, apply_angle_last: float, v_ego: float, steering_angle: float,
+ lat_active: bool, limits: AngleSteeringLimits) -> float:
# pick angle rate limits based on wind up/down
steer_up = apply_angle_last * apply_angle >= 0. and abs(apply_angle) > abs(apply_angle_last)
- rate_limits = LIMITS.ANGLE_RATE_LIMIT_UP if steer_up else LIMITS.ANGLE_RATE_LIMIT_DOWN
+ rate_limits = limits.ANGLE_RATE_LIMIT_UP if steer_up else limits.ANGLE_RATE_LIMIT_DOWN
- angle_rate_lim = np.interp(v_ego, rate_limits.speed_bp, rate_limits.angle_v)
+ angle_rate_lim = np.interp(v_ego, rate_limits[0], rate_limits[1])
new_apply_angle = np.clip(apply_angle, apply_angle_last - angle_rate_lim, apply_angle_last + angle_rate_lim)
# angle is current steering wheel angle when inactive on all angle cars
if not lat_active:
new_apply_angle = steering_angle
- return float(np.clip(new_apply_angle, -LIMITS.STEER_ANGLE_MAX, LIMITS.STEER_ANGLE_MAX))
+ return float(np.clip(new_apply_angle, -limits.STEER_ANGLE_MAX, limits.STEER_ANGLE_MAX))
def common_fault_avoidance(fault_condition: bool, request: bool, above_limit_frames: int,
diff --git a/opendbc/car/body/carcontroller.py b/opendbc/car/body/carcontroller.py
index 0afbea37cb..0fd87792e8 100644
--- a/opendbc/car/body/carcontroller.py
+++ b/opendbc/car/body/carcontroller.py
@@ -20,8 +20,8 @@ def __init__(self, dbc_names, CP, CP_SP):
self.packer = CANPacker(dbc_names[Bus.main])
# PIDs
- self.turn_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL)
- self.wheeled_speed_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL)
+ self.turn_pid = PIDController(110, k_i=11.5, rate=1 / DT_CTRL)
+ self.wheeled_speed_pid = PIDController(110, k_i=11.5, rate=1 / DT_CTRL)
self.torque_r_filtered = 0.
self.torque_l_filtered = 0.
diff --git a/opendbc/car/car.capnp b/opendbc/car/car.capnp
index 100ff3e6fb..b1b968a3c9 100644
--- a/opendbc/car/car.capnp
+++ b/opendbc/car/car.capnp
@@ -627,6 +627,7 @@ struct CarParams {
psa @31;
fcaGiorgio @32;
rivian @33;
+ volkswagenMeb @34;
}
enum SteerControlType {
diff --git a/opendbc/car/car_helpers.py b/opendbc/car/car_helpers.py
index 49d8433555..2ba13bd0dc 100644
--- a/opendbc/car/car_helpers.py
+++ b/opendbc/car/car_helpers.py
@@ -7,8 +7,8 @@
from opendbc.car.structs import CarParams, CarParamsT, CarParamsSP
from opendbc.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
from opendbc.car.fw_versions import ObdCallback, get_fw_versions_ordered, get_present_ecus, match_fw_to_car
-from opendbc.car.interfaces import get_interface_attr
from opendbc.car.mock.values import CAR as MOCK
+from opendbc.car.values import BRANDS
from opendbc.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN
FRAME_FINGERPRINT = 100 # 1s
@@ -30,8 +30,9 @@ def load_interfaces(brand_names):
def _get_interface_names() -> dict[str, list[str]]:
# returns a dict of brand name and its respective models
brand_names = {}
- for brand_name, brand_models in get_interface_attr("CAR").items():
- brand_names[brand_name] = [model.value for model in brand_models]
+ for brand in BRANDS:
+ brand_name = brand.__module__.split('.')[-2]
+ brand_names[brand_name] = [model.value for model in brand]
return brand_names
diff --git a/opendbc/car/chrysler/carcontroller.py b/opendbc/car/chrysler/carcontroller.py
index 53bffe83b6..3d7c540a25 100644
--- a/opendbc/car/chrysler/carcontroller.py
+++ b/opendbc/car/chrysler/carcontroller.py
@@ -28,7 +28,7 @@ def update(self, CC, CC_SP, CS, now_nanos):
lkas_active = CC.latActive and self.lkas_control_bit_prev
# cruise buttons
- if (self.frame - self.last_button_frame)*DT_CTRL > 0.05:
+ if (self.frame - self.last_button_frame) * DT_CTRL > 0.05:
das_bus = 2 if self.CP.carFingerprint in RAM_CARS else 0
# ACC cancellation
diff --git a/opendbc/car/docs.py b/opendbc/car/docs.py
index 5287fa1c2a..07fe514ece 100755
--- a/opendbc/car/docs.py
+++ b/opendbc/car/docs.py
@@ -11,9 +11,10 @@
from opendbc.car.common.basedir import BASEDIR
from opendbc.car import gen_empty_fingerprint
from opendbc.car.structs import CarParams
-from opendbc.car.docs_definitions import CarDocs, Device, ExtraCarDocs, Column, ExtraCarsColumn, CommonFootnote, PartType
-from opendbc.car.car_helpers import interfaces, get_interface_attr
-from opendbc.car.values import Platform, PLATFORMS
+from opendbc.car.docs_definitions import BaseCarHarness, CarDocs, Device, ExtraCarDocs, Column, ExtraCarsColumn, CommonFootnote, PartType, SupportType
+from opendbc.car.car_helpers import interfaces
+from opendbc.car.interfaces import get_interface_attr
+from opendbc.car.values import Platform
from opendbc.car.mock.values import CAR as MOCK
from opendbc.car.extra_cars import CAR as EXTRA
@@ -21,16 +22,17 @@
EXTRA_CARS_MD_OUT = os.path.join(BASEDIR, "../", "../", "docs", "CARS.md")
EXTRA_CARS_MD_TEMPLATE = os.path.join(BASEDIR, "CARS_template.md")
+# TODO: merge these platforms into normal car ports with SupportType flag
ExtraPlatform = Platform | EXTRA
EXTRA_BRANDS = get_args(ExtraPlatform)
EXTRA_PLATFORMS: dict[str, ExtraPlatform] = {str(platform): platform for brand in EXTRA_BRANDS for platform in brand}
-def get_params_for_docs(model, platform) -> CarParams:
- cp_model, cp_platform = (model, platform) if model in interfaces else ("MOCK", MOCK.MOCK)
- CP: CarParams = interfaces[cp_model][0].get_params(cp_platform, fingerprint=gen_empty_fingerprint(),
- car_fw=[CarParams.CarFw(ecu=CarParams.Ecu.unknown)],
- experimental_long=True, docs=True)
+def get_params_for_docs(platform) -> CarParams:
+ cp_platform = platform if platform in interfaces else MOCK.MOCK
+ CP: CarParams = interfaces[cp_platform][0].get_params(cp_platform, fingerprint=gen_empty_fingerprint(),
+ car_fw=[CarParams.CarFw(ecu=CarParams.Ecu.unknown)],
+ experimental_long=True, docs=True)
return CP
@@ -41,13 +43,13 @@ def get_all_footnotes() -> dict[Enum, int]:
return {fn: idx + 1 for idx, fn in enumerate(all_footnotes)}
-def build_sorted_car_docs_list(platforms, footnotes=None, include_dashcam=False):
+def build_sorted_car_docs_list(platforms, footnotes=None):
collected_car_docs: list[CarDocs | ExtraCarDocs] = []
- for model, platform in platforms.items():
+ for platform in platforms.values():
car_docs = platform.config.car_docs
- CP = get_params_for_docs(model, platform)
+ CP = get_params_for_docs(platform)
- if (CP.dashcamOnly and not include_dashcam) or not len(car_docs):
+ if not len(car_docs):
continue
# A platform can include multiple car models
@@ -65,12 +67,7 @@ def build_sorted_car_docs_list(platforms, footnotes=None, include_dashcam=False)
# CAUTION: This function is imported by shop.comma.ai and comma.ai/vehicles, test changes carefully
def get_all_car_docs() -> list[CarDocs]:
collected_footnotes = get_all_footnotes()
- sorted_list: list[CarDocs] = build_sorted_car_docs_list(PLATFORMS, footnotes=collected_footnotes)
- return sorted_list
-
-
-def get_car_docs_with_extras() -> list[CarDocs | ExtraCarDocs]:
- sorted_list: list[CarDocs] = build_sorted_car_docs_list(EXTRA_PLATFORMS, include_dashcam=True)
+ sorted_list: list[CarDocs] = build_sorted_car_docs_list(EXTRA_PLATFORMS, footnotes=collected_footnotes)
return sorted_list
@@ -89,16 +86,8 @@ def generate_cars_md(all_car_docs: list[CarDocs], template_fn: str) -> str:
footnotes = [fn.value.text for fn in get_all_footnotes()]
cars_md: str = template.render(all_car_docs=all_car_docs, PartType=PartType,
group_by_make=group_by_make, footnotes=footnotes,
- Device=Device, Column=Column)
- return cars_md
-
-
-def generate_cars_md_with_extras(car_docs_with_extras: list[CarDocs | ExtraCarDocs], template_fn: str) -> str:
- with open(template_fn) as f:
- template = jinja2.Template(f.read(), trim_blocks=True, lstrip_blocks=True)
-
- cars_md: str = template.render(car_docs_with_extras=car_docs_with_extras, PartType=PartType,
- group_by_make=group_by_make, ExtraCarsColumn=ExtraCarsColumn)
+ Device=Device, Column=Column, ExtraCarsColumn=ExtraCarsColumn,
+ BaseCarHarness=BaseCarHarness, SupportType=SupportType)
return cars_md
@@ -111,5 +100,5 @@ def generate_cars_md_with_extras(car_docs_with_extras: list[CarDocs | ExtraCarDo
args = parser.parse_args()
with open(args.out, 'w') as f:
- f.write(generate_cars_md_with_extras(get_car_docs_with_extras(), args.template))
+ f.write(generate_cars_md(get_all_car_docs(), args.template))
print(f"Generated and written to {args.out}")
diff --git a/opendbc/car/docs_definitions.py b/opendbc/car/docs_definitions.py
index 91ed5e85e8..bf44cd3d5a 100644
--- a/opendbc/car/docs_definitions.py
+++ b/opendbc/car/docs_definitions.py
@@ -98,6 +98,7 @@ class CarHarness(EnumBase):
nidec = BaseCarHarness("Honda Nidec connector")
bosch_a = BaseCarHarness("Honda Bosch A connector")
bosch_b = BaseCarHarness("Honda Bosch B connector")
+ bosch_c = BaseCarHarness("Honda Bosch C connector")
toyota_a = BaseCarHarness("Toyota A connector")
toyota_b = BaseCarHarness("Toyota B connector")
subaru_a = BaseCarHarness("Subaru A connector")
@@ -195,7 +196,7 @@ def all_parts(self):
class CommonFootnote(Enum):
EXP_LONG_AVAIL = CarFootnote(
"openpilot Longitudinal Control (Alpha) is available behind a toggle; " +
- "the toggle is only available in non-release branches such as `devel` or `master-ci`.",
+ "the toggle is only available in non-release branches such as `devel` or `nightly-dev`.",
Column.LONGITUDINAL, docs_only=True)
EXP_LONG_DSU = CarFootnote(
"By default, this car will use the stock Adaptive Cruise Control (ACC) for longitudinal control. " +
@@ -380,7 +381,7 @@ def get_detail_sentence(self, CP):
sentence_builder += " This car may not be able to take tight turns on its own."
# experimental mode
- exp_link = "Experimental mode"
+ exp_link = "Experimental mode"
if CP.openpilotLongitudinalControl and not CP.experimentalLongitudinalAvailable:
sentence_builder += f" Traffic light and stop sign handling is also available in {exp_link}."
@@ -388,7 +389,7 @@ def get_detail_sentence(self, CP):
else:
if CP.carFingerprint == "COMMA_BODY":
- return "The body is a robotics dev kit that can run openpilot. Learn more."
+ return "The body is a robotics dev kit that can run openpilot. Learn more."
else:
raise Exception(f"This notCar does not have a detail sentence: {CP.carFingerprint}")
diff --git a/opendbc/car/ford/carcontroller.py b/opendbc/car/ford/carcontroller.py
index 983b518fe4..b05bcd603b 100644
--- a/opendbc/car/ford/carcontroller.py
+++ b/opendbc/car/ford/carcontroller.py
@@ -9,15 +9,32 @@
LongCtrlState = structs.CarControl.Actuators.LongControlState
VisualAlert = structs.CarControl.HUDControl.VisualAlert
+# ISO 11270
+ISO_LATERAL_ACCEL = 3.0 # m/s^2 # TODO: import from test lateral limits file?
-def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw, steering_angle, lat_active):
+# Limit to average banked road since safety doesn't have the roll
+EARTH_G = 9.81
+AVERAGE_ROAD_ROLL = 0.06 # ~3.4 degrees, 6% superelevation
+MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - (EARTH_G * AVERAGE_ROAD_ROLL) # ~2.4 m/s^2
+
+
+def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_curvature, v_ego_raw, steering_angle, lat_active, CP):
# No blending at low speed due to lack of torque wind-up and inaccurate current curvature
if v_ego_raw > 9:
apply_curvature = np.clip(apply_curvature, current_curvature - CarControllerParams.CURVATURE_ERROR,
current_curvature + CarControllerParams.CURVATURE_ERROR)
# Curvature rate limit after driver torque limit
- return apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams)
+ apply_curvature = apply_std_steer_angle_limits(apply_curvature, apply_curvature_last, v_ego_raw, steering_angle, lat_active, CarControllerParams.ANGLE_LIMITS)
+
+ # Ford Q4/CAN FD has more torque available compared to Q3/CAN so we limit it based on lateral acceleration.
+ # Safety is not aware of the road roll so we subtract a conservative amount at all times
+ if CP.flags & FordFlags.CANFD:
+ # Limit curvature to conservative max lateral acceleration
+ curvature_accel_limit = MAX_LATERAL_ACCEL / (max(v_ego_raw, 1) ** 2)
+ apply_curvature = float(np.clip(apply_curvature, -curvature_accel_limit, curvature_accel_limit))
+
+ return apply_curvature
def apply_creep_compensation(accel: float, v_ego: float) -> float:
@@ -71,7 +88,7 @@ def update(self, CC, CC_SP, CS, now_nanos):
# apply rate limits, curvature error limit, and clip to signal range
current_curvature = -CS.out.yawRate / max(CS.out.vEgoRaw, 0.1)
self.apply_curvature_last = apply_ford_curvature_limits(actuators.curvature, self.apply_curvature_last, current_curvature,
- CS.out.vEgoRaw, 0., CC.latActive)
+ CS.out.vEgoRaw, 0., CC.latActive, self.CP)
if self.CP.flags & FordFlags.CANFD:
# TODO: extended mode
diff --git a/opendbc/car/ford/fingerprints.py b/opendbc/car/ford/fingerprints.py
index 13f1921751..03eedb1502 100644
--- a/opendbc/car/ford/fingerprints.py
+++ b/opendbc/car/ford/fingerprints.py
@@ -105,7 +105,7 @@
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x730, None): [
- b'RL38-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'
+ b'RL38-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
],
},
CAR.FORD_MUSTANG_MACH_E_MK1: {
diff --git a/opendbc/car/ford/interface.py b/opendbc/car/ford/interface.py
index 8b8a513d72..ef215f0dc9 100644
--- a/opendbc/car/ford/interface.py
+++ b/opendbc/car/ford/interface.py
@@ -1,5 +1,6 @@
import numpy as np
from opendbc.car import Bus, get_safety_config, structs
+from opendbc.car.carlog import carlog
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.ford.fordcan import CanBus
from opendbc.car.ford.values import CarControllerParams, DBC, Ecu, FordFlags, RADAR, FordSafetyFlags
@@ -47,17 +48,26 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime
if ret.flags & FordFlags.CANFD:
ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.CANFD.value
+
+ # TRON (SecOC) platforms are not supported
+ # LateralMotionControl2, ACCDATA are 16 bytes on these platforms
+ if len(fingerprint[CAN.camera]):
+ if fingerprint[CAN.camera].get(0x3d6) != 8 or fingerprint[CAN.camera].get(0x186) != 8:
+ carlog.error('dashcamOnly: SecOC is unsupported')
+ ret.dashcamOnly = True
else:
# Lock out if the car does not have needed lateral and longitudinal control APIs.
# Note that we also check CAN for adaptive cruise, but no known signal for LCA exists
pscm_config = next((fw for fw in car_fw if fw.ecu == Ecu.eps and b'\x22\xDE\x01' in fw.request), None)
if pscm_config:
if len(pscm_config.fwVersion) != 24:
+ carlog.error('dashcamOnly: Invalid EPS FW version')
ret.dashcamOnly = True
else:
config_tja = pscm_config.fwVersion[7] # Traffic Jam Assist
config_lca = pscm_config.fwVersion[8] # Lane Centering Assist
if config_tja != 0xFF or config_lca != 0xFF:
+ carlog.error('dashcamOnly: Car lacks required lateral control APIs')
ret.dashcamOnly = True
# Auto Transmission: 0x732 ECU or Gear_Shift_by_Wire_FD1
diff --git a/opendbc/car/ford/values.py b/opendbc/car/ford/values.py
index 4355fe6407..57f41ca2a7 100644
--- a/opendbc/car/ford/values.py
+++ b/opendbc/car/ford/values.py
@@ -3,10 +3,10 @@
from dataclasses import dataclass, field, replace
from enum import Enum, IntFlag
-from opendbc.car import AngleRateLimit, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
+from opendbc.car import AngleSteeringLimits, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
from opendbc.car.structs import CarParams
from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, \
- Device, SupportType
+ Device
from opendbc.car.fw_query_definitions import FwQueryConfig, LiveFwVersions, OfflineFwVersions, Request, StdQueries, p16
Ecu = CarParams.Ecu
@@ -20,16 +20,18 @@ class CarControllerParams:
ACC_UI_STEP = 20 # ACCDATA_3, 5Hz
BUTTONS_STEP = 5 # Steering_Data_FD1, 10Hz, but send twice as fast
- STEER_ANGLE_MAX = 0.02 # Max curvature for steering command, m^-1
STEER_DRIVER_ALLOWANCE = 1.0 # Driver intervention threshold, Nm
- # Curvature rate limits
- # Max curvature is limited by the EPS to an equivalent of ~2.0 m/s^2 at all speeds,
- # however max curvature rate linearly decreases as speed increases:
- # ~0.009 m^-1/sec at 7 m/s, ~0.002 m^-1/sec at 35 m/s
- # Limit to ~2 m/s^3 up, ~3.3 m/s^3 down at 75 mph and match EPS limit at low speed
- ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.00045, 0.0001])
- ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.00045, 0.00015])
+ ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
+ 0.02, # Max curvature for steering command, m^-1
+ # Curvature rate limits
+ # Max curvature is limited by the EPS to an equivalent of ~2.0 m/s^2 at all speeds,
+ # however max curvature rate linearly decreases as speed increases:
+ # ~0.009 m^-1/sec at 7 m/s, ~0.002 m^-1/sec at 35 m/s
+ # Limit to ~2 m/s^3 up, ~3.3 m/s^3 down at 75 mph and match EPS limit at low speed
+ ([5, 25], [0.00045, 0.0001]),
+ ([5, 25], [0.00045, 0.00015])
+ )
CURVATURE_ERROR = 0.002 # ~6 degrees at 10 m/s, ~10 degrees at 35 m/s
ACCEL_MAX = 2.0 # m/s^2 max acceleration
@@ -134,11 +136,11 @@ class CAR(Platforms):
CarSpecs(mass=2050, wheelbase=3.025, steerRatio=16.8),
)
FORD_F_150_MK14 = FordCANFDPlatformConfig(
- [FordCarDocs("Ford F-150 2021-23", "Co-Pilot360 Assist 2.0", hybrid=True, support_type=SupportType.REVIEW)],
+ [FordCarDocs("Ford F-150 2021-23", "Co-Pilot360 Assist 2.0", hybrid=True)],
CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0),
)
FORD_F_150_LIGHTNING_MK1 = FordF150LightningPlatform(
- [FordCarDocs("Ford F-150 Lightning 2022-23", "Co-Pilot360 Assist 2.0", support_type=SupportType.REVIEW)],
+ [FordCarDocs("Ford F-150 Lightning 2022-23", "Co-Pilot360 Assist 2.0")],
CarSpecs(mass=2948, wheelbase=3.70, steerRatio=16.9),
)
FORD_FOCUS_MK4 = FordPlatformConfig(
@@ -153,11 +155,11 @@ class CAR(Platforms):
CarSpecs(mass=1650, wheelbase=3.076, steerRatio=17.0),
)
FORD_MUSTANG_MACH_E_MK1 = FordCANFDPlatformConfig(
- [FordCarDocs("Ford Mustang Mach-E 2021-23", "All", support_type=SupportType.REVIEW)],
+ [FordCarDocs("Ford Mustang Mach-E 2021-23", "All")],
CarSpecs(mass=2200, wheelbase=2.984, steerRatio=17.0), # TODO: check steer ratio
)
FORD_RANGER_MK2 = FordCANFDPlatformConfig(
- [FordCarDocs("Ford Ranger 2024", "Adaptive Cruise Control with Lane Centering", support_type=SupportType.REVIEW)],
+ [FordCarDocs("Ford Ranger 2024", "Adaptive Cruise Control with Lane Centering")],
CarSpecs(mass=2000, wheelbase=3.27, steerRatio=17.0),
)
diff --git a/opendbc/car/honda/carcontroller.py b/opendbc/car/honda/carcontroller.py
index 6f05fa06d1..82f6fc09cd 100644
--- a/opendbc/car/honda/carcontroller.py
+++ b/opendbc/car/honda/carcontroller.py
@@ -117,7 +117,7 @@ def __init__(self, dbc_names, CP, CP_SP):
self.speed = 0.0
self.gas = 0.0
self.brake = 0.0
- self.last_steer = 0.0
+ self.last_torque = 0.0
def update(self, CC, CC_SP, CS, now_nanos):
MadsCarController.update(self, self.CP, CC, CC_SP)
@@ -135,9 +135,9 @@ def update(self, CC, CC_SP, CS, now_nanos):
gas, brake = 0.0, 0.0
# *** rate limit steer ***
- limited_steer = rate_limit(actuators.torque, self.last_steer, -self.params.STEER_DELTA_DOWN * DT_CTRL,
- self.params.STEER_DELTA_UP * DT_CTRL)
- self.last_steer = limited_steer
+ limited_torque = rate_limit(actuators.torque, self.last_torque, -self.params.STEER_DELTA_DOWN * DT_CTRL,
+ self.params.STEER_DELTA_UP * DT_CTRL)
+ self.last_torque = limited_torque
# *** apply brake hysteresis ***
pre_limit_brake, self.braking, self.brake_steady = actuator_hysteresis(brake, self.braking, self.brake_steady,
@@ -152,8 +152,8 @@ def update(self, CC, CC_SP, CS, now_nanos):
# **** process the car messages ****
# steer torque is converted back to CAN reference (positive when steering right)
- apply_torque = int(np.interp(-limited_steer * self.params.STEER_MAX,
- self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V))
+ apply_torque = int(np.interp(-limited_torque * self.params.STEER_MAX,
+ self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V))
# Send CAN commands
can_sends = []
@@ -246,7 +246,7 @@ def update(self, CC, CC_SP, CS, now_nanos):
new_actuators.accel = self.accel
new_actuators.gas = self.gas
new_actuators.brake = self.brake
- new_actuators.torque = self.last_steer
+ new_actuators.torque = self.last_torque
new_actuators.torqueOutputCan = apply_torque
self.frame += 1
diff --git a/opendbc/car/hyundai/carcontroller.py b/opendbc/car/hyundai/carcontroller.py
index 5ef7c3594a..85e5b0732b 100644
--- a/opendbc/car/hyundai/carcontroller.py
+++ b/opendbc/car/hyundai/carcontroller.py
@@ -203,7 +203,7 @@ def create_button_messages(self, CC: structs.CarControl, CS: CarState, use_clu11
self.last_button_frame = self.frame
else:
for _ in range(20):
- can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.CANCEL))
+ can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter + 1, Buttons.CANCEL))
self.last_button_frame = self.frame
# cruise standstill resume
@@ -213,7 +213,7 @@ def create_button_messages(self, CC: structs.CarControl, CS: CarState, use_clu11
pass
else:
for _ in range(20):
- can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter+1, Buttons.RES_ACCEL))
+ can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, CS.buttons_counter + 1, Buttons.RES_ACCEL))
self.last_button_frame = self.frame
return can_sends
diff --git a/opendbc/car/hyundai/carstate.py b/opendbc/car/hyundai/carstate.py
index 07052f02bf..2c8893140e 100644
--- a/opendbc/car/hyundai/carstate.py
+++ b/opendbc/car/hyundai/carstate.py
@@ -227,14 +227,15 @@ def update_canfd(self, can_parsers) -> structs.CarState:
# TODO: figure out positions
ret.wheelSpeeds = self.get_wheel_speeds(
- cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_1"],
- cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_2"],
- cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_3"],
- cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_4"],
+ cp.vl["WHEEL_SPEEDS"]["WHL_SpdFLVal"],
+ cp.vl["WHEEL_SPEEDS"]["WHL_SpdFRVal"],
+ cp.vl["WHEEL_SPEEDS"]["WHL_SpdRLVal"],
+ cp.vl["WHEEL_SPEEDS"]["WHL_SpdRRVal"],
)
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
- ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
+ ret.standstill = ret.wheelSpeeds.fl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.fr <= STANDSTILL_THRESHOLD and \
+ ret.wheelSpeeds.rl <= STANDSTILL_THRESHOLD and ret.wheelSpeeds.rr <= STANDSTILL_THRESHOLD
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"]
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"]
diff --git a/opendbc/car/hyundai/fingerprints.py b/opendbc/car/hyundai/fingerprints.py
index 464c116de6..9d7c783146 100644
--- a/opendbc/car/hyundai/fingerprints.py
+++ b/opendbc/car/hyundai/fingerprints.py
@@ -141,6 +141,7 @@
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00AEhe SCC F-CUP 1.00 1.00 99110-G2600 ',
b'\xf1\x00AEhe SCC FHCUP 1.00 1.00 99110-G2600 ',
+ b'\xf1\x00AEhe SCC FHCUP 1.00 1.02 99110-G2100 ',
],
(Ecu.eps, 0x7d4, None): [
b'\xf1\x00AE MDPS C 1.00 1.01 56310/G2510 4APHC101',
@@ -148,6 +149,7 @@
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00AEH MFC AT USA LHD 1.00 1.00 95740-G2700 201027',
+ b'\xf1\x00AEH MFC AT USA LHD 1.00 1.01 95740-G2600 190819',
],
},
CAR.HYUNDAI_SONATA: {
@@ -1009,6 +1011,7 @@
b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI000 210813',
b'\xf1\x00NE1 MFC AT EUR LHD 1.00 1.06 99211-GI010 230110',
b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.01 99211-GI010 211007',
+ b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.01 99211-GI100 240110',
b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.02 99211-GI010 211206',
b'\xf1\x00NE1 MFC AT IND RHD 1.00 1.07 99211-GI010 230620',
b'\xf1\x00NE1 MFC AT KOR LHD 1.00 1.00 99211-GI020 230719',
@@ -1021,7 +1024,6 @@
b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.03 99211-GI010 220401',
b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.05 99211-GI010 220614',
b'\xf1\x00NE1 MFC AT USA LHD 1.00 1.06 99211-GI010 230110',
- b'\xf1\x00NE1 MFC AT EUR RHD 1.00 1.01 99211-GI100 240110',
],
},
CAR.HYUNDAI_IONIQ_6: {
@@ -1089,16 +1091,16 @@
},
CAR.GENESIS_GV70_1ST_GEN: {
(Ecu.fwdCamera, 0x7c4, None): [
+ b'\xf1\x00JK1 MFC AT CAN LHD 1.00 1.02 99211-IY000 230627',
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.04 99211-AR000 210204',
- b'\xf1\x00JK1 MFC AT CAN LHD 1.00 1.02 99211-IY000 230627',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00JK1_ SCC FHCUP 1.00 1.00 99110-AR200 ',
b'\xf1\x00JK1_ SCC FHCUP 1.00 1.00 99110-AR300 ',
- b'\xf1\x00JK1_ SCC FHCUP 1.00 1.02 99110-AR000 ',
b'\xf1\x00JK1_ SCC FHCUP 1.00 1.00 99110-IY000 ',
+ b'\xf1\x00JK1_ SCC FHCUP 1.00 1.02 99110-AR000 ',
],
},
CAR.GENESIS_GV70_ELECTRIFIED_1ST_GEN: {
@@ -1203,32 +1205,32 @@
],
},
CAR.HYUNDAI_NEXO_1ST_GEN: {
- (Ecu.abs, 0x7D1, None): [
+ (Ecu.abs, 0x7d1, None): [
b'\xf1\x00FE IEB \x01 312 \x11\x13 58520-M5000',
],
- (Ecu.fwdCamera, 0x7C4, None): [
+ (Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00FE MFC AT KOR LHD 1.00 1.00 99211-M5100 201218',
],
- (Ecu.eps, 0x7D4, None): [
+ (Ecu.eps, 0x7d4, None): [
b'\xf1\x00FE MDPS C 1.00 1.05 56340-M5000 9903',
],
- (Ecu.fwdRadar, 0x7D0, None): [
+ (Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00FE__ SCC FHCUP 1.00 1.05 99110-M5000 ',
],
},
CAR.HYUNDAI_KONA_2022: {
- (Ecu.fwdCamera, 0x7C4, None): [
+ (Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00OSP LKA AT USA LHD 1.00 1.04 99211-J9200 904',
],
- (Ecu.eps, 0x7D4, None): [
+ (Ecu.eps, 0x7d4, None): [
b'\xf1\x00OSP MDPS C 1.00 1.04 56310/J9291 4OPCC104',
],
- (Ecu.fwdRadar, 0x7D0, None): [
+ (Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00YB__ FCA ----- 1.00 1.01 99110-J9000 \x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
- b'\xf1\x00T01G00BL T01I00A1 DOS2T16X4XI00NS0\x99L\xeeq',
b'\xf1\x00T01G00BL T01I00A1 DOS2T16X2XI00NS0\x8c`\xff\xe7',
+ b'\xf1\x00T01G00BL T01I00A1 DOS2T16X4XI00NS0\x99L\xeeq',
],
},
}
diff --git a/opendbc/car/hyundai/hyundaicanfd.py b/opendbc/car/hyundai/hyundaicanfd.py
index a3552d3db4..45dc06f461 100644
--- a/opendbc/car/hyundai/hyundaicanfd.py
+++ b/opendbc/car/hyundai/hyundaicanfd.py
@@ -1,3 +1,4 @@
+import copy
import numpy as np
from opendbc.car import CanBusBase
from opendbc.car.hyundai.values import HyundaiFlags
@@ -35,10 +36,7 @@ def CAM(self):
def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque, lkas_icon):
-
- ret = []
-
- values = {
+ common_values = {
"LKA_MODE": 2,
"LKA_ICON": lkas_icon,
"TORQUE_REQUEST": apply_torque,
@@ -46,17 +44,23 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque,
"STEER_REQ": 1 if lat_active else 0,
"STEER_MODE": 0,
"HAS_LANE_SAFETY": 0, # hide LKAS settings
- "NEW_SIGNAL_1": 0,
"NEW_SIGNAL_2": 0,
}
+ lkas_values = copy.copy(common_values)
+ lkas_values["LKA_AVAILABLE"] = 0
+
+ lfa_values = copy.copy(common_values)
+ lfa_values["NEW_SIGNAL_1"] = 0
+
+ ret = []
if CP.flags & HyundaiFlags.CANFD_LKA_STEERING:
lkas_msg = "LKAS_ALT" if CP.flags & HyundaiFlags.CANFD_LKA_STEERING_ALT else "LKAS"
if CP.openpilotLongitudinalControl:
- ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
- ret.append(packer.make_can_msg(lkas_msg, CAN.ACAN, values))
+ ret.append(packer.make_can_msg("LFA", CAN.ECAN, lfa_values))
+ ret.append(packer.make_can_msg(lkas_msg, CAN.ACAN, lkas_values))
else:
- ret.append(packer.make_can_msg("LFA", CAN.ECAN, values))
+ ret.append(packer.make_can_msg("LFA", CAN.ECAN, lfa_values))
return ret
@@ -120,7 +124,7 @@ def create_lfahda_cluster(packer, CAN, enabled, lfa_icon):
return packer.make_can_msg("LFAHDA_CLUSTER", CAN.ECAN, values)
-def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, main_cruise_enabled):
+def create_acc_control(packer, CAN, enabled, accel_last, accel, stopping, gas_override, set_speed, hud_control, main_cruise_enabled):
jerk = 5
jn = jerk / 50
if not enabled or gas_override:
diff --git a/opendbc/car/hyundai/values.py b/opendbc/car/hyundai/values.py
index 0d160d4941..856cd04cb1 100644
--- a/opendbc/car/hyundai/values.py
+++ b/opendbc/car/hyundai/values.py
@@ -223,7 +223,7 @@ class CAR(Platforms):
flags=HyundaiFlags.HYBRID | HyundaiFlags.MIN_STEER_32_MPH,
)
HYUNDAI_IONIQ_HEV_2022 = HyundaiPlatformConfig(
- [HyundaiCarDocs("Hyundai Ioniq Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_h]))], # TODO: confirm 2020-21 harness,
+ [HyundaiCarDocs("Hyundai Ioniq Hybrid 2020-22", car_parts=CarParts.common([CarHarness.hyundai_h]))],
CarSpecs(mass=1490, wheelbase=2.7, steerRatio=13.73, tireStiffnessFactor=0.385),
flags=HyundaiFlags.HYBRID | HyundaiFlags.LEGACY,
)
diff --git a/opendbc/car/mazda/carcontroller.py b/opendbc/car/mazda/carcontroller.py
index 65ff9a0a85..f851c86e45 100644
--- a/opendbc/car/mazda/carcontroller.py
+++ b/opendbc/car/mazda/carcontroller.py
@@ -23,7 +23,7 @@ def update(self, CC, CC_SP, CS, now_nanos):
# calculate steer and also set limits due to driver torque
new_torque = int(round(CC.actuators.torque * CarControllerParams.STEER_MAX))
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last,
- CS.out.steeringTorque, CarControllerParams)
+ CS.out.steeringTorque, CarControllerParams)
if CC.cruiseControl.cancel:
# If brake is pressed, let us wait >70ms before trying to disable crz to avoid
diff --git a/opendbc/car/nissan/carcontroller.py b/opendbc/car/nissan/carcontroller.py
index cb5b70d748..e066556f59 100644
--- a/opendbc/car/nissan/carcontroller.py
+++ b/opendbc/car/nissan/carcontroller.py
@@ -28,7 +28,7 @@ def update(self, CC, CC_SP, CS, now_nanos):
# windup slower
self.apply_angle_last = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw,
- CS.out.steeringAngleDeg, CC.latActive, CarControllerParams)
+ CS.out.steeringAngleDeg, CC.latActive, CarControllerParams.ANGLE_LIMITS)
lkas_max_torque = 0
if CC.latActive:
diff --git a/opendbc/car/nissan/values.py b/opendbc/car/nissan/values.py
index 267e7792c5..e64e41deec 100644
--- a/opendbc/car/nissan/values.py
+++ b/opendbc/car/nissan/values.py
@@ -1,7 +1,7 @@
from dataclasses import dataclass, field
from enum import IntFlag
-from opendbc.car import AngleRateLimit, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
+from opendbc.car import AngleSteeringLimits, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
from opendbc.car.structs import CarParams
from opendbc.car.docs_definitions import CarDocs, CarHarness, CarParts
from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
@@ -10,15 +10,17 @@
class CarControllerParams:
- ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., .8, .15])
- ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 15.], angle_v=[5., 3.5, 0.4])
+ ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
+ # When output steering Angle not within range -1311 and 1310,
+ # CANPacker packs wrong angle output to be decoded by panda
+ 600, # deg, reasonable limit
+ ([0., 5., 15.], [5., .8, .15]),
+ ([0., 5., 15.], [5., 3.5, 0.4]),
+ )
+
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower
STEER_THRESHOLD = 1.0
- # When output steering Angle not within range -1311 and 1310,
- # CANPacker packs wrong angle output to be decoded by panda
- STEER_ANGLE_MAX = 1310
-
def __init__(self, CP):
pass
diff --git a/opendbc/car/rivian/carcontroller.py b/opendbc/car/rivian/carcontroller.py
index 69cb59b66b..a7691ee146 100644
--- a/opendbc/car/rivian/carcontroller.py
+++ b/opendbc/car/rivian/carcontroller.py
@@ -1,7 +1,7 @@
from opendbc.can.packer import CANPacker
from opendbc.car import Bus, apply_driver_steer_torque_limits
from opendbc.car.interfaces import CarControllerBase
-from opendbc.car.rivian.riviancan import create_lka_steering, create_longitudinal, create_wheel_touch
+from opendbc.car.rivian.riviancan import create_lka_steering, create_longitudinal, create_wheel_touch, create_adas_status
from opendbc.car.rivian.values import CarControllerParams
@@ -11,6 +11,8 @@ def __init__(self, dbc_names, CP, CP_SP):
self.apply_torque_last = 0
self.packer = CANPacker(dbc_names[Bus.pt])
+ self.cancel_frames = 0
+
def update(self, CC, CC_SP, CS, now_nanos):
actuators = CC.actuators
can_sends = []
@@ -19,7 +21,7 @@ def update(self, CC, CC_SP, CS, now_nanos):
if CC.latActive:
new_torque = int(round(CC.actuators.torque * CarControllerParams.STEER_MAX))
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last,
- CS.out.steeringTorque, CarControllerParams)
+ CS.out.steeringTorque, CarControllerParams)
# send steering command
self.apply_torque_last = apply_torque
@@ -31,6 +33,17 @@ def update(self, CC, CC_SP, CS, now_nanos):
# Longitudinal control
if self.CP.openpilotLongitudinalControl:
can_sends.append(create_longitudinal(self.packer, self.frame % 15, actuators.accel, CC.enabled))
+ else:
+ interface_status = None
+ if CC.cruiseControl.cancel:
+ # if there is a noEntry, we need to send a status of "available" before the ACM will accept "unavailable"
+ # send "available" right away as the VDM itself takes a few frames to acknowledge
+ interface_status = 1 if self.cancel_frames < 5 else 0
+ self.cancel_frames += 1
+ else:
+ self.cancel_frames = 0
+
+ can_sends.append(create_adas_status(self.packer, CS.vdm_adas_status, interface_status))
new_actuators = actuators.as_builder()
new_actuators.torque = apply_torque / CarControllerParams.STEER_MAX
diff --git a/opendbc/car/rivian/carstate.py b/opendbc/car/rivian/carstate.py
index f0ea815347..d3860dbb2d 100644
--- a/opendbc/car/rivian/carstate.py
+++ b/opendbc/car/rivian/carstate.py
@@ -5,6 +5,8 @@
from opendbc.car.rivian.values import DBC, GEAR_MAP
from opendbc.car.common.conversions import Conversions as CV
+GearShifter = structs.CarState.GearShifter
+
class CarState(CarStateBase):
def __init__(self, CP, CP_SP):
@@ -13,6 +15,7 @@ def __init__(self, CP, CP_SP):
self.acm_lka_hba_cmd = None
self.sccm_wheel_touch = None
+ self.vdm_adas_status = None
def update(self, can_parsers) -> structs.CarState:
cp = can_parsers[Bus.pt]
@@ -49,13 +52,20 @@ def update(self, can_parsers) -> structs.CarState:
# TODO: find cruise set speed on CAN
ret.cruiseState.speed = self.last_speed * CV.MPH_TO_MS # detected speed limit
if not self.CP.openpilotLongitudinalControl:
- ret.cruiseState.speed = 0
+ ret.cruiseState.speed = -1
ret.cruiseState.available = True # cp.vl["VDM_AdasSts"]["VDM_AdasInterfaceStatus"] == 1
ret.cruiseState.standstill = cp.vl["VDM_AdasSts"]["VDM_AdasAccelRequestAcknowledged"] == 1
- ret.accFaulted = cp_cam.vl["ACM_Status"]["ACM_FaultStatus"] == 1
+
+ # TODO: log ACM_Unkown2=3 as a fault. need to filter it at the start and end of routes though
+ # ACM_FaultStatus hasn't been seen yet
+ ret.accFaulted = (cp_cam.vl["ACM_Status"]["ACM_FaultStatus"] == 1 or
+ # VDM_AdasFaultStatus=Brk_Intv is the default for some reason
+ # VDM_AdasFaultStatus=Imps_Cmd was seen when sending it rapidly changing ACC enable commands
+ # VDM_AdasFaultStatus=Cntr_Fault isn't fully understood, but we've seen it in the wild
+ cp.vl["VDM_AdasSts"]["VDM_AdasFaultStatus"] in (3,)) # 3=Imps_Cmd
# Gear
- ret.gearShifter = GEAR_MAP[int(cp.vl["VDM_PropStatus"]["VDM_Prndl_Status"])]
+ ret.gearShifter = GEAR_MAP.get(int(cp.vl["VDM_PropStatus"]["VDM_Prndl_Status"]), GearShifter.unknown)
# Doors
ret.doorOpen = (cp_adas.vl["IndicatorLights"]["RearDriverDoor"] != 2 or
@@ -80,6 +90,7 @@ def update(self, can_parsers) -> structs.CarState:
# Messages needed by carcontroller
self.acm_lka_hba_cmd = copy.copy(cp_cam.vl["ACM_lkaHbaCmd"])
self.sccm_wheel_touch = copy.copy(cp.vl["SCCM_WheelTouch"])
+ self.vdm_adas_status = copy.copy(cp.vl["VDM_AdasSts"])
return ret
diff --git a/opendbc/car/rivian/riviancan.py b/opendbc/car/rivian/riviancan.py
index 7c826a09ff..9961306c7e 100644
--- a/opendbc/car/rivian/riviancan.py
+++ b/opendbc/car/rivian/riviancan.py
@@ -83,3 +83,26 @@ def create_longitudinal(packer, frame, accel, enabled):
data = packer.make_can_msg("ACM_longitudinalRequest", 0, values)[1]
values["ACM_longitudinalRequest_Checksum"] = checksum(data[1:], 0x1D, 0x12)
return packer.make_can_msg("ACM_longitudinalRequest", 0, values)
+
+
+def create_adas_status(packer, vdm_adas_status, interface_status):
+ values = {s: vdm_adas_status[s] for s in (
+ "VDM_AdasStatus_Checksum",
+ "VDM_AdasStatus_Counter",
+ "VDM_AdasDecelLimit",
+ "VDM_AdasDriverAccelPriorityStatus",
+ "VDM_AdasFaultStatus",
+ "VDM_AdasAccelLimit",
+ "VDM_AdasDriverModeStatus",
+ "VDM_AdasAccelRequest",
+ "VDM_AdasInterfaceStatus",
+ "VDM_AdasAccelRequestAcknowledged",
+ "VDM_AdasVehicleHoldStatus",
+ )}
+
+ if interface_status is not None:
+ values["VDM_AdasInterfaceStatus"] = interface_status
+
+ data = packer.make_can_msg("VDM_AdasSts", 2, values)[1]
+ values["VDM_AdasStatus_Checksum"] = checksum(data[1:], 0x1D, 0xD1)
+ return packer.make_can_msg("VDM_AdasSts", 2, values)
diff --git a/opendbc/car/rivian/tests/__init__.py b/opendbc/car/rivian/tests/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/opendbc/car/rivian/tests/test_rivian.py b/opendbc/car/rivian/tests/test_rivian.py
new file mode 100644
index 0000000000..90dcfcd5dd
--- /dev/null
+++ b/opendbc/car/rivian/tests/test_rivian.py
@@ -0,0 +1,23 @@
+from opendbc.car.rivian.fingerprints import FW_VERSIONS
+from opendbc.car.rivian.values import CAR, FW_QUERY_CONFIG, WMI, ModelLine, ModelYear
+
+
+class TestRivian:
+ def test_custom_fuzzy_fingerprinting(self, subtests):
+ for platform in CAR:
+ with subtests.test(platform=platform.name):
+ for wmi in WMI:
+ for line in ModelLine:
+ for year in ModelYear:
+ for bad in (True, False):
+ vin = ["0"] * 17
+ vin[:3] = wmi
+ vin[3] = line.value
+ vin[9] = year.value
+ if bad:
+ vin[3] = "Z"
+ vin = "".join(vin)
+
+ matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy({}, vin, FW_VERSIONS)
+ should_match = year != ModelYear.S_2025 and not bad
+ assert (matches == {platform}) == should_match, "Bad match"
diff --git a/opendbc/car/rivian/values.py b/opendbc/car/rivian/values.py
index 1769489b36..c0fdb8bb20 100644
--- a/opendbc/car/rivian/values.py
+++ b/opendbc/car/rivian/values.py
@@ -1,13 +1,10 @@
from dataclasses import dataclass, field
from enum import StrEnum, IntFlag
-from opendbc.car.structs import CarParams
-from opendbc.car import Bus, structs
-from opendbc.car import CarSpecs, PlatformConfig, Platforms
+from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, structs
from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts, Device
from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
-
-Ecu = CarParams.Ecu
+from opendbc.car.vin import Vin
class WMI(StrEnum):
@@ -15,24 +12,60 @@ class WMI(StrEnum):
RIVIAN_MPV = "7PD"
+class ModelLine(StrEnum):
+ R1T = "T" # R1T 4-door Pickup Truck
+ R1S = "S" # R1S 4-door MPV
+
+
+class ModelYear(StrEnum):
+ N_2022 = "N"
+ P_2023 = "P"
+ R_2024 = "R"
+ S_2025 = "S"
+
+
@dataclass
class RivianCarDocs(CarDocs):
package: str = "All"
car_parts: CarParts = field(default_factory=CarParts([Device.threex_angled_mount, CarHarness.rivian]))
+@dataclass
+class RivianPlatformConfig(PlatformConfig):
+ dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'rivian_primary_actuator', Bus.radar: 'rivian_mando_front_radar_generated'})
+ wmis: set[WMI] = field(default_factory=set)
+ lines: set[ModelLine] = field(default_factory=set)
+ years: set[ModelYear] = field(default_factory=set)
+
+
class CAR(Platforms):
- RIVIAN_R1_GEN1 = PlatformConfig(
+ RIVIAN_R1_GEN1 = RivianPlatformConfig(
# TODO: verify this
[
RivianCarDocs("Rivian R1S 2022-24"),
RivianCarDocs("Rivian R1T 2022-24"),
],
CarSpecs(mass=3206., wheelbase=3.08, steerRatio=15.2),
- {Bus.pt: 'rivian_primary_actuator', Bus.radar: 'rivian_mando_front_radar_generated'}
+ wmis={WMI.RIVIAN_TRUCK, WMI.RIVIAN_MPV},
+ lines={ModelLine.R1T, ModelLine.R1S},
+ years={ModelYear.N_2022, ModelYear.P_2023, ModelYear.R_2024},
)
+def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str]:
+ # Rivian VIN reference: https://www.rivianforums.com/forum/threads/rivian-vin-decoder.1546
+ vin_obj = Vin(vin)
+ line = vin_obj.vds[:1]
+ year = vin_obj.vis[:1]
+
+ candidates = set()
+ for platform in CAR:
+ if vin_obj.wmi in platform.config.wmis and line in platform.config.lines and year in platform.config.years:
+ candidates.add(platform)
+
+ return {str(c) for c in candidates}
+
+
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
Request(
@@ -41,16 +74,17 @@ class CAR(Platforms):
rx_offset=0x40,
bus=0,
)
- ]
+ ],
+ match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
)
-GEAR_MAP = [
- structs.CarState.GearShifter.unknown,
- structs.CarState.GearShifter.park,
- structs.CarState.GearShifter.reverse,
- structs.CarState.GearShifter.neutral,
- structs.CarState.GearShifter.drive,
-]
+GEAR_MAP = {
+ 0: structs.CarState.GearShifter.unknown,
+ 1: structs.CarState.GearShifter.park,
+ 2: structs.CarState.GearShifter.reverse,
+ 3: structs.CarState.GearShifter.neutral,
+ 4: structs.CarState.GearShifter.drive,
+}
class CarControllerParams:
diff --git a/opendbc/car/subaru/carcontroller.py b/opendbc/car/subaru/carcontroller.py
index 83473683a7..bfe0bf6545 100644
--- a/opendbc/car/subaru/carcontroller.py
+++ b/opendbc/car/subaru/carcontroller.py
@@ -50,7 +50,7 @@ def update(self, CC, CC_SP, CS, now_nanos):
# Steering rate fault prevention
self.steer_rate_counter, apply_steer_req = \
common_fault_avoidance(abs(CS.out.steeringRateDeg) > MAX_STEER_RATE, apply_steer_req,
- self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
+ self.steer_rate_counter, MAX_STEER_RATE_FRAMES)
can_sends.append(subarucan.create_steering_control(self.packer, apply_torque, apply_steer_req))
diff --git a/opendbc/car/subaru/fingerprints.py b/opendbc/car/subaru/fingerprints.py
index d36916859e..53f1e6f0b9 100644
--- a/opendbc/car/subaru/fingerprints.py
+++ b/opendbc/car/subaru/fingerprints.py
@@ -523,16 +523,16 @@
(Ecu.fwdCamera, 0x787, None): [
b'\x04!\x01\x1eD\x07!\x00\x04,',
b'\x04!\x08\x01.\x07!\x08\x022',
- b'\r!\x08\x017\n!\x08\x003',
b'\r!\x08\x017\x00\x00\x00\x00\x00',
+ b'\r!\x08\x017\n!\x08\x003',
],
(Ecu.engine, 0x7e0, None): [
b'\xd5"`0\x07',
b'\xd5"a0\x07',
b'\xf1"`q\x07',
b'\xf1"aq\x07',
- b'\xfa"ap\x07',
b'\xfa"`p\x07',
+ b'\xfa"ap\x07',
],
(Ecu.transmission, 0x7e1, None): [
b'\x1d\x86B0\x00',
diff --git a/opendbc/car/tesla/carcontroller.py b/opendbc/car/tesla/carcontroller.py
index d3b8cba1fc..7c58697797 100644
--- a/opendbc/car/tesla/carcontroller.py
+++ b/opendbc/car/tesla/carcontroller.py
@@ -26,7 +26,7 @@ def update(self, CC, CC_SP, CS, now_nanos):
if self.frame % 2 == 0:
# Angular rate limit based on speed
self.apply_angle_last = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgo,
- CS.out.steeringAngleDeg, CC.latActive, CarControllerParams)
+ CS.out.steeringAngleDeg, CC.latActive, CarControllerParams.ANGLE_LIMITS)
can_sends.append(self.tesla_can.create_steering_control(self.apply_angle_last, lat_active, (self.frame // 2) % 16))
@@ -39,13 +39,13 @@ def update(self, CC, CC_SP, CS, now_nanos):
state = 13 if cruise_cancel else 4 # 4=ACC_ON, 13=ACC_CANCEL_GENERIC_SILENT
accel = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
cntr = (self.frame // 4) % 8
- can_sends.append(self.tesla_can.create_longitudinal_command(state, accel, cntr, CC.longActive))
+ can_sends.append(self.tesla_can.create_longitudinal_command(state, accel, cntr, CS.out.vEgo, CC.longActive))
else:
# Increment counter so cancel is prioritized even without openpilot longitudinal
if cruise_cancel:
cntr = (CS.das_control["DAS_controlCounter"] + 1) % 8
- can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, False))
+ can_sends.append(self.tesla_can.create_longitudinal_command(13, 0, cntr, CS.out.vEgo, False))
# TODO: HUD control
new_actuators = actuators.as_builder()
diff --git a/opendbc/car/tesla/carstate.py b/opendbc/car/tesla/carstate.py
index 152d1cd93e..ebb53fae4a 100644
--- a/opendbc/car/tesla/carstate.py
+++ b/opendbc/car/tesla/carstate.py
@@ -56,9 +56,9 @@ def update(self, can_parsers) -> structs.CarState:
ret.cruiseState.enabled = cruise_state in ("ENABLED", "STANDSTILL", "OVERRIDE", "PRE_FAULT", "PRE_CANCEL")
if speed_units == "KPH":
- ret.cruiseState.speed = cp_party.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS
+ ret.cruiseState.speed = max(cp_party.vl["DI_state"]["DI_digitalSpeed"] * CV.KPH_TO_MS, 1e-3)
elif speed_units == "MPH":
- ret.cruiseState.speed = cp_party.vl["DI_state"]["DI_digitalSpeed"] * CV.MPH_TO_MS
+ ret.cruiseState.speed = max(cp_party.vl["DI_state"]["DI_digitalSpeed"] * CV.MPH_TO_MS, 1e-3)
ret.cruiseState.available = cruise_state == "STANDBY" or ret.cruiseState.enabled
ret.cruiseState.standstill = False # This needs to be false, since we can resume from stop without sending anything special
ret.standstill = cruise_state == "STANDSTILL"
diff --git a/opendbc/car/tesla/fingerprints.py b/opendbc/car/tesla/fingerprints.py
index 81e686cb2c..b39c3f6339 100644
--- a/opendbc/car/tesla/fingerprints.py
+++ b/opendbc/car/tesla/fingerprints.py
@@ -24,6 +24,8 @@
b'TeMYG4_DCS_Update_0.0.0 (13),Y4002.27.1',
b'TeMYG4_DCS_Update_0.0.0 (13),Y4P002.27.1',
b'TeMYG4_DCS_Update_0.0.0 (9),Y4P002.25.0',
+ b'TeMYG4_Legacy3Y_0.0.0 (2),Y4003.02.0',
+ b'TeMYG4_Legacy3Y_0.0.0 (2),Y4P003.02.0',
b'TeMYG4_SingleECU_0.0.0 (33),Y4S002.26',
],
},
diff --git a/opendbc/car/tesla/teslacan.py b/opendbc/car/tesla/teslacan.py
index cbb5783014..328a681d76 100644
--- a/opendbc/car/tesla/teslacan.py
+++ b/opendbc/car/tesla/teslacan.py
@@ -1,3 +1,4 @@
+from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.interfaces import V_CRUISE_MAX
from opendbc.car.tesla.values import CANBUS, CarControllerParams
@@ -24,10 +25,14 @@ def create_steering_control(self, angle, enabled, counter):
values["DAS_steeringControlChecksum"] = self.checksum(0x488, data[:3])
return self.packer.make_can_msg("DAS_steeringControl", CANBUS.party, values)
- def create_longitudinal_command(self, acc_state, accel, cntr, active):
- values = {
+ def create_longitudinal_command(self, acc_state, accel, cntr, v_ego, active):
+ set_speed = v_ego * CV.MS_TO_KPH
+ if active:
# TODO: this causes jerking after gas override when above set speed
- "DAS_setSpeed": 0 if (accel < 0 or not active) else V_CRUISE_MAX,
+ set_speed = 0 if accel < 0 else V_CRUISE_MAX
+
+ values = {
+ "DAS_setSpeed": set_speed,
"DAS_accState": acc_state,
"DAS_aebEvent": 0,
"DAS_jerkMin": CarControllerParams.JERK_LIMIT_MIN,
diff --git a/opendbc/car/tesla/values.py b/opendbc/car/tesla/values.py
index f0b9cf95a9..8453a73f51 100644
--- a/opendbc/car/tesla/values.py
+++ b/opendbc/car/tesla/values.py
@@ -1,6 +1,6 @@
from dataclasses import dataclass, field
from enum import Enum, IntFlag
-from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, AngleRateLimit
+from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, AngleSteeringLimits
from opendbc.car.structs import CarParams, CarState
from opendbc.car.docs_definitions import CarDocs, CarFootnote, CarHarness, CarParts, Column
from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
@@ -82,20 +82,22 @@ class CANBUS:
class CarControllerParams:
- # Angle rate limits are set using the Tesla Model Y VehicleModel such that they maximally meet ISO 11270
- # At 5 m/s, FSD has been seen hitting up to ~4 deg/frame with ~5 deg/frame at very low creeping speeds
- # At 30 m/s, FSD has been seen hitting mostly 0.1 deg/frame, sometimes 0.2 deg/frame, and rarely 0.3 deg/frame
- ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[0., 5., 25.], angle_v=[2.5, 1.5, 0.2])
- ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[0., 5., 25.], angle_v=[5., 2.0, 0.3])
+ ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
+ # EPAS faults above this angle
+ 360, # deg
+ # Angle rate limits are set using the Tesla Model Y VehicleModel such that they maximally meet ISO 11270
+ # At 5 m/s, FSD has been seen hitting up to ~4 deg/frame with ~5 deg/frame at very low creeping speeds
+ # At 30 m/s, FSD has been seen hitting mostly 0.1 deg/frame, sometimes 0.2 deg/frame, and rarely 0.3 deg/frame
+ ([0., 5., 25.], [2.5, 1.5, 0.2]),
+ ([0., 5., 25.], [5., 2.0, 0.3]),
+ )
+
STEER_STEP = 2 # Angle command is sent at 50 Hz
ACCEL_MAX = 2.0 # m/s^2
ACCEL_MIN = -3.48 # m/s^2
JERK_LIMIT_MAX = 4.9 # m/s^3, ACC faults at 5.0
JERK_LIMIT_MIN = -4.9 # m/s^3, ACC faults at 5.0
- # EPAS faults above this angle
- STEER_ANGLE_MAX = 360 # deg
-
class TeslaSafetyFlags(IntFlag):
LONG_CONTROL = 1
diff --git a/opendbc/car/tests/test_can_fingerprint.py b/opendbc/car/tests/test_can_fingerprint.py
index 79863378f5..30dba60008 100644
--- a/opendbc/car/tests/test_can_fingerprint.py
+++ b/opendbc/car/tests/test_can_fingerprint.py
@@ -1,12 +1,11 @@
-from parameterized import parameterized
-
+import pytest
from opendbc.car.can_definitions import CanData
from opendbc.car.car_helpers import FRAME_FINGERPRINT, can_fingerprint
from opendbc.car.fingerprints import _FINGERPRINTS as FINGERPRINTS
class TestCanFingerprint:
- @parameterized.expand(list(FINGERPRINTS.items()))
+ @pytest.mark.parametrize("car_model, fingerprints", FINGERPRINTS.items())
def test_can_fingerprint(self, car_model, fingerprints):
"""Tests online fingerprinting function on offline fingerprints"""
diff --git a/opendbc/car/tests/test_car_interfaces.py b/opendbc/car/tests/test_car_interfaces.py
index b9fb380cf9..f3df16bfdf 100644
--- a/opendbc/car/tests/test_car_interfaces.py
+++ b/opendbc/car/tests/test_car_interfaces.py
@@ -1,8 +1,8 @@
import os
import math
import hypothesis.strategies as st
+import pytest
from hypothesis import Phase, given, settings
-from parameterized import parameterized
from collections.abc import Callable
from typing import Any
@@ -49,7 +49,7 @@ def get_fuzzy_car_interface_args(draw: DrawType) -> dict:
class TestCarInterfaces:
# FIXME: Due to the lists used in carParams, Phase.target is very slow and will cause
# many generated examples to overrun when max_examples > ~20, don't use it
- @parameterized.expand([(car,) for car in sorted(PLATFORMS)] + [MOCK.MOCK])
+ @pytest.mark.parametrize("car_name", sorted(PLATFORMS))
@settings(max_examples=MAX_EXAMPLES, deadline=None,
phases=(Phase.reuse, Phase.generate, Phase.shrink))
@given(data=st.data())
diff --git a/opendbc/car/tests/test_docs.py b/opendbc/car/tests/test_docs.py
index eeca3d9514..f09e847120 100644
--- a/opendbc/car/tests/test_docs.py
+++ b/opendbc/car/tests/test_docs.py
@@ -4,7 +4,7 @@
from opendbc.car.car_helpers import interfaces
from opendbc.car.docs import get_all_car_docs
-from opendbc.car.docs_definitions import Cable, Column, PartType, Star
+from opendbc.car.docs_definitions import Cable, Column, PartType, Star, SupportType
from opendbc.car.honda.values import CAR as HONDA
from opendbc.car.values import PLATFORMS
@@ -18,6 +18,9 @@ def test_duplicate_years(self, subtests):
make_model_years = defaultdict(list)
for car in self.all_cars:
with subtests.test(car_docs_name=car.name):
+ if car.support_type != SupportType.UPSTREAM:
+ pytest.skip()
+
make_model = (car.make, car.model)
for year in car.year_list:
assert year not in make_model_years[make_model], f"{car.name}: Duplicate model year"
@@ -63,7 +66,7 @@ def test_year_format(self, subtests):
def test_harnesses(self, subtests):
for car in self.all_cars:
with subtests.test(car=car.name):
- if car.name == "comma body":
+ if car.name == "comma body" or car.support_type != SupportType.UPSTREAM:
pytest.skip()
car_part_type = [p.part_type for p in car.car_parts.all_parts()]
diff --git a/opendbc/car/tests/test_fw_fingerprint.py b/opendbc/car/tests/test_fw_fingerprint.py
index 3c3341cdda..d46298dfe4 100644
--- a/opendbc/car/tests/test_fw_fingerprint.py
+++ b/opendbc/car/tests/test_fw_fingerprint.py
@@ -2,7 +2,6 @@
import random
import time
from collections import defaultdict
-from parameterized import parameterized
from opendbc.car.can_definitions import CanData
from opendbc.car.car_helpers import interfaces
@@ -24,7 +23,8 @@ def assertFingerprints(self, candidates, expected):
assert len(candidates) == 1, f"got more than one candidate: {candidates}"
assert candidates[0] == expected
- @parameterized.expand([(b, c, e[c], n) for b, e in VERSIONS.items() for c in e for n in (True, False)])
+ @pytest.mark.parametrize("brand, car_model, ecus, test_non_essential",
+ [(b, c, e[c], n) for b, e in VERSIONS.items() for c in e for n in (True, False)])
def test_exact_match(self, brand, car_model, ecus, test_non_essential):
config = FW_QUERY_CONFIGS[brand]
CP = CarParams()
@@ -48,7 +48,7 @@ def test_exact_match(self, brand, car_model, ecus, test_non_essential):
if len(matches) != 0:
self.assertFingerprints(matches, car_model)
- @parameterized.expand([(b, c, e[c]) for b, e in VERSIONS.items() for c in e])
+ @pytest.mark.parametrize("brand, car_model, ecus", [(b, c, e[c]) for b, e in VERSIONS.items() for c in e])
def test_custom_fuzzy_match(self, brand, car_model, ecus):
# Assert brand-specific fuzzy fingerprinting function doesn't disagree with standard fuzzy function
config = FW_QUERY_CONFIGS[brand]
@@ -70,7 +70,7 @@ def test_custom_fuzzy_match(self, brand, car_model, ecus):
if len(matches) == 1 and len(brand_matches) == 1:
assert matches == brand_matches
- @parameterized.expand([(b, c, e[c]) for b, e in VERSIONS.items() for c in e])
+ @pytest.mark.parametrize("brand, car_model, ecus", [(b, c, e[c]) for b, e in VERSIONS.items() for c in e])
def test_fuzzy_match_ecu_count(self, brand, car_model, ecus):
# Asserts that fuzzy matching does not count matching FW, but ECU address keys
valid_ecus = [e for e in ecus if e[0] not in FUZZY_EXCLUDE_ECUS]
diff --git a/opendbc/car/tests/test_routes.py b/opendbc/car/tests/test_routes.py
index 5c7ab12e83..92176909db 100644
--- a/opendbc/car/tests/test_routes.py
+++ b/opendbc/car/tests/test_routes.py
@@ -1,10 +1,10 @@
-from parameterized import parameterized
+import pytest
from opendbc.car.values import PLATFORMS
from opendbc.car.tests.routes import non_tested_cars, routes
-@parameterized.expand(PLATFORMS.keys())
+@pytest.mark.parametrize("platform", PLATFORMS.keys())
def test_test_route_present(platform):
tested_platforms = [r.car_model for r in routes]
assert platform in set(tested_platforms) | set(non_tested_cars), \
diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py
index c4bd0b406f..e90f77cdcb 100644
--- a/opendbc/car/toyota/carcontroller.py
+++ b/opendbc/car/toyota/carcontroller.py
@@ -51,7 +51,7 @@ class CarController(CarControllerBase):
def __init__(self, dbc_names, CP, CP_SP):
super().__init__(dbc_names, CP, CP_SP)
self.params = CarControllerParams(self.CP)
- self.last_steer = 0
+ self.last_torque = 0
self.last_angle = 0
self.alert_active = False
self.last_standstill = False
@@ -101,7 +101,7 @@ def update(self, CC, CC_SP, CS, now_nanos):
# *** steer torque ***
new_torque = int(round(actuators.torque * self.params.STEER_MAX))
- apply_torque = apply_meas_steer_torque_limits(new_torque, self.last_steer, CS.out.steeringTorqueEps, self.params)
+ apply_torque = apply_meas_steer_torque_limits(new_torque, self.last_torque, CS.out.steeringTorqueEps, self.params)
# >100 degree/sec steering fault prevention
self.steer_rate_counter, apply_steer_req = common_fault_avoidance(abs(CS.out.steeringRateDeg) >= MAX_STEER_RATE, lat_active,
@@ -122,9 +122,9 @@ def update(self, CC, CC_SP, CS, now_nanos):
# Angular rate limit based on speed
self.last_angle = apply_std_steer_angle_limits(apply_angle, self.last_angle, CS.out.vEgoRaw,
CS.out.steeringAngleDeg + CS.out.steeringAngleOffsetDeg,
- CC.latActive, self.params)
+ CC.latActive, self.params.ANGLE_LIMITS)
- self.last_steer = apply_torque
+ self.last_torque = apply_torque
# toyota can trace shows STEERING_LKA at 42Hz, with counter adding alternatively 1 and 2;
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
diff --git a/opendbc/car/toyota/carstate.py b/opendbc/car/toyota/carstate.py
index 366f0ee050..470f6212fb 100644
--- a/opendbc/car/toyota/carstate.py
+++ b/opendbc/car/toyota/carstate.py
@@ -161,7 +161,8 @@ def update(self, can_parsers) -> structs.CarState:
# send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1
if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in UNSUPPORTED_DSU_CAR) or \
(self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1):
- ret.accFaulted = ret.accFaulted or cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
+ if self.CP.openpilotLongitudinalControl:
+ ret.accFaulted = ret.accFaulted or cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
self.pcm_acc_status = cp.vl["PCM_CRUISE"]["CRUISE_STATE"]
if self.CP.carFingerprint not in (NO_STOP_TIMER_CAR - TSS2_CAR):
diff --git a/opendbc/car/toyota/values.py b/opendbc/car/toyota/values.py
index 80decf0636..889b9d0cb4 100644
--- a/opendbc/car/toyota/values.py
+++ b/opendbc/car/toyota/values.py
@@ -3,7 +3,7 @@
from dataclasses import dataclass, field
from enum import Enum, IntFlag
-from opendbc.car import Bus, CarSpecs, PlatformConfig, Platforms, AngleRateLimit
+from opendbc.car import Bus, CarSpecs, PlatformConfig, Platforms, AngleSteeringLimits
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.structs import CarParams
from opendbc.car.docs_definitions import CarFootnote, CarDocs, Column, CarParts, CarHarness
@@ -20,17 +20,18 @@ class CarControllerParams:
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
# Lane Tracing Assist (LTA) control limits
- # Assuming a steering ratio of 13.7:
- # Limit to ~2.0 m/s^3 up (7.5 deg/s), ~3.5 m/s^3 down (13 deg/s) at 75 mph
- # Worst case, the low speed limits will allow ~4.0 m/s^3 up (15 deg/s) and ~4.9 m/s^3 down (18 deg/s) at 75 mph,
- # however the EPS has its own internal limits at all speeds which are less than that:
- # Observed internal torque rate limit on TSS 2.5 Camry and RAV4 is ~1500 units/sec up and down when using LTA
- ANGLE_RATE_LIMIT_UP = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.3, 0.15])
- ANGLE_RATE_LIMIT_DOWN = AngleRateLimit(speed_bp=[5, 25], angle_v=[0.36, 0.26])
-
- # LTA limits
- # EPS ignores commands above this angle and causes PCS to fault
- STEER_ANGLE_MAX = 94.9461 # deg
+ ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
+ # EPS ignores commands above this angle and causes PCS to fault
+ 94.9461, # deg
+ # Assuming a steering ratio of 13.7:
+ # Limit to ~2.0 m/s^3 up (7.5 deg/s), ~3.5 m/s^3 down (13 deg/s) at 75 mph
+ # Worst case, the low speed limits will allow ~4.0 m/s^3 up (15 deg/s) and ~4.9 m/s^3 down (18 deg/s) at 75 mph,
+ # however the EPS has its own internal limits at all speeds which are less than that:
+ # Observed internal torque rate limit on TSS 2.5 Camry and RAV4 is ~1500 units/sec up and down when using LTA
+ ([5, 25], [0.3, 0.15]),
+ ([5, 25], [0.36, 0.26]),
+ )
+
MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows some resistance when changing lanes
def __init__(self, CP):
diff --git a/opendbc/car/vin.py b/opendbc/car/vin.py
index 61997be491..e373c76665 100644
--- a/opendbc/car/vin.py
+++ b/opendbc/car/vin.py
@@ -1,4 +1,5 @@
import re
+from dataclasses import dataclass, field
from opendbc.car import uds
from opendbc.car.carlog import carlog
@@ -9,6 +10,21 @@
VIN_RE = "[A-HJ-NPR-Z0-9]{17}"
+@dataclass
+class Vin:
+ vin: str
+ wmi: str = field(init=False)
+ vds: str = field(init=False)
+ vis: str = field(init=False)
+
+ def __post_init__(self):
+ # parses VIN in accordance with North America standard >2000 vehicles:
+ # https://en.wikipedia.org/wiki/Vehicle_identification_number#Components
+ self.wmi = self.vin[:3] # World Manufacturer Identifier
+ self.vds = self.vin[3:9] # Vehicle Descriptor Section
+ self.vis = self.vin[9:17] # Vehicle Identifier Section
+
+
def is_valid_vin(vin: str):
return re.fullmatch(VIN_RE, vin) is not None
diff --git a/opendbc/car/volkswagen/fingerprints.py b/opendbc/car/volkswagen/fingerprints.py
index ea21987e96..1162e3f945 100644
--- a/opendbc/car/volkswagen/fingerprints.py
+++ b/opendbc/car/volkswagen/fingerprints.py
@@ -352,9 +352,11 @@
CAR.VOLKSWAGEN_JETTA_MK6: {
(Ecu.srs, 0x715, None): [
b'\xf1\x875C0959655M \xf1\x890726\xf1\x82\t00NB1108--------24',
+ b'\xf1\x875K0959655H \xf1\x890724\xf1\x82\t00131108--------02',
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x877N0907572C \xf1\x890211\xf1\x82\x0151',
+ b'\xf1\x877N0907572C \xf1\x890211\xf1\x82\x0152',
],
},
CAR.VOLKSWAGEN_JETTA_MK7: {
diff --git a/opendbc/car/volkswagen/values.py b/opendbc/car/volkswagen/values.py
index aa50bc68c9..750a99e1c6 100644
--- a/opendbc/car/volkswagen/values.py
+++ b/opendbc/car/volkswagen/values.py
@@ -2,13 +2,13 @@
from dataclasses import dataclass, field
from enum import Enum, IntFlag, StrEnum
-from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
+from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, structs, uds
from opendbc.can.can_define import CANDefine
from opendbc.car.common.conversions import Conversions as CV
-from opendbc.car import structs
from opendbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, \
Device
from opendbc.car.fw_query_definitions import EcuAddrSubAddr, FwQueryConfig, Request, p16
+from opendbc.car.vin import Vin
Ecu = structs.CarParams.Ecu
NetworkLocation = structs.CarParams.NetworkLocation
@@ -453,8 +453,9 @@ def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str
all_ecu_versions[ecu] |= set(versions)
# Check the WMI and chassis code to determine the platform
- wmi = vin[:3]
- chassis_code = vin[6:8]
+ # https://www.clubvw.org.au/vwreference/vwvin
+ vin_obj = Vin(vin)
+ chassis_code = vin_obj.vds[3:5]
for platform in CAR:
valid_ecus = set()
@@ -474,7 +475,7 @@ def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str
if valid_ecus != CHECK_FUZZY_ECUS:
continue
- if wmi in platform.config.wmis and chassis_code in platform.config.chassis_codes:
+ if vin_obj.wmi in platform.config.wmis and chassis_code in platform.config.chassis_codes:
candidates.add(platform)
return {str(c) for c in candidates}
diff --git a/opendbc/dbc/generator/hyundai/_hyundai_common.dbc b/opendbc/dbc/generator/hyundai/_hyundai_common.dbc
index 4bf3ba3d14..edc5ded364 100644
--- a/opendbc/dbc/generator/hyundai/_hyundai_common.dbc
+++ b/opendbc/dbc/generator/hyundai/_hyundai_common.dbc
@@ -40,7 +40,7 @@ BO_ 80 LKAS: 16 XXX
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
SG_ LKA_MODE : 24|3@1+ (1,0) [0|7] "" XXX
- SG_ NEW_SIGNAL_1 : 27|2@1+ (1,0) [0|255] "" XXX
+ SG_ LKA_AVAILABLE : 27|2@1+ (1,0) [0|255] "" XXX
SG_ LKA_WARNING : 32|1@1+ (1,0) [0|1] "" XXX
SG_ LKA_ICON : 38|2@1+ (1,0) [0|255] "" XXX
SG_ FCA_SYSWARN : 40|1@0+ (1,0) [0|1] "" XXX
diff --git a/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc b/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc
index fd1cc62518..31f5a662e2 100644
--- a/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc
+++ b/opendbc/dbc/generator/hyundai/hyundai_canfd.dbc
@@ -41,10 +41,17 @@ BO_ 160 WHEEL_SPEEDS: 24 XXX
SG_ MOVING_BACKWARD : 57|1@0+ (1,0) [0|1] "" XXX
SG_ MOVING_FORWARD2 : 58|1@0+ (1,0) [0|1] "" XXX
SG_ MOVING_BACKWARD2 : 59|1@0+ (1,0) [0|1] "" XXX
- SG_ WHEEL_SPEED_1 : 64|16@1+ (0.03125,0) [0|65535] "kph" XXX
- SG_ WHEEL_SPEED_2 : 80|16@1+ (0.03125,0) [0|65535] "kph" XXX
- SG_ WHEEL_SPEED_3 : 96|16@1+ (0.03125,0) [0|65535] "kph" XXX
- SG_ WHEEL_SPEED_4 : 112|16@1+ (0.03125,0) [0|65535] "kph" XXX
+ SG_ WHL_SpdFLVal : 64|14@1+ (0.03125,0) [0|0] "km^h" XXX
+ SG_ WHL_SpdFRVal : 80|14@1+ (0.03125,0) [0|0] "km^h" XXX
+ SG_ WHL_SpdRLVal : 96|14@1+ (0.03125,0) [0|0] "km^h" XXX
+ SG_ WHL_SpdRRVal : 112|14@1+ (0.03125,0) [0|0] "km^h" XXX
+
+BO_ 203 LFA_ALT: 24 ADRV
+ SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
+ SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
+ SG_ LKAS_ANGLE_ACTIVE : 29|2@0+ (1,0) [0|1] "" XXX
+ SG_ LKAS_ANGLE_CMD : 32|14@1- (0.1,0) [0|511] "" XXX
+ SG_ LKAS_ANGLE_MAX_TORQUE : 55|8@0+ (1,0) [0|255] "" XXX
BO_ 234 MDPS: 24 XXX
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
@@ -55,6 +62,8 @@ BO_ 234 MDPS: 24 XXX
SG_ STEERING_COL_TORQUE : 80|13@1+ (1,-4095) [0|4095] "" XXX
SG_ STEERING_ANGLE : 96|16@1- (0.1,0) [0|255] "deg" XXX
SG_ STEERING_ANGLE_2 : 128|16@1- (0.1,0) [0|65535] "deg" XXX
+ SG_ LKA_ANGLE_ACTIVE : 145|2@0+ (1,0) [0|3] "" XXX
+ SG_ LKA_ANGLE_FAULT : 149|1@0+ (1,0) [0|1] "" XXX
BO_ 256 ACCELERATOR_BRAKE_ALT: 32 XXX
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
@@ -71,7 +80,7 @@ BO_ 272 LKAS_ALT: 32 XXX
SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
SG_ LKA_MODE : 24|3@1+ (1,0) [0|7] "" XXX
- SG_ NEW_SIGNAL_1 : 27|2@1+ (1,0) [0|255] "" XXX
+ SG_ LKA_AVAILABLE : 27|2@1+ (1,0) [0|255] "" XXX
SG_ LKA_WARNING : 32|1@1+ (1,0) [0|1] "" XXX
SG_ LKA_ICON : 38|2@1+ (1,0) [0|255] "" XXX
SG_ FCA_SYSWARN : 40|1@0+ (1,0) [0|1] "" XXX
@@ -81,7 +90,10 @@ BO_ 272 LKAS_ALT: 32 XXX
SG_ LKA_ASSIST : 62|1@1+ (1,0) [0|1] "" XXX
SG_ STEER_MODE : 65|3@1+ (1,0) [0|1] "" XXX
SG_ NEW_SIGNAL_2 : 70|2@0+ (1,0) [0|3] "" XXX
+ SG_ LKAS_ANGLE_ACTIVE : 77|2@0+ (1,0) [0|3] "" XXX
SG_ HAS_LANE_SAFETY : 80|1@0+ (1,0) [0|1] "" XXX
+ SG_ LKAS_ANGLE_CMD : 82|14@1- (0.1,0) [0|511] "" XXX
+ SG_ LKAS_ANGLE_MAX_TORQUE : 96|8@1+ (1,0) [0|255] "" XXX
SG_ NEW_SIGNAL_3 : 111|8@0+ (1,0) [0|255] "" XXX
BO_ 293 STEERING_SENSORS: 16 XXX
@@ -577,6 +589,8 @@ BO_ 1264 LOCAL_TIME: 8 XXX
CM_ SG_ 96 BRAKE_PRESSURE "User applied brake pedal pressure. Ramps from computer applied pressure on falling edge of cruise. Cruise cancels if !=0";
CM_ SG_ 101 BRAKE_POSITION "User applied brake pedal position, max is ~700. Signed on some vehicles";
CM_ BO_ 272 "Alternative LKAS message, used on cars such as 2023 Ioniq 6, 2nd gen Kona. Matches LKAS except size is 32 bytes";
+CM_ SG_ 272 LKA_AVAILABLE "Angle control cars: 3 when LKA is generally available, goes to 0 during changes with LFA";
+CM_ SG_ 272 LKAS_ANGLE_CMD "tracks MDPS->STEERING_ANGLE when not engaged, not STEERING_SENSORS->STEERING_ANGLE";
CM_ SG_ 298 NEW_SIGNAL_4 "todo: figure out why always set to 9";
CM_ SG_ 352 SET_ME_9 "has something to do with AEB settings";
CM_ SG_ 373 ACCEnable "Likely a copy of CAN's TCS13->ACCEnable";
@@ -602,6 +616,7 @@ VAL_ 112 GEAR 0 "P" 5 "D" 6 "N" 7 "R";
VAL_ 234 LKA_FAULT 0 "ok" 1 "lka fault";
VAL_ 272 LKA_MODE 1 "warning only" 2 "assist" 6 "off";
VAL_ 272 LKA_ICON 0 "hidden" 1 "grey" 2 "green" 3 "flashing green";
+VAL_ 272 LKAS_ANGLE_ACTIVE 0 "off" 1 "not active" 2 "active";
VAL_ 298 LKA_MODE 1 "warning only" 2 "assist" 6 "off";
VAL_ 298 LKA_ICON 0 "hidden" 1 "grey" 2 "green" 3 "flashing green";
VAL_ 304 PARK_BUTTON 1 "Pressed" 2 "Not Pressed";
diff --git a/opendbc/dbc/rivian_primary_actuator.dbc b/opendbc/dbc/rivian_primary_actuator.dbc
index 9cbbcf516a..686ec6fe32 100644
--- a/opendbc/dbc/rivian_primary_actuator.dbc
+++ b/opendbc/dbc/rivian_primary_actuator.dbc
@@ -185,7 +185,7 @@ BO_ 354 VDM_AdasSts: 8 VDM
SG_ VDM_AdasStatus_Checksum : 7|8@0+ (1,0) [0|0] "" ACM
SG_ VDM_AdasStatus_Counter : 11|4@0+ (1,0) [0|0] "" ACM
SG_ VDM_AdasDecelLimit : 17|10@0+ (0.01,0) [0|10.23] "m/s^2" ACM
- SG_ VDM_AdasDriverAccelPriorityStatu : 19|2@0+ (1,0) [0|3] "" ACM
+ SG_ VDM_AdasDriverAccelPriorityStatus : 19|2@0+ (1,0) [0|3] "" ACM
SG_ VDM_AdasFaultStatus : 23|4@0+ (1,0) [0|15] "" ACM
SG_ VDM_AdasAccelLimit : 33|10@0+ (0.01,0) [0|10.23] "m/s^2" ACM
SG_ VDM_AdasDriverModeStatus : 36|3@0+ (1,0) [0|7] "" ACM
@@ -874,7 +874,7 @@ VAL_ 352 ACM_VehicleHoldRequired 0 "ACM_VEHICLEHOLDREQ_NO_REQUEST" 1 "ACM_VEHICL
VAL_ 352 ACM_PrndRequired 0 "ACM_PRNDREQ_PARK" 1 "ACM_PRNDREQ_REVERSE" 2 "ACM_PRNDREQ_NEUTRAL" 3 "ACM_PRNDREQ_DRIVE" 4 "ACM_PRNDREQ_NOT_USED";
VAL_ 352 ACM_longInterfaceEnable 0 "ACM_LONGIFEN_INIT" 1 "ACM_LONGIFEN_LONGITUDINAL_INTERFACE_ENABLE" 2 "ACM_LONGIFEN_LONGITUDINAL_INTERFACE_DISABLE" 3 "ACM_LONGIFEN_SNA";
VAL_ 352 ACM_AccelerationRequestType 0 "ACM_ACCELREQTYPE_INIT" 1 "ACM_ACCELREQTYPE_ACCEL_NEGATIVE" 2 "ACM_ACCELREQTYPE_ACCEL_POSITIVE" 3 "ACM_ACCELREQTYPE_SNA";
-VAL_ 354 VDM_AdasDriverAccelPriorityStatu 0 "VDM_AdasDriverAccelPriorityStatus_Driver" 1 "VDM_AdasDriverAccelPriorityStatus_Adas";
+VAL_ 354 VDM_AdasDriverAccelPriorityStatus 0 "VDM_AdasDriverAccelPriorityStatus_Driver" 1 "VDM_AdasDriverAccelPriorityStatus_Adas";
VAL_ 354 VDM_AdasFaultStatus 0 "VDM_AdasFlaultStatus_No_Fault" 1 "VDM_AdasFaultStatus_Brk_Intv" 2 "VDM_AdasFlaultStatus_Cntr_Fault" 3 "VDM_AdasFlaultStatus_Imps_Cmd" 15 "VDM_AdasFlaultStatus_Sna";
VAL_ 354 VDM_AdasDriverModeStatus 0 "VDM_AdasDriverModeStatus_Human" 1 "VDM_AdasDriverModeStatus_Adas" 2 "VDM_AdasDriverModeStatus_Reserved" 3 "VDM_AdasDriverModeStatus_Sna";
VAL_ 354 VDM_AdasInterfaceStatus 0 "VDM_AdasInterfaceStatus_Unavailable" 1 "VDM_AdasInterfaceStatus_Available" 2 "VDM_AdasInterfaceStatus_Enabled" 3 "VDM_AdasInterfaceStatus_Faulted";
diff --git a/opendbc/safety/safety.h b/opendbc/safety/safety.h
index a422e105c0..f5bca715d4 100644
--- a/opendbc/safety/safety.h
+++ b/opendbc/safety/safety.h
@@ -57,6 +57,7 @@
#define SAFETY_BODY 27U
#define SAFETY_HYUNDAI_CANFD 28U
#define SAFETY_RIVIAN 33U
+#define SAFETY_VOLKSWAGEN_MEB 34U
uint32_t GET_BYTES(const CANPacket_t *msg, int start, int len) {
uint32_t ret = 0U;
@@ -178,28 +179,28 @@ static void update_counter(RxCheck addr_list[], int index, uint8_t counter) {
}
static bool rx_msg_safety_check(const CANPacket_t *to_push,
- const safety_config *cfg,
- const safety_hooks *safety_hooks) {
+ const safety_config *cfg,
+ const safety_hooks *safety_hooks) {
int index = get_addr_check_index(to_push, cfg->rx_checks, cfg->rx_checks_len);
update_addr_timestamp(cfg->rx_checks, index);
if (index != -1) {
// checksum check
- if ((safety_hooks->get_checksum != NULL) && (safety_hooks->compute_checksum != NULL) && cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].check_checksum) {
+ if ((safety_hooks->get_checksum != NULL) && (safety_hooks->compute_checksum != NULL) && !cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_checksum) {
uint32_t checksum = safety_hooks->get_checksum(to_push);
uint32_t checksum_comp = safety_hooks->compute_checksum(to_push);
cfg->rx_checks[index].status.valid_checksum = checksum_comp == checksum;
} else {
- cfg->rx_checks[index].status.valid_checksum = true;
+ cfg->rx_checks[index].status.valid_checksum = cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_checksum;
}
- // counter check (max_counter == 0 means skip check)
+ // counter check
if ((safety_hooks->get_counter != NULL) && (cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].max_counter > 0U)) {
uint8_t counter = safety_hooks->get_counter(to_push);
update_counter(cfg->rx_checks, index, counter);
} else {
- cfg->rx_checks[index].status.wrong_counters = 0U;
+ cfg->rx_checks[index].status.wrong_counters = cfg->rx_checks[index].msg[cfg->rx_checks[index].status.index].ignore_counter ? 0 : MAX_WRONG_COUNTERS;
}
// quality flag check
@@ -228,7 +229,7 @@ bool safety_rx_hook(const CANPacket_t *to_push) {
return valid;
}
-static bool msg_allowed(const CANPacket_t *to_send, const CanMsg msg_list[], int len) {
+static bool tx_msg_safety_check(const CANPacket_t *to_send, const CanMsg msg_list[], int len) {
int addr = GET_ADDR(to_send);
int bus = GET_BUS(to_send);
int length = GET_LEN(to_send);
@@ -244,13 +245,17 @@ static bool msg_allowed(const CANPacket_t *to_send, const CanMsg msg_list[], int
}
bool safety_tx_hook(CANPacket_t *to_send) {
- bool whitelisted = msg_allowed(to_send, current_safety_config.tx_msgs, current_safety_config.tx_msgs_len);
+ bool allowed = tx_msg_safety_check(to_send, current_safety_config.tx_msgs, current_safety_config.tx_msgs_len);
if ((current_safety_mode == SAFETY_ALLOUTPUT) || (current_safety_mode == SAFETY_ELM327)) {
- whitelisted = true;
+ allowed = true;
}
- const bool safety_allowed = current_hooks->tx(to_send);
- return !relay_malfunction && whitelisted && safety_allowed;
+ bool safety_allowed = false;
+ if (allowed) {
+ safety_allowed = current_hooks->tx(to_send);
+ }
+
+ return !relay_malfunction && allowed && safety_allowed;
}
int safety_fwd_hook(int bus_num, int addr) {
@@ -744,6 +749,32 @@ bool steer_angle_cmd_checks(int desired_angle, bool steer_control_enabled, const
highest_desired_angle = CLAMP(highest_desired_angle, -limits.max_angle, limits.max_angle);
}
+ // check not above ISO 11270 lateral accel assuming worst case road roll
+ if (limits.angle_is_curvature) {
+ // ISO 11270
+ static const float ISO_LATERAL_ACCEL = 3.0; // m/s^2
+
+ // Limit to average banked road since safety doesn't have the roll
+ static const float EARTH_G = 9.81;
+ static const float AVERAGE_ROAD_ROLL = 0.06; // ~3.4 degrees, 6% superelevation
+ 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 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);
+ } 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);
+ }
+ }
+
// check for violation;
violation |= max_limit_check(desired_angle, highest_desired_angle, lowest_desired_angle);
}
diff --git a/opendbc/safety/safety/safety_body.h b/opendbc/safety/safety/safety_body.h
index fcba1b577a..79d4b475ac 100644
--- a/opendbc/safety/safety/safety_body.h
+++ b/opendbc/safety/safety/safety_body.h
@@ -31,7 +31,7 @@ static bool body_tx_hook(const CANPacket_t *to_send) {
static safety_config body_init(uint16_t param) {
static RxCheck body_rx_checks[] = {
- {.msg = {{0x201, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}},
+ {.msg = {{0x201, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}, { 0 }, { 0 }}},
};
static const CanMsg BODY_TX_MSGS[] = {{0x250, 0, 8}, {0x250, 0, 6}, {0x251, 0, 5}, // body
diff --git a/opendbc/safety/safety/safety_chrysler.h b/opendbc/safety/safety/safety_chrysler.h
index 96590a2758..a2a79613ce 100644
--- a/opendbc/safety/safety/safety_chrysler.h
+++ b/opendbc/safety/safety/safety_chrysler.h
@@ -235,20 +235,20 @@ static safety_config chrysler_init(uint16_t param) {
};
static RxCheck chrysler_ram_dt_rx_checks[] = {
- {.msg = {{CHRYSLER_RAM_DT_ADDRS.EPS_2, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
- {.msg = {{CHRYSLER_RAM_DT_ADDRS.ESP_1, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{CHRYSLER_RAM_DT_ADDRS.ESP_8, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{CHRYSLER_RAM_DT_ADDRS.ECM_5, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{CHRYSLER_RAM_DT_ADDRS.DAS_3, 2, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{CHRYSLER_RAM_DT_ADDRS.EPS_2, 0, 8, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
+ {.msg = {{CHRYSLER_RAM_DT_ADDRS.ESP_1, 0, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{CHRYSLER_RAM_DT_ADDRS.ESP_8, 0, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{CHRYSLER_RAM_DT_ADDRS.ECM_5, 0, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{CHRYSLER_RAM_DT_ADDRS.DAS_3, 2, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
};
static RxCheck chrysler_rx_checks[] = {
- {.msg = {{CHRYSLER_ADDRS.EPS_2, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
- {.msg = {{CHRYSLER_ADDRS.ESP_1, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
- //{.msg = {{ESP_8, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}}},
- {.msg = {{514, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}},
- {.msg = {{CHRYSLER_ADDRS.ECM_5, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{CHRYSLER_ADDRS.DAS_3, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{CHRYSLER_ADDRS.EPS_2, 0, 8, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
+ {.msg = {{CHRYSLER_ADDRS.ESP_1, 0, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ //{.msg = {{ESP_8, 0, 8, .max_counter = 15U, .frequency = 50U}}},
+ {.msg = {{514, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}, { 0 }, { 0 }}},
+ {.msg = {{CHRYSLER_ADDRS.ECM_5, 0, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{CHRYSLER_ADDRS.DAS_3, 0, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
};
static const CanMsg CHRYSLER_TX_MSGS[] = {
@@ -280,11 +280,11 @@ static safety_config chrysler_init(uint16_t param) {
};
static RxCheck chrysler_ram_hd_rx_checks[] = {
- {.msg = {{CHRYSLER_RAM_HD_ADDRS.EPS_2, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
- {.msg = {{CHRYSLER_RAM_HD_ADDRS.ESP_1, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{CHRYSLER_RAM_HD_ADDRS.ESP_8, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{CHRYSLER_RAM_HD_ADDRS.ECM_5, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{CHRYSLER_RAM_HD_ADDRS.DAS_3, 2, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{CHRYSLER_RAM_HD_ADDRS.EPS_2, 0, 8, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
+ {.msg = {{CHRYSLER_RAM_HD_ADDRS.ESP_1, 0, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{CHRYSLER_RAM_HD_ADDRS.ESP_8, 0, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{CHRYSLER_RAM_HD_ADDRS.ECM_5, 0, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{CHRYSLER_RAM_HD_ADDRS.DAS_3, 2, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
};
static const CanMsg CHRYSLER_RAM_HD_TX_MSGS[] = {
diff --git a/opendbc/safety/safety/safety_defaults.h b/opendbc/safety/safety/safety_defaults.h
index 6716b57b3c..ca7a18c6f0 100644
--- a/opendbc/safety/safety/safety_defaults.h
+++ b/opendbc/safety/safety/safety_defaults.h
@@ -13,10 +13,13 @@ static safety_config nooutput_init(uint16_t param) {
return (safety_config){NULL, 0, NULL, 0};
}
+// GCOV_EXCL_START
+// Unreachable by design (doesn't define any tx msgs)
static bool nooutput_tx_hook(const CANPacket_t *to_send) {
UNUSED(to_send);
return false;
}
+// GCOV_EXCL_STOP
static int default_fwd_hook(int bus_num, int addr) {
UNUSED(bus_num);
diff --git a/opendbc/safety/safety/safety_ford.h b/opendbc/safety/safety/safety_ford.h
index b2ae5c2407..9bda8f0bb0 100644
--- a/opendbc/safety/safety/safety_ford.h
+++ b/opendbc/safety/safety/safety_ford.h
@@ -108,25 +108,28 @@ static bool ford_lkas_msg_check(int addr) {
}
// Curvature rate limits
-static const AngleSteeringLimits FORD_STEERING_LIMITS = {
- .max_angle = 1000, // 0.02 curvature
- .angle_deg_to_can = 50000, // 1 / (2e-5) rad to can
- .max_angle_error = 100, // 0.002 * FORD_STEERING_LIMITS.angle_deg_to_can
- .angle_rate_up_lookup = {
- {5., 25., 25.},
- {0.00045, 0.0001, 0.0001}
- },
- .angle_rate_down_lookup = {
- {5., 25., 25.},
- {0.00045, 0.00015, 0.00015}
- },
-
- // no blending at low speed due to lack of torque wind-up and inaccurate current curvature
- .angle_error_min_speed = 10.0, // m/s
-
- .enforce_angle_error = true,
- .inactive_angle_is_zero = true,
-};
+#define FORD_LIMITS(limit_lateral_acceleration) { \
+ .max_angle = 1000, /* 0.02 curvature */ \
+ .angle_deg_to_can = 50000, /* 1 / (2e-5) rad to can */ \
+ .max_angle_error = 100, /* 0.002 * FORD_STEERING_LIMITS.angle_deg_to_can */ \
+ .angle_rate_up_lookup = { \
+ {5., 25., 25.}, \
+ {0.00045, 0.0001, 0.0001} \
+ }, \
+ .angle_rate_down_lookup = { \
+ {5., 25., 25.}, \
+ {0.00045, 0.00015, 0.00015} \
+ }, \
+ \
+ /* no blending at low speed due to lack of torque wind-up and inaccurate current curvature */ \
+ .angle_error_min_speed = 10.0, /* m/s */ \
+ \
+ .angle_is_curvature = (limit_lateral_acceleration), \
+ .enforce_angle_error = true, \
+ .inactive_angle_is_zero = true, \
+}
+
+static const AngleSteeringLimits FORD_STEERING_LIMITS = FORD_LIMITS(false);
static void ford_rx_hook(const CANPacket_t *to_push) {
if (GET_BUS(to_push) == FORD_MAIN_BUS) {
@@ -158,6 +161,7 @@ static void ford_rx_hook(const CANPacket_t *to_push) {
// Update vehicle yaw rate
if (addr == FORD_Yaw_Data_FD1) {
// 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 = (((GET_BYTE(to_push, 2) << 8U) | GET_BYTE(to_push, 3)) * 0.0002) - 6.5;
float current_curvature = ford_yaw_rate / MAX(vehicle_speed.values[0] / VEHICLE_SPEED_FACTOR, 0.1);
// convert current curvature into units on CAN for comparison with desired curvature
@@ -296,6 +300,8 @@ static bool ford_tx_hook(const CANPacket_t *to_send) {
// Safety check for LateralMotionControl2 action
if (addr == FORD_LateralMotionControl2) {
+ static const AngleSteeringLimits FORD_CANFD_STEERING_LIMITS = FORD_LIMITS(true);
+
// Signal: LatCtl_D2_Rq
bool steer_control_enabled = ((GET_BYTE(to_send, 0) >> 4) & 0x7U) != 0U;
unsigned int raw_curvature = (GET_BYTE(to_send, 2) << 3) | (GET_BYTE(to_send, 3) >> 5);
@@ -308,7 +314,7 @@ static bool ford_tx_hook(const CANPacket_t *to_send) {
// Check angle error and steer_control_enabled
int desired_curvature = raw_curvature - FORD_INACTIVE_CURVATURE; // /FORD_STEERING_LIMITS.angle_deg_to_can to get real curvature
- violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_STEERING_LIMITS);
+ violation |= steer_angle_cmd_checks(desired_curvature, steer_control_enabled, FORD_CANFD_STEERING_LIMITS);
if (violation) {
tx = false;
@@ -356,16 +362,16 @@ static safety_config ford_init(uint16_t param) {
// warning: quality flags are not yet checked in openpilot's CAN parser,
// this may be the cause of blocked messages
static RxCheck ford_rx_checks[] = {
- {.msg = {{FORD_BrakeSysFeatures, 0, 8, .check_checksum = true, .max_counter = 15U, .quality_flag=true, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{FORD_BrakeSysFeatures, 0, 8, .max_counter = 15U, .quality_flag=true, .frequency = 50U}, { 0 }, { 0 }}},
// FORD_EngVehicleSpThrottle2 has a counter that either randomly skips or by 2, likely ECU bug
// Some hybrid models also experience a bug where this checksum mismatches for one or two frames under heavy acceleration with ACC
// It has been confirmed that the Bronco Sport's camera only disallows ACC for bad quality flags, not counters or checksums, so we match that
- {.msg = {{FORD_EngVehicleSpThrottle2, 0, 8, .check_checksum = false, .quality_flag=true, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{FORD_Yaw_Data_FD1, 0, 8, .check_checksum = true, .max_counter = 255U, .quality_flag=true, .frequency = 100U}, { 0 }, { 0 }}},
+ {.msg = {{FORD_EngVehicleSpThrottle2, 0, 8, .ignore_checksum = true, .ignore_counter = true, .quality_flag=true, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{FORD_Yaw_Data_FD1, 0, 8, .max_counter = 255U, .quality_flag=true, .frequency = 100U}, { 0 }, { 0 }}},
// These messages have no counter or checksum
- {.msg = {{FORD_EngBrakeData, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},
- {.msg = {{FORD_EngVehicleSpThrottle, 0, 8, .frequency = 100U}, { 0 }, { 0 }}},
- {.msg = {{FORD_DesiredTorqBrk, 0, 8, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{FORD_EngBrakeData, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U}, { 0 }, { 0 }}},
+ {.msg = {{FORD_EngVehicleSpThrottle, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}, { 0 }, { 0 }}},
+ {.msg = {{FORD_DesiredTorqBrk, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 50U}, { 0 }, { 0 }}},
};
#define FORD_COMMON_TX_MSGS \
@@ -397,26 +403,24 @@ static safety_config ford_init(uint16_t param) {
{FORD_LateralMotionControl, 0, 8},
};
- UNUSED(param);
+ const uint16_t FORD_PARAM_CANFD = 2;
+ ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD);
+
+ ford_longitudinal = false;
+
#ifdef ALLOW_DEBUG
const uint16_t FORD_PARAM_LONGITUDINAL = 1;
- const uint16_t FORD_PARAM_CANFD = 2;
ford_longitudinal = GET_FLAG(param, FORD_PARAM_LONGITUDINAL);
- ford_canfd = GET_FLAG(param, FORD_PARAM_CANFD);
#endif
// Longitudinal is the default for CAN, and optional for CAN FD w/ ALLOW_DEBUG
ford_longitudinal = !ford_canfd || ford_longitudinal;
safety_config ret;
- // FIXME: cppcheck thinks that ford_canfd is always false. This is not true
- // if ALLOW_DEBUG is defined but cppcheck is run without ALLOW_DEBUG
- // cppcheck-suppress knownConditionTrueFalse
if (ford_canfd) {
ret = ford_longitudinal ? BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(ford_rx_checks, FORD_CANFD_STOCK_TX_MSGS);
} else {
- // cppcheck-suppress knownConditionTrueFalse
ret = ford_longitudinal ? BUILD_SAFETY_CFG(ford_rx_checks, FORD_LONG_TX_MSGS) : \
BUILD_SAFETY_CFG(ford_rx_checks, FORD_STOCK_TX_MSGS);
}
diff --git a/opendbc/safety/safety/safety_gm.h b/opendbc/safety/safety/safety_gm.h
index 9ae83a3328..6de31cfcca 100644
--- a/opendbc/safety/safety/safety_gm.h
+++ b/opendbc/safety/safety/safety_gm.h
@@ -217,14 +217,14 @@ static safety_config gm_init(uint16_t param) {
// TODO: do checksum and counter checks. Add correct timestep, 0.1s for now.
static RxCheck gm_rx_checks[] = {
- {.msg = {{0x184, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},
- {.msg = {{0x34A, 0, 5, .frequency = 10U}, { 0 }, { 0 }}},
- {.msg = {{0x1E1, 0, 7, .frequency = 10U}, { 0 }, { 0 }}},
- {.msg = {{0xBE, 0, 6, .frequency = 10U}, // Volt, Silverado, Acadia Denali
- {0xBE, 0, 7, .frequency = 10U}, // Bolt EUV
- {0xBE, 0, 8, .frequency = 10U}}}, // Escalade
- {.msg = {{0x1C4, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},
- {.msg = {{0xC9, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},
+ {.msg = {{0x184, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U}, { 0 }, { 0 }}},
+ {.msg = {{0x34A, 0, 5, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U}, { 0 }, { 0 }}},
+ {.msg = {{0x1E1, 0, 7, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U}, { 0 }, { 0 }}},
+ {.msg = {{0xBE, 0, 6, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U}, // Volt, Silverado, Acadia Denali
+ {0xBE, 0, 7, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U}, // Bolt EUV
+ {0xBE, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U}}}, // Escalade
+ {.msg = {{0x1C4, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U}, { 0 }, { 0 }}},
+ {.msg = {{0xC9, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U}, { 0 }, { 0 }}},
};
static const CanMsg GM_CAM_TX_MSGS[] = {{0x180, 0, 4}, // pt bus
diff --git a/opendbc/safety/safety/safety_honda.h b/opendbc/safety/safety/safety_honda.h
index 1c220dbc01..a414c1961d 100644
--- a/opendbc/safety/safety/safety_honda.h
+++ b/opendbc/safety/safety/safety_honda.h
@@ -3,19 +3,19 @@
#include "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) \
- {.msg = {{0x1A6, (pt_bus), 8, .check_checksum = true, .max_counter = 3U, .frequency = 25U}, /* SCM_BUTTONS */ \
- {0x296, (pt_bus), 4, .check_checksum = true, .max_counter = 3U, .frequency = 25U}, { 0 }}}, \
- {.msg = {{0x158, (pt_bus), 8, .check_checksum = true, .max_counter = 3U, .frequency = 100U}, { 0 }, { 0 }}}, /* ENGINE_DATA */ \
- {.msg = {{0x17C, (pt_bus), 8, .check_checksum = true, .max_counter = 3U, .frequency = 100U}, { 0 }, { 0 }}}, /* POWERTRAIN_DATA */ \
+#define HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(pt_bus) \
+ {.msg = {{0x1A6, (pt_bus), 8, .max_counter = 3U, .frequency = 25U}, /* SCM_BUTTONS */ \
+ {0x296, (pt_bus), 4, .max_counter = 3U, .frequency = 25U}, { 0 }}}, \
+ {.msg = {{0x158, (pt_bus), 8, .max_counter = 3U, .frequency = 100U}, { 0 }, { 0 }}}, /* ENGINE_DATA */ \
+ {.msg = {{0x17C, (pt_bus), 8, .max_counter = 3U, .frequency = 100U}, { 0 }, { 0 }}}, /* POWERTRAIN_DATA */ \
-#define HONDA_COMMON_RX_CHECKS(pt_bus) \
- HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(pt_bus) \
- {.msg = {{0x326, (pt_bus), 8, .check_checksum = true, .max_counter = 3U, .frequency = 10U}, { 0 }, { 0 }}}, /* SCM_FEEDBACK */ \
+#define HONDA_COMMON_RX_CHECKS(pt_bus) \
+ HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(pt_bus) \
+ {.msg = {{0x326, (pt_bus), 8, .max_counter = 3U, .frequency = 10U}, { 0 }, { 0 }}}, /* SCM_FEEDBACK */ \
// Alternate brake message is used on some Honda Bosch, and Honda Bosch radarless (where PT bus is 0)
-#define HONDA_ALT_BRAKE_ADDR_CHECK(pt_bus) \
- {.msg = {{0x1BE, (pt_bus), 3, .check_checksum = true, .max_counter = 3U, .frequency = 50U}, { 0 }, { 0 }}}, /* BRAKE_MODULE */ \
+#define HONDA_ALT_BRAKE_ADDR_CHECK(pt_bus) \
+ {.msg = {{0x1BE, (pt_bus), 3, .max_counter = 3U, .frequency = 50U}, { 0 }, { 0 }}}, /* BRAKE_MODULE */ \
// Nidec and bosch radarless has the powertrain bus on bus 0
@@ -329,7 +329,7 @@ static safety_config honda_nidec_init(uint16_t param) {
if (enable_nidec_alt) {
// For Nidecs with main on signal on an alternate msg (missing 0x326)
- static RxCheck honda_nidec_alt_rx_checks[] = {
+ static RxCheck honda_nidec_alt_rx_checks[] = {
HONDA_COMMON_NO_SCM_FEEDBACK_RX_CHECKS(0)
};
diff --git a/opendbc/safety/safety/safety_hyundai.h b/opendbc/safety/safety/safety_hyundai.h
index 53bc1e6366..4ae824699e 100644
--- a/opendbc/safety/safety/safety_hyundai.h
+++ b/opendbc/safety/safety/safety_hyundai.h
@@ -39,15 +39,15 @@ const LongitudinalLimits HYUNDAI_LONG_LIMITS = {
{0x389, 0, 8}, /* SCC14 Bus 0 */ \
{0x4A2, 0, 2}, /* FRT_RADAR11 Bus 0 */ \
-#define HYUNDAI_COMMON_RX_CHECKS(legacy) \
- {.msg = {{0x260, 0, 8, .check_checksum = true, .max_counter = 3U, .frequency = 100U}, \
- {0x371, 0, 8, .frequency = 100U}, \
- {0x91, 0, 8, .frequency = 100U}}}, \
- {.msg = {{0x386, 0, 8, .check_checksum = !(legacy), .max_counter = (legacy) ? 0U : 15U, .frequency = 100U}, { 0 }, { 0 }}}, \
- {.msg = {{0x394, 0, 8, .check_checksum = !(legacy), .max_counter = (legacy) ? 0U : 7U, .frequency = 100U}, { 0 }, { 0 }}}, \
+#define HYUNDAI_COMMON_RX_CHECKS(legacy) \
+ {.msg = {{0x260, 0, 8, .max_counter = 3U, .frequency = 100U}, \
+ {0x371, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}, \
+ {0x91, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}}}, \
+ {.msg = {{0x386, 0, 8, .ignore_checksum = (legacy), .ignore_counter = (legacy), .max_counter = (legacy) ? 0U : 15U, .frequency = 100U}, { 0 }, { 0 }}}, \
+ {.msg = {{0x394, 0, 8, .ignore_checksum = (legacy), .ignore_counter = (legacy), .max_counter = (legacy) ? 0U : 7U, .frequency = 100U}, { 0 }, { 0 }}}, \
-#define HYUNDAI_SCC12_ADDR_CHECK(scc_bus) \
- {.msg = {{0x421, (scc_bus), 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
+#define HYUNDAI_SCC12_ADDR_CHECK(scc_bus) \
+ {.msg = {{0x421, (scc_bus), 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
static const CanMsg HYUNDAI_TX_MSGS[] = {
HYUNDAI_COMMON_TX_MSGS(0)
@@ -336,7 +336,7 @@ static safety_config hyundai_init(uint16_t param) {
static RxCheck hyundai_long_rx_checks[] = {
HYUNDAI_COMMON_RX_CHECKS(false)
// Use CLU11 (buttons) to manage controls allowed instead of SCC cruise state
- {.msg = {{0x4F1, 0, 4, .check_checksum = false, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{0x4F1, 0, 4, .ignore_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
};
if (hyundai_escc)
diff --git a/opendbc/safety/safety/safety_hyundai_canfd.h b/opendbc/safety/safety/safety_hyundai_canfd.h
index 765aeac98d..58facac8f0 100644
--- a/opendbc/safety/safety/safety_hyundai_canfd.h
+++ b/opendbc/safety/safety/safety_hyundai_canfd.h
@@ -25,19 +25,19 @@
// *** Addresses checked in rx hook ***
// EV, ICE, HYBRID: ACCELERATOR (0x35), ACCELERATOR_BRAKE_ALT (0x100), ACCELERATOR_ALT (0x105)
-#define HYUNDAI_CANFD_COMMON_RX_CHECKS(pt_bus) \
- {.msg = {{0x35, (pt_bus), 32, .check_checksum = true, .max_counter = 0xffU, .frequency = 100U}, \
- {0x100, (pt_bus), 32, .check_checksum = true, .max_counter = 0xffU, .frequency = 100U}, \
- {0x105, (pt_bus), 32, .check_checksum = true, .max_counter = 0xffU, .frequency = 100U}}}, \
- {.msg = {{0x175, (pt_bus), 24, .check_checksum = true, .max_counter = 0xffU, .frequency = 50U}, { 0 }, { 0 }}}, \
- {.msg = {{0xa0, (pt_bus), 24, .check_checksum = true, .max_counter = 0xffU, .frequency = 100U}, { 0 }, { 0 }}}, \
- {.msg = {{0xea, (pt_bus), 24, .check_checksum = true, .max_counter = 0xffU, .frequency = 100U}, { 0 }, { 0 }}}, \
- {.msg = {{0x1cf, (pt_bus), 8, .check_checksum = false, .max_counter = 0xfU, .frequency = 50U}, \
- {0x1aa, (pt_bus), 16, .check_checksum = false, .max_counter = 0xffU, .frequency = 50U}, { 0 }}}, \
+#define HYUNDAI_CANFD_COMMON_RX_CHECKS(pt_bus) \
+ {.msg = {{0x35, (pt_bus), 32, .max_counter = 0xffU, .frequency = 100U}, \
+ {0x100, (pt_bus), 32, .max_counter = 0xffU, .frequency = 100U}, \
+ {0x105, (pt_bus), 32, .max_counter = 0xffU, .frequency = 100U}}}, \
+ {.msg = {{0x175, (pt_bus), 24, .max_counter = 0xffU, .frequency = 50U}, { 0 }, { 0 }}}, \
+ {.msg = {{0xa0, (pt_bus), 24, .max_counter = 0xffU, .frequency = 100U}, { 0 }, { 0 }}}, \
+ {.msg = {{0xea, (pt_bus), 24, .max_counter = 0xffU, .frequency = 100U}, { 0 }, { 0 }}}, \
+ {.msg = {{0x1cf, (pt_bus), 8, .ignore_checksum = true, .max_counter = 0xfU, .frequency = 50U}, \
+ {0x1aa, (pt_bus), 16, .ignore_checksum = true, .max_counter = 0xffU, .frequency = 50U}, { 0 }}}, \
// SCC_CONTROL (from ADAS unit or camera)
-#define HYUNDAI_CANFD_SCC_ADDR_CHECK(scc_bus) \
- {.msg = {{0x1a0, (scc_bus), 32, .check_checksum = true, .max_counter = 0xffU, .frequency = 50U}, { 0 }, { 0 }}}, \
+#define HYUNDAI_CANFD_SCC_ADDR_CHECK(scc_bus) \
+ {.msg = {{0x1a0, (scc_bus), 32, .max_counter = 0xffU, .frequency = 50U}, { 0 }, { 0 }}}, \
static bool hyundai_canfd_alt_buttons = false;
static bool hyundai_canfd_lka_steering_alt = false;
@@ -110,9 +110,15 @@ static void hyundai_canfd_rx_hook(const CANPacket_t *to_push) {
// vehicle moving
if (addr == 0xa0) {
- uint32_t front_left_speed = GET_BYTES(to_push, 8, 2);
- uint32_t rear_right_speed = GET_BYTES(to_push, 14, 2);
- vehicle_moving = (front_left_speed > HYUNDAI_STANDSTILL_THRSLD) || (rear_right_speed > HYUNDAI_STANDSTILL_THRSLD);
+ uint32_t fl = (GET_BYTES(to_push, 8, 2)) & 0x3FFFU;
+ uint32_t fr = (GET_BYTES(to_push, 10, 2)) & 0x3FFFU;
+ uint32_t rl = (GET_BYTES(to_push, 12, 2)) & 0x3FFFU;
+ uint32_t rr = (GET_BYTES(to_push, 14, 2)) & 0x3FFFU;
+ vehicle_moving = (fl > HYUNDAI_STANDSTILL_THRSLD) || (fr > HYUNDAI_STANDSTILL_THRSLD) ||
+ (rl > HYUNDAI_STANDSTILL_THRSLD) || (rr > HYUNDAI_STANDSTILL_THRSLD);
+
+ // average of all 4 wheel speeds. Conversion: raw * 0.03125 / 3.6 = m/s
+ UPDATE_VEHICLE_SPEED((fr + rr + rl + fl) / 4.0 * 0.03125 / 3.6);
}
}
@@ -204,6 +210,11 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *to_send) {
violation |= longitudinal_accel_checks(desired_accel_val, HYUNDAI_LONG_LIMITS);
} else {
// only used to cancel on here
+ const int acc_mode = (GET_BYTE(to_send, 8) >> 4) & 0x7U;
+ if (acc_mode != 4) {
+ violation = true;
+ }
+
if ((desired_accel_raw != 0) || (desired_accel_val != 0)) {
violation = true;
}
diff --git a/opendbc/safety/safety/safety_mazda.h b/opendbc/safety/safety/safety_mazda.h
index 3e01ca96f5..b09bef21f5 100644
--- a/opendbc/safety/safety/safety_mazda.h
+++ b/opendbc/safety/safety/safety_mazda.h
@@ -113,11 +113,11 @@ static safety_config mazda_init(uint16_t param) {
static const CanMsg MAZDA_TX_MSGS[] = {{MAZDA_LKAS, 0, 8}, {MAZDA_CRZ_BTNS, 0, 8}, {MAZDA_LKAS_HUD, 0, 8}};
static RxCheck mazda_rx_checks[] = {
- {.msg = {{MAZDA_CRZ_CTRL, 0, 8, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{MAZDA_CRZ_BTNS, 0, 8, .frequency = 10U}, { 0 }, { 0 }}},
- {.msg = {{MAZDA_STEER_TORQUE, 0, 8, .frequency = 83U}, { 0 }, { 0 }}},
- {.msg = {{MAZDA_ENGINE_DATA, 0, 8, .frequency = 100U}, { 0 }, { 0 }}},
- {.msg = {{MAZDA_PEDALS, 0, 8, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{MAZDA_CRZ_CTRL, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{MAZDA_CRZ_BTNS, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U}, { 0 }, { 0 }}},
+ {.msg = {{MAZDA_STEER_TORQUE, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 83U}, { 0 }, { 0 }}},
+ {.msg = {{MAZDA_ENGINE_DATA, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}, { 0 }, { 0 }}},
+ {.msg = {{MAZDA_PEDALS, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 50U}, { 0 }, { 0 }}},
};
UNUSED(param);
diff --git a/opendbc/safety/safety/safety_nissan.h b/opendbc/safety/safety/safety_nissan.h
index ca428c38da..99f346d8cf 100644
--- a/opendbc/safety/safety/safety_nissan.h
+++ b/opendbc/safety/safety/safety_nissan.h
@@ -68,7 +68,7 @@ static void nissan_rx_hook(const CANPacket_t *to_push) {
static bool nissan_tx_hook(const CANPacket_t *to_send) {
const AngleSteeringLimits NISSAN_STEERING_LIMITS = {
- .max_angle = 131000, // 1310 deg, max CAN signal value
+ .max_angle = 60000, // 600 deg, reasonable limit
.angle_deg_to_can = 100,
.angle_rate_up_lookup = {
{0., 5., 15.},
@@ -144,18 +144,18 @@ static safety_config nissan_init(uint16_t param) {
// Signals duplicated below due to the fact that these messages can come in on either CAN bus, depending on car model.
static RxCheck nissan_rx_checks[] = {
- {.msg = {{0x2, 0, 5, .frequency = 100U},
- {0x2, 1, 5, .frequency = 100U}, { 0 }}}, // STEER_ANGLE_SENSOR
- {.msg = {{0x285, 0, 8, .frequency = 50U},
- {0x285, 1, 8, .frequency = 50U}, { 0 }}}, // WHEEL_SPEEDS_REAR
- {.msg = {{0x30f, 2, 3, .frequency = 10U},
- {0x30f, 1, 3, .frequency = 10U}, { 0 }}}, // CRUISE_STATE
- {.msg = {{0x15c, 0, 8, .frequency = 50U},
- {0x15c, 1, 8, .frequency = 50U},
- {0x239, 0, 8, .frequency = 50U}}}, // GAS_PEDAL
- {.msg = {{0x454, 0, 8, .frequency = 10U},
- {0x454, 1, 8, .frequency = 10U},
- {0x1cc, 0, 4, .frequency = 100U}}}, // DOORS_LIGHTS / BRAKE
+ {.msg = {{0x2, 0, 5, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U},
+ {0x2, 1, 5, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}, { 0 }}}, // STEER_ANGLE_SENSOR
+ {.msg = {{0x285, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 50U},
+ {0x285, 1, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 50U}, { 0 }}}, // WHEEL_SPEEDS_REAR
+ {.msg = {{0x30f, 2, 3, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U},
+ {0x30f, 1, 3, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U}, { 0 }}}, // CRUISE_STATE
+ {.msg = {{0x15c, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 50U},
+ {0x15c, 1, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 50U},
+ {0x239, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 50U}}}, // GAS_PEDAL
+ {.msg = {{0x454, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U},
+ {0x454, 1, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U},
+ {0x1cc, 0, 4, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}}}, // DOORS_LIGHTS / BRAKE
};
// EPS Location. false = V-CAN, true = C-CAN
diff --git a/opendbc/safety/safety/safety_rivian.h b/opendbc/safety/safety/safety_rivian.h
index 0e01f8f1a4..9f1f908b85 100644
--- a/opendbc/safety/safety/safety_rivian.h
+++ b/opendbc/safety/safety/safety_rivian.h
@@ -39,7 +39,8 @@ static void rivian_rx_hook(const CANPacket_t *to_push) {
if (bus == 2) {
// Cruise state
if (addr == 0x100) {
- pcm_cruise_check(GET_BIT(to_push, 21U));
+ const int feature_status = GET_BYTE(to_push, 2) >> 5U;
+ pcm_cruise_check(feature_status == 1);
}
}
}
@@ -80,12 +81,8 @@ static bool rivian_tx_hook(const CANPacket_t *to_send) {
// Longitudinal control
if (addr == 0x160) {
- if (rivian_longitudinal) {
- int raw_accel = ((GET_BYTE(to_send, 2) << 3) | (GET_BYTE(to_send, 3) >> 5)) - 1024U;
- if (longitudinal_accel_checks(raw_accel, RIVIAN_LONG_LIMITS)) {
- tx = false;
- }
- } else {
+ int raw_accel = ((GET_BYTE(to_send, 2) << 3) | (GET_BYTE(to_send, 3) >> 5)) - 1024U;
+ if (longitudinal_accel_checks(raw_accel, RIVIAN_LONG_LIMITS)) {
tx = false;
}
}
@@ -99,23 +96,28 @@ static int rivian_fwd_hook(int bus, int addr) {
bool block_msg = false;
if (bus == 0) {
- // SCCM_WheelTouch
+ // SCCM_WheelTouch: for hiding hold wheel alert
if (addr == 0x321) {
block_msg = true;
}
+ // VDM_AdasSts: for canceling stock ACC
+ if ((addr == 0x162) && !rivian_longitudinal) {
+ block_msg = true;
+ }
+
if (!block_msg) {
bus_fwd = 2;
}
}
if (bus == 2) {
- // ACM_lkaHbaCmd
+ // ACM_lkaHbaCmd: lateral control message
if (addr == 0x120) {
block_msg = true;
}
- // ACM_longitudinalRequest
+ // ACM_longitudinalRequest: longitudinal control message
if (rivian_longitudinal && (addr == 0x160)) {
block_msg = true;
}
@@ -129,15 +131,17 @@ static int rivian_fwd_hook(int bus, int addr) {
}
static safety_config rivian_init(uint16_t param) {
- // 0x120 = ACM_lkaHbaCmd, 0x160 = ACM_longitudinalRequest, 0x321 = SCCM_WheelTouch
- static const CanMsg RIVIAN_TX_MSGS[] = {{0x120, 0, 8}, {0x321, 2, 7}};
+ // 0x120 = ACM_lkaHbaCmd, 0x321 = SCCM_WheelTouch, 0x162 = VDM_AdasSts
+ static const CanMsg RIVIAN_TX_MSGS[] = {{0x120, 0, 8}, {0x321, 2, 7}, {0x162, 2, 8}};
+ // 0x160 = ACM_longitudinalRequest
static const CanMsg RIVIAN_LONG_TX_MSGS[] = {{0x120, 0, 8}, {0x321, 2, 7}, {0x160, 0, 5}};
static RxCheck rivian_rx_checks[] = {
- {.msg = {{0x208, 0, 8, .frequency = 50U}, { 0 }, { 0 }}}, // ESP_Status (speed)
- {.msg = {{0x150, 0, 7, .frequency = 50U}, { 0 }, { 0 }}}, // VDM_PropStatus (gas pedal)
- {.msg = {{0x38f, 0, 6, .frequency = 50U}, { 0 }, { 0 }}}, // iBESP2 (brakes)
- {.msg = {{0x100, 2, 8, .frequency = 100U}, { 0 }, { 0 }}}, // ACM_Status (cruise state)
+ {.msg = {{0x208, 0, 8, .frequency = 50U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, // ESP_Status (speed)
+ {.msg = {{0x380, 0, 5, .frequency = 100U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, // EPAS_SystemStatus (driver torque)
+ {.msg = {{0x150, 0, 7, .frequency = 50U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, // VDM_PropStatus (gas pedal)
+ {.msg = {{0x38f, 0, 6, .frequency = 50U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, // iBESP2 (brakes)
+ {.msg = {{0x100, 2, 8, .frequency = 100U, .ignore_checksum = true, .ignore_counter = true}, { 0 }, { 0 }}}, // ACM_Status (cruise state)
};
UNUSED(param);
diff --git a/opendbc/safety/safety/safety_subaru.h b/opendbc/safety/safety/safety_subaru.h
index 332478c86d..b6cd035c69 100644
--- a/opendbc/safety/safety/safety_subaru.h
+++ b/opendbc/safety/safety/safety_subaru.h
@@ -61,12 +61,12 @@
{MSG_SUBARU_ES_STATIC_1, SUBARU_MAIN_BUS, 8}, \
{MSG_SUBARU_ES_STATIC_2, SUBARU_MAIN_BUS, 8}, \
-#define SUBARU_COMMON_RX_CHECKS(alt_bus) \
- {.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}}, \
- {.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
- {.msg = {{MSG_SUBARU_Wheel_Speeds, alt_bus, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
- {.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
- {.msg = {{MSG_SUBARU_CruiseControl, alt_bus, 8, .check_checksum = true, .max_counter = 15U, .frequency = 20U}, { 0 }, { 0 }}}, \
+#define SUBARU_COMMON_RX_CHECKS(alt_bus) \
+ {.msg = {{MSG_SUBARU_Throttle, SUBARU_MAIN_BUS, 8, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}}, \
+ {.msg = {{MSG_SUBARU_Steering_Torque, SUBARU_MAIN_BUS, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
+ {.msg = {{MSG_SUBARU_Wheel_Speeds, alt_bus, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
+ {.msg = {{MSG_SUBARU_Brake_Status, alt_bus, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}}, \
+ {.msg = {{MSG_SUBARU_CruiseControl, alt_bus, 8, .max_counter = 15U, .frequency = 20U}, { 0 }, { 0 }}}, \
static bool subaru_gen2 = false;
static bool subaru_longitudinal = false;
diff --git a/opendbc/safety/safety/safety_subaru_preglobal.h b/opendbc/safety/safety/safety_subaru_preglobal.h
index c6a437f45a..9d07244653 100644
--- a/opendbc/safety/safety/safety_subaru_preglobal.h
+++ b/opendbc/safety/safety/safety_subaru_preglobal.h
@@ -111,9 +111,9 @@ static safety_config subaru_preglobal_init(uint16_t param) {
// TODO: do checksum and counter checks after adding the signals to the outback dbc file
static RxCheck subaru_preglobal_rx_checks[] = {
- {.msg = {{MSG_SUBARU_PG_Throttle, SUBARU_PG_MAIN_BUS, 8, .frequency = 100U}, { 0 }, { 0 }}},
- {.msg = {{MSG_SUBARU_PG_Steering_Torque, SUBARU_PG_MAIN_BUS, 8, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{MSG_SUBARU_PG_CruiseControl, SUBARU_PG_MAIN_BUS, 8, .frequency = 20U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_SUBARU_PG_Throttle, SUBARU_PG_MAIN_BUS, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_SUBARU_PG_Steering_Torque, SUBARU_PG_MAIN_BUS, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_SUBARU_PG_CruiseControl, SUBARU_PG_MAIN_BUS, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 20U}, { 0 }, { 0 }}},
};
const int SUBARU_PG_PARAM_REVERSED_DRIVER_TORQUE = 4;
diff --git a/opendbc/safety/safety/safety_tesla.h b/opendbc/safety/safety/safety_tesla.h
index 0715bf1e03..611a4edeb4 100644
--- a/opendbc/safety/safety/safety_tesla.h
+++ b/opendbc/safety/safety/safety_tesla.h
@@ -194,13 +194,13 @@ static safety_config tesla_init(uint16_t param) {
tesla_stock_aeb = false;
static RxCheck tesla_model3_y_rx_checks[] = {
- {.msg = {{0x2b9, 2, 8, .frequency = 25U}, { 0 }, { 0 }}}, // DAS_control
- {.msg = {{0x257, 0, 8, .frequency = 50U}, { 0 }, { 0 }}}, // DI_speed (speed in kph)
- {.msg = {{0x370, 0, 8, .frequency = 100U}, { 0 }, { 0 }}}, // EPAS3S_internalSAS (steering angle)
- {.msg = {{0x118, 0, 8, .frequency = 100U}, { 0 }, { 0 }}}, // DI_systemStatus (gas pedal)
- {.msg = {{0x39d, 0, 5, .frequency = 25U}, { 0 }, { 0 }}}, // IBST_status (brakes)
- {.msg = {{0x286, 0, 8, .frequency = 10U}, { 0 }, { 0 }}}, // DI_state (acc state)
- {.msg = {{0x311, 0, 7, .frequency = 10U}, { 0 }, { 0 }}}, // UI_warning (blinkers, buckle switch & doors)
+ {.msg = {{0x2b9, 2, 8, .ignore_checksum = true, .ignore_counter = true,.frequency = 25U}, { 0 }, { 0 }}}, // DAS_control
+ {.msg = {{0x257, 0, 8, .ignore_checksum = true, .ignore_counter = true,.frequency = 50U}, { 0 }, { 0 }}}, // DI_speed (speed in kph)
+ {.msg = {{0x370, 0, 8, .ignore_checksum = true, .ignore_counter = true,.frequency = 100U}, { 0 }, { 0 }}}, // EPAS3S_internalSAS (steering angle)
+ {.msg = {{0x118, 0, 8, .ignore_checksum = true, .ignore_counter = true,.frequency = 100U}, { 0 }, { 0 }}}, // DI_systemStatus (gas pedal)
+ {.msg = {{0x39d, 0, 5, .ignore_checksum = true, .ignore_counter = true,.frequency = 25U}, { 0 }, { 0 }}}, // IBST_status (brakes)
+ {.msg = {{0x286, 0, 8, .ignore_checksum = true, .ignore_counter = true,.frequency = 10U}, { 0 }, { 0 }}}, // DI_state (acc state)
+ {.msg = {{0x311, 0, 7, .ignore_checksum = true, .ignore_counter = true,.frequency = 10U}, { 0 }, { 0 }}}, // UI_warning (blinkers, buckle switch & doors)
};
return BUILD_SAFETY_CFG(tesla_model3_y_rx_checks, TESLA_M3_Y_TX_MSGS);
diff --git a/opendbc/safety/safety/safety_toyota.h b/opendbc/safety/safety/safety_toyota.h
index f26c1a586d..da052febed 100644
--- a/opendbc/safety/safety/safety_toyota.h
+++ b/opendbc/safety/safety/safety_toyota.h
@@ -21,14 +21,14 @@
{0x411, 0, 8}, /* PCS_HUD */ \
{0x750, 0, 8}, /* radar diagnostic address */ \
-#define TOYOTA_COMMON_RX_CHECKS(lta) \
- {.msg = {{ 0xaa, 0, 8, .check_checksum = false, .frequency = 83U}, { 0 }, { 0 }}}, \
- {.msg = {{0x260, 0, 8, .check_checksum = true, .quality_flag = (lta), .frequency = 50U}, { 0 }, { 0 }}}, \
- {.msg = {{0x1D2, 0, 8, .check_checksum = true, .frequency = 33U}, \
- {0x176, 0, 8, .check_checksum = true, .frequency = 32U}, { 0 }}}, \
- {.msg = {{0x101, 0, 8, .check_checksum = false, .frequency = 50U}, \
- {0x224, 0, 8, .check_checksum = false, .frequency = 40U}, \
- {0x226, 0, 8, .check_checksum = false, .frequency = 40U}}}, \
+#define TOYOTA_COMMON_RX_CHECKS(lta) \
+ {.msg = {{ 0xaa, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 83U}, { 0 }, { 0 }}}, \
+ {.msg = {{0x260, 0, 8, .ignore_counter = true, .quality_flag = (lta), .frequency = 50U}, { 0 }, { 0 }}}, \
+ {.msg = {{0x1D2, 0, 8, .ignore_counter = true, .frequency = 33U}, \
+ {0x176, 0, 8, .ignore_counter = true, .frequency = 32U}, { 0 }}}, \
+ {.msg = {{0x101, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 50U}, \
+ {0x224, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 40U}, \
+ {0x226, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 40U}}}, \
static bool toyota_secoc = false;
static bool toyota_alt_brake = false;
diff --git a/opendbc/safety/safety/safety_volkswagen_mqb.h b/opendbc/safety/safety/safety_volkswagen_mqb.h
index 6598fa7ff1..be13c6fd9c 100644
--- a/opendbc/safety/safety/safety_volkswagen_mqb.h
+++ b/opendbc/safety/safety/safety_volkswagen_mqb.h
@@ -16,13 +16,13 @@ static safety_config volkswagen_mqb_init(uint16_t param) {
{MSG_ACC_02, 0, 8}, {MSG_ACC_06, 0, 8}, {MSG_ACC_07, 0, 8}};
static RxCheck volkswagen_mqb_rx_checks[] = {
- {.msg = {{MSG_ESP_19, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}},
- {.msg = {{MSG_LH_EPS_03, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
- {.msg = {{MSG_ESP_05, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{MSG_TSK_06, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{MSG_MOTOR_20, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{MSG_MOTOR_14, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 10U}, { 0 }, { 0 }}},
- {.msg = {{MSG_GRA_ACC_01, 0, 8, .check_checksum = true, .max_counter = 15U, .frequency = 33U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_ESP_19, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_LH_EPS_03, 0, 8, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_ESP_05, 0, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_TSK_06, 0, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_MOTOR_20, 0, 8, .max_counter = 15U, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_MOTOR_14, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 10U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_GRA_ACC_01, 0, 8, .max_counter = 15U, .frequency = 33U}, { 0 }, { 0 }}},
};
UNUSED(param);
diff --git a/opendbc/safety/safety/safety_volkswagen_pq.h b/opendbc/safety/safety/safety_volkswagen_pq.h
index d8a4447b0b..672edba781 100644
--- a/opendbc/safety/safety/safety_volkswagen_pq.h
+++ b/opendbc/safety/safety/safety_volkswagen_pq.h
@@ -59,12 +59,12 @@ static safety_config volkswagen_pq_init(uint16_t param) {
{MSG_ACC_SYSTEM, 0, 8}, {MSG_ACC_GRA_ANZEIGE, 0, 8}};
static RxCheck volkswagen_pq_rx_checks[] = {
- {.msg = {{MSG_LENKHILFE_3, 0, 6, .check_checksum = true, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
- {.msg = {{MSG_BREMSE_1, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}},
- {.msg = {{MSG_MOTOR_2, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{MSG_MOTOR_3, 0, 8, .check_checksum = false, .max_counter = 0U, .frequency = 100U}, { 0 }, { 0 }}},
- {.msg = {{MSG_MOTOR_5, 0, 8, .check_checksum = true, .max_counter = 0U, .frequency = 50U}, { 0 }, { 0 }}},
- {.msg = {{MSG_GRA_NEU, 0, 4, .check_checksum = true, .max_counter = 15U, .frequency = 30U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_LENKHILFE_3, 0, 6, .max_counter = 15U, .frequency = 100U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_BREMSE_1, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_MOTOR_2, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_MOTOR_3, 0, 8, .ignore_checksum = true, .ignore_counter = true, .frequency = 100U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_MOTOR_5, 0, 8, .ignore_counter = true, .frequency = 50U}, { 0 }, { 0 }}},
+ {.msg = {{MSG_GRA_NEU, 0, 4, .max_counter = 15U, .frequency = 30U}, { 0 }, { 0 }}},
};
UNUSED(param);
diff --git a/opendbc/safety/safety_declarations.h b/opendbc/safety/safety_declarations.h
index 986164fa2d..27c06bb899 100644
--- a/opendbc/safety/safety_declarations.h
+++ b/opendbc/safety/safety_declarations.h
@@ -81,7 +81,7 @@ typedef struct {
} TorqueSteeringLimits;
typedef struct {
- // angle cmd limits
+ // angle cmd limits (also used by curvature control cars)
const int max_angle;
const float angle_deg_to_can;
@@ -90,6 +90,7 @@ typedef struct {
const int max_angle_error; // used to limit error between meas and cmd while enabled
const float angle_error_min_speed; // minimum speed to start limiting angle error
+ const bool angle_is_curvature; // if true, we can apply max lateral acceleration limits
const bool enforce_angle_error; // enables max_angle_error check
const bool inactive_angle_is_zero; // if false, enforces angle near meas when disabled (default)
} AngleSteeringLimits;
@@ -120,10 +121,11 @@ typedef struct {
const int addr;
const int bus;
const int len;
- const bool check_checksum; // true is checksum check is performed
+ const bool ignore_checksum; // checksum check is not performed when set to true
+ const bool ignore_counter; // counter check is not performed when set to true
const uint8_t max_counter; // maximum value of the counter. 0 means that the counter check is skipped
const bool quality_flag; // true is quality flag check is performed
- const uint32_t frequency; // expected frequency of the message [Hz]
+ const uint32_t frequency; // expected frequency of the message [Hz]
} CanMsgCheck;
typedef struct {
diff --git a/opendbc/safety/tests/common.py b/opendbc/safety/tests/common.py
index e860ec246e..787245c7a4 100644
--- a/opendbc/safety/tests/common.py
+++ b/opendbc/safety/tests/common.py
@@ -580,6 +580,7 @@ def test_reset_torque_measurements(self):
class AngleSteeringSafetyTest(PandaSafetyTestBase):
+ STEER_ANGLE_MAX: float = 300
DEG_TO_CAN: float
ANGLE_RATE_BP: list[float]
ANGLE_RATE_UP: list[float] # windup limit
@@ -616,15 +617,17 @@ def _reset_speed_measurement(self, speed):
self._rx(self._speed_msg(speed))
def test_vehicle_speed_measurements(self):
+ # TODO: lower tolerance on these tests
self._common_measurement_test(self._speed_msg, 0, 80, 1, self.safety.get_vehicle_speed_min, self.safety.get_vehicle_speed_max)
- def test_steering_angle_measurements(self, max_angle=300):
- self._common_measurement_test(self._angle_meas_msg, -max_angle, max_angle, self.DEG_TO_CAN, self.safety.get_angle_meas_min, self.safety.get_angle_meas_max)
+ def test_steering_angle_measurements(self):
+ self._common_measurement_test(self._angle_meas_msg, -self.STEER_ANGLE_MAX, self.STEER_ANGLE_MAX, self.DEG_TO_CAN,
+ self.safety.get_angle_meas_min, self.safety.get_angle_meas_max)
- def test_angle_cmd_when_enabled(self, max_angle=300):
+ def test_angle_cmd_when_enabled(self):
# when controls are allowed, angle cmd rate limit is enforced
speeds = [0., 1., 5., 10., 15., 50.]
- angles = np.concatenate((np.arange(-max_angle, max_angle, 5), [0]))
+ angles = np.concatenate((np.arange(-self.STEER_ANGLE_MAX * 2, self.STEER_ANGLE_MAX * 2, 5), [0]))
for a in angles:
for s in speeds:
max_delta_up = np.interp(s, self.ANGLE_RATE_BP, self.ANGLE_RATE_UP)
@@ -666,7 +669,8 @@ def test_angle_cmd_when_enabled(self, max_angle=300):
# Check desired steer should be the same as steer angle when controls are off
self.safety.set_controls_allowed(0)
- self.assertTrue(self._tx(self._angle_cmd_msg(a, False)))
+ should_tx = abs(a) <= abs(self.STEER_ANGLE_MAX)
+ self.assertEqual(should_tx, self._tx(self._angle_cmd_msg(a, False)))
def test_angle_cmd_when_disabled(self):
# Tests that only angles close to the meas are allowed while
@@ -693,7 +697,7 @@ class PandaSafetyTest(PandaSafetyTestBase):
*range(0x18DB00F1, 0x18DC00F1, 0x100), # 29-bit UDS functional addressing
*range(0x3300, 0x3400)] # Honda
FWD_BLACKLISTED_ADDRS: dict[int, list[int]] = {} # {bus: [addr]}
- FWD_BUS_LOOKUP: dict[int, int] = {}
+ FWD_BUS_LOOKUP: dict[int, int] = {0: 2, 2: 0}
@classmethod
def setUpClass(cls):
@@ -794,7 +798,6 @@ def test_tx_hook_on_wrong_safety_mode(self):
# exceptions for common msgs across different hondas
tx = list(filter(lambda m: m[0] not in [0x1FA, 0x30C, 0x33D, 0x33DB], tx))
- # TODO-SP: Temporary, should be fixed in panda firmware, safety_hyundai.h
if attr.startswith('TestHyundaiLongitudinal'):
# exceptions for common msgs across different Hyundai CAN platforms
tx = list(filter(lambda m: m[0] not in [0x420, 0x50A, 0x389, 0x4A2], tx))
@@ -815,7 +818,7 @@ def test_tx_hook_on_wrong_safety_mode(self):
@add_regen_tests
class PandaCarSafetyTest(PandaSafetyTest, MadsCommonBase):
- STANDSTILL_THRESHOLD: float | None = None
+ STANDSTILL_THRESHOLD: float = 0.0
GAS_PRESSED_THRESHOLD = 0
RELAY_MALFUNCTION_ADDRS: dict[int, tuple[int, ...]] | None = None
diff --git a/opendbc/safety/tests/hyundai_common.py b/opendbc/safety/tests/hyundai_common.py
index e3967a7117..a6ce087cc1 100644
--- a/opendbc/safety/tests/hyundai_common.py
+++ b/opendbc/safety/tests/hyundai_common.py
@@ -111,6 +111,9 @@ def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
def _acc_state_msg(self, enable):
raise NotImplementedError
+ def _tx_acc_state_msg(self, enable):
+ raise NotImplementedError
+
def test_pcm_main_cruise_state_availability(self):
pass
diff --git a/opendbc/safety/tests/libsafety/safety_helpers.py b/opendbc/safety/tests/libsafety/safety_helpers.py
index 8f598fce5d..f1546a38df 100644
--- a/opendbc/safety/tests/libsafety/safety_helpers.py
+++ b/opendbc/safety/tests/libsafety/safety_helpers.py
@@ -68,6 +68,7 @@ def setup_safety_helpers(ffi):
uint32_t get_acc_main_on_mismatches(void);
void set_mads_params(bool enable_mads, bool disengage_lat_on_brake);
void set_heartbeat_engaged_mads(bool c);
+ void mads_heartbeat_engaged_check(void);
""")
class PandaSafety(Protocol):
@@ -136,4 +137,5 @@ def get_acc_main_on_mismatches(self) -> int: ...
def set_mads_params(self, enable_mads: bool, disengage_lat_on_brake: bool) -> None: ...
def set_heartbeat_engaged_mads(self, c: bool) -> None: ...
+ def mads_heartbeat_engaged_check(self) -> None: ...
# def get_temp_debug(self) -> int: ...
diff --git a/opendbc/safety/tests/mads_common.py b/opendbc/safety/tests/mads_common.py
index 2463067aef..6b3a98ad73 100644
--- a/opendbc/safety/tests/mads_common.py
+++ b/opendbc/safety/tests/mads_common.py
@@ -19,6 +19,29 @@ def _mads_states_cleanup(self):
self.safety.set_mads_params(False, False)
self.safety.set_heartbeat_engaged_mads(True)
+ def test_heartbeat_engaged_mads_check(self):
+ """Test MADS heartbeat engaged check behavior"""
+ for boolean in (True, False):
+ # If boolean is True, the heartbeat is engaged and should remain engaged, otherwise it should disengage.
+ with self.subTest(heartbeat_engaged=boolean, should_remain_engaged=boolean):
+ # Setup initial conditions
+ self._mads_states_cleanup()
+ self.safety.set_mads_params(True, False) # Enable MADS
+ self.safety.set_controls_allowed_lat(True)
+ self.assertTrue(self.safety.get_controls_allowed_lat())
+
+ # Set heartbeat engaged state based on test case
+ self.safety.set_heartbeat_engaged_mads(boolean)
+
+ # Call the heartbeat check function multiple times
+ # We know from the implementation that it takes 3 mismatches to disengage
+ for _ in range(4): # More than 3 times to ensure we pass the threshold
+ self.safety.mads_heartbeat_engaged_check()
+
+ # Verify engagement state matches expectation
+ self.assertEqual(self.safety.get_controls_allowed_lat(), boolean,
+ f"Expected controls_allowed_lat to be [{boolean}] but got [{self.safety.get_controls_allowed_lat()}]")
+
def test_enable_control_allowed_with_mads_button(self):
"""Toggle MADS with MADS button"""
try:
diff --git a/opendbc/safety/tests/safety_replay/helpers.py b/opendbc/safety/tests/safety_replay/helpers.py
index e8dac6888d..c56a15270f 100644
--- a/opendbc/safety/tests/safety_replay/helpers.py
+++ b/opendbc/safety/tests/safety_replay/helpers.py
@@ -1,3 +1,4 @@
+from opendbc.car.ford.values import FordSafetyFlags
from opendbc.car.toyota.values import ToyotaSafetyFlags
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
@@ -18,12 +19,15 @@ def is_steering_msg(mode, param, addr):
ret = addr == 384
elif mode == CarParams.SafetyModel.hyundai:
ret = addr == 832
+ elif mode == CarParams.SafetyModel.hyundaiCanfd:
+ # TODO: other params
+ ret = addr == 0x50
elif mode == CarParams.SafetyModel.chrysler:
ret = addr == 0x292
elif mode == CarParams.SafetyModel.subaru:
ret = addr == 0x122
elif mode == CarParams.SafetyModel.ford:
- ret = addr == 0x3d3
+ ret = addr == 0x3d6 if param & FordSafetyFlags.CANFD else addr == 0x3d3
elif mode == CarParams.SafetyModel.nissan:
ret = addr == 0x169
elif mode == CarParams.SafetyModel.rivian:
@@ -31,6 +35,7 @@ def is_steering_msg(mode, param, addr):
return ret
def get_steer_value(mode, param, to_send):
+ # TODO: use CANParser
torque, angle = 0, 0
if mode in (CarParams.SafetyModel.hondaNidec, CarParams.SafetyModel.hondaBosch):
torque = (to_send.data[0] << 8) | to_send.data[1]
@@ -47,13 +52,18 @@ def get_steer_value(mode, param, to_send):
torque = to_signed(torque, 11)
elif mode == CarParams.SafetyModel.hyundai:
torque = (((to_send.data[3] & 0x7) << 8) | to_send.data[2]) - 1024
+ elif mode == CarParams.SafetyModel.hyundaiCanfd:
+ torque = ((to_send.data[5] >> 1) | (to_send.data[6] & 0xF) << 7) - 1024
elif mode == CarParams.SafetyModel.chrysler:
torque = (((to_send.data[0] & 0x7) << 8) | to_send.data[1]) - 1024
elif mode == CarParams.SafetyModel.subaru:
torque = ((to_send.data[3] & 0x1F) << 8) | to_send.data[2]
torque = -to_signed(torque, 13)
elif mode == CarParams.SafetyModel.ford:
- angle = ((to_send.data[0] << 3) | (to_send.data[1] >> 5)) - 1000
+ if param & FordSafetyFlags.CANFD:
+ angle = ((to_send.data[2] << 3) | (to_send.data[3] >> 5)) - 1000
+ else:
+ angle = ((to_send.data[0] << 3) | (to_send.data[1] >> 5)) - 1000
elif mode == CarParams.SafetyModel.nissan:
angle = (to_send.data[0] << 10) | (to_send.data[1] << 2) | (to_send.data[2] >> 6)
angle = -angle + (1310 * 100)
@@ -64,13 +74,13 @@ def get_steer_value(mode, param, to_send):
def package_can_msg(msg):
return libsafety_py.make_CANPacket(msg.address, msg.src % 4, msg.dat)
-def init_segment(safety, lr, mode, param):
- sendcan = (msg for msg in lr if msg.which() == 'sendcan')
+def init_segment(safety, msgs, mode, param):
+ sendcan = (msg for msg in msgs if msg.which() == 'sendcan')
steering_msgs = (can for msg in sendcan for can in msg.sendcan if is_steering_msg(mode, param, can.address))
msg = next(steering_msgs, None)
if msg is None:
- # no steering msgs
+ print("no steering msgs found!")
return
to_send = package_can_msg(msg)
@@ -78,6 +88,9 @@ def init_segment(safety, lr, mode, param):
if torque != 0:
safety.set_controls_allowed(1)
safety.set_desired_torque_last(torque)
+ safety.set_rt_torque_last(torque)
+ safety.set_torque_meas(torque, torque)
+ safety.set_torque_driver(torque, torque)
elif angle != 0:
safety.set_controls_allowed(1)
safety.set_desired_angle_last(angle)
diff --git a/opendbc/safety/tests/safety_replay/replay_drive.py b/opendbc/safety/tests/safety_replay/replay_drive.py
index eb004dc73d..f2e4d4db66 100755
--- a/opendbc/safety/tests/safety_replay/replay_drive.py
+++ b/opendbc/safety/tests/safety_replay/replay_drive.py
@@ -1,32 +1,59 @@
#!/usr/bin/env python3
import argparse
-from collections import Counter
+import os
+from collections import Counter, defaultdict
+from tqdm import tqdm
-from opendbc.car.carlog import carlog
+from opendbc.safety import ALTERNATIVE_EXPERIENCE
from opendbc.safety.tests.libsafety import libsafety_py
+from opendbc.car.carlog import carlog
from opendbc.safety.tests.safety_replay.helpers import package_can_msg, init_segment
+# Define debug variables and their getter methods
+DEBUG_VARS = {
+ 'lat_active': lambda safety: safety.get_lat_active(),
+ 'controls_allowed': lambda safety: safety.get_controls_allowed(),
+ 'controls_requested_lat': lambda safety: safety.get_controls_requested_lat(),
+ 'controls_allowed_lat': lambda safety: safety.get_controls_allowed_lat(),
+ 'current_disengage_reason': lambda safety: safety.mads_get_current_disengage_reason(),
+ 'stock_acc_main': lambda safety: safety.get_acc_main_on(),
+ 'mads_acc_main': lambda safety: safety.get_mads_acc_main(),
+}
+
+
# replay a drive to check for safety violations
-def replay_drive(lr, safety_mode, param, alternative_experience, segment=False):
+def replay_drive(msgs, safety_mode, param, alternative_experience):
safety = libsafety_py.libsafety
+ msgs.sort(key=lambda m: m.logMonoTime)
err = safety.set_safety_hooks(safety_mode, param)
assert err == 0, "invalid safety mode: %d" % safety_mode
safety.set_alternative_experience(alternative_experience)
- if segment:
- init_segment(safety, lr, safety_mode, param)
- lr.reset()
+ _enable_mads = bool(alternative_experience & ALTERNATIVE_EXPERIENCE.ENABLE_MADS)
+ _disengage_lateral_on_brake = bool(alternative_experience & ALTERNATIVE_EXPERIENCE.DISENGAGE_LATERAL_ON_BRAKE)
+ safety.set_mads_params(_enable_mads, _disengage_lateral_on_brake)
+ print("alternative experience:")
+ print(f" enable mads: {_enable_mads}")
+ print(f" disengage lateral on brake: {_disengage_lateral_on_brake}")
+
+ init_segment(safety, msgs, safety_mode, param)
- rx_tot, rx_invalid, tx_tot, tx_blocked, tx_controls, tx_controls_blocked = 0, 0, 0, 0, 0, 0
+ rx_tot, rx_invalid, tx_tot, tx_blocked, tx_controls, tx_controls_lat, tx_controls_blocked, tx_controls_lat_blocked, mads_mismatch = 0, 0, 0, 0, 0, 0, 0, 0, 0
safety_tick_rx_invalid = False
blocked_addrs = Counter()
invalid_addrs = set()
- can_msgs = [m for m in lr if m.which() in ('can', 'sendcan')]
+ # Track last good state for each address
+ last_good_states = defaultdict(lambda: {
+ 'timestamp': None,
+ **{var: None for var in DEBUG_VARS}
+ })
+
+ can_msgs = [m for m in msgs if m.which() in ('can', 'sendcan')]
start_t = can_msgs[0].logMonoTime
end_t = can_msgs[-1].logMonoTime
- for msg in can_msgs:
+ for msg in tqdm(can_msgs):
safety.set_timer((msg.logMonoTime // 1000) % 0xFFFFFFFF)
# skip start and end of route, warm up/down period
@@ -38,13 +65,46 @@ def replay_drive(lr, safety_mode, param, alternative_experience, segment=False):
for canmsg in msg.sendcan:
to_send = package_can_msg(canmsg)
sent = safety.safety_tx_hook(to_send)
+
+ # mismatched
+ if (safety.get_controls_allowed() and not safety.get_controls_allowed_lat()):
+ mads_mismatch += 1
+ print(f"controls allowed but not controls allowed lat [{mads_mismatch}]")
+ print(f"msg:{canmsg.address} ({hex(canmsg.address)})")
+ for var, getter in DEBUG_VARS.items():
+ print(f" {var}: {getter(safety)}")
+
if not sent:
tx_blocked += 1
tx_controls_blocked += safety.get_controls_allowed()
+ tx_controls_lat_blocked += safety.get_controls_allowed_lat()
blocked_addrs[canmsg.address] += 1
carlog.debug("blocked bus %d msg %d at %f" % (canmsg.src, canmsg.address, (msg.logMonoTime - start_t) / 1e9))
+
+ if "DEBUG" in os.environ:
+ last_good = last_good_states[canmsg.address]
+ print(f"\nBlocked message at {(msg.logMonoTime - start_t) / 1e9:.3f}s:")
+ print(f"Address: {hex(canmsg.address)} (bus {canmsg.src})")
+ print("Current state:")
+ for var, getter in DEBUG_VARS.items():
+ print(f" {var}: {getter(safety)}")
+
+ if last_good['timestamp'] is not None:
+ print(f"\nLast good state ({last_good['timestamp']:.3f}s):")
+ for var in DEBUG_VARS:
+ print(f" {var}: {last_good[var]}")
+ else:
+ print("\nNo previous good state found for this address")
+ print("-" * 80)
+ else: # Update last good state if message is allowed
+ last_good_states[canmsg.address].update({
+ 'timestamp': (msg.logMonoTime - start_t) / 1e9,
+ **{var: getter(safety) for var, getter in DEBUG_VARS.items()}
+ })
+
tx_controls += safety.get_controls_allowed()
+ tx_controls_lat += safety.get_controls_allowed_lat()
tx_tot += 1
elif msg.which() == 'can':
# ignore msgs we sent
@@ -64,11 +124,14 @@ def replay_drive(lr, safety_mode, param, alternative_experience, segment=False):
print("\nTX")
print("total openpilot msgs:", tx_tot)
print("total msgs with controls allowed:", tx_controls)
+ print("total msgs with controls_lat allowed:", tx_controls_lat)
print("blocked msgs:", tx_blocked)
print("blocked with controls allowed:", tx_controls_blocked)
+ print("blocked with controls_lat allowed:", tx_controls_lat_blocked)
print("blocked addrs:", blocked_addrs)
+ print("mads enabled:", safety.get_enable_mads())
- return tx_controls_blocked == 0 and rx_invalid == 0 and not safety_tick_rx_invalid
+ return tx_controls_blocked == 0 and tx_controls_lat_blocked == 0 and rx_invalid == 0 and not safety_tick_rx_invalid
if __name__ == "__main__":
from openpilot.tools.lib.logreader import LogReader
@@ -81,7 +144,7 @@ def replay_drive(lr, safety_mode, param, alternative_experience, segment=False):
parser.add_argument("--alternative-experience", type=int, help="Override the alternative experience from the log")
args = parser.parse_args()
- lr = LogReader(args.route_or_segment_name[0], sort_by_time=True)
+ lr = LogReader(args.route_or_segment_name[0])
if None in (args.mode, args.param, args.alternative_experience):
CP = lr.first('carParams')
@@ -93,4 +156,4 @@ def replay_drive(lr, safety_mode, param, alternative_experience, segment=False):
args.alternative_experience = CP.alternativeExperience
print(f"replaying {args.route_or_segment_name[0]} with safety mode {args.mode}, param {args.param}, alternative experience {args.alternative_experience}")
- replay_drive(lr, args.mode, args.param, args.alternative_experience, segment=len(lr.logreader_identifiers) == 1)
+ replay_drive(list(lr), args.mode, args.param, args.alternative_experience)
diff --git a/opendbc/safety/tests/test.sh b/opendbc/safety/tests/test.sh
index 812970aed5..af695bb7fa 100755
--- a/opendbc/safety/tests/test.sh
+++ b/opendbc/safety/tests/test.sh
@@ -19,17 +19,15 @@ pytest -n8
# generate and open report
if [ "$1" == "--report" ]; then
- geninfo ./libsafety/ -o coverage.info
- genhtml coverage.info -o coverage-out
+ mkdir -p coverage-out
+ gcovr -r ../ --html-nested coverage-out/index.html
sensible-browser coverage-out/index.html
fi
# test coverage
-GCOV_OUTPUT=$(gcov -n ./libsafety/safety.c)
-INCOMPLETE_COVERAGE=$(echo "$GCOV_OUTPUT" | paste -s -d' \n' | grep -E "File.*(\/safety\/safety_.*)|(safety)\.h" | grep -v "100.00%" || true)
-if [ -n "$INCOMPLETE_COVERAGE" ]; then
- echo "FAILED: Some files have less than 100% coverage:"
- echo "$INCOMPLETE_COVERAGE"
+GCOV="gcovr -r ../ --fail-under-line=100 -e ^libsafety -e ^../board"
+if ! GCOV_OUTPUT="$($GCOV)"; then
+ echo -e "FAILED:\n$GCOV_OUTPUT"
exit 1
else
echo "SUCCESS: All checked files have 100% coverage!"
diff --git a/opendbc/safety/tests/test_body.py b/opendbc/safety/tests/test_body.py
index 5083399647..91b68187d2 100755
--- a/opendbc/safety/tests/test_body.py
+++ b/opendbc/safety/tests/test_body.py
@@ -10,6 +10,7 @@
class TestBody(common.PandaSafetyTest):
TX_MSGS = [[0x250, 0], [0x251, 0], [0x350, 0], [0x351, 0],
[0x1, 0], [0x1, 1], [0x1, 2], [0x1, 3]]
+ FWD_BUS_LOOKUP = {}
def setUp(self):
self.packer = CANPackerPanda("comma_body")
diff --git a/opendbc/safety/tests/test_chrysler.py b/opendbc/safety/tests/test_chrysler.py
index 2d1c162c37..247885057d 100755
--- a/opendbc/safety/tests/test_chrysler.py
+++ b/opendbc/safety/tests/test_chrysler.py
@@ -10,10 +10,8 @@
class TestChryslerSafety(common.PandaCarSafetyTest, common.MotorTorqueSteeringSafetyTest):
TX_MSGS = [[0x23B, 0], [0x292, 0], [0x2A6, 0], [0x2D9, 0]]
- STANDSTILL_THRESHOLD = 0
RELAY_MALFUNCTION_ADDRS = {0: (0x292,)}
FWD_BLACKLISTED_ADDRS = {2: [0x292, 0x2A6, 0x2D9]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
MAX_RATE_UP = 3
MAX_RATE_DOWN = 3
diff --git a/opendbc/safety/tests/test_defaults.py b/opendbc/safety/tests/test_defaults.py
index 8b5b2c2dd3..e44a1cd0a4 100755
--- a/opendbc/safety/tests/test_defaults.py
+++ b/opendbc/safety/tests/test_defaults.py
@@ -7,6 +7,8 @@
class TestDefaultRxHookBase(common.PandaSafetyTest):
+ FWD_BUS_LOOKUP = {}
+
def test_rx_hook(self):
# default rx hook allows all msgs
for bus in range(4):
diff --git a/opendbc/safety/tests/test_elm327.py b/opendbc/safety/tests/test_elm327.py
index 45ddf6cbb9..a499a051c4 100755
--- a/opendbc/safety/tests/test_elm327.py
+++ b/opendbc/safety/tests/test_elm327.py
@@ -15,6 +15,7 @@ class TestElm327(TestDefaultRxHookBase):
*range(0x18DA00F1, 0x18DB00F1, 0x100), # 29-bit UDS physical addressing
*[0x18DB33F1], # 29-bit UDS functional address
] for bus in range(4)]
+ FWD_BUS_LOOKUP = {}
def setUp(self):
self.safety = libsafety_py.libsafety
diff --git a/opendbc/safety/tests/test_ford.py b/opendbc/safety/tests/test_ford.py
index 1c1632f41a..0982153a56 100755
--- a/opendbc/safety/tests/test_ford.py
+++ b/opendbc/safety/tests/test_ford.py
@@ -4,6 +4,7 @@
import unittest
import opendbc.safety.tests.common as common
+from opendbc.car.ford.carcontroller import MAX_LATERAL_ACCEL
from opendbc.car.ford.values import FordSafetyFlags
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
@@ -71,7 +72,6 @@ class TestFordSafetyBase(common.PandaCarSafetyTest):
FWD_BLACKLISTED_ADDRS = {2: [MSG_ACCDATA_3, MSG_Lane_Assist_Data1, MSG_LateralMotionControl,
MSG_LateralMotionControl2, MSG_IPMA_Data]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
# Max allowed delta between car speeds
MAX_SPEED_DELTA = 2.0 # m/s
@@ -100,6 +100,13 @@ def setUpClass(cls):
if cls.__name__ == "TestFordSafetyBase":
raise unittest.SkipTest
+ def get_canfd_curvature_limits(self, speed):
+ # Round it in accordance with the safety
+ curvature_accel_limit = MAX_LATERAL_ACCEL / (max(speed, 1) ** 2)
+ curvature_accel_limit_lower = int(curvature_accel_limit * self.DEG_TO_CAN - 1) / self.DEG_TO_CAN
+ curvature_accel_limit_upper = int(curvature_accel_limit * self.DEG_TO_CAN + 1) / self.DEG_TO_CAN
+ return curvature_accel_limit_lower, curvature_accel_limit_upper
+
def _set_prev_desired_angle(self, t):
t = round(t * self.DEG_TO_CAN)
self.safety.set_desired_angle_last(t)
@@ -252,14 +259,34 @@ def test_angle_measurements(self):
self.assertEqual(self.safety.get_angle_meas_min(), 0)
self.assertEqual(self.safety.get_angle_meas_max(), 0)
+ def test_max_lateral_acceleration(self):
+ # Ford CAN FD can achieve a higher max lateral acceleration than CAN so we limit curvature based on speed
+ for speed in np.arange(0, 40, 0.5):
+ # Clip so we test curvature limiting at low speed due to low max curvature
+ _, curvature_accel_limit_upper = self.get_canfd_curvature_limits(speed)
+ curvature_accel_limit_upper = np.clip(curvature_accel_limit_upper, -self.MAX_CURVATURE, self.MAX_CURVATURE)
+
+ for sign in (-1, 1):
+ # Test above and below the lateral by 20%, max is clipped since
+ # max curvature at low speed is higher than the signal max
+ for curvature in np.arange(curvature_accel_limit_upper * 0.8, min(curvature_accel_limit_upper * 1.2, self.MAX_CURVATURE), 1 / self.DEG_TO_CAN):
+ curvature = sign * round(curvature * self.DEG_TO_CAN) / self.DEG_TO_CAN # fix np rounding errors
+ self.safety.set_controls_allowed(True)
+ self._set_prev_desired_angle(curvature)
+ self._reset_curvature_measurement(curvature, speed)
+
+ should_tx = abs(curvature) <= curvature_accel_limit_upper
+ self.assertEqual(should_tx, self._tx(self._lat_ctl_msg(True, 0, 0, curvature, 0)))
+
def test_steer_allowed(self):
- path_offsets = np.arange(-5.12, 5.11, 1).round()
- path_angles = np.arange(-0.5, 0.5235, 0.1).round(1)
+ path_offsets = np.arange(-5.12, 5.11, 2.5).round()
+ path_angles = np.arange(-0.5, 0.5235, 0.25).round(1)
curvature_rates = np.arange(-0.001024, 0.00102375, 0.001).round(3)
curvatures = np.arange(-0.02, 0.02094, 0.01).round(2)
for speed in (self.CURVATURE_ERROR_MIN_SPEED - 1,
self.CURVATURE_ERROR_MIN_SPEED + 1):
+ _, curvature_accel_limit_upper = self.get_canfd_curvature_limits(speed)
for controls_allowed in (True, False):
for steer_control_enabled in (True, False):
for path_offset in path_offsets:
@@ -274,6 +301,11 @@ def test_steer_allowed(self):
# when request bit is 0, only allow curvature of 0 since the signal range
# is not large enough to enforce it tracking measured
should_tx = should_tx and (controls_allowed if steer_control_enabled else curvature == 0)
+
+ # Only CAN FD has the max lateral acceleration limit
+ if self.STEER_MESSAGE == MSG_LateralMotionControl2:
+ should_tx = should_tx and abs(curvature) <= curvature_accel_limit_upper
+
with self.subTest(controls_allowed=controls_allowed, steer_control_enabled=steer_control_enabled,
path_offset=path_offset, path_angle=path_angle, curvature_rate=curvature_rate,
curvature=curvature):
@@ -289,6 +321,7 @@ def test_curvature_rate_limits(self):
small_curvature = 1 / self.DEG_TO_CAN # significant small amount of curvature to cross boundary
for speed in np.arange(0, 40, 0.5):
+ curvature_accel_limit_lower, curvature_accel_limit_upper = self.get_canfd_curvature_limits(speed)
limit_command = speed > self.CURVATURE_ERROR_MIN_SPEED
# ensure our limits match the safety's rounded limits
max_delta_up = int(np.interp(speed - 1, self.ANGLE_RATE_BP, self.ANGLE_RATE_UP) * self.DEG_TO_CAN + 1) / self.DEG_TO_CAN
@@ -325,6 +358,17 @@ def test_curvature_rate_limits(self):
for angle_meas, cases in (up_cases, down_cases):
self._reset_curvature_measurement(sign * angle_meas, speed)
for should_tx, initial_curvature, desired_curvature in cases:
+
+ # Only CAN FD has the max lateral acceleration limit
+ if self.STEER_MESSAGE == MSG_LateralMotionControl2:
+ if should_tx:
+ # can not send if the curvature is above the max lateral acceleration
+ should_tx = should_tx and abs(desired_curvature) <= curvature_accel_limit_upper
+ else:
+ # if desired curvature violates driver curvature error, it can only send if
+ # the curvature is being limited by max lateral acceleration
+ should_tx = should_tx or curvature_accel_limit_lower <= abs(desired_curvature) <= curvature_accel_limit_upper
+
# small curvature ensures we're using up limits. at 0, safety allows down limits to allow to account for rounding errors
curvature_offset = small_curvature if initial_curvature == 0 else 0
self._set_prev_desired_angle(sign * (curvature_offset + initial_curvature))
@@ -459,6 +503,10 @@ def setUp(self):
self.safety.set_safety_hooks(CarParams.SafetyModel.ford, 0)
self.safety.init_tests()
+ def test_max_lateral_acceleration(self):
+ # CAN does not limit curvature from lateral acceleration
+ pass
+
class TestFordCANFDLongitudinalSafety(TestFordLongitudinalSafetyBase):
STEER_MESSAGE = MSG_LateralMotionControl2
diff --git a/opendbc/safety/tests/test_gm.py b/opendbc/safety/tests/test_gm.py
index 2671860631..988be81a9c 100755
--- a/opendbc/safety/tests/test_gm.py
+++ b/opendbc/safety/tests/test_gm.py
@@ -165,7 +165,6 @@ def setUp(self):
class TestGmCameraSafetyBase(TestGmSafetyBase):
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
@classmethod
def setUpClass(cls):
diff --git a/opendbc/safety/tests/test_honda.py b/opendbc/safety/tests/test_honda.py
index 3e5fde622d..3b6d0e9332 100755
--- a/opendbc/safety/tests/test_honda.py
+++ b/opendbc/safety/tests/test_honda.py
@@ -172,9 +172,7 @@ class HondaBase(common.PandaCarSafetyTest):
STEER_BUS: int | None = None # must be set when inherited
BUTTONS_BUS: int | None = None # must be set when inherited, tx on this bus, rx on PT_BUS
- STANDSTILL_THRESHOLD = 0
RELAY_MALFUNCTION_ADDRS = {0: (0xE4, 0x194)} # STEERING_CONTROL
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
cnt_speed = 0
cnt_button = 0
diff --git a/opendbc/safety/tests/test_hyundai.py b/opendbc/safety/tests/test_hyundai.py
index bd58075276..0b3ae0ff08 100755
--- a/opendbc/safety/tests/test_hyundai.py
+++ b/opendbc/safety/tests/test_hyundai.py
@@ -50,7 +50,6 @@ class TestHyundaiSafety(HyundaiButtonBase, common.PandaCarSafetyTest, common.Dri
STANDSTILL_THRESHOLD = 12 # 0.375 kph
RELAY_MALFUNCTION_ADDRS = {0: (0x340,)} # LKAS11
FWD_BLACKLISTED_ADDRS = {2: [0x340, 0x485]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
MAX_RATE_UP = 3
MAX_RATE_DOWN = 7
diff --git a/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc/safety/tests/test_hyundai_canfd.py
index 8ff75e2299..75eb335c2f 100755
--- a/opendbc/safety/tests/test_hyundai_canfd.py
+++ b/opendbc/safety/tests/test_hyundai_canfd.py
@@ -15,7 +15,6 @@ class TestHyundaiCanfdBase(HyundaiButtonBase, common.PandaCarSafetyTest, common.
TX_MSGS = [[0x50, 0], [0x1CF, 1], [0x2A4, 0]]
STANDSTILL_THRESHOLD = 12 # 0.375 kph
FWD_BLACKLISTED_ADDRS = {2: [0x50, 0x2a4]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
MAX_RATE_UP = 2
MAX_RATE_DOWN = 3
@@ -56,7 +55,7 @@ def _torque_cmd_msg(self, torque, steer_req=1):
return self.packer.make_can_msg_panda(self.STEER_MSG, self.STEER_BUS, values)
def _speed_msg(self, speed):
- values = {f"WHEEL_SPEED_{i}": speed * 0.03125 for i in range(1, 5)}
+ values = {f"WHL_Spd{pos}Val": speed * 0.03125 for pos in ["FL", "FR", "RL", "RR"]}
return self.packer.make_can_msg_panda("WHEEL_SPEEDS", self.PT_BUS, values)
def _user_brake_msg(self, brake):
@@ -97,7 +96,6 @@ class TestHyundaiCanfdLFASteeringBase(TestHyundaiCanfdBase):
TX_MSGS = [[0x12A, 0], [0x1A0, 1], [0x1CF, 0], [0x1E0, 0]]
RELAY_MALFUNCTION_ADDRS = {0: (0x12A,)} # LFA
FWD_BLACKLISTED_ADDRS = {2: [0x12A, 0x1E0]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
STEER_MSG = "LFA"
BUTTONS_TX_BUS = 2
@@ -167,6 +165,10 @@ def _lkas_button_msg(self, enabled):
values = {"LDA_BTN": enabled}
return self.packer.make_can_msg_panda("CRUISE_BUTTONS_ALT", self.PT_BUS, values)
+ def _acc_cancel_msg(self, cancel, accel=0):
+ values = {"ACCMode": 4 if cancel else 0, "aReqRaw": accel, "aReqValue": accel}
+ return self.packer.make_can_msg_panda("SCC_CONTROL", self.PT_BUS, values)
+
def test_button_sends(self):
"""
No button send allowed with alt buttons.
@@ -176,13 +178,20 @@ def test_button_sends(self):
self.safety.set_controls_allowed(enabled)
self.assertFalse(self._tx(self._button_msg(btn)))
+ def test_acc_cancel(self):
+ # FIXME: the CANFD_ALT_BUTTONS cars are the only ones that use SCC_CONTROL to cancel, why can't we use buttons?
+ for enabled in (True, False):
+ self.safety.set_controls_allowed(enabled)
+ self.assertTrue(self._tx(self._acc_cancel_msg(True)))
+ self.assertFalse(self._tx(self._acc_cancel_msg(True, accel=1)))
+ self.assertFalse(self._tx(self._acc_cancel_msg(False)))
+
class TestHyundaiCanfdLKASteeringEV(TestHyundaiCanfdBase):
TX_MSGS = [[0x50, 0], [0x1CF, 1], [0x2A4, 0]]
RELAY_MALFUNCTION_ADDRS = {0: (0x50,)} # LKAS
FWD_BLACKLISTED_ADDRS = {2: [0x50, 0x2a4]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
PT_BUS = 1
SCC_BUS = 1
@@ -202,7 +211,6 @@ class TestHyundaiCanfdLKASteeringAltEV(TestHyundaiCanfdBase):
TX_MSGS = [[0x110, 0], [0x1CF, 1], [0x362, 0]]
RELAY_MALFUNCTION_ADDRS = {0: (0x110,)} # LKAS_ALT
FWD_BLACKLISTED_ADDRS = {2: [0x110, 0x362]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
PT_BUS = 1
SCC_BUS = 1
@@ -243,11 +251,11 @@ def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
"aReqRaw": accel,
"aReqValue": accel,
}
- return self.packer.make_can_msg_panda("SCC_CONTROL", 1, values)
+ return self.packer.make_can_msg_panda("SCC_CONTROL", self.PT_BUS, values)
def _tx_acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
- return self.packer.make_can_msg_panda("SCC_CONTROL", 0, values)
+ return self.packer.make_can_msg_panda("SCC_CONTROL", self.PT_BUS, values)
# Tests longitudinal for ICE, hybrid, EV cars with LFA steering
@parameterized_class([
@@ -286,11 +294,11 @@ def _accel_msg(self, accel, aeb_req=False, aeb_decel=0):
"aReqRaw": accel,
"aReqValue": accel,
}
- return self.packer.make_can_msg_panda("SCC_CONTROL", 0, values)
+ return self.packer.make_can_msg_panda("SCC_CONTROL", self.PT_BUS, values)
def _tx_acc_state_msg(self, enable):
values = {"MainMode_ACC": enable}
- return self.packer.make_can_msg_panda("SCC_CONTROL", 0, values)
+ return self.packer.make_can_msg_panda("SCC_CONTROL", self.PT_BUS, values)
# no knockout
def test_tester_present_allowed(self):
diff --git a/opendbc/safety/tests/test_mazda.py b/opendbc/safety/tests/test_mazda.py
index b1a0d94ebf..e60248d2d1 100755
--- a/opendbc/safety/tests/test_mazda.py
+++ b/opendbc/safety/tests/test_mazda.py
@@ -13,7 +13,6 @@ class TestMazdaSafety(common.PandaCarSafetyTest, common.DriverTorqueSteeringSafe
STANDSTILL_THRESHOLD = .1
RELAY_MALFUNCTION_ADDRS = {0: (0x243,)}
FWD_BLACKLISTED_ADDRS = {2: [0x243, 0x440]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
MAX_RATE_UP = 10
MAX_RATE_DOWN = 25
diff --git a/opendbc/safety/tests/test_nissan.py b/opendbc/safety/tests/test_nissan.py
index d1be459738..69eae69771 100755
--- a/opendbc/safety/tests/test_nissan.py
+++ b/opendbc/safety/tests/test_nissan.py
@@ -11,17 +11,16 @@
class TestNissanSafety(common.PandaCarSafetyTest, common.AngleSteeringSafetyTest):
TX_MSGS = [[0x169, 0], [0x2b1, 0], [0x4cc, 0], [0x20b, 2], [0x280, 2]]
- STANDSTILL_THRESHOLD = 0
GAS_PRESSED_THRESHOLD = 3
RELAY_MALFUNCTION_ADDRS = {0: (0x169,)}
FWD_BLACKLISTED_ADDRS = {0: [0x280], 2: [0x169, 0x2b1, 0x4cc]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
EPS_BUS = 0
CRUISE_BUS = 2
ACC_MAIN_BUS = 1
# Angle control limits
+ STEER_ANGLE_MAX = 600 # deg, reasonable limit
DEG_TO_CAN = 100
ANGLE_RATE_BP = [0., 5., 15.]
diff --git a/opendbc/safety/tests/test_rivian.py b/opendbc/safety/tests/test_rivian.py
index 53b913e31b..7dc4072415 100755
--- a/opendbc/safety/tests/test_rivian.py
+++ b/opendbc/safety/tests/test_rivian.py
@@ -8,13 +8,11 @@
from opendbc.car.rivian.values import RivianSafetyFlags
-class TestRivianSafetyBase(common.PandaCarSafetyTest, common.DriverTorqueSteeringSafetyTest):
+class TestRivianSafetyBase(common.PandaCarSafetyTest, common.DriverTorqueSteeringSafetyTest, common.LongitudinalAccelSafetyTest):
- TX_MSGS = [[0x120, 0], [0x321, 2]]
- STANDSTILL_THRESHOLD = 0
+ TX_MSGS = [[0x120, 0], [0x321, 2], [0x162, 2]]
RELAY_MALFUNCTION_ADDRS = {0: (0x120,)}
- FWD_BLACKLISTED_ADDRS = {0: [0x321], 2: [0x120]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
+ FWD_BLACKLISTED_ADDRS = {0: [0x321, 0x162], 2: [0x120]}
MAX_TORQUE = 250
MAX_RATE_UP = 3
@@ -64,7 +62,7 @@ def _accel_msg(self, accel: float):
return self.packer.make_can_msg_panda("ACM_longitudinalRequest", 0, values)
def test_wheel_touch(self):
- self.safety.set_controls_allowed(True)
+ # For hiding hold wheel alert on engage
for controls_allowed in (True, False):
self.safety.set_controls_allowed(controls_allowed)
values = {
@@ -77,19 +75,29 @@ def test_wheel_touch(self):
class TestRivianStockSafety(TestRivianSafetyBase):
+ LONGITUDINAL = False
+
def setUp(self):
self.packer = CANPackerPanda("rivian_primary_actuator")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.rivian, 0)
self.safety.init_tests()
+ def test_adas_status(self):
+ # For canceling stock ACC
+ for controls_allowed in (True, False):
+ self.safety.set_controls_allowed(controls_allowed)
+ for interface_status in range(4):
+ values = {"VDM_AdasInterfaceStatus": interface_status}
+ self.assertTrue(self._tx(self.packer.make_can_msg_panda("VDM_AdasSts", 2, values)))
+
+
+class TestRivianLongitudinalSafety(TestRivianSafetyBase):
-class TestRivianLongitudinalSafety(TestRivianSafetyBase, common.LongitudinalAccelSafetyTest):
+ TX_MSGS = [[0x120, 0], [0x321, 2], [0x160, 0]]
RELAY_MALFUNCTION_ADDRS = {0: (0x120, 0x160)}
FWD_BLACKLISTED_ADDRS = {0: [0x321], 2: [0x120, 0x160]}
- LONGITUDINAL = True
-
def setUp(self):
self.packer = CANPackerPanda("rivian_primary_actuator")
self.safety = libsafety_py.libsafety
diff --git a/opendbc/safety/tests/test_subaru.py b/opendbc/safety/tests/test_subaru.py
index ee64a38c22..85b3045ed0 100755
--- a/opendbc/safety/tests/test_subaru.py
+++ b/opendbc/safety/tests/test_subaru.py
@@ -56,9 +56,7 @@ def fwd_blacklisted_addr(lkas_msg=SubaruMsg.ES_LKAS):
class TestSubaruSafetyBase(common.PandaCarSafetyTest):
FLAGS = 0
- STANDSTILL_THRESHOLD = 0 # kph
RELAY_MALFUNCTION_ADDRS = {SUBARU_MAIN_BUS: (SubaruMsg.ES_LKAS,)}
- FWD_BUS_LOOKUP = {SUBARU_MAIN_BUS: SUBARU_CAM_BUS, SUBARU_CAM_BUS: SUBARU_MAIN_BUS}
FWD_BLACKLISTED_ADDRS = fwd_blacklisted_addr()
MAX_RT_DELTA = 940
diff --git a/opendbc/safety/tests/test_subaru_preglobal.py b/opendbc/safety/tests/test_subaru_preglobal.py
index 3879aaffc5..b2f482bf57 100755
--- a/opendbc/safety/tests/test_subaru_preglobal.py
+++ b/opendbc/safety/tests/test_subaru_preglobal.py
@@ -12,10 +12,8 @@ class TestSubaruPreglobalSafety(common.PandaCarSafetyTest, common.DriverTorqueSt
FLAGS = 0
DBC = "subaru_outback_2015_generated"
TX_MSGS = [[0x161, 0], [0x164, 0]]
- STANDSTILL_THRESHOLD = 0 # kph
RELAY_MALFUNCTION_ADDRS = {0: (0x164,)}
FWD_BLACKLISTED_ADDRS = {2: [0x161, 0x164]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
MAX_RATE_UP = 50
MAX_RATE_DOWN = 70
diff --git a/opendbc/safety/tests/test_tesla.py b/opendbc/safety/tests/test_tesla.py
index 8941c45b41..654a815bfb 100755
--- a/opendbc/safety/tests/test_tesla.py
+++ b/opendbc/safety/tests/test_tesla.py
@@ -20,9 +20,9 @@ class TestTeslaSafetyBase(common.PandaCarSafetyTest, common.AngleSteeringSafetyT
STANDSTILL_THRESHOLD = 0.1
GAS_PRESSED_THRESHOLD = 3
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
# Angle control limits
+ STEER_ANGLE_MAX = 360 # deg
DEG_TO_CAN = 10
ANGLE_RATE_BP = [0., 5., 25.]
diff --git a/opendbc/safety/tests/test_toyota.py b/opendbc/safety/tests/test_toyota.py
index 878378cf36..035c4dc09a 100755
--- a/opendbc/safety/tests/test_toyota.py
+++ b/opendbc/safety/tests/test_toyota.py
@@ -21,10 +21,8 @@
class TestToyotaSafetyBase(common.PandaCarSafetyTest, common.LongitudinalAccelSafetyTest):
TX_MSGS = TOYOTA_COMMON_TX_MSGS + TOYOTA_COMMON_LONG_TX_MSGS
- STANDSTILL_THRESHOLD = 0 # kph
RELAY_MALFUNCTION_ADDRS = {0: (0x2E4, 0x343)}
FWD_BLACKLISTED_ADDRS = {2: [0x2E4, 0x412, 0x191, 0x343]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
EPS_SCALE = 73
packer: CANPackerPanda
@@ -154,6 +152,7 @@ def setUp(self):
class TestToyotaSafetyAngle(TestToyotaSafetyBase, common.AngleSteeringSafetyTest):
# Angle control limits
+ STEER_ANGLE_MAX = 94.9461 # deg
DEG_TO_CAN = 17.452007 # 1 / 0.0573 deg to can
ANGLE_RATE_BP = [5., 25., 25.]
@@ -231,20 +230,13 @@ def test_lta_steer_cmd(self):
should_tx = not (req or req2) and torque_wind_down == 0
self.assertEqual(should_tx, self._tx(self._lta_msg(req, req2, angle, torque_wind_down)))
- def test_steering_angle_measurements(self, max_angle=None):
- # Measurement test tests max angle + 0.5 which will fail
- super().test_steering_angle_measurements(max_angle=self.MAX_LTA_ANGLE - 0.5)
-
- def test_angle_cmd_when_enabled(self, max_angle=None):
- super().test_angle_cmd_when_enabled(max_angle=self.MAX_LTA_ANGLE)
-
def test_angle_measurements(self):
"""
* Tests angle meas quality flag dictates whether angle measurement is parsed, and if rx is valid
* Tests rx hook correctly clips the angle measurement, since it is to be compared to LTA cmd when inactive
"""
for steer_angle_initializing in (True, False):
- for angle in np.arange(0, self.MAX_LTA_ANGLE * 2, 1):
+ for angle in np.arange(0, self.STEER_ANGLE_MAX * 2, 1):
# If init flag is set, do not rx or parse any angle measurements
for a in (angle, -angle, 0, 0, 0, 0):
self.assertEqual(not steer_angle_initializing,
diff --git a/opendbc/safety/tests/test_volkswagen_mqb.py b/opendbc/safety/tests/test_volkswagen_mqb.py
index 65f2ef922f..1d66fbd745 100755
--- a/opendbc/safety/tests/test_volkswagen_mqb.py
+++ b/opendbc/safety/tests/test_volkswagen_mqb.py
@@ -24,7 +24,6 @@
class TestVolkswagenMqbSafety(common.PandaCarSafetyTest, common.DriverTorqueSteeringSafetyTest):
- STANDSTILL_THRESHOLD = 0
RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_01,)}
MAX_RATE_UP = 4
@@ -139,7 +138,6 @@ def test_torque_measurements(self):
class TestVolkswagenMqbStockSafety(TestVolkswagenMqbSafety):
TX_MSGS = [[MSG_HCA_01, 0], [MSG_LDW_02, 0], [MSG_LH_EPS_03, 2], [MSG_GRA_ACC_01, 0], [MSG_GRA_ACC_01, 2]]
FWD_BLACKLISTED_ADDRS = {0: [MSG_LH_EPS_03], 2: [MSG_HCA_01, MSG_LDW_02]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
def setUp(self):
self.packer = CANPackerPanda("vw_mqb_2010")
@@ -160,7 +158,6 @@ def test_spam_cancel_safety_check(self):
class TestVolkswagenMqbLongSafety(TestVolkswagenMqbSafety):
TX_MSGS = [[MSG_HCA_01, 0], [MSG_LDW_02, 0], [MSG_LH_EPS_03, 2], [MSG_ACC_02, 0], [MSG_ACC_06, 0], [MSG_ACC_07, 0]]
FWD_BLACKLISTED_ADDRS = {0: [MSG_LH_EPS_03], 2: [MSG_HCA_01, MSG_LDW_02, MSG_ACC_02, MSG_ACC_06, MSG_ACC_07]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
INACTIVE_ACCEL = 3.01
def setUp(self):
diff --git a/opendbc/safety/tests/test_volkswagen_pq.py b/opendbc/safety/tests/test_volkswagen_pq.py
index c9a4db59dc..72d0876426 100755
--- a/opendbc/safety/tests/test_volkswagen_pq.py
+++ b/opendbc/safety/tests/test_volkswagen_pq.py
@@ -22,7 +22,6 @@
class TestVolkswagenPqSafety(common.PandaCarSafetyTest, common.DriverTorqueSteeringSafetyTest):
cruise_engaged = False
- STANDSTILL_THRESHOLD = 0
RELAY_MALFUNCTION_ADDRS = {0: (MSG_HCA_1,)}
MAX_RATE_UP = 6
@@ -122,7 +121,6 @@ class TestVolkswagenPqStockSafety(TestVolkswagenPqSafety):
# Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
TX_MSGS = [[MSG_HCA_1, 0], [MSG_GRA_NEU, 0], [MSG_GRA_NEU, 2], [MSG_LDW_1, 0]]
FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_1, MSG_LDW_1]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
def setUp(self):
self.packer = CANPackerPanda("vw_golf_mk4")
@@ -143,7 +141,6 @@ def test_spam_cancel_safety_check(self):
class TestVolkswagenPqLongSafety(TestVolkswagenPqSafety, common.LongitudinalAccelSafetyTest):
TX_MSGS = [[MSG_HCA_1, 0], [MSG_LDW_1, 0], [MSG_ACC_SYSTEM, 0], [MSG_ACC_GRA_ANZEIGE, 0]]
FWD_BLACKLISTED_ADDRS = {2: [MSG_HCA_1, MSG_LDW_1, MSG_ACC_SYSTEM, MSG_ACC_GRA_ANZEIGE]}
- FWD_BUS_LOOKUP = {0: 2, 2: 0}
INACTIVE_ACCEL = 3.01
def setUp(self):
diff --git a/opendbc/sunnypilot/car/platform_list.py b/opendbc/sunnypilot/car/platform_list.py
index e576c9b2fc..56b53ba9e9 100644
--- a/opendbc/sunnypilot/car/platform_list.py
+++ b/opendbc/sunnypilot/car/platform_list.py
@@ -19,7 +19,7 @@ def build_sorted_car_list(platforms, footnotes) -> dict[str, dict[str, list[str]
cars: dict[str, dict[str, list[str] | str]] = {}
for model, platform in platforms.items():
car_docs = platform.config.get_all_docs()
- CP = get_params_for_docs(model, platform)
+ CP = get_params_for_docs(platform)
if CP.dashcamOnly or not len(car_docs):
continue
diff --git a/pyproject.toml b/pyproject.toml
index d87df7e270..a67c061a5c 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -26,6 +26,7 @@ dependencies = [
testing = [
"cffi",
"ruff",
+ "gcovr",
"pytest",
"pytest-coverage",
"pytest-mock",