Merge branch 'upstream/opendbc/master' into sync-20250823

# Conflicts:
#	.github/workflows/tests.yml
#	opendbc/safety/tests/safety_replay/replay_drive.py
This commit is contained in:
Jason Wen
2025-08-23 20:41:49 -04:00
45 changed files with 831 additions and 211 deletions

View File

@@ -3,7 +3,11 @@ runs:
using: "composite"
steps:
- name: Restore cached cppcheck
id: cppcache
uses: actions/cache@v4
with:
path: opendbc/safety/tests/misra/cppcheck/
key: cppcheck-cache-${{ runner.os }}-${{ hashFiles('opendbc/safety/tests/misra/install.sh') }}
- name: Set cache-hit output
run: echo "cache-hit=${{ steps.cppcache.outputs.cache-hit }}" >> $GITHUB_OUTPUT
shell: bash

View File

@@ -17,11 +17,12 @@ jobs:
- os: ${{ github.repository == 'commaai/opendbc' && 'namespace-profile-amd64-8x16' || 'ubuntu-24.04' }}
- os: ${{ github.repository == 'commaai/opendbc' && 'namespace-profile-macos-8x14' || 'macos-latest' }}
steps:
- uses: commaai/timeout@v1
with:
timeout: ${{ github.repository == 'commaai/opendbc' && '60' || '999' }}
- uses: actions/checkout@v4
- uses: ./.github/workflows/cache
id: cache
- uses: commaai/timeout@v1
with:
timeout: ${{ github.repository == 'commaai/opendbc' && (steps.cache.outputs.cache-hit == 'true' && '60' || '90') || '999' }}
- run: ./test.sh
safety_tests:
@@ -43,7 +44,7 @@ jobs:
mutation:
name: Safety mutation tests
runs-on: ${{ github.repository == 'commaai/opendbc' && 'namespace-profile-amd64-8x16' || 'ubuntu-latest' }}
timeout-minutes: ${{ github.repository == 'commaai/opendbc' && 20 || 999 }}
timeout-minutes: ${{ github.repository == 'commaai/opendbc' && 45 || 999 }}
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:
@@ -52,7 +53,6 @@ jobs:
fetch-depth: 0 # need master to get diff
- uses: ./.github/workflows/cache
- name: Run mutation tests
timeout-minutes: ${{ github.repository == 'commaai/opendbc' && 5 || 999 }}
run: |
source setup.sh
scons -j8

View File

@@ -9,7 +9,7 @@
|Acura|Integra 2023-25|All|[Community](#community)|
|Acura|MDX 2015-16|Advance Package|[Community](#community)|
|Acura|MDX 2017-20|All|[Community](#community)|
|Acura|MDX 2025|All|[Community](#community)|
|Acura|MDX 2025|All except Type S|[Upstream](#upstream)|
|Acura|RDX 2016-18|AcuraWatch Plus or Advance Package|[Upstream](#upstream)|
|Acura|RDX 2019-21|All|[Upstream](#upstream)|
|Acura|RDX 2022-25|All|[Community](#community)|
@@ -82,9 +82,9 @@
|GMC|Yukon 2019-20|Adaptive Cruise Control (ACC) & LKAS|[Dashcam mode](#dashcam)|
|Honda|Accord 2016-17|Honda Sensing|[Community](#community)|
|Honda|Accord 2018-22|All|[Upstream](#upstream)|
|Honda|Accord 2023|All|[Upstream](#upstream)|
|Honda|Accord 2024-25|All|[Community](#community)|
|Honda|Accord 2023-25|All|[Upstream](#upstream)|
|Honda|Accord Hybrid 2018-22|All|[Upstream](#upstream)|
|Honda|Accord Hybrid 2023-25|All|[Upstream](#upstream)|
|Honda|Civic 2016-18|Honda Sensing|[Upstream](#upstream)|
|Honda|Civic 2019-21|All|[Upstream](#upstream)|
|Honda|Civic 2022-24|All|[Upstream](#upstream)|
@@ -96,9 +96,9 @@
|Honda|Clarity 2018-21|All|[Community](#community)|
|Honda|CR-V 2015-16|Touring Trim|[Upstream](#upstream)|
|Honda|CR-V 2017-22|Honda Sensing|[Upstream](#upstream)|
|Honda|CR-V 2023-25|All|[Community](#community)|
|Honda|CR-V 2023-25|All|[Upstream](#upstream)|
|Honda|CR-V Hybrid 2017-22|Honda Sensing|[Upstream](#upstream)|
|Honda|CR-V Hybrid 2023-25|All|[Community](#community)|
|Honda|CR-V Hybrid 2023-25|All|[Upstream](#upstream)|
|Honda|e 2020|All|[Upstream](#upstream)|
|Honda|Fit 2018-20|Honda Sensing|[Upstream](#upstream)|
|Honda|Freed 2020|Honda Sensing|[Upstream](#upstream)|
@@ -111,8 +111,7 @@
|Honda|Passport 2019-25|All|[Upstream](#upstream)|
|Honda|Passport 2026|All|[Community](#community)|
|Honda|Pilot 2016-22|Honda Sensing|[Upstream](#upstream)|
|Honda|Pilot 2023|All|[Dashcam mode](#dashcam)|
|Honda|Pilot 2023-25|All|[Community](#community)|
|Honda|Pilot 2023-25|All|[Upstream](#upstream)|
|Honda|Prologue 2024-25|All|[Not compatible](#can-bus-security)|
|Honda|Ridgeline 2017-25|Honda Sensing|[Upstream](#upstream)|
|Hyundai|Azera 2022|All|[Upstream](#upstream)|
@@ -137,7 +136,7 @@
|Hyundai|Ioniq Plug-in Hybrid 2019|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|Hyundai|Ioniq Plug-in Hybrid 2020-22|All|[Upstream](#upstream)|
|Hyundai|Kona 2020|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|Hyundai|Kona 2022|Smart Cruise Control (SCC)|[Dashcam mode](#dashcam)|
|Hyundai|Kona 2022-23|Smart Cruise Control (SCC)|[Dashcam mode](#dashcam)|
|Hyundai|Kona Electric 2018-21|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|Hyundai|Kona Electric 2022-23|Smart Cruise Control (SCC)|[Upstream](#upstream)|
|Hyundai|Kona Electric (with HDA II, Korea only) 2023|Smart Cruise Control (SCC)|[Upstream](#upstream)|
@@ -241,6 +240,7 @@
|Nissan|Leaf 2018-23|ProPILOT Assist|[Upstream](#upstream)|
|Nissan|Rogue 2018-20|ProPILOT Assist|[Upstream](#upstream)|
|Nissan|X-Trail 2017|ProPILOT Assist|[Upstream](#upstream)|
|Peugeot|208 2019-25|Adaptive Cruise Control (ACC) & Lane Assist|[Dashcam mode](#dashcam)|
|Ram|1500 2019-24|Adaptive Cruise Control (ACC)|[Upstream](#upstream)|
|Ram|2500 2020-24|Adaptive Cruise Control (ACC)|[Dashcam mode](#dashcam)|
|Ram|3500 2019-22|Adaptive Cruise Control (ACC)|[Dashcam mode](#dashcam)|
@@ -339,7 +339,7 @@
|Toyota|Sienna 2024-25|Any|[Not compatible](#can-bus-security)|
|Toyota|Tundra 2022-25|Any|[Not compatible](#can-bus-security)|
|Toyota|Venza 2021-25|Any|[Not compatible](#can-bus-security)|
|Toyota|Yaris (Non-US only) 2023|All|[Community](#community)|
|Toyota|Yaris (Non-US only) 2020, 2023|All|[Community](#community)|
|Volkswagen|Arteon 2018-23|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|Volkswagen|Arteon eHybrid 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|
|Volkswagen|Arteon R 2020-23|Adaptive Cruise Control (ACC) & Lane Assist|[Upstream](#upstream)|

View File

@@ -14,6 +14,7 @@ from opendbc.car.hyundai.hyundaicanfd import hkg_can_fd_checksum
from opendbc.car.volkswagen.mqbcan import volkswagen_mqb_meb_checksum, xor_checksum
from opendbc.car.tesla.teslacan import tesla_checksum
from opendbc.car.body.bodycan import body_checksum
from opendbc.car.psa.psacan import psa_checksum
class SignalType:
@@ -29,6 +30,7 @@ class SignalType:
HKG_CAN_FD_CHECKSUM = 9
FCA_GIORGIO_CHECKSUM = 10
TESLA_CHECKSUM = 11
PSA_CHECKSUM = 12
@dataclass
@@ -196,6 +198,8 @@ def get_checksum_state(dbc_name: str) -> ChecksumState | None:
return ChecksumState(8, 4, 7, 3, False, SignalType.BODY_CHECKSUM, body_checksum)
elif dbc_name.startswith("tesla_model3_party"):
return ChecksumState(8, -1, 0, -1, True, SignalType.TESLA_CHECKSUM, tesla_checksum, tesla_setup_signal)
elif dbc_name.startswith("psa_"):
return ChecksumState(4, 4, 7, 3, False, SignalType.PSA_CHECKSUM, psa_checksum)
return None

View File

@@ -142,10 +142,9 @@ class CANParser:
self._add_message(name_or_addr, freq)
self.can_valid: bool = False
self.bus_timeout: bool = False
self.can_invalid_cnt: int = CAN_INVALID_CNT
self.last_nonempty_nanos: int = 0
self._last_update_nanos: int = 0
def _add_message(self, name_or_addr: str | int, freq: int = None) -> None:
if isinstance(name_or_addr, numbers.Number):
@@ -181,17 +180,29 @@ class CANParser:
self.message_states[msg.address] = state
def update_valid(self, nanos: int) -> None:
@property
def bus_timeout(self) -> bool:
ignore_alive = all(s.ignore_alive for s in self.message_states.values())
bus_timeout_threshold = 500 * 1_000_000
for st in self.message_states.values():
if st.timeout_threshold > 0:
bus_timeout_threshold = min(bus_timeout_threshold, st.timeout_threshold)
return ((self._last_update_nanos - self.last_nonempty_nanos) > bus_timeout_threshold) and not ignore_alive
@property
def can_valid(self) -> bool:
valid = True
counters_valid = True
bus_timeout = self.bus_timeout
for state in self.message_states.values():
if state.counter_fail >= MAX_BAD_COUNTER:
counters_valid = False
if not state.valid(nanos, self.bus_timeout):
if not state.valid(self._last_update_nanos, bus_timeout):
valid = False
# TODO: probably only want to increment this once per update() call
self.can_invalid_cnt = 0 if valid else min(self.can_invalid_cnt + 1, CAN_INVALID_CNT)
self.can_valid = self.can_invalid_cnt < CAN_INVALID_CNT and counters_valid
return self.can_invalid_cnt < CAN_INVALID_CNT and counters_valid
def update(self, strings, sendcan: bool = False):
if strings and not isinstance(strings[0], list | tuple):
@@ -228,13 +239,7 @@ class CANParser:
if not bus_empty:
self.last_nonempty_nanos = t
ignore_alive = all(s.ignore_alive for s in self.message_states.values())
bus_timeout_threshold = 500 * 1_000_000
for st in self.message_states.values():
if st.timeout_threshold > 0:
bus_timeout_threshold = min(bus_timeout_threshold, st.timeout_threshold)
self.bus_timeout = ((t - self.last_nonempty_nanos) > bus_timeout_threshold) and not ignore_alive
self.update_valid(t)
self._last_update_nanos = t
return updated_addrs

View File

@@ -145,6 +145,7 @@ class CarHarness(EnumBase):
rivian = BaseCarHarness("Rivian A connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler])
tesla_a = BaseCarHarness("Tesla A connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler])
tesla_b = BaseCarHarness("Tesla B connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable, Cable.usbc_coupler])
psa_a = BaseCarHarness("PSA A connector", parts=[Accessory.harness_box, Cable.long_obdc_cable, Cable.usbc_coupler])
# custom harness
honda_clarity = BaseCarHarness("Honda Nidec connector + Honda Clarity Proxy Board")

View File

@@ -7,10 +7,11 @@ from opendbc.car.fw_query_definitions import EcuAddrBusType
def _is_tester_present_response(msg: CanData, subaddr: int = None) -> bool:
# ISO-TP messages are always padded to 8 bytes
# ISO-TP messages may use CAN frame optimization (not always 8 bytes)
# tester present response is always a single frame
dat_offset = 1 if subaddr is not None else 0
if len(msg.dat) == 8 and 1 <= msg.dat[dat_offset] <= 7:
min_length = 4 if subaddr is not None else 3 # bytes: frame len, (pos/neg) sid, (optional negative sid)/0x00 sub-function
if min_length <= len(msg.dat) <= 8 and 1 <= msg.dat[dat_offset] <= 7:
# success response
if msg.dat[dat_offset + 1] == (uds.SERVICE_TYPE.TESTER_PRESENT + 0x40):
return True

View File

@@ -40,20 +40,15 @@ class CAR(Platforms):
CommunityCarDocs("Acura Integra 2023-25", "All"),
CommunityCarDocs("Acura MDX 2015-16", "Advance Package"),
CommunityCarDocs("Acura MDX 2017-20", "All"),
CommunityCarDocs("Acura MDX 2025", "All"),
CommunityCarDocs("Acura RDX 2022-25", "All"),
CommunityCarDocs("Acura RLX 2017", "Advance Package or Technology Package"),
CommunityCarDocs("Acura TLX 2015-17", "Advance Package"),
CommunityCarDocs("Acura TLX 2018-20", "All"),
GMSecurityCarDocs("Acura ZDX 2024", "All"),
CommunityCarDocs("Honda Accord 2016-17", "Honda Sensing"),
CommunityCarDocs("Honda Accord 2024-25", "All"),
CommunityCarDocs("Honda Clarity 2018-21", "All"),
CommunityCarDocs("Honda CR-V 2023-25", "All"),
CommunityCarDocs("Honda CR-V Hybrid 2023-25", "All"),
CommunityCarDocs("Honda Odyssey 2021-25", "All"),
CommunityCarDocs("Honda Passport 2026", "All"),
CommunityCarDocs("Honda Pilot 2023-25", "All"),
GMSecurityCarDocs("Honda Prologue 2024-25", "All"),
],
)

View File

@@ -1,15 +1,14 @@
#!/usr/bin/env python3
import os
from math import fabs, exp
import numpy as np
from opendbc.car import get_safety_config, structs
from opendbc.car.common.basedir import BASEDIR
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.gm.carcontroller import CarController
from opendbc.car.gm.carstate import CarState
from opendbc.car.gm.radar_interface import RadarInterface, RADAR_HEADER_MSG, CAMERA_DATA_HEADER_MSG
from opendbc.car.gm.values import CAR, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, SDGM_CAR, ALT_ACCS, CanBus, GMSafetyFlags
from opendbc.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, LatControlInputs, NanoFFModel
from opendbc.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, LateralAccelFromTorqueCallbackType
TransmissionType = structs.CarParams.TransmissionType
NetworkLocation = structs.CarParams.NetworkLocation
@@ -20,8 +19,6 @@ NON_LINEAR_TORQUE_PARAMS = {
CAR.CHEVROLET_SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122]
}
NEURAL_PARAMS_PATH = os.path.join(BASEDIR, 'torque_data/neural_ff_weights.json')
class CarInterface(CarInterfaceBase):
CarState = CarState
@@ -45,42 +42,45 @@ class CarInterface(CarInterfaceBase):
else:
return CarInterfaceBase.get_steer_feedforward_default
def torque_from_lateral_accel_siglin(self, latcontrol_inputs: LatControlInputs, torque_params: structs.CarParams.LateralTorqueTuning,
gravity_adjusted: bool) -> float:
def sig(val):
# https://timvieira.github.io/blog/post/2014/02/11/exp-normalize-trick
if val >= 0:
return 1 / (1 + exp(-val)) - 0.5
else:
z = exp(val)
return z / (1 + z) - 0.5
def get_lataccel_torque_siglin(self) -> float:
# The "lat_accel vs torque" relationship is assumed to be the sum of "sigmoid + linear" curves
# An important thing to consider is that the slope at 0 should be > 0 (ideally >1)
# This has big effect on the stability about 0 (noise when going straight)
# ToDo: To generalize to other GMs, explore tanh function as the nonlinear
non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
assert non_linear_torque_params, "The params are not defined"
a, b, c, _ = non_linear_torque_params
steer_torque = (sig(latcontrol_inputs.lateral_acceleration * a) * b) + (latcontrol_inputs.lateral_acceleration * c)
return float(steer_torque)
def torque_from_lateral_accel_siglin_func(lateral_acceleration: float) -> float:
# The "lat_accel vs torque" relationship is assumed to be the sum of "sigmoid + linear" curves
# An important thing to consider is that the slope at 0 should be > 0 (ideally >1)
# This has big effect on the stability about 0 (noise when going straight)
non_linear_torque_params = NON_LINEAR_TORQUE_PARAMS.get(self.CP.carFingerprint)
assert non_linear_torque_params, "The params are not defined"
a, b, c, _ = non_linear_torque_params
sig_input = a * lateral_acceleration
sig = np.sign(sig_input) * (1 / (1 + exp(-fabs(sig_input))) - 0.5)
steer_torque = (sig * b) + (lateral_acceleration * c)
return float(steer_torque)
def torque_from_lateral_accel_neural(self, latcontrol_inputs: LatControlInputs, torque_params: structs.CarParams.LateralTorqueTuning,
gravity_adjusted: bool) -> float:
inputs = list(latcontrol_inputs)
if gravity_adjusted:
inputs[0] += inputs[1]
return float(self.neural_ff_model.predict(inputs))
lataccel_values = np.arange(-5.0, 5.0, 0.01)
torque_values = [torque_from_lateral_accel_siglin_func(x) for x in lataccel_values]
assert min(torque_values) < -1 and max(torque_values) > 1, "The torque values should cover the range [-1, 1]"
return torque_values, lataccel_values
def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
if self.CP.carFingerprint == CAR.CHEVROLET_BOLT_EUV:
self.neural_ff_model = NanoFFModel(NEURAL_PARAMS_PATH, self.CP.carFingerprint)
return self.torque_from_lateral_accel_neural
elif self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
return self.torque_from_lateral_accel_siglin
if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
torque_values, lataccel_values = self.get_lataccel_torque_siglin()
def torque_from_lateral_accel_siglin(lateral_acceleration: float, torque_params: structs.CarParams.LateralTorqueTuning):
return np.interp(lateral_acceleration, lataccel_values, torque_values)
return torque_from_lateral_accel_siglin
else:
return self.torque_from_lateral_accel_linear
def lateral_accel_from_torque(self) -> LateralAccelFromTorqueCallbackType:
if self.CP.carFingerprint in NON_LINEAR_TORQUE_PARAMS:
torque_values, lataccel_values = self.get_lataccel_torque_siglin()
def lateral_accel_from_torque_siglin(torque: float, torque_params: structs.CarParams.LateralTorqueTuning):
return np.interp(torque, torque_values, lataccel_values)
return lateral_accel_from_torque_siglin
else:
return self.lateral_accel_from_torque_linear
@staticmethod
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
ret.brand = "gm"

View File

@@ -70,7 +70,6 @@ FW_VERSIONS = {
],
(Ecu.eps, 0x18da30f1, None): [
b'39990-TBX-H120\x00\x00',
b'39990-TVA,A150\x00\x00',
b'39990-TVA-A140\x00\x00',
b'39990-TVA-A150\x00\x00',
b'39990-TVA-A160\x00\x00',
@@ -150,7 +149,6 @@ FW_VERSIONS = {
b'57114-TEA-Q220\x00\x00',
],
(Ecu.eps, 0x18da30f1, None): [
b'39990-TBA,A030\x00\x00',
b'39990-TBA-A030\x00\x00',
b'39990-TBG-A030\x00\x00',
b'39990-TEA-T020\x00\x00',
@@ -223,8 +221,6 @@ FW_VERSIONS = {
b'39990-TEA-T330\x00\x00',
b'39990-TEA-T820\x00\x00',
b'39990-TEZ-T020\x00\x00',
b'39990-TGG,A020\x00\x00',
b'39990-TGG,A120\x00\x00',
b'39990-TGG-A020\x00\x00',
b'39990-TGG-A120\x00\x00',
b'39990-TGG-J510\x00\x00',
@@ -357,7 +353,6 @@ FW_VERSIONS = {
b'57114-TMC-Z050\x00\x00',
],
(Ecu.eps, 0x18da30f1, None): [
b'39990-TLA,A040\x00\x00',
b'39990-TLA-A040\x00\x00',
b'39990-TLA-A110\x00\x00',
b'39990-TLA-A220\x00\x00',
@@ -505,6 +500,14 @@ FW_VERSIONS = {
b'36161-TDK-J530\x00\x00',
],
},
CAR.ACURA_MDX_4G_MMR: {
(Ecu.fwdCamera, 0x18dab5f1, None): [
b'8S102-TYA-A020\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'8S302-TYA-A020\x00\x00',
],
},
CAR.HONDA_ODYSSEY: {
(Ecu.gateway, 0x18daeff1, None): [
b'38897-THR-A010\x00\x00',
@@ -807,6 +810,7 @@ FW_VERSIONS = {
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'8S102-3M3-T050\x00\x00',
b'8S102-3M6-P030\x00\x00',
b'8S102-3M6-PA20\x00\x00',
b'8S102-3W0-A060\x00\x00',
b'8S102-3W0-AB10\x00\x00',
b'8S102-3W0-AB20\x00\x00',
@@ -941,21 +945,13 @@ FW_VERSIONS = {
(Ecu.fwdCamera, 0x18dab5f1, None): [
b'8S102-T90-A050\x00\x00',
b'8S102-T90-A060\x00\x00',
b'8S102-T90-A070\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'8S302-T90-A040\x00\x00',
],
},
CAR.HONDA_ACCORD_11G: {
(Ecu.eps, 0x18da30f1, None): [
b'39991-30B-A060\x00\x00',
],
(Ecu.gateway, 0x18daeff1, None): [
b'5J802-30B-AA10\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-30B-A750\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'8S302-30A-A040\x00\x00',
],
@@ -963,8 +959,38 @@ FW_VERSIONS = {
b'8S102-30A-A050\x00\x00',
b'8S102-30A-A060\x00\x00',
],
},
CAR.HONDA_CRV_6G: {
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'8S302-3C0-Q050\x00\x00',
b'8S302-3D4-A050\x00\x00',
],
(Ecu.fwdCamera, 0x18dab5f1, None): [
b'8S102-3C0-Q060\x00\x00',
b'8S102-3D4-A060\x00\x00',
b'8S102-3D4-A070\x00\x00',
b'8S102-3D4-A080\x00\x00',
b'8S102-3D4-A090\x00\x00',
],
},
CAR.HONDA_CITY_7G: {
(Ecu.eps, 0x18da30f1, None): [
b'39990-T14-B030\x00\x00',
],
(Ecu.gateway, 0x18daeff1, None): [
b'38897-T14-M110\x00\x00',
],
(Ecu.srs, 0x18da53f1, None): [
b'77959-T00-B830\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'36161-T14-P050\x00\x00',
],
(Ecu.vsa, 0x18da28f1, None): [
b'57114-30B-A030\x00\x00',
b'57114-T14-B030\x00\x00',
],
(Ecu.transmission, 0x18da1ef1, None): [
b'28101-63B-M420\x00\x00',
],
},
}

View File

@@ -38,9 +38,6 @@ class CarInterface(CarInterfaceBase):
CAN = CanBus(ret, fingerprint)
# Pilot 4G needs a rescaled lateral actuator, switch to lat accel torque control, and an updated test route
ret.dashcamOnly = candidate in [CAR.HONDA_PILOT_4G]
if candidate in HONDA_BOSCH:
cfgs = [get_safety_config(structs.CarParams.SafetyModel.hondaBosch)]
if candidate in HONDA_BOSCH_CANFD and CAN.pt >= 4:
@@ -99,24 +96,14 @@ class CarInterface(CarInterfaceBase):
ret.longitudinalTuning.kiBP = [0., 5., 35.]
ret.longitudinalTuning.kiV = [1.2, 0.8, 0.5]
eps_modified = False
# Disable control if EPS mod detected
for fw in car_fw:
if fw.ecu == "eps" and b"," in fw.fwVersion:
eps_modified = True
ret.dashcamOnly = True
if candidate == CAR.HONDA_CIVIC:
if eps_modified:
# stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE
# stock request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x1680, 0x1680
# modified request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x2880, 0x3180
# stock filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108
# modified filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0400, 0x0480
# note: max request allowed is 4096, but request is capped at 3840 in firmware, so modifications result in 2x max
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]]
else:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]]
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]]
elif candidate in (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_BOSCH_DIESEL):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
@@ -129,11 +116,7 @@ class CarInterface(CarInterfaceBase):
elif candidate == CAR.HONDA_ACCORD:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
if eps_modified:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]]
else:
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
elif candidate == CAR.HONDA_ACCORD_11G:
ret.steerActuatorDelay = 0.22
@@ -144,21 +127,19 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
elif candidate == CAR.HONDA_CITY_7G:
ret.steerActuatorDelay = 0.15
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate in (CAR.HONDA_CRV, CAR.HONDA_CRV_EU):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.HONDA_CRV_5G:
if eps_modified:
# stock request input values: 0x0000, 0x00DB, 0x01BB, 0x0296, 0x0377, 0x0454, 0x0532, 0x0610, 0x067F
# stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400
# modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]]
else:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
ret.wheelSpeedFactor = 1.025
elif candidate == CAR.HONDA_CRV_HYBRID:
@@ -166,6 +147,11 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
ret.wheelSpeedFactor = 1.025
elif candidate in (CAR.HONDA_CRV_6G):
ret.steerActuatorDelay = 0.15
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.HONDA_FIT:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
@@ -194,10 +180,20 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
elif candidate in (CAR.HONDA_PILOT, CAR.HONDA_PILOT_4G):
elif candidate == CAR.HONDA_PILOT:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
elif candidate == CAR.HONDA_PILOT_4G:
ret.steerActuatorDelay = 0.15
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.ACURA_MDX_4G_MMR:
ret.steerActuatorDelay = 0.15
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
elif candidate == CAR.HONDA_RIDGELINE:
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
@@ -252,12 +248,11 @@ class CarInterface(CarInterfaceBase):
def _get_params_sp(stock_cp: structs.CarParams, ret: structs.CarParamsSP, candidate, fingerprint: dict[int, dict[int, int]],
car_fw: list[structs.CarParams.CarFw], alpha_long: bool, docs: bool) -> structs.CarParamsSP:
eps_modified = False
# FIXME-SP: uncomment when the sync brings in https://github.com/commaai/opendbc/pull/2672
# for fw in car_fw:
# if fw.ecu == "eps" and b"," in fw.fwVersion:
# ret.flags |= HondaFlagsSP.EPS_MOD.value
# eps_modified = True
# stock_cp.dashcamOnly = False
for fw in car_fw:
if fw.ecu == "eps" and b"," in fw.fwVersion:
ret.flags |= HondaFlagsSP.EPS_MOD.value
eps_modified = True
stock_cp.dashcamOnly = False
if candidate == CAR.HONDA_CIVIC:
if eps_modified:
@@ -283,12 +278,6 @@ class CarInterface(CarInterfaceBase):
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.21], [0.07]]
elif candidate == CAR.HONDA_CLARITY:
# FIXME-SP: remove when the sync brings in https://github.com/commaai/opendbc/pull/2672
for fw in car_fw:
if fw.ecu == "eps" and b"," in fw.fwVersion:
ret.flags |= HondaFlagsSP.EPS_MOD.value
eps_modified = True
ret.safetyParam |= HondaSafetyFlagsSP.CLARITY
stock_cp.autoResumeSng = True
stock_cp.minEnableSpeed = -1

View File

@@ -175,14 +175,18 @@ class CAR(Platforms):
{Bus.pt: 'honda_accord_2018_can_generated'},
)
HONDA_ACCORD_11G = HondaBoschCANFDPlatformConfig(
[HondaCarDocs("Honda Accord 2023", "All")],
[
HondaCarDocs("Honda Accord 2023-25", "All"),
HondaCarDocs("Honda Accord Hybrid 2023-25", "All"),
],
CarSpecs(mass=3477 * CV.LB_TO_KG, wheelbase=2.83, steerRatio=16.0, centerToFrontRatio=0.39),
)
HONDA_CIVIC_BOSCH = HondaBoschPlatformConfig(
[
HondaCarDocs("Honda Civic 2019-21", "All", video="https://www.youtube.com/watch?v=4Iz1Mz5LGF8",
footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS),
HondaCarDocs("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS),
HondaCarDocs("Honda Civic Hatchback 2017-18", min_steer_speed=12. * CV.MPH_TO_MS),
HondaCarDocs("Honda Civic Hatchback 2019-21", "All", min_steer_speed=12. * CV.MPH_TO_MS),
],
CarSpecs(mass=1326, wheelbase=2.7, steerRatio=15.38, centerToFrontRatio=0.4), # steerRatio: 10.93 is end-to-end spec
{Bus.pt: 'honda_civic_hatchback_ex_2017_can_generated'},
@@ -212,6 +216,13 @@ class CAR(Platforms):
{Bus.pt: 'honda_crv_ex_2017_can_generated', Bus.body: 'honda_crv_ex_2017_body_generated'},
flags=HondaFlags.BOSCH_ALT_BRAKE,
)
HONDA_CRV_6G = HondaBoschCANFDPlatformConfig(
[
HondaCarDocs("Honda CR-V 2023-25", "All"),
HondaCarDocs("Honda CR-V Hybrid 2023-25", "All"),
],
CarSpecs(mass=1703, wheelbase=2.7, steerRatio=16.2, centerToFrontRatio=0.42),
)
HONDA_CRV_HYBRID = HondaBoschPlatformConfig(
[HondaCarDocs("Honda CR-V Hybrid 2017-22", min_steer_speed=12. * CV.MPH_TO_MS)],
# mass: mean of 4 models in kg, steerRatio: 12.3 is spec end-to-end
@@ -224,6 +235,12 @@ class CAR(Platforms):
{Bus.pt: 'honda_civic_ex_2022_can_generated'},
flags=HondaFlags.BOSCH_RADARLESS,
)
HONDA_CITY_7G = HondaBoschPlatformConfig(
[HondaCarDocs("Honda City (Brazil only) 2023", "All")],
CarSpecs(mass=3125 * CV.LB_TO_KG, wheelbase=2.6, steerRatio=19.0, centerToFrontRatio=0.41, minSteerSpeed=23. * CV.KPH_TO_MS),
{Bus.pt: 'honda_civic_ex_2022_can_generated'},
flags=HondaFlags.BOSCH_RADARLESS,
)
ACURA_RDX_3G = HondaBoschPlatformConfig(
[HondaCarDocs("Acura RDX 2019-21", "All", min_steer_speed=3. * CV.MPH_TO_MS)],
CarSpecs(mass=4068 * CV.LB_TO_KG, wheelbase=2.75, steerRatio=11.95, centerToFrontRatio=0.41, tireStiffnessFactor=0.677), # as spec
@@ -241,9 +258,13 @@ class CAR(Platforms):
{Bus.pt: 'acura_rdx_2020_can_generated'},
)
HONDA_PILOT_4G = HondaBoschCANFDPlatformConfig(
[HondaCarDocs("Honda Pilot 2023", "All")],
CarSpecs(mass=4278 * CV.LB_TO_KG, wheelbase=2.86, centerToFrontRatio=0.428, steerRatio=16.0, tireStiffnessFactor=0.444), # as spec
flags=HondaFlags.BOSCH_ALT_BRAKE,
[HondaCarDocs("Honda Pilot 2023-25", "All")],
CarSpecs(mass=4660 * CV.LB_TO_KG, wheelbase=2.89, centerToFrontRatio=0.442, steerRatio=17.5),
)
# mid-model refresh
ACURA_MDX_4G_MMR = HondaBoschCANFDPlatformConfig(
[HondaCarDocs("Acura MDX 2025", "All except Type S")],
CarSpecs(mass=4544 * CV.LB_TO_KG, wheelbase=2.89, centerToFrontRatio=0.428, steerRatio=16.2),
)
# Nidec Cars
@@ -345,6 +366,10 @@ STEER_THRESHOLD = {
CAR.ACURA_RDX: 400,
CAR.HONDA_CRV_EU: 400,
CAR.HONDA_ACCORD_11G: 600,
CAR.HONDA_PILOT_4G: 600,
CAR.ACURA_MDX_4G_MMR: 600,
CAR.HONDA_CRV_6G: 600,
CAR.HONDA_CITY_7G: 600,
}
@@ -389,9 +414,9 @@ FW_QUERY_CONFIG = FwQueryConfig(
# Note that we still attempt to match with them when they are present
# This is or'd with (ALL_ECUS - ESSENTIAL_ECUS) from fw_versions.py
non_essential_ecus={
Ecu.eps: [CAR.ACURA_RDX_3G, CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_2022, CAR.HONDA_E, CAR.HONDA_HRV_3G, *HONDA_BOSCH_CANFD],
Ecu.eps: [CAR.ACURA_RDX_3G, CAR.HONDA_ACCORD, CAR.HONDA_CIVIC_2022, CAR.HONDA_E, CAR.HONDA_HRV_3G, CAR.HONDA_CITY_7G, *HONDA_BOSCH_CANFD],
Ecu.vsa: [CAR.ACURA_RDX_3G, CAR.HONDA_ACCORD, CAR.HONDA_CIVIC, CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_2022, CAR.HONDA_CRV_5G, CAR.HONDA_CRV_HYBRID,
CAR.HONDA_E, CAR.HONDA_HRV_3G, CAR.HONDA_INSIGHT, *HONDA_BOSCH_CANFD],
CAR.HONDA_E, CAR.HONDA_HRV_3G, CAR.HONDA_INSIGHT, CAR.HONDA_CITY_7G, *HONDA_BOSCH_CANFD],
},
extra_ecus=[
(Ecu.combinationMeter, 0x18da60f1, None),

View File

@@ -322,7 +322,8 @@ class CarState(CarStateBase, EsccCarStateBase, MadsCarState, CarStateExt):
if not (CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS):
# TODO: this can be removed once we add dynamic support to vl_all
msgs += [
("CRUISE_BUTTONS", 50)
# this message is 50Hz but the ECU frequently stops transmitting for ~0.5s
("CRUISE_BUTTONS", 1)
]
return {
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], msgs, CanBus(CP).ECAN),

View File

@@ -731,6 +731,7 @@ FW_VERSIONS = {
b'\xf1\x00DE MDPS C 1.00 1.04 56310Q4100\x00 4DEEC104',
b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4000\x00 4DEEC105',
b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4100\x00 4DEEC105',
b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4200\x00 4DEEC105',
],
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00DEE MFC AT EUR LHD 1.00 1.00 99211-Q4000 191211',
@@ -986,6 +987,7 @@ FW_VERSIONS = {
},
CAR.KIA_SORENTO: {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00UMP LKAS AT AUS RHD 1.00 1.00 96400-C6550 S30',
b'\xf1\x00UMP LKAS AT KOR LHD 1.00 1.00 95740-C5550 S30',
b'\xf1\x00UMP LKAS AT USA LHD 1.00 1.00 95740-C6550 d00',
b'\xf1\x00UMP LKAS AT USA LHD 1.01 1.01 95740-C6550 d01',
@@ -1249,16 +1251,19 @@ FW_VERSIONS = {
},
CAR.HYUNDAI_KONA_2022: {
(Ecu.fwdCamera, 0x7c4, None): [
b'\xf1\x00OSP LKA AT CND LHD 1.00 1.04 99211-J9200 904',
b'\xf1\x00OSP LKA AT USA LHD 1.00 1.04 99211-J9200 904',
],
(Ecu.eps, 0x7d4, None): [
b'\xf1\x00OSP MDPS C 1.00 1.04 56310/J9290 4OPCC104',
b'\xf1\x00OSP MDPS C 1.00 1.04 56310/J9291 4OPCC104',
b'\xf1\x00OSP MDPS C 1.00 1.04 56310J9291\x00 4OPCC104',
],
(Ecu.fwdRadar, 0x7d0, None): [
b'\xf1\x00YB__ FCA ----- 1.00 1.01 99110-J9000 \x00\x00\x00',
],
(Ecu.transmission, 0x7e1, None): [
b'\xf1\x00HT6WA280BLHT6VA650A1COS4N20NS1\x00\x00\x00\x00\x00\x00\x15\xf5\x87~',
b'\xf1\x00T01960BL T01E60A1 DOS2T16X4XE60NS4N\x90\xe6\xcb',
b'\xf1\x00T01G00BL T01I00A1 DOS2T16X2XI00NS0\x8c`\xff\xe7',
b'\xf1\x00T01G00BL T01I00A1 DOS2T16X4XI00NS0\x99L\xeeq',

View File

@@ -253,7 +253,7 @@ class CAR(Platforms):
flags=HyundaiFlags.CLUSTER_GEARS | HyundaiFlags.ALT_LIMITS,
)
HYUNDAI_KONA_2022 = HyundaiPlatformConfig(
[HyundaiCarDocs("Hyundai Kona 2022", car_parts=CarParts.common([CarHarness.hyundai_o]))],
[HyundaiCarDocs("Hyundai Kona 2022-23", car_parts=CarParts.common([CarHarness.hyundai_o]))],
CarSpecs(mass=1491, wheelbase=2.6, steerRatio=13.42, tireStiffnessFactor=0.385),
flags=HyundaiFlags.CAMERA_SCC | HyundaiFlags.ALT_LIMITS_2,
)

View File

@@ -1,11 +1,10 @@
import json
import os
import numpy as np
import time
import tomllib
from abc import abstractmethod, ABC
from enum import StrEnum
from typing import Any, NamedTuple
from typing import Any
from collections.abc import Callable
from functools import cache
@@ -43,15 +42,8 @@ GEAR_SHIFTER_MAP: dict[str, structs.CarState.GearShifter] = {
'B': GearShifter.brake, 'BRAKE': GearShifter.brake,
}
class LatControlInputs(NamedTuple):
lateral_acceleration: float
roll_compensation: float
vego: float
aego: float
TorqueFromLateralAccelCallbackType = Callable[[LatControlInputs, structs.CarParams.LateralTorqueTuning, bool], float]
TorqueFromLateralAccelCallbackType = Callable[[float, structs.CarParams.LateralTorqueTuning, bool], float]
LateralAccelFromTorqueCallbackType = Callable[[float, structs.CarParams.LateralTorqueTuning, bool], float]
@cache
@@ -214,14 +206,19 @@ class CarInterfaceBase(ABC):
def get_steer_feedforward_function(self):
return self.get_steer_feedforward_default
def torque_from_lateral_accel_linear(self, latcontrol_inputs: LatControlInputs, torque_params: structs.CarParams.LateralTorqueTuning,
gravity_adjusted: bool) -> float:
def torque_from_lateral_accel_linear(self, lateral_acceleration: float, torque_params: structs.CarParams.LateralTorqueTuning) -> float:
# The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction)
return latcontrol_inputs.lateral_acceleration / float(torque_params.latAccelFactor)
return lateral_acceleration / float(torque_params.latAccelFactor)
def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
return self.torque_from_lateral_accel_linear
def lateral_accel_from_torque_linear(self, torque: float, torque_params: structs.CarParams.LateralTorqueTuning) -> float:
return torque * float(torque_params.latAccelFactor)
def lateral_accel_from_torque(self) -> LateralAccelFromTorqueCallbackType:
return self.lateral_accel_from_torque_linear
# returns a set of default params to avoid repetition in car specific params
@staticmethod
def get_std_params(candidate: str) -> structs.CarParams:
@@ -261,8 +258,8 @@ class CarInterfaceBase(ABC):
params = get_torque_params()[candidate]
tune.init('torque')
tune.torque.kp = 1.0
tune.torque.kf = 1.0
tune.torque.kp = 1.0
tune.torque.ki = 0.3
tune.torque.friction = params['FRICTION']
tune.torque.latAccelFactor = params['LAT_ACCEL_FACTOR']
@@ -445,35 +442,3 @@ def get_interface_attr(attr: str, combine_brands: bool = False, ignore_none: boo
pass
return result
class NanoFFModel:
def __init__(self, weights_loc: str, platform: str):
self.weights_loc = weights_loc
self.platform = platform
self.load_weights(platform)
def load_weights(self, platform: str):
with open(self.weights_loc) as fob:
self.weights = {k: np.array(v) for k, v in json.load(fob)[platform].items()}
def relu(self, x: np.ndarray):
return np.maximum(0.0, x)
def forward(self, x: np.ndarray):
assert x.ndim == 1
x = (x - self.weights['input_norm_mat'][:, 0]) / (self.weights['input_norm_mat'][:, 1] - self.weights['input_norm_mat'][:, 0])
x = self.relu(np.dot(x, self.weights['w_1']) + self.weights['b_1'])
x = self.relu(np.dot(x, self.weights['w_2']) + self.weights['b_2'])
x = self.relu(np.dot(x, self.weights['w_3']) + self.weights['b_3'])
x = np.dot(x, self.weights['w_4']) + self.weights['b_4']
return x
def predict(self, x: list[float], do_sample: bool = False):
x = self.forward(np.array(x))
if do_sample:
pred = np.random.laplace(x[0], np.exp(x[1]) / self.weights['temperature'])
else:
pred = x[0]
pred = pred * (self.weights['output_norm_mat'][1] - self.weights['output_norm_mat'][0]) + self.weights['output_norm_mat'][0]
return pred

View File

@@ -160,9 +160,10 @@ def apply_center_deadzone(error, deadzone):
def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, friction_threshold: float,
torque_params: structs.CarParams.LateralTorqueTuning) -> float:
# TODO torque params' friction should be in lataxel space, not torque space
friction_interp = np.interp(
apply_center_deadzone(lateral_accel_error, lateral_accel_deadzone),
[-friction_threshold, friction_threshold],
[-torque_params.friction, torque_params.friction]
[-torque_params.friction * torque_params.latAccelFactor, torque_params.friction * torque_params.latAccelFactor]
)
return float(friction_interp)

View File

@@ -59,6 +59,7 @@ FW_VERSIONS = {
b'476605SD2E',
b'476605SH1D',
b'476605SH7D',
b'476605SH7E',
b'476605SK2A',
],
(Ecu.eps, 0x742, None): [

View File

View File

@@ -0,0 +1,41 @@
from opendbc.can.packer import CANPacker
from opendbc.car import Bus
from opendbc.car.lateral import apply_std_steer_angle_limits
from opendbc.car.interfaces import CarControllerBase
from opendbc.car.psa.psacan import create_lka_steering
from opendbc.car.psa.values import CarControllerParams
class CarController(CarControllerBase):
def __init__(self, dbc_names, CP, CP_SP):
super().__init__(dbc_names, CP, CP_SP)
self.packer = CANPacker(dbc_names[Bus.main])
self.apply_angle_last = 0
self.status = 2
def update(self, CC, CC_SP, CS, now_nanos):
can_sends = []
actuators = CC.actuators
# lateral control
if self.frame % 5 == 0:
apply_angle = apply_std_steer_angle_limits(actuators.steeringAngleDeg, self.apply_angle_last, CS.out.vEgoRaw,
CS.out.steeringAngleDeg, CC.latActive, CarControllerParams.ANGLE_LIMITS)
# EPS disengages on steering override, activation sequence 2->3->4 to re-engage
# STATUS - 0: UNAVAILABLE, 1: UNSELECTED, 2: READY, 3: AUTHORIZED, 4: ACTIVE
if not CC.latActive:
self.status = 2
elif not CS.eps_active and not CS.out.steeringPressed:
self.status = 2 if self.status == 4 else self.status + 1
else:
self.status = 4
can_sends.append(create_lka_steering(self.packer, CC.latActive, apply_angle, self.status))
self.apply_angle_last = apply_angle
new_actuators = actuators.as_builder()
new_actuators.steeringAngleDeg = self.apply_angle_last
self.frame += 1
return new_actuators, can_sends

View File

@@ -0,0 +1,74 @@
from opendbc.car import structs, Bus
from opendbc.can.parser import CANParser
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.psa.values import DBC, CarControllerParams
from opendbc.car.interfaces import CarStateBase
GearShifter = structs.CarState.GearShifter
TransmissionType = structs.CarParams.TransmissionType
class CarState(CarStateBase):
def update(self, can_parsers) -> structs.CarState:
cp = can_parsers[Bus.main]
cp_adas = can_parsers[Bus.adas]
cp_cam = can_parsers[Bus.cam]
ret = structs.CarState()
ret_sp = structs.CarStateSP()
# car speed
self.parse_wheel_speeds(ret,
cp.vl['Dyn4_FRE']['P263_VehV_VPsvValWhlFrtL'],
cp.vl['Dyn4_FRE']['P264_VehV_VPsvValWhlFrtR'],
cp.vl['Dyn4_FRE']['P265_VehV_VPsvValWhlBckL'],
cp.vl['Dyn4_FRE']['P266_VehV_VPsvValWhlBckR'],
)
ret.yawRate = cp_adas.vl['HS2_DYN_UCF_MDD_32D']['VITESSE_LACET_BRUTE'] * CV.DEG_TO_RAD
ret.standstill = bool(cp_adas.vl['HS2_DYN_UCF_MDD_32D']['VEHICLE_STANDSTILL'])
# gas
ret.gasPressed = cp.vl['Dyn_CMM']['P002_Com_rAPP'] > 0
# brake
ret.brakePressed = bool(cp_cam.vl['Dat_BSI']['P013_MainBrake'])
ret.parkingBrake = cp.vl['Dyn_EasyMove']['P337_Com_stPrkBrk'] == 1 # 0: disengaged, 1: engaged, 3: brake actuator moving
# steering wheel
ret.steeringAngleDeg = cp.vl['STEERING_ALT']['ANGLE'] # EPS
ret.steeringRateDeg = cp.vl['STEERING_ALT']['RATE'] * (2 * cp.vl['STEERING_ALT']['RATE_SIGN'] - 1) # convert [0,1] to [-1,1] EPS: rot. speed * rot. sign
ret.steeringTorque = cp.vl['STEERING']['DRIVER_TORQUE']
ret.steeringTorqueEps = cp.vl['IS_DAT_DIRA']['EPS_TORQUE']
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
self.eps_active = cp.vl['IS_DAT_DIRA']['EPS_STATE_LKA'] == 3 # 0: Unauthorized, 1: Authorized, 2: Available, 3: Active, 4: Defect
# cruise
ret.cruiseState.speed = cp_adas.vl['HS2_DAT_MDD_CMD_452']['SPEED_SETPOINT'] * CV.KPH_TO_MS # set to 255 when ACC is off, -2 kph offset from dash speed
ret.cruiseState.enabled = cp_adas.vl['HS2_DAT_MDD_CMD_452']['RVV_ACC_ACTIVATION_REQ'] == 1
ret.cruiseState.available = cp_adas.vl['HS2_DYN1_MDD_ETAT_2B6']['ACC_STATUS'] > 2
ret.cruiseState.nonAdaptive = cp_adas.vl['HS2_DAT_MDD_CMD_452']['LONGITUDINAL_REGULATION_TYPE'] != 3 # 0: None, 1: CC, 2: Limiter, 3: ACC
ret.cruiseState.standstill = bool(cp_adas.vl['HS2_DYN_UCF_MDD_32D']['VEHICLE_STANDSTILL'])
ret.accFaulted = cp_adas.vl['HS2_DYN_UCF_MDD_32D']['ACC_ETAT_DECEL_OR_ESP_STATUS'] == 3 # 0: Inhibited, 1: Waiting, 2: Active, 3: Fault
# gear
if bool(cp_cam.vl['Dat_BSI']['P103_Com_bRevGear']):
ret.gearShifter = GearShifter.reverse
else:
ret.gearShifter = GearShifter.drive
# blinkers
blinker = cp_cam.vl['HS2_DAT7_BSI_612']['CDE_CLG_ET_HDC']
ret.leftBlinker = blinker == 1
ret.rightBlinker = blinker == 2
# lock info
ret.doorOpen = any((cp_cam.vl['Dat_BSI']['DRIVER_DOOR'], cp_cam.vl['Dat_BSI']['PASSENGER_DOOR']))
ret.seatbeltUnlatched = cp_cam.vl['RESTRAINTS']['DRIVER_SEATBELT'] != 2
return ret, ret_sp
@staticmethod
def get_can_parsers(CP, CP_SP):
return {
Bus.main: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 0),
Bus.adas: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 1),
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
}

View File

@@ -0,0 +1,13 @@
""" AUTO-FORMATTED USING opendbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
from opendbc.car.structs import CarParams
from opendbc.car.psa.values import CAR
Ecu = CarParams.Ecu
FW_VERSIONS = {
CAR.PSA_PEUGEOT_208: {
(Ecu.fwdRadar, 0x6b6, None): [
b'212053276',
],
},
}

View File

@@ -0,0 +1,30 @@
from opendbc.car import structs, get_safety_config
from opendbc.car.interfaces import CarInterfaceBase
from opendbc.car.psa.carcontroller import CarController
from opendbc.car.psa.carstate import CarState
TransmissionType = structs.CarParams.TransmissionType
class CarInterface(CarInterfaceBase):
CarState = CarState
CarController = CarController
@staticmethod
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
ret.brand = 'psa'
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.psa)]
ret.dashcamOnly = True
ret.steerActuatorDelay = 0.3
ret.steerLimitTimer = 0.1
ret.steerAtStandstill = True
ret.steerControlType = structs.CarParams.SteerControlType.angle
ret.radarUnavailable = True
ret.alphaLongitudinalAvailable = False
return ret

18
opendbc/car/psa/psacan.py Normal file
View File

@@ -0,0 +1,18 @@
def psa_checksum(address: int, sig, d: bytearray) -> int:
chk_ini = {0x452: 0x4, 0x38D: 0x7, 0x42D: 0xC}.get(address, 0xB)
byte = sig.start_bit // 8
d[byte] &= 0x0F if sig.start_bit % 8 >= 4 else 0xF0
checksum = sum((b >> 4) + (b & 0xF) for b in d)
return (chk_ini - checksum) & 0xF
def create_lka_steering(packer, lat_active: bool, apply_angle: float, status: int):
values = {
'DRIVE': 1,
'STATUS': status,
'LXA_ACTIVATION': 1,
'TORQUE_FACTOR': lat_active * 100,
'SET_ANGLE': apply_angle,
}
return packer.make_can_msg('LANE_KEEP_ASSIST', 0, values)

54
opendbc/car/psa/values.py Normal file
View File

@@ -0,0 +1,54 @@
from dataclasses import dataclass, field
from opendbc.car.structs import CarParams
from opendbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms
from opendbc.car.lateral import AngleSteeringLimits
from opendbc.car.docs_definitions import CarDocs, CarHarness, CarParts
from opendbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
Ecu = CarParams.Ecu
class CarControllerParams:
STEER_STEP = 1
ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
390, # deg
([0., 5., 25.], [2.5, 1.5, .2]),
([0., 5., 25.], [5., 2., .3]),
)
STEER_DRIVER_ALLOWANCE = 5 # Driver intervention threshold, 0.5 Nm
@dataclass
class PSACarDocs(CarDocs):
package: str = "Adaptive Cruise Control (ACC) & Lane Assist"
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.psa_a]))
@dataclass
class PSAPlatformConfig(PlatformConfig):
dbc_dict: DbcDict = field(default_factory=lambda: {
Bus.pt: 'psa_aee2010_r3',
})
class CAR(Platforms):
PSA_PEUGEOT_208 = PSAPlatformConfig(
[PSACarDocs("Peugeot 208 2019-25")],
CarSpecs(mass=1530, wheelbase=2.54, steerRatio=17.6),
)
# Placeholder, FW Query will be added in separate PR
FW_QUERY_CONFIG = FwQueryConfig(
requests=[
Request(
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.UDS_VERSION_REQUEST],
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.UDS_VERSION_RESPONSE],
bus=0,
),
],
)
DBC = CAR.create_dbc_map()

View File

@@ -14,6 +14,7 @@ FW_VERSIONS = {
b'TeMYG4_DCS_Update_0.0.0 (9),E4014.26.0',
b'TeMYG4_Legacy3Y_0.0.0 (2),E4015.02.0',
b'TeMYG4_Legacy3Y_0.0.0 (5),E4015.03.2',
b'TeMYG4_Legacy3Y_0.0.0 (5),E4L015.03.2',
b'TeMYG4_Main_0.0.0 (59),E4H014.29.0',
b'TeMYG4_Main_0.0.0 (65),E4H015.01.0',
b'TeMYG4_Main_0.0.0 (67),E4H015.02.1',

View File

@@ -15,6 +15,7 @@ from opendbc.car.toyota.values import CAR as TOYOTA
from opendbc.car.values import Platform
from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN
from opendbc.car.body.values import CAR as COMMA
from opendbc.car.psa.values import CAR as PSA
# FIXME: add routes for these cars
non_tested_cars = [
@@ -122,6 +123,9 @@ routes = [
CarTestRoute("b1c832ad56b6bc9d/00000010--debfcf5867", HONDA.HONDA_CIVIC_2022), # 2025 Civic Hatch Hybrid with new eCVT transmission
CarTestRoute("f9c43864cf057d05/2024-01-15--23-01-20", HONDA.HONDA_PILOT_4G), # TODO: Replace with a newer route
CarTestRoute("f39cf149898833ff/0000002b--54f3fae045", HONDA.HONDA_ACCORD_11G),
CarTestRoute("ad9840558640c31d/0000001a--d6cd4871c2", HONDA.ACURA_MDX_4G_MMR), # 2025 MDX
CarTestRoute("63568e3e2f56c8ad/0000000a--a254e90429", HONDA.HONDA_CRV_6G),
CarTestRoute("56b2cf1dacdcd033/00000048--3267801001", HONDA.HONDA_CITY_7G), # Brazilian model
CarTestRoute("87d7f06ade479c2e/2023-09-11--23-30-11", HYUNDAI.HYUNDAI_AZERA_6TH_GEN),
CarTestRoute("66189dd8ec7b50e6/2023-09-20--07-02-12", HYUNDAI.HYUNDAI_AZERA_HEV_6TH_GEN),
@@ -194,7 +198,7 @@ routes = [
CarTestRoute("db04d2c63990e3ba/2023-02-08--16-52-39", HYUNDAI.KIA_NIRO_HEV_2ND_GEN),
CarTestRoute("50a2212c41f65c7b/2021-05-24--16-22-06", HYUNDAI.KIA_FORTE),
CarTestRoute("192283cdbb7a58c2/2022-10-15--01-43-18", HYUNDAI.KIA_SPORTAGE_5TH_GEN),
CarTestRoute("09559f1fcaed4704/2023-11-16--02-24-57", HYUNDAI.KIA_SPORTAGE_5TH_GEN), # openpilot longitudinal
CarTestRoute("09559f1fcaed4704/2023-11-16--02-24-57", HYUNDAI.KIA_SPORTAGE_5TH_GEN, segment=0), # openpilot longitudinal
CarTestRoute("b3537035ffe6a7d6/2022-10-17--15-23-49", HYUNDAI.KIA_SPORTAGE_5TH_GEN), # hybrid
CarTestRoute("c5ac319aa9583f83/2021-06-01--18-18-31", HYUNDAI.HYUNDAI_ELANTRA),
CarTestRoute("734ef96182ddf940/2022-10-02--16-41-44", HYUNDAI.HYUNDAI_ELANTRA_GT_I30),
@@ -324,6 +328,8 @@ routes = [
CarTestRoute("f6d5b1a9d7a1c92e/2021-07-08--06-56-59", MAZDA.MAZDA_CX9_2021),
CarTestRoute("a4af1602d8e668ac/2022-02-03--12-17-07", MAZDA.MAZDA_CX5_2022),
CarTestRoute("6a7075a4fdd765ee/0000004e--1f612006dd", PSA.PSA_PEUGEOT_208),
CarTestRoute("bc095dc92e101734/000000db--ee9fe46e57", RIVIAN.RIVIAN_R1_GEN1),
CarTestRoute("7dc058789994da80/00000112--adb970f6a8", TESLA.TESLA_MODEL_3),

View File

@@ -260,7 +260,7 @@ class TestFwFingerprintTiming:
print(f'get_vin {name} case, query time={self.total_time / self.N} seconds')
def test_fw_query_timing(self, subtests, mocker):
total_ref_time = {1: 7.3, 2: 7.9}
total_ref_time = {1: 7.4, 2: 8.0}
brand_ref_times = {
1: {
'gm': 1.0,
@@ -276,6 +276,7 @@ class TestFwFingerprintTiming:
'toyota': 0.7,
'volkswagen': 0.65,
'rivian': 0.3,
'psa': 0.1,
},
2: {
'ford': 1.6,

File diff suppressed because one or more lines are too long

View File

@@ -7,6 +7,9 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
"NISSAN_LEAF" = [nan, 1.5, nan]
"NISSAN_ROGUE" = [nan, 1.5, nan]
# PSA angle based controllers
"PSA_PEUGEOT_208" = [nan, 2.0, nan]
# New subarus angle based controllers
"SUBARU_FORESTER_2022" = [nan, 3.0, nan]
"SUBARU_OUTBACK_2023" = [nan, 3.0, nan]
@@ -80,6 +83,10 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
"RIVIAN_R1_GEN1" = [2.8, 2.5, 0.07]
"HYUNDAI_NEXO_1ST_GEN" = [2.5, 2.5, 0.1]
"HONDA_ACCORD_11G" = [1.35, 1.35, 0.17]
"HONDA_PILOT_4G" = [1.25, 1.25, 0.21]
"ACURA_MDX_4G_MMR" = [1.25, 1.25, 0.15]
"HONDA_CRV_6G" = [1.3, 1.3, 0.2]
"HONDA_CITY_7G" = [1.2, 1.2, 0.23]
# Dashcam or fallback configured as ideal car
"MOCK" = [10.0, 10, 0.0]
@@ -87,4 +94,3 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
# Manually checked
"HONDA_CIVIC_2022" = [2.5, 1.2, 0.15]
"HONDA_HRV_3G" = [2.5, 1.2, 0.2]
"HONDA_PILOT_4G" = [1.0, 1.0, 0.2]

View File

@@ -500,6 +500,7 @@ FW_VERSIONS = {
b'\x02896630ZZ0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966312K6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966312L0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966312L0100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966312Q3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966312Q3100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
b'\x028966312Q4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
@@ -1187,6 +1188,7 @@ FW_VERSIONS = {
b'\x01F15264283200\x00\x00\x00\x00',
b'\x01F15264286100\x00\x00\x00\x00',
b'\x01F15264286200\x00\x00\x00\x00',
b'\x01F152642870\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'\x028965B0R01500\x00\x00\x00\x008965B0R02500\x00\x00\x00\x00',
@@ -1837,17 +1839,22 @@ FW_VERSIONS = {
CAR.TOYOTA_YARIS: {
(Ecu.engine, 0x700, None): [
b'\x0189663K015300\x00\x00\x00\x00',
b'\x0189663K023000\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'\x018965BK002100\x00\x00\x00\x00',
b'\x018965BK003200\x00\x00\x00\x00',
],
(Ecu.abs, 0x7b0, None): [
b'\x01F1526K005600\x00\x00\x00\x00',
b'\x01F1526K007500\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
b'\x018821F0D04100\x00\x00\x00\x00',
b'\x018821F0D05300\x00\x00\x00\x00',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'\x028646F0W04100\x00\x00\x00\x008646G0W04100\x00\x00\x00\x00',
b'\x028646F5205200\x00\x00\x00\x008646G5202200\x00\x00\x00\x00',
],
},

View File

@@ -290,7 +290,7 @@ class CAR(Platforms):
CarSpecs(mass=4372. * CV.LB_TO_KG, wheelbase=2.68, steerRatio=16.88, tireStiffnessFactor=0.5533),
)
TOYOTA_YARIS = ToyotaSecOCPlatformConfig(
[ToyotaCommunityCarDocs("Toyota Yaris (Non-US only) 2023", min_enable_speed=MIN_ACC_SPEED)],
[ToyotaCommunityCarDocs("Toyota Yaris (Non-US only) 2020, 2023", min_enable_speed=MIN_ACC_SPEED)],
CarSpecs(mass=1170, wheelbase=2.55, steerRatio=14.80, tireStiffnessFactor=0.5533),
flags=ToyotaFlags.RADAR_ACC,
)

View File

@@ -8,13 +8,14 @@ from opendbc.car.hyundai.values import CAR as HYUNDAI
from opendbc.car.mazda.values import CAR as MAZDA
from opendbc.car.mock.values import CAR as MOCK
from opendbc.car.nissan.values import CAR as NISSAN
from opendbc.car.psa.values import CAR as PSA
from opendbc.car.rivian.values import CAR as RIVIAN
from opendbc.car.subaru.values import CAR as SUBARU
from opendbc.car.tesla.values import CAR as TESLA
from opendbc.car.toyota.values import CAR as TOYOTA
from opendbc.car.volkswagen.values import CAR as VOLKSWAGEN
Platform = BODY | CHRYSLER | FORD | GM | HONDA | HYUNDAI | MAZDA | MOCK | NISSAN | RIVIAN | SUBARU | TESLA | TOYOTA | VOLKSWAGEN
Platform = BODY | CHRYSLER | FORD | GM | HONDA | HYUNDAI | MAZDA | MOCK | NISSAN | PSA | RIVIAN | SUBARU | TESLA | TOYOTA | VOLKSWAGEN
BRANDS = get_args(Platform)
PLATFORMS: dict[str, Platform] = {str(platform): platform for brand in BRANDS for platform in brand}

View File

@@ -441,6 +441,7 @@ FW_VERSIONS = {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8703N906026E \xf1\x892114',
b'\xf1\x8704E906023AH\xf1\x893379',
b'\xf1\x8704E906023BF\xf1\x893842',
b'\xf1\x8704E906023BM\xf1\x894522',
b'\xf1\x8704L906026DP\xf1\x891538',
b'\xf1\x8704L906026ET\xf1\x891990',
@@ -465,6 +466,7 @@ FW_VERSIONS = {
b'\xf1\x870D9300041A \xf1\x894801',
b'\xf1\x870D9300042H \xf1\x894901',
b'\xf1\x870DD300045T \xf1\x891601',
b'\xf1\x870DD300046B \xf1\x891601',
b'\xf1\x870DD300046H \xf1\x891601',
b'\xf1\x870DL300011H \xf1\x895201',
b'\xf1\x870GC300042H \xf1\x891404',

View File

@@ -384,8 +384,8 @@ BO_ 437 FR_CMR_03_50ms: 32 FR_CMR
SG_ Info_RtLnDptSta : 99|2@1+ (1,0) [0|3] "" RR_C_RDR,CGW
SG_ Info_RtLnPosVal : 101|14@1- (0.0039625,0) [-32.4608|32.4568375] "m" RR_C_RDR,CGW
SG_ Info_RtLnHdingAnglVal : 115|10@1- (0.000976563,0) [-0.500000256|0.499023693] "rad" RR_C_RDR,CGW
SG_ Info_RtLnCvtrVal : 128|16@1- (1,0) [0|65535] "" CGW
SG_ Info_RtLnCrvtrDrvtvVal : 144|16@1- (1,0) [0|65535] "" CGW
SG_ Info_RtLnCvtrVal : 128|16@1- (1e-06,0) [-0.032768|0.032767] "1/m" CGW
SG_ Info_RtLnCrvtrDrvtvVal : 144|16@1- (4e-09,0) [-0.000131072|0.000131068] "1/m2" CGW
SG_ ID_CIPV : 192|8@1+ (1,0) [0|255] "" XXX
SG_ Relative_Velocity : 200|12@1+ (0.05,-100) [-100|104.75] "m/s" XXX
SG_ Longitudinal_Distance : 212|12@1+ (0.05,0) [0|204.75] "m" XXX

View File

@@ -162,8 +162,8 @@ BO_ 773 STEERING_ALT: 7 XXX
SG_ ANGLE : 7|16@0- (0.1,0) [-3276.8|3276.7] "degrees" XXX
SG_ RATE : 23|8@0+ (1,0) [0|255] "" XXX
SG_ RATE_SIGN : 31|1@0+ (1,0) [0|1] "" XXX
SG_ COUNTER : 35|4@0+ (1,0) [0|255] "" XXX
SG_ CHECKSUM : 39|4@0+ (1,0) [0|15] "" XXX
SG_ 0_COUNTER : 35|4@0+ (1,0) [0|255] "" XXX
SG_ 0_CHECKSUM : 39|4@0+ (1,0) [0|15] "" XXX
SG_ RATE_ALT : 47|8@0+ (1,0) [0|255] "" XXX
BO_ 781 Dyn4_FRE: 8 CDS___Fahrdynamikregelung
@@ -833,6 +833,7 @@ CM_ SG_ 758 CHECKSUM_TRANSM_DYN_ACC2 "CHK_INI = 0x7";
CM_ SG_ 758 PROCESS_COUNTER_4B_ACC2 "This counter increments from 0 to 15 at each activation of the calculation process.";
CM_ SG_ 773 ANGLE "-565 to 560, seems like estimate and not init";
CM_ SG_ 773 RATE_SIGN "1=moving clockwise, 0=moving anticlockwise";
CM_ SG_ 773 0_CHECKSUM "TODO: find checksum";
CM_ BO_ 840 "ID 348: Engine Dynamic Frame 2 (Period time 20 ms)";
CM_ SG_ 840 P165_FlFCD_stWtLvlSensDebVal "P165: Water in fuel indication (1 = water in fuel detected / 0 = no water in fuel)";
CM_ SG_ 840 P152_Gearbx_stGear "P152: Gear detected by ratio vehicule speed to engine speed";

151
opendbc/safety/modes/psa.h Normal file
View File

@@ -0,0 +1,151 @@
#pragma once
#include "opendbc/safety/safety_declarations.h"
#define PSA_STEERING 757U // RX from XXX, driver torque
#define PSA_STEERING_ALT 773U // RX from EPS, steering angle
#define PSA_DYN_CMM 520U // RX from CMM, gas pedal
#define PSA_HS2_DYN_ABR_38D 909U // RX from UC_FREIN, speed
#define PSA_HS2_DAT_MDD_CMD_452 1106U // RX from BSI, cruise state
#define PSA_DAT_BSI 1042U // RX from BSI, brake
#define PSA_LANE_KEEP_ASSIST 1010U // TX from OP, EPS
// CAN bus
#define PSA_MAIN_BUS 0U
#define PSA_ADAS_BUS 1U
#define PSA_CAM_BUS 2U
static uint8_t psa_get_counter(const CANPacket_t *msg) {
uint8_t cnt = 0;
if (msg->addr == PSA_HS2_DAT_MDD_CMD_452) {
cnt = (msg->data[3] >> 4) & 0xFU;
} else if (msg->addr == PSA_HS2_DYN_ABR_38D) {
cnt = (msg->data[5] >> 4) & 0xFU;
} else {
}
return cnt;
}
static uint32_t psa_get_checksum(const CANPacket_t *msg) {
uint8_t chksum = 0;
if (msg->addr == PSA_HS2_DAT_MDD_CMD_452) {
chksum = msg->data[5] & 0xFU;
} else if (msg->addr == PSA_HS2_DYN_ABR_38D) {
chksum = msg->data[5] & 0xFU;
} else {
}
return chksum;
}
static uint8_t _psa_compute_checksum(const CANPacket_t *msg, uint8_t chk_ini, int chk_pos) {
int len = GET_LEN(msg);
uint8_t sum = 0;
for (int i = 0; i < len; i++) {
uint8_t b = msg->data[i];
if (i == chk_pos) {
// set checksum in low nibble to 0
b &= 0xF0U;
}
sum += (b >> 4) + (b & 0xFU);
}
return (chk_ini - sum) & 0xFU;
}
static uint32_t psa_compute_checksum(const CANPacket_t *msg) {
uint8_t chk = 0;
if (msg->addr == PSA_HS2_DAT_MDD_CMD_452) {
chk = _psa_compute_checksum(msg, 0x4, 5);
} else if (msg->addr == PSA_HS2_DYN_ABR_38D) {
chk = _psa_compute_checksum(msg, 0x7, 5);
} else {
}
return chk;
}
static void psa_rx_hook(const CANPacket_t *msg) {
if (msg->bus == PSA_MAIN_BUS) {
if (msg->addr == PSA_DYN_CMM) {
gas_pressed = msg->data[3] > 0U; // P002_Com_rAPP
}
if (msg->addr == PSA_STEERING_ALT) {
int angle_meas_new = to_signed((msg->data[0] << 8) | msg->data[1], 16); // ANGLE
update_sample(&angle_meas, angle_meas_new);
}
if (msg->addr == PSA_HS2_DYN_ABR_38D) {
int speed = (msg->data[0] << 8) | msg->data[1];
vehicle_moving = speed > 0;
UPDATE_VEHICLE_SPEED(speed * 0.01 * KPH_TO_MS); // VITESSE_VEHICULE_ROUES
}
}
if (msg->bus == PSA_ADAS_BUS) {
if (msg->addr == PSA_HS2_DAT_MDD_CMD_452) {
pcm_cruise_check((msg->data[2U] >> 7U) & 1U); // RVV_ACC_ACTIVATION_REQ
}
}
if (msg->bus == PSA_CAM_BUS) {
if (msg->addr == PSA_DAT_BSI) {
brake_pressed = (msg->data[0U] >> 5U) & 1U; // P013_MainBrake
}
}
}
static bool psa_tx_hook(const CANPacket_t *msg) {
bool tx = true;
static const AngleSteeringLimits PSA_STEERING_LIMITS = {
.max_angle = 3900,
.angle_deg_to_can = 10,
.angle_rate_up_lookup = {
{0., 5., 25.},
{2.5, 1.5, .2},
},
.angle_rate_down_lookup = {
{0., 5., 25.},
{5., 2., .3},
},
};
// Safety check for LKA
if (msg->addr == PSA_LANE_KEEP_ASSIST) {
// SET_ANGLE
int desired_angle = to_signed((msg->data[6] << 6) | ((msg->data[7] & 0xFCU) >> 2), 14);
// TORQUE_FACTOR
bool lka_active = ((msg->data[5] & 0xFEU) >> 1) == 100U;
if (steer_angle_cmd_checks(desired_angle, lka_active, PSA_STEERING_LIMITS)) {
tx = false;
}
}
return tx;
}
static safety_config psa_init(uint16_t param) {
UNUSED(param);
static const CanMsg PSA_TX_MSGS[] = {
{PSA_LANE_KEEP_ASSIST, PSA_MAIN_BUS, 8, .check_relay = true}, // EPS steering
};
static RxCheck psa_rx_checks[] = {
{.msg = {{PSA_HS2_DAT_MDD_CMD_452, PSA_ADAS_BUS, 6, 20U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // cruise state
{.msg = {{PSA_HS2_DYN_ABR_38D, PSA_MAIN_BUS, 8, 25U, .max_counter = 15U, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // speed
{.msg = {{PSA_STEERING_ALT, PSA_MAIN_BUS, 7, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // steering angle
{.msg = {{PSA_STEERING, PSA_MAIN_BUS, 7, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // driver torque
{.msg = {{PSA_DYN_CMM, PSA_MAIN_BUS, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // gas pedal
{.msg = {{PSA_DAT_BSI, PSA_CAM_BUS, 8, 20U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, // brake
};
return BUILD_SAFETY_CFG(psa_rx_checks, PSA_TX_MSGS);
}
const safety_hooks psa_hooks = {
.init = psa_init,
.rx = psa_rx_hook,
.tx = psa_tx_hook,
.get_counter = psa_get_counter,
.get_checksum = psa_get_checksum,
.compute_checksum = psa_compute_checksum,
};

View File

@@ -24,6 +24,7 @@
#include "opendbc/safety/modes/volkswagen_pq.h"
#include "opendbc/safety/modes/elm327.h"
#include "opendbc/safety/modes/body.h"
#include "opendbc/safety/modes/psa.h"
// CAN-FD only safety modes
#ifdef CANFD
@@ -415,6 +416,7 @@ int set_safety_hooks(uint16_t mode, uint16_t param) {
{SAFETY_HYUNDAI_CANFD, &hyundai_canfd_hooks},
#endif
#ifdef ALLOW_DEBUG
{SAFETY_PSA, &psa_hooks},
{SAFETY_SUBARU_PREGLOBAL, &subaru_preglobal_hooks},
{SAFETY_VOLKSWAGEN_PQ, &volkswagen_pq_hooks},
{SAFETY_ALLOUTPUT, &alloutput_hooks},

View File

@@ -30,6 +30,7 @@
#define SAFETY_FAW 26U
#define SAFETY_BODY 27U
#define SAFETY_HYUNDAI_CANFD 28U
#define SAFETY_PSA 31U
#define SAFETY_RIVIAN 33U
#define SAFETY_VOLKSWAGEN_MEB 34U
@@ -341,3 +342,4 @@ extern const safety_hooks toyota_hooks;
extern const safety_hooks volkswagen_mqb_hooks;
extern const safety_hooks volkswagen_pq_hooks;
extern const safety_hooks rivian_hooks;
extern const safety_hooks psa_hooks;

View File

@@ -4,6 +4,7 @@ set -e
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
cd $DIR
source $DIR/../../../setup.sh
$DIR/install_mull.sh
GIT_REF="${GIT_REF:-origin/master}"
@@ -11,6 +12,6 @@ GIT_ROOT=$(git rev-parse --show-toplevel)
MULL_OPS="mutators: [cxx_increment, cxx_decrement, cxx_comparison, cxx_boundary, cxx_bitwise_assignment, cxx_bitwise, cxx_arithmetic_assignment, cxx_arithmetic, cxx_remove_negation]"
echo -e "$MULL_OPS" > $GIT_ROOT/mull.yml
scons --mutation -j$(nproc) -D
echo -e "timeout: 100000\ngitDiffRef: $GIT_REF\ngitProjectRoot: $GIT_ROOT" >> $GIT_ROOT/mull.yml
echo -e "timeout: 1000000\ngitDiffRef: $GIT_REF\ngitProjectRoot: $GIT_ROOT" >> $GIT_ROOT/mull.yml
mull-runner-17 --ld-search-path /lib/x86_64-linux-gnu/ ./libsafety/libsafety.so -test-program=pytest -- -n8 --ignore-glob=misra/*

View File

@@ -66,8 +66,8 @@ def replay_drive(msgs, safety_mode, param, alternative_experience, param_sp):
if msg.which() == 'sendcan':
for canmsg in msg.sendcan:
msg = package_can_msg(canmsg)
sent = safety.safety_tx_hook(msg)
_msg = package_can_msg(canmsg)
sent = safety.safety_tx_hook(_msg)
# mismatched
if safety.get_controls_allowed() and not safety.get_controls_allowed_lat():
@@ -112,8 +112,8 @@ def replay_drive(msgs, safety_mode, param, alternative_experience, param_sp):
# ignore msgs we sent
for canmsg in filter(lambda m: m.src < 128, msg.can):
safety.safety_fwd_hook(canmsg.src, canmsg.address)
msg = package_can_msg(canmsg)
recv = safety.safety_rx_hook(msg)
_msg = package_can_msg(canmsg)
recv = safety.safety_rx_hook(_msg)
if not recv:
rx_invalid += 1
invalid_addrs.add(canmsg.address)

View File

@@ -0,0 +1,90 @@
#!/usr/bin/env python3
import unittest
from opendbc.car.structs import CarParams
from opendbc.safety.tests.libsafety import libsafety_py
import opendbc.safety.tests.common as common
from opendbc.safety.tests.common import CANPackerPanda
LANE_KEEP_ASSIST = 0x3F2
class TestPsaSafetyBase(common.PandaCarSafetyTest, common.AngleSteeringSafetyTest):
RELAY_MALFUNCTION_ADDRS = {0: (LANE_KEEP_ASSIST,)}
FWD_BLACKLISTED_ADDRS = {2: [LANE_KEEP_ASSIST]}
TX_MSGS = [[1010, 0]]
MAIN_BUS = 0
ADAS_BUS = 1
CAM_BUS = 2
STEER_ANGLE_MAX = 390
DEG_TO_CAN = 10
ANGLE_RATE_BP = [0., 5., 25.]
ANGLE_RATE_UP = [2.5, 1.5, .2]
ANGLE_RATE_DOWN = [5., 2., .3]
def setUp(self):
self.packer = CANPackerPanda("psa_aee2010_r3")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.psa, 0)
self.safety.init_tests()
def _angle_cmd_msg(self, angle: float, enabled: bool):
values = {"SET_ANGLE": angle, "TORQUE_FACTOR": 100 if enabled else 0}
return self.packer.make_can_msg_panda("LANE_KEEP_ASSIST", self.MAIN_BUS, values)
def _angle_meas_msg(self, angle: float):
values = {"ANGLE": angle}
return self.packer.make_can_msg_panda("STEERING_ALT", self.MAIN_BUS, values)
def _pcm_status_msg(self, enable):
values = {"RVV_ACC_ACTIVATION_REQ": enable}
return self.packer.make_can_msg_panda("HS2_DAT_MDD_CMD_452", self.ADAS_BUS, values)
def _speed_msg(self, speed):
values = {"VITESSE_VEHICULE_ROUES": speed * 3.6}
return self.packer.make_can_msg_panda("HS2_DYN_ABR_38D", self.MAIN_BUS, values)
def _user_brake_msg(self, brake):
values = {"P013_MainBrake": brake}
return self.packer.make_can_msg_panda("Dat_BSI", self.CAM_BUS, values)
def _user_gas_msg(self, gas):
values = {"P002_Com_rAPP": int(gas * 100)}
return self.packer.make_can_msg_panda("Dyn_CMM", self.MAIN_BUS, values)
def test_rx_hook(self):
# speed
for _ in range(10):
self.assertTrue(self._rx(self._speed_msg(0)))
msg = self._speed_msg(0)
# invalidate checksum
msg[0].data[5] = 0x00
self.assertFalse(self._rx(msg))
# cruise
for _ in range(10):
self.assertTrue(self._rx(self._pcm_status_msg(0)))
msg = self._pcm_status_msg(0)
# invalidate checksum
msg[0].data[5] = 0x00
self.assertFalse(self._rx(msg))
msg = self._pcm_status_msg(0)
# write to unused payload byte
msg[0].data[6] = 0xAB
self.assertTrue(self._rx(msg))
class TestPsaStockSafety(TestPsaSafetyBase):
def setUp(self):
self.packer = CANPackerPanda("psa_aee2010_r3")
self.safety = libsafety_py.libsafety
self.safety.set_safety_hooks(CarParams.SafetyModel.psa, 0)
self.safety.init_tests()
if __name__ == "__main__":
unittest.main()

View File

@@ -21,6 +21,16 @@
],
"package": "All"
},
"Acura MDX 2025": {
"platform": "ACURA_MDX_4G_MMR",
"make": "Acura",
"brand": "honda",
"model": "MDX",
"year": [
"2025"
],
"package": "All except Type S"
},
"Acura RDX 2016-18": {
"platform": "ACURA_RDX",
"make": "Acura",
@@ -842,13 +852,15 @@
],
"package": "All"
},
"Honda Accord 2023": {
"Honda Accord 2023-25": {
"platform": "HONDA_ACCORD_11G",
"make": "Honda",
"brand": "honda",
"model": "Accord",
"year": [
"2023"
"2023",
"2024",
"2025"
],
"package": "All"
},
@@ -866,6 +878,28 @@
],
"package": "All"
},
"Honda Accord Hybrid 2023-25": {
"platform": "HONDA_ACCORD_11G",
"make": "Honda",
"brand": "honda",
"model": "Accord Hybrid",
"year": [
"2023",
"2024",
"2025"
],
"package": "All"
},
"Honda City (Brazil only) 2023": {
"platform": "HONDA_CITY_7G",
"make": "Honda",
"brand": "honda",
"model": "City (Brazil only)",
"year": [
"2023"
],
"package": "All"
},
"Honda Civic 2016-18": {
"platform": "HONDA_CIVIC",
"make": "Honda",
@@ -902,19 +936,28 @@
],
"package": "All"
},
"Honda Civic Hatchback 2017-21": {
"Honda Civic Hatchback 2017-18": {
"platform": "HONDA_CIVIC_BOSCH",
"make": "Honda",
"brand": "honda",
"model": "Civic Hatchback",
"year": [
"2017",
"2018",
"2018"
],
"package": "Honda Sensing"
},
"Honda Civic Hatchback 2019-21": {
"platform": "HONDA_CIVIC_BOSCH",
"make": "Honda",
"brand": "honda",
"model": "Civic Hatchback",
"year": [
"2019",
"2020",
"2021"
],
"package": "Honda Sensing"
"package": "All"
},
"Honda Civic Hatchback 2022-24": {
"platform": "HONDA_CIVIC_2022",
@@ -997,6 +1040,18 @@
],
"package": "Honda Sensing"
},
"Honda CR-V 2023-25": {
"platform": "HONDA_CRV_6G",
"make": "Honda",
"brand": "honda",
"model": "CR-V",
"year": [
"2023",
"2024",
"2025"
],
"package": "All"
},
"Honda CR-V Hybrid 2017-22": {
"platform": "HONDA_CRV_HYBRID",
"make": "Honda",
@@ -1012,6 +1067,18 @@
],
"package": "Honda Sensing"
},
"Honda CR-V Hybrid 2023-25": {
"platform": "HONDA_CRV_6G",
"make": "Honda",
"brand": "honda",
"model": "CR-V Hybrid",
"year": [
"2023",
"2024",
"2025"
],
"package": "All"
},
"Honda e 2020": {
"platform": "HONDA_E",
"make": "Honda",
@@ -1136,6 +1203,18 @@
],
"package": "Honda Sensing"
},
"Honda Pilot 2023-25": {
"platform": "HONDA_PILOT_4G",
"make": "Honda",
"brand": "honda",
"model": "Pilot",
"year": [
"2023",
"2024",
"2025"
],
"package": "All"
},
"Honda Ridgeline 2017-25": {
"platform": "HONDA_RIDGELINE",
"make": "Honda",
@@ -1408,13 +1487,14 @@
],
"package": "Smart Cruise Control (SCC)"
},
"Hyundai Kona 2022": {
"Hyundai Kona 2022-23": {
"platform": "HYUNDAI_KONA_2022",
"make": "Hyundai",
"brand": "hyundai",
"model": "Kona",
"year": [
"2022"
"2022",
"2023"
],
"package": "Smart Cruise Control (SCC)"
},
@@ -3505,12 +3585,13 @@
],
"package": "All"
},
"Toyota Yaris (Non-US only) 2023": {
"Toyota Yaris (Non-US only) 2020, 2023": {
"platform": "TOYOTA_YARIS",
"make": "Toyota",
"brand": "toyota",
"model": "Yaris (Non-US only)",
"year": [
"2020",
"2023"
],
"package": "All"

View File

@@ -11,6 +11,27 @@ from opendbc.car.honda.values import CAR
Ecu = CarParams.Ecu
FW_VERSIONS_EXT = {
CAR.HONDA_ACCORD: {
(Ecu.eps, 0x18da30f1, None): [
b'39990-TVA,A150\x00\x00',
],
},
CAR.HONDA_CIVIC: {
(Ecu.eps, 0x18da30f1, None): [
b'39990-TBA,A030\x00\x00',
],
},
CAR.HONDA_CIVIC_BOSCH: {
(Ecu.eps, 0x18da30f1, None): [
b'39990-TGG,A020\x00\x00',
b'39990-TGG,A120\x00\x00',
],
},
CAR.HONDA_CRV_5G: {
(Ecu.eps, 0x18da30f1, None): [
b'39990-TLA,A040\x00\x00',
],
},
CAR.HONDA_CLARITY: {
(Ecu.shiftByWire, 0x18da0bf1, None): [
b'54008-TRW-A910\x00\x00',

View File

@@ -5,11 +5,6 @@ Ecu = CarParams.Ecu
FW_VERSIONS_EXT = {
CAR.KIA_NIRO_EV: {
(Ecu.eps, 0x7d4, None): [
b'\xf1\x00DE MDPS C 1.00 1.05 56310Q4200\x00 4DEEC105',
],
},
CAR.KIA_CEED_PHEV_2022_NON_SCC: {
(Ecu.eps, 0x7D4, None): [
b'\xf1\x00CD MDPS C 1.00 1.01 56310-XX000 4CPHC101',