forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Release Commit @ f2a861c
This commit is contained in:
IQ.Lvbs CI [bot]
35
artifacts/package_sources/iqdbc/.gitignore
vendored
Normal file
35
artifacts/package_sources/iqdbc/.gitignore
vendored
Normal file
@@ -0,0 +1,35 @@
|
||||
.cache/
|
||||
/build/
|
||||
*.pyc
|
||||
*.os
|
||||
*.o
|
||||
*.so
|
||||
*.a
|
||||
*.tmp
|
||||
*.dylib
|
||||
.*.swp
|
||||
.DS_Store
|
||||
.sconsign.dblite
|
||||
.hypothesis
|
||||
*.egg-info/
|
||||
*.html
|
||||
*.gcda
|
||||
*.gcno
|
||||
*.dump
|
||||
*.gcov
|
||||
/dist/
|
||||
.vscode/
|
||||
__pycache__/
|
||||
mull.yml
|
||||
*.profraw
|
||||
|
||||
iqdbc/can/build/
|
||||
iqdbc/can/obj/
|
||||
iqdbc/dbc/*_generated.dbc
|
||||
|
||||
cppcheck-addon-ctu-file-list
|
||||
iqdbc/safety/tests/coverage-out
|
||||
|
||||
compile_commands.json
|
||||
.mull/
|
||||
/iqdbc/car/volkswagen/tests/test_mqb_iq_alc_state.py
|
||||
106
artifacts/package_sources/iqdbc/LICENSE
Normal file
106
artifacts/package_sources/iqdbc/LICENSE
Normal file
@@ -0,0 +1,106 @@
|
||||
IQ.Lvbs License v0.1a
|
||||
|
||||
Copyright (c) 2026 IQ.Lvbs LLC, a part of Project Teal Lvbs Inc. All Rights Reserved.
|
||||
|
||||
DEFINITIONS
|
||||
|
||||
"Software" refers to IQ.Pilot, konn3kt, and all associated source code,
|
||||
documentation, and assets owned by the Copyright Holder.
|
||||
|
||||
"Open Components" refers to portions of the Software explicitly marked as
|
||||
open source.
|
||||
|
||||
"Proprietary Components" refers to all portions of the Software not made
|
||||
available to the public in source form.
|
||||
|
||||
"Copyright Holder" refers to IQ.Lvbs LLC, a part of Project Teal Lvbs Inc.
|
||||
|
||||
GRANT OF LICENSE
|
||||
|
||||
Subject to the terms of this license, you are granted a limited,
|
||||
non-exclusive, revocable license to:
|
||||
|
||||
1. View, study, and learn from the Open Components
|
||||
2. Modify the Open Components for personal, internal, or open-source public use
|
||||
3. Run the Software for personal, non-commercial purposes
|
||||
|
||||
RESTRICTIONS
|
||||
|
||||
You may NOT:
|
||||
|
||||
1. Claim ownership of any part of the Software, excluding your own
|
||||
modifications that do not incorporate Proprietary Components.
|
||||
|
||||
2. Reverse engineer, decompile, disassemble, or in any way attempt to
|
||||
circumvent the obfuscation of the Proprietary Components.
|
||||
|
||||
3. Use the Software or any derivative for commercial purposes without
|
||||
explicit written permission from the Copyright Holder.
|
||||
|
||||
4. Remove or alter any copyright notices or this license.
|
||||
|
||||
5. Sublicense, sell, or transfer rights to the Software.
|
||||
|
||||
6. Use the Software and/or its source code to compete with or create a
|
||||
substantially similar product.
|
||||
|
||||
7. Use the Software in closed source software not licensed by IQ.Lvbs LLC.
|
||||
|
||||
CONSEQUENCES OF VIOLATION
|
||||
|
||||
In the event any Restriction is violated, any product created using inspiration from, or source code from, IQ.Pilot or Konn3kt shall be subject to a licensing fee determined solely by the Copyright Holder. Additionally, the violating party hereby grants IQ.Lvbs LLC an exclusive, irrevocable, worldwide, royalty-free license to use any and all assets from the infringing product on IQ.Lvbs webpages, advertising materials, and in any other manner IQ.Lvbs sees fit.
|
||||
|
||||
OWNERSHIP
|
||||
|
||||
All rights, title, and interest in the Software remain exclusively with the
|
||||
Copyright Holder. Any modifications, improvements, or derivative works you
|
||||
create based on the Software are owned by the Copyright Holder. By
|
||||
contributing modifications, you irrevocably assign all rights to the
|
||||
Copyright Holder.
|
||||
|
||||
PROPRIETARY COMPONENTS
|
||||
|
||||
The Proprietary Components are provided in binary or obfuscated form only.
|
||||
Reverse engineering, decompilation, or disassembly of Proprietary Components
|
||||
is strictly prohibited. Violation of this provision entitles IQ.Lvbs LLC to
|
||||
pursue all available legal remedies to protect its intellectual property and
|
||||
trade secrets.
|
||||
|
||||
NO WARRANTY
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT
|
||||
HOLDER DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING BUT NOT
|
||||
LIMITED TO MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT.
|
||||
|
||||
LIMITATION OF LIABILITY
|
||||
|
||||
IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY CLAIM, DAMAGES, OR
|
||||
OTHER LIABILITY ARISING FROM THE USE OF THE SOFTWARE. THE USER ACCEPTS FULL
|
||||
RESPONSIBILITY FOR ANY AND ALL LIABILITIES WHEN USING IQ.LVBS SOFTWARE.
|
||||
|
||||
TERMINATION
|
||||
|
||||
This license terminates automatically if you violate any of its terms. Upon
|
||||
termination, you must destroy all copies of the Software in your possession.
|
||||
|
||||
The Copyright Holder reserves the right to revoke this license at any time
|
||||
for any reason.
|
||||
|
||||
GOVERNING LAW
|
||||
|
||||
This license shall be governed by the laws of the State of Illinois, United
|
||||
States of America. Any disputes arising under this license shall be subject
|
||||
to the exclusive jurisdiction of the courts located in Henry County, Illinois.
|
||||
|
||||
---
|
||||
|
||||
For commercial licensing inquiries, contact: support@iqlvbs.com
|
||||
|
||||
Copyright (c) 2020, Comma.ai, Inc.
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
106
artifacts/package_sources/iqdbc/LICENSE.md
Normal file
106
artifacts/package_sources/iqdbc/LICENSE.md
Normal file
@@ -0,0 +1,106 @@
|
||||
IQ.Lvbs License v0.1a
|
||||
|
||||
Copyright (c) 2026 IQ.Lvbs LLC, a part of Project Teal Lvbs Inc. All Rights Reserved.
|
||||
|
||||
DEFINITIONS
|
||||
|
||||
"Software" refers to IQ.Pilot, konn3kt, and all associated source code,
|
||||
documentation, and assets owned by the Copyright Holder.
|
||||
|
||||
"Open Components" refers to portions of the Software explicitly marked as
|
||||
open source.
|
||||
|
||||
"Proprietary Components" refers to all portions of the Software not made
|
||||
available to the public in source form.
|
||||
|
||||
"Copyright Holder" refers to IQ.Lvbs LLC, a part of Project Teal Lvbs Inc.
|
||||
|
||||
GRANT OF LICENSE
|
||||
|
||||
Subject to the terms of this license, you are granted a limited,
|
||||
non-exclusive, revocable license to:
|
||||
|
||||
1. View, study, and learn from the Open Components
|
||||
2. Modify the Open Components for personal, internal, or open-source public use
|
||||
3. Run the Software for personal, non-commercial purposes
|
||||
|
||||
RESTRICTIONS
|
||||
|
||||
You may NOT:
|
||||
|
||||
1. Claim ownership of any part of the Software, excluding your own
|
||||
modifications that do not incorporate Proprietary Components.
|
||||
|
||||
2. Reverse engineer, decompile, disassemble, or in any way attempt to
|
||||
circumvent the obfuscation of the Proprietary Components.
|
||||
|
||||
3. Use the Software or any derivative for commercial purposes without
|
||||
explicit written permission from the Copyright Holder.
|
||||
|
||||
4. Remove or alter any copyright notices or this license.
|
||||
|
||||
5. Sublicense, sell, or transfer rights to the Software.
|
||||
|
||||
6. Use the Software and/or its source code to compete with or create a
|
||||
substantially similar product.
|
||||
|
||||
7. Use the Software in closed source software not licensed by IQ.Lvbs LLC.
|
||||
|
||||
CONSEQUENCES OF VIOLATION
|
||||
|
||||
In the event any Restriction is violated, any product created using inspiration from, or source code from, IQ.Pilot or Konn3kt shall be subject to a licensing fee determined solely by the Copyright Holder. Additionally, the violating party hereby grants IQ.Lvbs LLC an exclusive, irrevocable, worldwide, royalty-free license to use any and all assets from the infringing product on IQ.Lvbs webpages, advertising materials, and in any other manner IQ.Lvbs sees fit.
|
||||
|
||||
OWNERSHIP
|
||||
|
||||
All rights, title, and interest in the Software remain exclusively with the
|
||||
Copyright Holder. Any modifications, improvements, or derivative works you
|
||||
create based on the Software are owned by the Copyright Holder. By
|
||||
contributing modifications, you irrevocably assign all rights to the
|
||||
Copyright Holder.
|
||||
|
||||
PROPRIETARY COMPONENTS
|
||||
|
||||
The Proprietary Components are provided in binary or obfuscated form only.
|
||||
Reverse engineering, decompilation, or disassembly of Proprietary Components
|
||||
is strictly prohibited. Violation of this provision entitles IQ.Lvbs LLC to
|
||||
pursue all available legal remedies to protect its intellectual property and
|
||||
trade secrets.
|
||||
|
||||
NO WARRANTY
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND. THE COPYRIGHT
|
||||
HOLDER DISCLAIMS ALL WARRANTIES, EXPRESS OR IMPLIED, INCLUDING BUT NOT
|
||||
LIMITED TO MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, AND
|
||||
NON-INFRINGEMENT.
|
||||
|
||||
LIMITATION OF LIABILITY
|
||||
|
||||
IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY CLAIM, DAMAGES, OR
|
||||
OTHER LIABILITY ARISING FROM THE USE OF THE SOFTWARE. THE USER ACCEPTS FULL
|
||||
RESPONSIBILITY FOR ANY AND ALL LIABILITIES WHEN USING IQ.LVBS SOFTWARE.
|
||||
|
||||
TERMINATION
|
||||
|
||||
This license terminates automatically if you violate any of its terms. Upon
|
||||
termination, you must destroy all copies of the Software in your possession.
|
||||
|
||||
The Copyright Holder reserves the right to revoke this license at any time
|
||||
for any reason.
|
||||
|
||||
GOVERNING LAW
|
||||
|
||||
This license shall be governed by the laws of the State of Illinois, United
|
||||
States of America. Any disputes arising under this license shall be subject
|
||||
to the exclusive jurisdiction of the courts located in Henry County, Illinois.
|
||||
|
||||
---
|
||||
|
||||
For commercial licensing inquiries, contact: support@iqlvbs.com
|
||||
|
||||
Copyright (c) 2020, Comma.ai, Inc.
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
2
artifacts/package_sources/iqdbc/MANIFEST.in
Normal file
2
artifacts/package_sources/iqdbc/MANIFEST.in
Normal file
@@ -0,0 +1,2 @@
|
||||
graft iqdbc
|
||||
global-exclude __pycache__ *.pyc *.o *.os *.d *.so
|
||||
5
artifacts/package_sources/iqdbc/SConscript
Normal file
5
artifacts/package_sources/iqdbc/SConscript
Normal file
@@ -0,0 +1,5 @@
|
||||
SConscript(['iqdbc/dbc/SConscript'])
|
||||
|
||||
# test files
|
||||
if GetOption('extras'):
|
||||
SConscript('iqdbc/safety/tests/libsafety/SConscript')
|
||||
12
artifacts/package_sources/iqdbc/SConstruct
Normal file
12
artifacts/package_sources/iqdbc/SConstruct
Normal file
@@ -0,0 +1,12 @@
|
||||
import os
|
||||
|
||||
env = Environment(ENV=os.environ)
|
||||
Export('env')
|
||||
|
||||
AddOption('--minimal',
|
||||
action='store_false',
|
||||
dest='extras',
|
||||
default=True,
|
||||
help='the minimum build. no tests, tools, etc.')
|
||||
|
||||
SConscript(['SConscript'])
|
||||
107
artifacts/package_sources/iqdbc/conftest.py
Normal file
107
artifacts/package_sources/iqdbc/conftest.py
Normal file
@@ -0,0 +1,107 @@
|
||||
# pytest attempts to execute shell scripts while collecting
|
||||
collect_ignore_glob = [
|
||||
"iqdbc/safety/tests/misra/*.sh",
|
||||
"iqdbc/safety/tests/misra/cppcheck/",
|
||||
]
|
||||
|
||||
# --- host test harness -------------------------------------------------------
|
||||
# tesla/volkswagen carstate+carcontroller import the closed-source ALC / odometer
|
||||
# runtime via import_verified_module(), which gates on a rootfs integrity manifest
|
||||
# that only exists on-device. Off-device (host CI / dev), install minimal stubs so
|
||||
# the whole car interface set is importable and test_car_interfaces can collect and
|
||||
# run. On-device the manifest is present and these stubs are never installed, so the
|
||||
# real verified modules are used.
|
||||
import os as _os
|
||||
|
||||
if not _os.path.exists("/usr/libexec/iqpilot/runtime_integrity.json"):
|
||||
import sys as _sys
|
||||
import types as _types
|
||||
|
||||
def _stub_module(name, attrs):
|
||||
mod = _types.ModuleType(name)
|
||||
for key, value in attrs.items():
|
||||
setattr(mod, key, value)
|
||||
_sys.modules[name] = mod
|
||||
|
||||
_noop = lambda *args, **kwargs: None # noqa: E731
|
||||
|
||||
class _VehicleOdometerStoreStub:
|
||||
def __init__(self, *args, **kwargs):
|
||||
pass
|
||||
|
||||
def record(self, km):
|
||||
return km
|
||||
|
||||
_stub_module("iqpilot_private.konn3kt.iqlvbs.vehicle_state",
|
||||
{"VehicleOdometerStore": _VehicleOdometerStoreStub})
|
||||
_stub_module("iqpilot_private.konn3kt.iqlvbs.alc", {
|
||||
"angle_lateral_control_enabled": lambda *a, **k: False,
|
||||
"update_vw_alc": _noop,
|
||||
"append_private_apd": _noop,
|
||||
"update_mqb_carstate_alc_state": _noop,
|
||||
"update_mlb_carstate_alc_state": _noop,
|
||||
"update_pq_carstate_alc_state": _noop,
|
||||
"update_turn_signals": _noop,
|
||||
"create_vehicle_odometer_store": lambda *a, **k: _VehicleOdometerStoreStub(),
|
||||
"vw_driver_override_threshold_cnm": lambda cs, platform, default: default,
|
||||
})
|
||||
_stub_module("iqpilot_private.konn3kt.iqlvbs.iqlvbs_commander", {
|
||||
"update_turn_signals": _noop,
|
||||
})
|
||||
_stub_module("iqpilot_private.konn3kt.hephaestus.vw_pq_flasher", {})
|
||||
|
||||
try:
|
||||
from iqpilot.system.proprietary_runtime import _verified_import as _vi
|
||||
_real_import_verified_module = _vi.import_verified_module
|
||||
except Exception:
|
||||
_vi = _types.ModuleType("iqpilot.system.proprietary_runtime._verified_import")
|
||||
_sys.modules["iqpilot.system.proprietary_runtime._verified_import"] = _vi
|
||||
_real_import_verified_module = None
|
||||
|
||||
def _import_verified_module(bundle, module):
|
||||
stub = _sys.modules.get(module)
|
||||
if stub is not None:
|
||||
return stub
|
||||
if _real_import_verified_module is None:
|
||||
raise ImportError(module)
|
||||
return _real_import_verified_module(bundle, module)
|
||||
|
||||
_vi.import_verified_module = _import_verified_module
|
||||
# --- end host test harness ---------------------------------------------------
|
||||
|
||||
_ABSTRACT_BASES = frozenset(("CarSafetyTest", "AolSafetyTestBase", "SafetyTest", "SafetyTestBase"))
|
||||
|
||||
def _method_from_base(cls, method_name):
|
||||
for klass in cls.__mro__:
|
||||
if method_name in klass.__dict__:
|
||||
return klass.__name__ in _ABSTRACT_BASES
|
||||
return True
|
||||
|
||||
_NEEDS_LKAS = frozenset((
|
||||
"test_enable_control_allowed_with_aol_button",
|
||||
"test_enable_control_allowed_with_aol_button_and_disable_with_main_cruise",
|
||||
"test_engage_with_brake_pressed_0_aol_button",
|
||||
))
|
||||
_NEEDS_ACC_STATE = frozenset((
|
||||
"test_enable_control_allowed_with_manual_acc_main_on_state",
|
||||
"test_enable_control_allowed_with_aol_button_and_disable_with_main_cruise",
|
||||
"test_engage_with_brake_pressed_1_acc_main_on",
|
||||
))
|
||||
|
||||
def pytest_collection_modifyitems(items):
|
||||
keep = []
|
||||
for item in items:
|
||||
cls = item.cls
|
||||
if cls is None:
|
||||
keep.append(item)
|
||||
continue
|
||||
if cls.__name__.endswith("Base"):
|
||||
continue
|
||||
if "regen" in item.name and _method_from_base(cls, "_user_regen_msg"):
|
||||
continue
|
||||
if item.name in _NEEDS_LKAS and _method_from_base(cls, "_lkas_button_msg"):
|
||||
continue
|
||||
if item.name in _NEEDS_ACC_STATE and _method_from_base(cls, "_acc_state_msg"):
|
||||
continue
|
||||
keep.append(item)
|
||||
items[:] = keep
|
||||
55
artifacts/package_sources/iqdbc/credits/CREDITS.MD
Normal file
55
artifacts/package_sources/iqdbc/credits/CREDITS.MD
Normal file
@@ -0,0 +1,55 @@
|
||||
## IQ.Pilot Car-Port Credits
|
||||
|
||||
This file is intended to track per-platform and per-tuning attribution inside the car-port-side included inside of IQ.Pilot.
|
||||
|
||||
It does not replace the repository-wide license files for IQ.Pilot itself or any other components not listed here.
|
||||
|
||||
## Credits:
|
||||
|
||||
### Hyundai / Kia / Genesis (HKG) - carrotpilot
|
||||
|
||||
#### Contributors
|
||||
|
||||
Full credit for the Hyundai / Kia / Genesis port and its HKG-specific tuning belongs to:
|
||||
|
||||
- carrotpilot
|
||||
- ajouatom
|
||||
|
||||
#### Attribution
|
||||
|
||||
This repository carries the carrotpilot HKG implementation from `ajouatom/openpilot`, imported from commit `b54d44efd514bc61f86f08b01c23a6c190885c07`. IQ.Lvbs claims no copyright, credit, or authorship over that implementation.
|
||||
|
||||
#### Upstream License Notice
|
||||
|
||||
The HKG work attributed above is acknowledged here under LICENSE notice:
|
||||
|
||||
```text
|
||||
Copyright (c) 2018, Comma.ai, Inc.
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
||||
```
|
||||
|
||||
#### Scope
|
||||
|
||||
This licensing is limited to the Hyundai/Kia/Genesis car, DBC, and safety implementations and the supporting schema and compatibility changes required by that port.
|
||||
|
||||
### Honda
|
||||
|
||||
#### Contributors
|
||||
|
||||
Full credit for the Honda-specific tuning lineage carried in this tree belongs to:
|
||||
|
||||
- MVL-Boston on GitHub
|
||||
- mvl3c on Discord
|
||||
|
||||
#### Attribution
|
||||
|
||||
This repository carries Honda tuning work derived from MVL's tuning effort's, that originated from, and/or is verbatim their work, and are accredited as such, IQ.Lvbs claims no copyright, credit, or authorship of the components listed below.
|
||||
|
||||
#### Scope
|
||||
|
||||
This licensing is limited to `iqdbc_repo/iqdbc/car/honda` and `iqdbc_repo/iqdbc/iqpilot/car/honda`.
|
||||
6
artifacts/package_sources/iqdbc/iqdbc/__init__.py
Normal file
6
artifacts/package_sources/iqdbc/iqdbc/__init__.py
Normal file
@@ -0,0 +1,6 @@
|
||||
import os
|
||||
|
||||
DBC_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), 'dbc')
|
||||
|
||||
# -I include path for e.g. "#include <iqdbc/safety/safety.h>"
|
||||
INCLUDE_PATH = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../"))
|
||||
8
artifacts/package_sources/iqdbc/iqdbc/can/__init__.py
Normal file
8
artifacts/package_sources/iqdbc/iqdbc/can/__init__.py
Normal file
@@ -0,0 +1,8 @@
|
||||
from iqdbc.can.packer import CANPacker
|
||||
from iqdbc.can.parser import CANParser, CANDefine
|
||||
|
||||
__all__ = [
|
||||
"CANDefine",
|
||||
"CANParser",
|
||||
"CANPacker",
|
||||
]
|
||||
221
artifacts/package_sources/iqdbc/iqdbc/can/dbc.py
Normal file
221
artifacts/package_sources/iqdbc/iqdbc/can/dbc.py
Normal file
@@ -0,0 +1,221 @@
|
||||
import re
|
||||
import os
|
||||
from collections.abc import Callable
|
||||
from dataclasses import dataclass
|
||||
from functools import cache
|
||||
|
||||
from iqdbc import DBC_PATH
|
||||
|
||||
# TODO: these should just be passed in along with the DBC file
|
||||
from iqdbc.car.honda.hondacan import honda_checksum
|
||||
from iqdbc.car.toyota.toyotacan import toyota_checksum
|
||||
from iqdbc.car.subaru.subarucan import subaru_checksum
|
||||
from iqdbc.car.chrysler.chryslercan import chrysler_checksum, fca_giorgio_checksum
|
||||
from iqdbc.car.hyundai.hyundaicanfd import hkg_can_fd_checksum
|
||||
from iqdbc.car.volkswagen.mlbcan import volkswagen_mlb_checksum
|
||||
from iqdbc.car.volkswagen.mqbcan import volkswagen_mqb_meb_checksum, volkswagen_mqb_meb_gen2_checksum, xor_checksum
|
||||
from iqdbc.car.tesla.teslacan import tesla_checksum
|
||||
from iqdbc.car.body.bodycan import body_checksum
|
||||
from iqdbc.car.psa.psacan import psa_checksum
|
||||
from iqdbc.car.byd.bydcan import byd_checksum
|
||||
|
||||
|
||||
class SignalType:
|
||||
DEFAULT = 0
|
||||
COUNTER = 1
|
||||
HONDA_CHECKSUM = 2
|
||||
TOYOTA_CHECKSUM = 3
|
||||
BODY_CHECKSUM = 4
|
||||
VOLKSWAGEN_MQB_MEB_CHECKSUM = 5
|
||||
XOR_CHECKSUM = 6
|
||||
SUBARU_CHECKSUM = 7
|
||||
CHRYSLER_CHECKSUM = 8
|
||||
HKG_CAN_FD_CHECKSUM = 9
|
||||
FCA_GIORGIO_CHECKSUM = 10
|
||||
TESLA_CHECKSUM = 11
|
||||
PSA_CHECKSUM = 12
|
||||
VOLKSWAGEN_MLB_CHECKSUM = 13
|
||||
VOLKSWAGEN_MQB_MEB_GEN2_CHECKSUM = 14
|
||||
BYD_CHECKSUM = 15
|
||||
|
||||
|
||||
@dataclass
|
||||
class Signal:
|
||||
name: str
|
||||
start_bit: int
|
||||
msb: int
|
||||
lsb: int
|
||||
size: int
|
||||
is_signed: bool
|
||||
factor: float
|
||||
offset: float
|
||||
is_little_endian: bool
|
||||
type: int = SignalType.DEFAULT
|
||||
calc_checksum: 'Callable[[int, Signal, bytearray], int] | None' = None
|
||||
|
||||
|
||||
@dataclass
|
||||
class Msg:
|
||||
name: str
|
||||
address: int
|
||||
size: int
|
||||
sigs: dict[str, Signal]
|
||||
|
||||
|
||||
@dataclass
|
||||
class Val:
|
||||
name: str
|
||||
address: int
|
||||
def_val: str
|
||||
sigs: dict[str, Signal] | None = None
|
||||
|
||||
|
||||
BO_RE = re.compile(r"^BO_ (\w+) (\w+) *: (\w+) (\w+)")
|
||||
SG_RE = re.compile(r"^SG_ (\w+) : (\d+)\|(\d+)@(\d)([+-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[[0-9.+\-eE]+\|[0-9.+\-eE]+\] \".*\" .*")
|
||||
SGM_RE = re.compile(r"^SG_ (\w+) (\w+) *: (\d+)\|(\d+)@(\d)([+-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[[0-9.+\-eE]+\|[0-9.+\-eE]+\] \".*\" .*")
|
||||
VAL_RE = re.compile(r"^VAL_ (\w+) (\w+) (.*);")
|
||||
VAL_SPLIT_RE = re.compile(r'["]+')
|
||||
|
||||
|
||||
@cache
|
||||
class DBC:
|
||||
def __init__(self, name: str):
|
||||
dbc_path = name
|
||||
if not os.path.exists(dbc_path):
|
||||
dbc_path = os.path.join(DBC_PATH, name + ".dbc")
|
||||
|
||||
self._parse(dbc_path)
|
||||
|
||||
def _parse(self, path: str):
|
||||
self.name = os.path.basename(path).replace(".dbc", "")
|
||||
with open(path) as f:
|
||||
lines = f.readlines()
|
||||
|
||||
checksum_state = get_checksum_state(self.name)
|
||||
be_bits = [j + i * 8 for i in range(64) for j in range(7, -1, -1)]
|
||||
self.msgs: dict[int, Msg] = {}
|
||||
self.addr_to_msg: dict[int, Msg] = {}
|
||||
self.name_to_msg: dict[str, Msg] = {}
|
||||
self.vals: list[Val] = []
|
||||
address = 0
|
||||
signals_temp: dict[int, dict[str, Signal]] = {}
|
||||
for line_num, line in enumerate(lines, 1):
|
||||
line = line.strip()
|
||||
if line.startswith("BO_ "):
|
||||
m = BO_RE.match(line)
|
||||
if not m:
|
||||
continue
|
||||
address = int(m.group(1), 0)
|
||||
msg_name = m.group(2)
|
||||
size = int(m.group(3), 0)
|
||||
sigs = {}
|
||||
self.msgs[address] = Msg(msg_name, address, size, sigs)
|
||||
self.addr_to_msg[address] = self.msgs[address]
|
||||
self.name_to_msg[msg_name] = self.msgs[address]
|
||||
signals_temp[address] = sigs
|
||||
elif line.startswith("SG_ "):
|
||||
m = SG_RE.search(line)
|
||||
offset = 0
|
||||
if not m:
|
||||
m = SGM_RE.search(line)
|
||||
if not m:
|
||||
continue
|
||||
offset = 1
|
||||
sig_name = m.group(1)
|
||||
start_bit = int(m.group(2 + offset))
|
||||
size = int(m.group(3 + offset))
|
||||
is_little_endian = m.group(4 + offset) == "1"
|
||||
is_signed = m.group(5 + offset) == "-"
|
||||
factor = float(m.group(6 + offset))
|
||||
offset_val = float(m.group(7 + offset))
|
||||
|
||||
if is_little_endian:
|
||||
lsb = start_bit
|
||||
msb = start_bit + size - 1
|
||||
else:
|
||||
idx = be_bits.index(start_bit)
|
||||
lsb = be_bits[idx + size - 1]
|
||||
msb = start_bit
|
||||
|
||||
sig = Signal(sig_name, start_bit, msb, lsb, size, is_signed, factor, offset_val, is_little_endian)
|
||||
set_signal_type(sig, checksum_state, self.name, line_num)
|
||||
signals_temp[address][sig_name] = sig
|
||||
elif line.startswith("VAL_ "):
|
||||
m = VAL_RE.search(line)
|
||||
if not m:
|
||||
continue
|
||||
val_addr = int(m.group(1), 0)
|
||||
sgname = m.group(2)
|
||||
defs = m.group(3)
|
||||
words = [w.strip() for w in VAL_SPLIT_RE.split(defs) if w.strip()]
|
||||
words = [w.upper().replace(" ", "_") for w in words]
|
||||
val_def = " ".join(words).strip()
|
||||
self.vals.append(Val(sgname, val_addr, val_def))
|
||||
for addr, sigs in signals_temp.items():
|
||||
self.msgs[addr].sigs = sigs
|
||||
|
||||
|
||||
# ***** checksum functions *****
|
||||
|
||||
def tesla_setup_signal(sig: Signal, dbc_name: str, line_num: int) -> None:
|
||||
if sig.name.endswith("Counter"):
|
||||
sig.type = SignalType.COUNTER
|
||||
elif sig.name.endswith("Checksum"):
|
||||
sig.type = SignalType.TESLA_CHECKSUM
|
||||
sig.calc_checksum = tesla_checksum
|
||||
|
||||
|
||||
@dataclass
|
||||
class ChecksumState:
|
||||
checksum_size: int
|
||||
counter_size: int
|
||||
checksum_start_bit: int
|
||||
counter_start_bit: int
|
||||
little_endian: bool
|
||||
checksum_type: int
|
||||
calc_checksum: Callable[[int, Signal, bytearray], int] | None
|
||||
setup_signal: Callable[[Signal, str, int], None] | None = None
|
||||
|
||||
|
||||
def get_checksum_state(dbc_name: str) -> ChecksumState | None:
|
||||
if dbc_name.startswith(("honda_", "acura_")):
|
||||
return ChecksumState(4, 2, 3, 5, False, SignalType.HONDA_CHECKSUM, honda_checksum)
|
||||
elif dbc_name.startswith(("toyota_", "lexus_")):
|
||||
return ChecksumState(8, -1, 7, -1, False, SignalType.TOYOTA_CHECKSUM, toyota_checksum)
|
||||
elif dbc_name.startswith("hyundai_canfd_generated"):
|
||||
return ChecksumState(16, -1, 0, -1, True, SignalType.HKG_CAN_FD_CHECKSUM, hkg_can_fd_checksum)
|
||||
elif dbc_name.startswith("vw_meb_2024"):
|
||||
return ChecksumState(8, 4, 0, 0, True, SignalType.VOLKSWAGEN_MQB_MEB_GEN2_CHECKSUM, volkswagen_mqb_meb_gen2_checksum)
|
||||
elif dbc_name.startswith(("vw_mqb", "vw_mqbevo", "vw_meb")):
|
||||
return ChecksumState(8, 4, 0, 0, True, SignalType.VOLKSWAGEN_MQB_MEB_CHECKSUM, volkswagen_mqb_meb_checksum)
|
||||
elif dbc_name.startswith("vw_mlb"):
|
||||
return ChecksumState(8, 4, 0, 0, True, SignalType.VOLKSWAGEN_MLB_CHECKSUM, volkswagen_mlb_checksum)
|
||||
elif dbc_name.startswith("vw_pq"):
|
||||
return ChecksumState(8, 4, 0, -1, True, SignalType.XOR_CHECKSUM, xor_checksum)
|
||||
elif dbc_name.startswith("subaru_global_"):
|
||||
return ChecksumState(8, -1, 0, -1, True, SignalType.SUBARU_CHECKSUM, subaru_checksum)
|
||||
elif dbc_name.startswith("chrysler_"):
|
||||
return ChecksumState(8, -1, 7, -1, False, SignalType.CHRYSLER_CHECKSUM, chrysler_checksum)
|
||||
elif dbc_name.startswith("fca_giorgio"):
|
||||
return ChecksumState(8, -1, 7, -1, False, SignalType.FCA_GIORGIO_CHECKSUM, fca_giorgio_checksum)
|
||||
elif dbc_name.startswith("comma_body"):
|
||||
return ChecksumState(8, 4, 7, 3, False, SignalType.BODY_CHECKSUM, body_checksum)
|
||||
elif dbc_name.startswith(("tesla_model3_party", "tesla_model3_vehicle")):
|
||||
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)
|
||||
elif dbc_name.startswith("byd_"):
|
||||
return ChecksumState(8, 4, 56, 55, True, SignalType.BYD_CHECKSUM, byd_checksum)
|
||||
return None
|
||||
|
||||
|
||||
def set_signal_type(sig: Signal, chk: ChecksumState | None, dbc_name: str, line_num: int) -> None:
|
||||
sig.calc_checksum = None
|
||||
if chk:
|
||||
if chk.setup_signal:
|
||||
chk.setup_signal(sig, dbc_name, line_num)
|
||||
if sig.name == "CHECKSUM":
|
||||
sig.type = chk.checksum_type
|
||||
sig.calc_checksum = chk.calc_checksum
|
||||
elif sig.name == "COUNTER":
|
||||
sig.type = SignalType.COUNTER
|
||||
72
artifacts/package_sources/iqdbc/iqdbc/can/packer.py
Normal file
72
artifacts/package_sources/iqdbc/iqdbc/can/packer.py
Normal file
@@ -0,0 +1,72 @@
|
||||
import math
|
||||
|
||||
from iqdbc.car.carlog import carlog
|
||||
from iqdbc.can.dbc import DBC, Signal, SignalType
|
||||
|
||||
|
||||
class CANPacker:
|
||||
def __init__(self, dbc_name: str):
|
||||
self.dbc = DBC(dbc_name)
|
||||
self.counters: dict[int, int] = {}
|
||||
|
||||
def pack(self, address: int, values: dict[str, float]) -> bytearray:
|
||||
msg = self.dbc.addr_to_msg.get(address)
|
||||
if msg is None:
|
||||
carlog.error(f"msg not found for {address=}")
|
||||
return bytearray()
|
||||
dat = bytearray(msg.size)
|
||||
counter_set = False
|
||||
for name, value in values.items():
|
||||
sig = msg.sigs.get(name)
|
||||
if sig is None:
|
||||
carlog.error(f"unknown signal {name=} in {msg.name}")
|
||||
continue
|
||||
ival = int(math.floor((value - sig.offset) / sig.factor + 0.5))
|
||||
if ival < 0:
|
||||
ival = (1 << sig.size) + ival
|
||||
set_value(dat, sig, ival)
|
||||
if sig.type == SignalType.COUNTER or sig.name == "COUNTER":
|
||||
self.counters[address] = int(value)
|
||||
counter_set = True
|
||||
sig_counter = next((s for s in msg.sigs.values() if s.type == SignalType.COUNTER or s.name == "COUNTER"), None)
|
||||
if sig_counter and not counter_set:
|
||||
if address not in self.counters:
|
||||
self.counters[address] = 0
|
||||
set_value(dat, sig_counter, self.counters[address])
|
||||
self.counters[address] = (self.counters[address] + 1) % (1 << sig_counter.size)
|
||||
sig_checksum = next((s for s in msg.sigs.values() if s.type > SignalType.COUNTER), None)
|
||||
if sig_checksum and sig_checksum.calc_checksum:
|
||||
checksum = sig_checksum.calc_checksum(address, sig_checksum, dat)
|
||||
set_value(dat, sig_checksum, checksum)
|
||||
return dat
|
||||
|
||||
def make_can_msg(self, name_or_addr, bus: int, values: dict[str, float], rx_counter: int | None = None):
|
||||
if isinstance(name_or_addr, int):
|
||||
addr = name_or_addr
|
||||
else:
|
||||
msg = self.dbc.name_to_msg.get(name_or_addr)
|
||||
if msg is None:
|
||||
carlog.error(f"msg not found for {name_or_addr=}")
|
||||
return 0, b'', bus
|
||||
addr = msg.address
|
||||
pack_values = values if rx_counter is None else values | {"COUNTER": rx_counter}
|
||||
dat = self.pack(addr, pack_values)
|
||||
if len(dat) == 0:
|
||||
return 0, b'', bus
|
||||
return addr, bytes(dat), bus
|
||||
|
||||
|
||||
def set_value(msg: bytearray, sig: Signal, ival: int) -> None:
|
||||
i = sig.lsb // 8
|
||||
bits = sig.size
|
||||
if sig.size < 64:
|
||||
ival &= (1 << sig.size) - 1
|
||||
while 0 <= i < len(msg) and bits > 0:
|
||||
shift = sig.lsb % 8 if (sig.lsb // 8) == i else 0
|
||||
size = min(bits, 8 - shift)
|
||||
mask = ((1 << size) - 1) << shift
|
||||
msg[i] &= ~mask
|
||||
msg[i] |= (ival & ((1 << size) - 1)) << shift
|
||||
bits -= size
|
||||
ival >>= size
|
||||
i = i + 1 if sig.is_little_endian else i - 1
|
||||
285
artifacts/package_sources/iqdbc/iqdbc/can/parser.py
Normal file
285
artifacts/package_sources/iqdbc/iqdbc/can/parser.py
Normal file
@@ -0,0 +1,285 @@
|
||||
import math
|
||||
import numbers
|
||||
import time
|
||||
from collections import defaultdict, deque
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from iqdbc.car.carlog import carlog
|
||||
from iqdbc.can.dbc import DBC, Signal
|
||||
|
||||
|
||||
MAX_BAD_COUNTER = 5
|
||||
CAN_INVALID_CNT = 5
|
||||
|
||||
|
||||
def get_raw_value(dat: bytes | bytearray, sig: Signal) -> int:
|
||||
ret = 0
|
||||
i = sig.msb // 8
|
||||
bits = sig.size
|
||||
while 0 <= i < len(dat) and bits > 0:
|
||||
lsb = sig.lsb if (sig.lsb // 8) == i else i * 8
|
||||
msb = sig.msb if (sig.msb // 8) == i else (i + 1) * 8 - 1
|
||||
size = msb - lsb + 1
|
||||
d = (dat[i] >> (lsb - (i * 8))) & ((1 << size) - 1)
|
||||
ret |= d << (bits - size)
|
||||
bits -= size
|
||||
i = i - 1 if sig.is_little_endian else i + 1
|
||||
return ret
|
||||
|
||||
|
||||
@dataclass
|
||||
class MessageState:
|
||||
address: int
|
||||
name: str
|
||||
size: int
|
||||
signals: list[Signal]
|
||||
ignore_alive: bool = False
|
||||
ignore_checksum: bool = False
|
||||
ignore_counter: bool = False
|
||||
frequency: float = 0.0
|
||||
timeout_threshold: float = 1e5 # default to 1Hz threshold
|
||||
vals: list[float] = field(default_factory=list)
|
||||
all_vals: list[list[float]] = field(default_factory=list)
|
||||
timestamps: deque[int] = field(default_factory=lambda: deque(maxlen=500))
|
||||
counter: int = 0
|
||||
counter_fail: int = 0
|
||||
first_seen_nanos: int = 0
|
||||
last_warning_log_nanos: int = 0
|
||||
|
||||
def rate_limited_log(self, last_update_nanos: int, msg: str) -> None:
|
||||
if (last_update_nanos - self.last_warning_log_nanos) >= 1_000_000_000:
|
||||
carlog.warning(f"CANParser: {hex(self.address)} {self.name} {msg}")
|
||||
self.last_warning_log_nanos = last_update_nanos
|
||||
|
||||
def parse(self, nanos: int, dat: bytes) -> bool:
|
||||
tmp_vals: list[float] = [0.0] * len(self.signals)
|
||||
checksum_failed = False
|
||||
counter_failed = False
|
||||
|
||||
if self.first_seen_nanos == 0:
|
||||
self.first_seen_nanos = nanos
|
||||
|
||||
for i, sig in enumerate(self.signals):
|
||||
tmp = get_raw_value(dat, sig)
|
||||
if sig.is_signed:
|
||||
tmp -= ((tmp >> (sig.size - 1)) & 0x1) * (1 << sig.size)
|
||||
|
||||
if not self.ignore_checksum and sig.calc_checksum is not None:
|
||||
expected_checksum = sig.calc_checksum(self.address, sig, bytearray(dat))
|
||||
if tmp != expected_checksum:
|
||||
checksum_failed = True
|
||||
self.rate_limited_log(nanos, f"checksum failed: received {hex(tmp)}, calculated {hex(expected_checksum)}")
|
||||
|
||||
if not self.ignore_counter and sig.type == 1: # COUNTER
|
||||
if not self.update_counter(tmp, sig.size):
|
||||
counter_failed = True
|
||||
|
||||
tmp_vals[i] = tmp * sig.factor + sig.offset
|
||||
|
||||
# must have good counter and checksum to update data
|
||||
if checksum_failed or counter_failed:
|
||||
return False
|
||||
|
||||
if not self.vals:
|
||||
self.vals = [0.0] * len(self.signals)
|
||||
self.all_vals = [[] for _ in self.signals]
|
||||
|
||||
for i, v in enumerate(tmp_vals):
|
||||
self.vals[i] = v
|
||||
self.all_vals[i].append(v)
|
||||
|
||||
self.timestamps.append(nanos)
|
||||
|
||||
if self.frequency < 1e-5 and len(self.timestamps) >= 3:
|
||||
dt = (self.timestamps[-1] - self.timestamps[0]) * 1e-9
|
||||
if (dt > 1.0 or (self.timestamps.maxlen is not None and len(self.timestamps) >= self.timestamps.maxlen)) and dt != 0:
|
||||
self.frequency = min(len(self.timestamps) / dt, 100.0)
|
||||
self.timeout_threshold = (1_000_000_000 / self.frequency) * 10
|
||||
return True
|
||||
|
||||
def update_counter(self, cur_count: int, cnt_size: int) -> bool:
|
||||
if ((self.counter + 1) & ((1 << cnt_size) - 1)) != cur_count:
|
||||
self.counter_fail = min(self.counter_fail + 1, MAX_BAD_COUNTER)
|
||||
elif self.counter_fail > 0:
|
||||
self.counter_fail -= 1
|
||||
self.counter = cur_count
|
||||
return self.counter_fail < MAX_BAD_COUNTER
|
||||
|
||||
def valid(self, current_nanos: int, bus_timeout: bool) -> bool:
|
||||
if self.ignore_alive:
|
||||
return True
|
||||
if not self.timestamps:
|
||||
return False
|
||||
if (current_nanos - self.timestamps[-1]) > self.timeout_threshold:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
class VLDict(dict):
|
||||
def __init__(self, parser):
|
||||
super().__init__()
|
||||
self.parser = parser
|
||||
|
||||
def __getitem__(self, key):
|
||||
if key not in self:
|
||||
self.parser._add_message(key)
|
||||
return super().__getitem__(key)
|
||||
|
||||
|
||||
class CANParser:
|
||||
def __init__(self, dbc_name: str, messages: list[tuple[str | int, int]], bus: int):
|
||||
self.dbc_name: str = dbc_name
|
||||
self.bus: int = bus
|
||||
self.dbc: DBC = DBC(dbc_name)
|
||||
|
||||
self.vl: dict[int | str, dict[str, float]] = VLDict(self)
|
||||
self.vl_all: dict[int | str, dict[str, list[float]]] = {}
|
||||
self.ts_nanos: dict[int | str, dict[str, int]] = {}
|
||||
self.dat: dict[int | str, bytes] = {}
|
||||
self.addresses: set[int] = set()
|
||||
self.seen_addresses: set[int] = set()
|
||||
self.enable_capture: bool = True
|
||||
self.controls_ready: bool = False
|
||||
self.message_states: dict[int, MessageState] = {}
|
||||
|
||||
for name_or_addr, freq in messages:
|
||||
if isinstance(name_or_addr, numbers.Number):
|
||||
msg = self.dbc.addr_to_msg.get(int(name_or_addr))
|
||||
else:
|
||||
msg = self.dbc.name_to_msg.get(name_or_addr)
|
||||
if msg is None:
|
||||
raise RuntimeError(f"could not find message {name_or_addr!r} in DBC {dbc_name}")
|
||||
if msg.address in self.addresses:
|
||||
raise RuntimeError("Duplicate Message Check: %d" % msg.address)
|
||||
|
||||
self._add_message(name_or_addr, freq)
|
||||
|
||||
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, ignore_counter: bool = False) -> None:
|
||||
if isinstance(name_or_addr, numbers.Number):
|
||||
msg = self.dbc.addr_to_msg.get(int(name_or_addr))
|
||||
else:
|
||||
msg = self.dbc.name_to_msg.get(name_or_addr)
|
||||
assert msg is not None
|
||||
assert msg.address not in self.addresses
|
||||
|
||||
self.addresses.add(msg.address)
|
||||
signal_names = list(msg.sigs.keys())
|
||||
signals_dict = {s: 0.0 for s in signal_names}
|
||||
dict.__setitem__(self.vl, msg.address, signals_dict)
|
||||
dict.__setitem__(self.vl, msg.name, signals_dict)
|
||||
self.vl_all[msg.address] = defaultdict(list)
|
||||
self.vl_all[msg.name] = self.vl_all[msg.address]
|
||||
self.ts_nanos[msg.address] = {s: 0 for s in signal_names}
|
||||
self.ts_nanos[msg.name] = self.ts_nanos[msg.address]
|
||||
state = MessageState(
|
||||
address=msg.address,
|
||||
name=msg.name,
|
||||
size=msg.size,
|
||||
signals=list(msg.sigs.values()),
|
||||
ignore_alive=freq is not None and math.isnan(freq),
|
||||
ignore_counter=ignore_counter,
|
||||
)
|
||||
state.first_seen_nanos = time.monotonic_ns()
|
||||
if freq is not None and freq > 0:
|
||||
state.frequency = freq
|
||||
else:
|
||||
# if frequency not specified, assume 1Hz until we learn it
|
||||
freq = 1
|
||||
state.timeout_threshold = (1_000_000_000 / freq) * 10
|
||||
|
||||
self.message_states[msg.address] = state
|
||||
|
||||
@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
|
||||
state.rate_limited_log(self._last_update_nanos, f"counter invalid, {state.counter_fail=} {MAX_BAD_COUNTER=}")
|
||||
if not state.valid(self._last_update_nanos, bus_timeout):
|
||||
valid = False
|
||||
state.rate_limited_log(self._last_update_nanos, "not valid (timeout or missing)")
|
||||
|
||||
# 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)
|
||||
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):
|
||||
strings = [strings]
|
||||
|
||||
for addr in self.addresses:
|
||||
for k in self.vl_all[addr]:
|
||||
self.vl_all[addr][k].clear()
|
||||
|
||||
updated_addrs: set[int] = set()
|
||||
for entry in strings:
|
||||
t = entry[0]
|
||||
frames = entry[1]
|
||||
bus_empty = True
|
||||
for address, dat, src in frames:
|
||||
if src != self.bus:
|
||||
continue
|
||||
bus_empty = False
|
||||
if self.enable_capture:
|
||||
self.seen_addresses.add(address)
|
||||
state = self.message_states.get(address)
|
||||
if state is None or len(dat) > 64:
|
||||
continue
|
||||
if state.parse(t, dat):
|
||||
updated_addrs.add(address)
|
||||
|
||||
vl_addr = self.vl[address]
|
||||
vl_all_addr = self.vl_all[address]
|
||||
ts_addr = self.ts_nanos[address]
|
||||
raw_dat = bytes(dat)
|
||||
self.dat[address] = raw_dat
|
||||
self.dat[state.name] = raw_dat
|
||||
|
||||
for i, sig in enumerate(state.signals):
|
||||
vl_addr[sig.name] = state.vals[i]
|
||||
vl_all_addr[sig.name] = state.all_vals[i]
|
||||
ts_addr[sig.name] = state.timestamps[-1]
|
||||
|
||||
if not bus_empty:
|
||||
self.last_nonempty_nanos = t
|
||||
|
||||
self._last_update_nanos = t
|
||||
|
||||
return updated_addrs
|
||||
|
||||
|
||||
class CANDefine:
|
||||
def __init__(self, dbc_name: str):
|
||||
dbc = DBC(dbc_name)
|
||||
|
||||
dv = defaultdict(dict)
|
||||
for val in dbc.vals:
|
||||
sgname = val.name
|
||||
address = val.address
|
||||
msg = dbc.addr_to_msg.get(address)
|
||||
if msg is None:
|
||||
raise KeyError(address)
|
||||
msgname = msg.name
|
||||
parts = val.def_val.split()
|
||||
values = [int(v) for v in parts[::2]]
|
||||
defs = parts[1::2]
|
||||
dv[address][sgname] = dict(zip(values, defs, strict=True))
|
||||
dv[msgname][sgname] = dv[address][sgname]
|
||||
|
||||
self.dv = dict(dv)
|
||||
1
artifacts/package_sources/iqdbc/iqdbc/can/tests/.gitignore
vendored
Normal file
1
artifacts/package_sources/iqdbc/iqdbc/can/tests/.gitignore
vendored
Normal file
@@ -0,0 +1 @@
|
||||
*.bz2
|
||||
@@ -0,0 +1,8 @@
|
||||
import glob
|
||||
import os
|
||||
|
||||
from iqdbc import DBC_PATH
|
||||
|
||||
ALL_DBCS = [os.path.basename(dbc).split('.')[0] for dbc in
|
||||
glob.glob(f"{DBC_PATH}/*.dbc")]
|
||||
TEST_DBC = os.path.abspath(os.path.join(os.path.dirname(__file__), "test.dbc"))
|
||||
46
artifacts/package_sources/iqdbc/iqdbc/can/tests/benchmark.py
Executable file
46
artifacts/package_sources/iqdbc/iqdbc/can/tests/benchmark.py
Executable file
@@ -0,0 +1,46 @@
|
||||
#!/usr/bin/env python3
|
||||
import time
|
||||
from iqdbc.can import CANPacker, CANParser
|
||||
|
||||
|
||||
def _benchmark(checks, n):
|
||||
parser = CANParser('toyota_new_mc_pt_generated', checks, 0)
|
||||
packer = CANPacker('toyota_new_mc_pt_generated')
|
||||
|
||||
t1 = time.process_time_ns()
|
||||
can_msgs = []
|
||||
for i in range(10000):
|
||||
values = {"ACC_CONTROL": {"ACC_TYPE": 1, "ALLOW_LONG_PRESS": 3}}
|
||||
msgs = [packer.make_can_msg(k, 0, v) for k, v in values.items()]
|
||||
can_msgs.append([int(0.01 * i * 1e9), msgs])
|
||||
t2 = time.process_time_ns()
|
||||
pack_dt = t2 - t1
|
||||
|
||||
ets = []
|
||||
for _ in range(25):
|
||||
if n > 1:
|
||||
strings = []
|
||||
for i in range(0, len(can_msgs), n):
|
||||
strings.append(can_msgs[i:i + n])
|
||||
t1 = time.process_time_ns()
|
||||
for m in strings:
|
||||
parser.update(m)
|
||||
t2 = time.process_time_ns()
|
||||
else:
|
||||
t1 = time.process_time_ns()
|
||||
for m in can_msgs:
|
||||
parser.update([m])
|
||||
t2 = time.process_time_ns()
|
||||
|
||||
ets.append(t2 - t1)
|
||||
|
||||
et = sum(ets) / len(ets)
|
||||
avg_nanos = et / len(can_msgs)
|
||||
print('[%d] %.1fms to pack, %.1fms to parse %s messages, avg: %dns' % (n, pack_dt/1e6, et/1e6, len(can_msgs), avg_nanos))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
# python -m cProfile -s cumulative benchmark.py
|
||||
_benchmark([('ACC_CONTROL', 10)], 1)
|
||||
_benchmark([('ACC_CONTROL', 10)], 5)
|
||||
_benchmark([('ACC_CONTROL', 10)], 10)
|
||||
27
artifacts/package_sources/iqdbc/iqdbc/can/tests/test.dbc
Normal file
27
artifacts/package_sources/iqdbc/iqdbc/can/tests/test.dbc
Normal file
@@ -0,0 +1,27 @@
|
||||
CM_ "This DBC is used for the CAN parser and packer tests.";
|
||||
|
||||
BO_ 228 STEERING_CONTROL: 5 EON
|
||||
SG_ STEER_TORQUE_REQUEST : 23|1@0+ (1,0) [0|1] "" EPS
|
||||
SG_ SET_ME_X00 : 22|7@0+ (1,0) [0|127] "" EPS
|
||||
SG_ SET_ME_X00_2 : 31|8@0+ (1,0) [0|0] "" EPS
|
||||
SG_ STEER_TORQUE : 7|16@0- (1,0) [-4096|4096] "" EPS
|
||||
SG_ STEER_DOWN_TO_ZERO : 38|1@0+ (1,0) [0|1] "" EPS
|
||||
SG_ COUNTER : 37|2@0+ (1,0) [0|3] "" EPS
|
||||
SG_ CHECKSUM : 35|4@0+ (1,0) [0|15] "" EPS
|
||||
|
||||
BO_ 316 Brake_Status: 8 XXX
|
||||
SG_ CHECKSUM : 0|8@1+ (1,0) [0|255] "" XXX
|
||||
SG_ COUNTER : 8|4@1+ (1,0) [0|15] "" XXX
|
||||
SG_ Signal1 : 12|46@1+ (1,0) [0|1] "" XXX
|
||||
SG_ ES_Brake : 58|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ Signal2 : 59|3@1+ (1,0) [0|1] "" XXX
|
||||
SG_ Brake : 62|1@1+ (1,0) [0|1] "" XXX
|
||||
SG_ Signal3 : 63|1@1+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 245 CAN_FD_MESSAGE: 32 XXX
|
||||
SG_ COUNTER : 7|8@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SIGNED : 22|16@0- (1,0) [0|1] "" XXX
|
||||
SG_ 64_BIT_LE : 159|64@1+ (1,0) [0|1] "" XXX
|
||||
SG_ 64_BIT_BE : 80|64@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
VAL_ 80 NON_EXISTENT_ADDR 0 "test";
|
||||
@@ -0,0 +1,594 @@
|
||||
import copy
|
||||
from iqdbc.can import CANPacker, CANParser
|
||||
|
||||
|
||||
class TestCanChecksums:
|
||||
|
||||
def verify_checksum(self, subtests, dbc_file: str, msg_name: str, msg_addr: int, test_messages: list[bytes],
|
||||
checksum_field: str = 'CHECKSUM', counter_field = 'COUNTER'):
|
||||
"""
|
||||
Verify that iqdbc calculates payload CRCs/checksums matching those received in known-good sample messages
|
||||
Depends on all non-zero bits in the sample message having a corresponding DBC signal, add UNKNOWN signals if needed
|
||||
"""
|
||||
parser = CANParser(dbc_file, [(msg_name, 0)], 0)
|
||||
packer = CANPacker(dbc_file)
|
||||
|
||||
for data in test_messages:
|
||||
expected_msg = (msg_addr, data, 0)
|
||||
parser.update([0, [expected_msg]])
|
||||
expected = copy.deepcopy(parser.vl[msg_name])
|
||||
|
||||
modified = copy.deepcopy(expected)
|
||||
modified.pop(checksum_field, None)
|
||||
modified_msg = packer.make_can_msg(msg_name, 0, modified)
|
||||
|
||||
parser.update([0, [modified_msg]])
|
||||
tested = parser.vl[msg_name]
|
||||
with subtests.test(counter=expected[counter_field]):
|
||||
assert tested[checksum_field] == expected[checksum_field]
|
||||
|
||||
def verify_fca_giorgio_crc(self, subtests, msg_name: str, msg_addr: int, test_messages: list[bytes]):
|
||||
"""Test modified SAE J1850 CRCs, with special final XOR cases for EPS messages"""
|
||||
assert len(test_messages) == 3
|
||||
self.verify_checksum(subtests, "fca_giorgio", msg_name, msg_addr, test_messages)
|
||||
|
||||
def test_fca_giorgio_eps_1(self, subtests):
|
||||
self.verify_fca_giorgio_crc(subtests, "EPS_1", 0xDE, [
|
||||
b'\x17\x51\x97\xcc\x00\xdf',
|
||||
b'\x17\x51\x97\xc9\x01\xa3',
|
||||
b'\x17\x51\x97\xcc\x02\xe5',
|
||||
])
|
||||
|
||||
def test_fca_giorgio_eps_2(self, subtests):
|
||||
self.verify_fca_giorgio_crc(subtests, "EPS_2", 0x106, [
|
||||
b'\x7c\x43\x57\x60\x00\x00\xa1',
|
||||
b'\x7c\x63\x58\xe0\x00\x01\xd5',
|
||||
b'\x7c\x63\x58\xe0\x00\x02\xf2',
|
||||
])
|
||||
|
||||
def test_fca_giorgio_eps_3(self, subtests):
|
||||
self.verify_fca_giorgio_crc(subtests, "EPS_3", 0x122, [
|
||||
b'\x7b\x30\x00\xf8',
|
||||
b'\x7b\x10\x01\x90',
|
||||
b'\x7b\xf0\x02\x6e',
|
||||
])
|
||||
|
||||
def test_fca_giorgio_abs_2(self, subtests):
|
||||
self.verify_fca_giorgio_crc(subtests, "ABS_2", 0xFE, [
|
||||
b'\x7e\x38\x00\x7d\x10\x31\x80\x32',
|
||||
b'\x7e\x38\x00\x7d\x10\x31\x81\x2f',
|
||||
b'\x7e\x38\x00\x7d\x20\x31\x82\x20',
|
||||
])
|
||||
|
||||
def test_honda_checksum(self):
|
||||
"""Test checksums for Honda standard and extended CAN ids"""
|
||||
# TODO: refactor to use self.verify_checksum()
|
||||
dbc_file = "honda_civic_hatchback_ex_2017_can_generated"
|
||||
msgs = [("LKAS_HUD", 0), ("LKAS_HUD_A", 0)]
|
||||
parser = CANParser(dbc_file, msgs, 0)
|
||||
packer = CANPacker(dbc_file)
|
||||
|
||||
values = {
|
||||
'LKAS_READY': 1,
|
||||
'LKAS_STATE_CHANGE': 1,
|
||||
'STEERING_REQUIRED': 1,
|
||||
'SOLID_LANES': 1,
|
||||
'BEEP': 0,
|
||||
}
|
||||
|
||||
# known correct checksums according to the above values
|
||||
checksum_std = [11, 10, 9, 8]
|
||||
checksum_ext = [4, 3, 2, 1]
|
||||
|
||||
for std, ext in zip(checksum_std, checksum_ext, strict=True):
|
||||
msgs = [
|
||||
packer.make_can_msg("LKAS_HUD", 0, values),
|
||||
packer.make_can_msg("LKAS_HUD_A", 0, values),
|
||||
]
|
||||
parser.update([0, msgs])
|
||||
|
||||
assert parser.vl['LKAS_HUD']['CHECKSUM'] == std
|
||||
assert parser.vl['LKAS_HUD_A']['CHECKSUM'] == ext
|
||||
|
||||
def test_honda_checksum_high_extended(self):
|
||||
"""Extended CAN ids above 0x100000 use a +10 checksum constant instead of +3"""
|
||||
dbc_file = "honda_common_canfd_generated"
|
||||
msgs = [("LANE_PATH", 0), ("RADAR_LEAD", 0)]
|
||||
parser = CANParser(dbc_file, msgs, 0)
|
||||
packer = CANPacker(dbc_file)
|
||||
|
||||
lane_path_values = {
|
||||
'MUX': 1,
|
||||
'PATH_OFFSET_1': 0,
|
||||
'PATH_OFFSET_2': 0,
|
||||
'PATH_OFFSET_3': 2047,
|
||||
'PATH_OFFSET_4': 2047,
|
||||
}
|
||||
radar_lead_values = {
|
||||
'CNTR_REF': 2,
|
||||
'SET_ME_X01': 1,
|
||||
'TARGET_SPEED_MAYBE': 140,
|
||||
'LEFT_LANE': 3,
|
||||
'RIGHT_LANE': 3,
|
||||
'LANE_PATH_LENGTH': 6,
|
||||
}
|
||||
|
||||
# known correct checksums according to the above values
|
||||
checksum_lane_path = [14, 13, 12, 11]
|
||||
checksum_radar_lead = [4, 3, 2, 1]
|
||||
|
||||
for lane_path, radar_lead in zip(checksum_lane_path, checksum_radar_lead, strict=True):
|
||||
msgs = [
|
||||
packer.make_can_msg("LANE_PATH", 0, lane_path_values),
|
||||
packer.make_can_msg("RADAR_LEAD", 0, radar_lead_values),
|
||||
]
|
||||
parser.update([0, msgs])
|
||||
|
||||
assert parser.vl['LANE_PATH']['CHECKSUM'] == lane_path
|
||||
assert parser.vl['RADAR_LEAD']['CHECKSUM'] == radar_lead
|
||||
assert parser.can_valid
|
||||
|
||||
def verify_volkswagen_mqb_crc(self, subtests, msg_name: str, msg_addr: int, test_messages: list[bytes], counter_field: str = 'COUNTER'):
|
||||
"""Test AUTOSAR E2E Profile 2 CRCs"""
|
||||
assert len(test_messages) == 16 # All counter values must be tested
|
||||
self.verify_checksum(subtests, "vw_mqb", msg_name, msg_addr, test_messages, counter_field=counter_field)
|
||||
|
||||
def test_volkswagen_mqb_crc_lwi_01(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "LWI_01", 0x86, [
|
||||
b'\x6b\x00\xbd\x00\x00\x00\x00\x00',
|
||||
b'\xee\x01\x0a\x00\x00\x00\x00\x00',
|
||||
b'\xd8\x02\xa9\x00\x00\x00\x00\x00',
|
||||
b'\x03\x03\xbe\xa2\x12\x00\x00\x00',
|
||||
b'\x7b\x04\x31\x20\x03\x00\x00\x00',
|
||||
b'\x8b\x05\xe2\x85\x09\x00\x00\x00',
|
||||
b'\x63\x06\x13\x21\x00\x00\x00\x00',
|
||||
b'\x66\x07\x05\x00\x00\x00\x00\x00',
|
||||
b'\x49\x08\x0d\x00\x00\x00\x00\x00',
|
||||
b'\x5f\x09\x7e\x60\x01\x00\x00\x00',
|
||||
b'\xaf\x0a\x72\x20\x00\x00\x00\x00',
|
||||
b'\x59\x0b\x1b\x00\x00\x00\x00\x00',
|
||||
b'\xa8\x0c\x06\x00\x00\x00\x00\x00',
|
||||
b'\xbc\x0d\x72\x20\x00\x00\x00\x00',
|
||||
b'\xf9\x0e\x0f\x00\x00\x00\x00\x00',
|
||||
b'\x60\x0f\x62\xc0\x00\x00\x00\x00',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_airbag_01(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "Airbag_01", 0x40, [
|
||||
b'\xaf\x00\x00\x80\xc0\x00\x20\x3e',
|
||||
b'\x54\x01\x00\x80\xc0\x00\x20\x1a',
|
||||
b'\x54\x02\x00\x80\xc0\x00\x60\x00',
|
||||
b'\x31\x03\x00\x80\xc0\x00\x60\xf2',
|
||||
b'\xe0\x04\x00\x80\xc0\x00\x60\xcc',
|
||||
b'\xb3\x05\x00\x80\xc0\x00\x40\xde',
|
||||
b'\xa4\x06\x00\x80\xc0\x00\x40\x18',
|
||||
b'\x94\x07\x00\x80\xc0\x00\x20\x38',
|
||||
b'\x2d\x08\x00\x80\xc0\x00\x60\xae',
|
||||
b'\xc2\x09\x00\x80\xc0\x00\x00\x1c',
|
||||
b'\x1f\x0a\x00\x80\xc0\x00\x60\x2c',
|
||||
b'\x7f\x0b\x00\x80\xc0\x00\x00\x00',
|
||||
b'\x03\x0c\x00\x80\xc0\x00\x40\xd6',
|
||||
b'\x56\x0d\x00\x80\xc0\x00\x20\x50',
|
||||
b'\x4a\x0e\x00\x80\xc0\x00\x20\xf2',
|
||||
b'\xe5\x0f\x00\x80\xc0\x00\x40\xf6',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_lh_eps_03(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "LH_EPS_03", 0x9F, [
|
||||
b'\x11\x30\x2e\x00\x05\x1c\x80\x30',
|
||||
b'\x5b\x31\x8e\x03\x05\x53\x00\x30',
|
||||
b'\xcb\x32\xd3\x06\x05\x73\x00\x30',
|
||||
b'\xf2\x33\x28\x00\x05\x26\x00\x30',
|
||||
b'\x0b\x34\x44\x00\x05\x5b\x80\x30',
|
||||
b'\xed\x35\x80\x00\x03\x34\x00\x30',
|
||||
b'\xf0\x36\x88\x00\x05\x3d\x80\x30',
|
||||
b'\x9e\x37\x44\x03\x05\x41\x00\x30',
|
||||
b'\x68\x38\x06\x01\x05\x18\x80\x30',
|
||||
b'\x87\x39\x51\x00\x05\x11\x80\x30',
|
||||
b'\x8c\x3a\x29\x00\x05\xac\x00\x30',
|
||||
b'\x08\x3b\xbd\x00\x05\x8e\x00\x30',
|
||||
b'\xd4\x3c\x19\x00\x05\x05\x80\x30',
|
||||
b'\x29\x3d\x54\x00\x05\x5b\x00\x30',
|
||||
b'\xa1\x3e\x49\x01\x03\x04\x80\x30',
|
||||
b'\xe2\x3f\x05\x00\x05\x0a\x00\x30',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_getriebe_11(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "Getriebe_11", 0xAD, [
|
||||
b'\xf8\xe0\xbf\xff\x5f\x20\x20\x20',
|
||||
b'\xb0\xe1\xbf\xff\xc6\x98\x21\x80',
|
||||
b'\xd2\xe2\xbf\xff\x5f\x20\x20\x20',
|
||||
b'\x00\xe3\xbf\xff\xaa\x20\x20\x10',
|
||||
b'\xf1\xe4\xbf\xff\x5f\x20\x20\x20',
|
||||
b'\xc4\xe5\xbf\xff\x5f\x20\x20\x20',
|
||||
b'\xda\xe6\xbf\xff\x5f\x20\x20\x20',
|
||||
b'\x85\xe7\xbf\xff\x5f\x20\x20\x20',
|
||||
b'\x12\xe8\xbf\xff\x5f\x20\x20\x20',
|
||||
b'\x45\xe9\xbf\xff\xaa\x20\x20\x10',
|
||||
b'\x03\xea\xbf\xff\xcc\x20\x20\x10',
|
||||
b'\xfc\xeb\xbf\xff\x5f\x20\x21\x20',
|
||||
b'\xfe\xec\xbf\xff\xad\x20\x20\x10',
|
||||
b'\xbd\xed\xbf\xff\xaa\x20\x20\x10',
|
||||
b'\x67\xee\xbf\xff\xaa\x20\x20\x10',
|
||||
b'\x36\xef\xbf\xff\xaa\x20\x20\x10',
|
||||
], counter_field="COUNTER_DISABLED") # see iqdbc#1235
|
||||
|
||||
def test_volkswagen_mqb_crc_esp_21(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "ESP_21", 0xFD, [
|
||||
b'\x66\xd0\x1f\x80\x45\x05\x00\x00',
|
||||
b'\x87\xd1\x1f\x80\x52\x05\x00\x00',
|
||||
b'\xcd\xd2\x1f\x80\x50\x06\x00\x00',
|
||||
b'\xfd\xd3\x1f\x80\x35\x02\x00\x00',
|
||||
b'\xfa\xd4\x1f\x80\x22\x05\x00\x00',
|
||||
b'\xfd\xd5\x1f\x80\x84\x04\x00\x00',
|
||||
b'\x2e\xd6\x1f\x80\xf0\x03\x00\x00',
|
||||
b'\x9f\xd7\x1f\x80\x00\x00\x00\x00',
|
||||
b'\x1e\xd8\x1f\x80\xb3\x03\x00\x00',
|
||||
b'\x61\xd9\x1f\x80\x6d\x05\x00\x00',
|
||||
b'\x44\xda\x1f\x80\x47\x02\x00\x00',
|
||||
b'\x86\xdb\x1f\x80\x3a\x02\x00\x00',
|
||||
b'\x39\xdc\x1f\x80\xcb\x01\x00\x00',
|
||||
b'\x19\xdd\x1f\x80\x00\x00\x00\x00',
|
||||
b'\x8c\xde\x1f\x80\xba\x04\x00\x00',
|
||||
b'\xfb\xdf\x1f\x80\x46\x00\x00\x00',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_esp_02(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "ESP_02", 0x101, [
|
||||
b'\xf2\x00\x7e\xff\xa1\x2a\x40\x00',
|
||||
b'\xd3\x01\x7d\x00\xa2\x0c\x02\x00',
|
||||
b'\x03\x02\x7a\x06\xa2\x49\x42\x00',
|
||||
b'\xfd\x03\x70\xfb\xa1\xde\x00\x00',
|
||||
b'\x8e\x04\x7b\xf7\xa1\xd2\x01\x00',
|
||||
b'\x0f\x05\x7d\xfd\xa1\x31\x40\x00',
|
||||
b'\xb6\x06\x7d\x01\xa2\x0a\x40\x00',
|
||||
b'\xe8\x07\x7e\xfd\xa1\x12\x40\x00',
|
||||
b'\x74\x08\x7a\x01\xa2\x40\x01\x00',
|
||||
b'\xe3\x09\x81\x00\xa2\xb5\x01\x00',
|
||||
b'\xab\x0a\x74\x09\xa2\x9f\x42\x00',
|
||||
b'\xf3\x0b\x80\x12\xa2\x94\x00\x00',
|
||||
b'\x88\x0c\x7f\x07\xa2\x46\x00\x00',
|
||||
b'\x6f\x0d\x7f\xff\xa1\x53\x40\x00',
|
||||
b'\x38\x0e\x73\xd6\xa1\x6a\x40\x00',
|
||||
b'\x49\x0f\x85\x12\xa2\xf6\x01\x00',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_esp_05(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "ESP_05", 0x106, [
|
||||
b'\x90\x80\x64\x00\x00\x00\xe7\x10',
|
||||
b'\xf4\x81\x64\x00\x00\x00\xe7\x10',
|
||||
b'\x90\x82\x63\x00\x00\x00\xe8\x10',
|
||||
b'\xa0\x83\x63\x00\x00\x00\xe6\x10',
|
||||
b'\xe7\x84\x63\x00\x00\x00\xe8\x10',
|
||||
b'\x2e\x85\x78\x04\x00\x00\xea\x30',
|
||||
b'\x7b\x86\x63\x00\x00\x00\xe6\x10',
|
||||
b'\x71\x87\x79\x04\x00\x00\xd0\x30',
|
||||
b'\x50\x88\x79\x04\x00\x00\xea\x30',
|
||||
b'\x81\x89\x64\x00\x00\x00\xe1\x10',
|
||||
b'\x6a\x8a\x68\x00\x00\x04\xd0\x10',
|
||||
b'\x17\x8b\x6a\x04\x00\x00\xe6\x10',
|
||||
b'\xc7\x8c\x63\x00\x00\x00\xd1\x10',
|
||||
b'\x53\x8d\x64\x04\x00\x00\xe2\x10',
|
||||
b'\x24\x8e\x63\x00\x00\x00\xe7\x10',
|
||||
b'\x3f\x8f\x82\x04\x00\x00\xe6\x30',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_esp_10(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "ESP_10", 0x116, [
|
||||
b'\x2d\x00\xd5\x98\x9f\x26\x25\x0f',
|
||||
b'\x24\x01\x60\x63\x2c\x5e\x3b\x0f',
|
||||
b'\x08\x02\xb2\x2f\xee\x9a\x29\x0f',
|
||||
b'\x7c\x03\x17\x07\x1d\xe5\x8c\x0f',
|
||||
b'\xaa\x04\xd6\xe3\xeb\x98\xe8\x0f',
|
||||
b'\x4e\x05\xbb\xd9\x65\x43\xca\x0f',
|
||||
b'\x59\x06\x78\xbd\x25\xc6\xf2\xff',
|
||||
b'\xaf\x07\x42\x85\x53\xbe\xbe\x0f',
|
||||
b'\x2a\x08\xa6\xcd\x95\x8c\x12\x0f',
|
||||
b'\xce\x09\x6e\x17\x6d\x1b\x2f\x0f',
|
||||
b'\x60\x0a\xd3\xe6\x3a\x8d\xf0\x0f',
|
||||
b'\xc5\x0b\xfc\x69\x57\x50\x21\x0f',
|
||||
b'\x70\x0c\xde\xf3\x9d\xe9\x6b\xff',
|
||||
b'\x62\x0d\xc4\x1a\xdb\x61\x7a\x0f',
|
||||
b'\x76\x0e\x79\x69\xe3\x32\x67\x0f',
|
||||
b'\x15\x0f\x51\x59\x56\x35\xb1\x0f',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_acc_10(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "ACC_10", 0x117, [
|
||||
b'\x9b\x00\x00\x40\x68\x00\x00\xff',
|
||||
b'\xff\x01\x00\x40\x68\x00\x00\xff',
|
||||
b'\x53\x02\x00\x40\x68\x00\x00\xff',
|
||||
b'\x37\x03\x00\x40\x68\x00\x00\xff',
|
||||
b'\x24\x04\x00\x40\x68\x00\x00\xff',
|
||||
b'\x40\x05\x00\x40\x68\x00\x00\xff',
|
||||
b'\xec\x06\x00\x40\x68\x00\x00\xff',
|
||||
b'\x88\x07\x00\x40\x68\x00\x00\xff',
|
||||
b'\xca\x08\x00\x40\x68\x00\x00\xff',
|
||||
b'\xae\x09\x00\x40\x68\x00\x00\xff',
|
||||
b'\x02\x0a\x00\x40\x68\x00\x00\xff',
|
||||
b'\x66\x0b\x00\x40\x68\x00\x00\xff',
|
||||
b'\x75\x0c\x00\x40\x68\x00\x00\xff',
|
||||
b'\x11\x0d\x00\x40\x68\x00\x00\xff',
|
||||
b'\xbd\x0e\x00\x40\x68\x00\x00\xff',
|
||||
b'\xd9\x0f\x00\x40\x68\x00\x00\xff',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_tsk_06(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "TSK_06", 0x120, [
|
||||
b'\xc1\x00\x00\x02\x00\x08\xff\x21',
|
||||
b'\x34\x01\x00\x02\x00\x08\xff\x21',
|
||||
b'\xcc\x02\x00\x02\x00\x08\xff\x21',
|
||||
b'\x1e\x03\x00\x02\x00\x08\xff\x21',
|
||||
b'\x48\x04\x00\x02\x00\x08\xff\x21',
|
||||
b'\x4a\x05\x00\x02\x00\x08\xff\x21',
|
||||
b'\xa5\x06\x00\x02\x00\x08\xff\x21',
|
||||
b'\xa7\x07\x00\x02\x00\x08\xff\x21',
|
||||
b'\xfe\x08\x00\x02\x00\x08\xff\x21',
|
||||
b'\xa8\x09\x00\x02\x00\x08\xff\x21',
|
||||
b'\x73\x0a\x00\x02\x00\x08\xff\x21',
|
||||
b'\xdf\x0b\x00\x02\x00\x08\xff\x21',
|
||||
b'\x05\x0c\x00\x02\x00\x08\xff\x21',
|
||||
b'\xb5\x0d\x00\x02\x00\x08\xff\x21',
|
||||
b'\xde\x0e\x00\x02\x00\x08\xff\x21',
|
||||
b'\x0b\x0f\x00\x02\x00\x08\xff\x21',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_motor_20(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "Motor_20", 0x121, [
|
||||
b'\xb9\x00\x00\xc0\x39\x46\x7e\xfe',
|
||||
b'\x85\x31\x20\x00\x1a\x46\x7e\xfe',
|
||||
b'\xc7\x12\x00\x40\x1a\x46\x7e\xfe',
|
||||
b'\x53\x93\x00\x00\x19\x46\x7e\xfe',
|
||||
b'\xa4\x34\x00\x80\x1a\x46\x7e\xfe',
|
||||
b'\x0e\x55\x20\x60\x18\x46\x7e\xfe',
|
||||
b'\x3f\x06\x00\xc0\x37\x4c\x7e\xfe',
|
||||
b'\x0c\x07\x00\x40\x39\x46\x7e\xfe',
|
||||
b'\x2a\x08\x00\x00\x3a\x46\x7e\xfe',
|
||||
b'\x7f\x49\x20\x80\x1a\x46\x7e\xfe',
|
||||
b'\x2f\x0a\x00\xc0\x39\x46\x7e\xfe',
|
||||
b'\x70\xbb\x00\x00\x17\x46\x7e\xfe',
|
||||
b'\x06\x0c\x00\x00\x39\x46\x7e\xfe',
|
||||
b'\x4b\x9d\x20\xe0\x16\x4c\x7e\xfe',
|
||||
b'\x73\xfe\x00\x40\x16\x46\x7e\xfe',
|
||||
b'\xaf\x0f\x20\x80\x39\x4c\x7e\xfe',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_acc_06(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "ACC_06", 0x122, [
|
||||
b'\x14\x80\x00\xfe\x07\x00\x00\x18',
|
||||
b'\x9f\x81\x00\xfe\x07\x00\x00\x18',
|
||||
b'\x0a\x82\x00\xfe\x07\x00\x00\x28',
|
||||
b'\x40\x83\x00\xfe\x07\x00\x00\x18',
|
||||
b'\x2d\x84\x00\xfe\x07\x00\x00\x28',
|
||||
b'\xdb\x85\x00\xfe\x07\x00\x00\x18',
|
||||
b'\x4d\x86\x00\xfe\x07\x00\x00\x28',
|
||||
b'\x35\x87\x00\xfe\x07\x00\x00\x18',
|
||||
b'\x23\x88\x00\xfe\x07\x00\x00\x28',
|
||||
b'\x4a\x89\x00\xfe\x07\x00\x00\x28',
|
||||
b'\xe1\x8a\x00\xfe\x07\x00\x00\x28',
|
||||
b'\x30\x8b\x00\xfe\x07\x00\x00\x28',
|
||||
b'\x60\x8c\x00\xfe\x07\x00\x00\x28',
|
||||
b'\x0d\x8d\x00\xfe\x07\x00\x00\x18',
|
||||
b'\x8c\x8e\x00\xfe\x07\x00\x00\x18',
|
||||
b'\x6f\x8f\x00\xfe\x07\x00\x00\x28',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_hca_01(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "HCA_01", 0x126, [
|
||||
b'\x00\x30\x0d\xc0\x05\xfe\x07\x00',
|
||||
b'\x3e\x31\x54\xc0\x05\xfe\x07\x00',
|
||||
b'\xa7\x32\xbb\x40\x05\xfe\x07\x00',
|
||||
b'\x96\x33\x29\xc0\x05\xfe\x07\x00',
|
||||
b'\x5f\x34\x00\x00\x03\xfe\x07\x00',
|
||||
b'\x3b\x35\xae\x40\x05\xfe\x07\x00',
|
||||
b'\xc7\x36\x7a\x40\x05\xfe\x07\x00',
|
||||
b'\x6f\x37\x76\x40\x05\xfe\x07\x00',
|
||||
b'\xb1\x38\x00\x00\x03\xfe\x07\x00',
|
||||
b'\xd5\x39\x00\x00\x03\xfe\x07\x00',
|
||||
b'\xba\x3a\x69\xc0\x05\xfe\x07\x00',
|
||||
b'\x65\x3b\x10\x40\x05\xfe\x07\x00',
|
||||
b'\x49\x3c\x72\xc0\x05\xfe\x07\x00',
|
||||
b'\xc6\x3d\xdf\x40\x05\xfe\x07\x00',
|
||||
b'\x1d\x3e\x2c\xc1\x05\xfe\x07\x00',
|
||||
b'\x9b\x3f\x20\x40\x05\xfe\x07\x00',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_gra_acc_01(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "GRA_ACC_01", 0x12B, [
|
||||
b'\x86\x40\x80\x2a\x00\x00\x00\x00',
|
||||
b'\xf4\x41\x80\x2a\x00\x00\x00\x00',
|
||||
b'\x50\x42\x80\x2a\x00\x00\x00\x00',
|
||||
b'\x08\x43\x80\x2a\x00\x00\x00\x00',
|
||||
b'\x88\x44\x80\x2a\x00\x00\x00\x00',
|
||||
b'\x2d\x45\x80\x2a\x00\x00\x00\x00',
|
||||
b'\x34\x46\x80\x2a\x00\x00\x00\x00',
|
||||
b'\x11\x47\x80\x2a\x00\x00\x00\x00',
|
||||
b'\xc4\x48\x80\x2a\x00\x00\x00\x00',
|
||||
b'\xcc\x49\x80\x2a\x00\x00\x00\x00',
|
||||
b'\xdc\x4a\x80\x2a\x00\x00\x00\x00',
|
||||
b'\x79\x4b\x80\x2a\x00\x00\x00\x00',
|
||||
b'\x3c\x4c\x80\x2a\x00\x00\x00\x00',
|
||||
b'\x68\x4d\x80\x2a\x00\x00\x00\x00',
|
||||
b'\x27\x4e\x80\x2a\x00\x00\x00\x00',
|
||||
b'\x0d\x4f\x80\x2a\x00\x00\x00\x00',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_acc_07(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "ACC_07", 0x12E, [
|
||||
b'\xac\xe0\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\xa2\xe1\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\x6b\xe2\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\xf2\xe3\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\xd5\xe4\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\x35\xe5\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\x7f\xe6\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\x6c\xe7\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\x05\xe8\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\x79\xe9\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\x25\xea\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\xd1\xeb\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\x72\xec\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\x58\xed\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\x82\xee\x7f\x00\xfe\x00\xc0\xff',
|
||||
b'\x85\xef\x7f\x00\xfe\x00\xc0\xff',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_motor_ev_01(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "Motor_EV_01", 0x187, [
|
||||
b'\x70\x80\x15\x00\x00\x00\x00\xF0',
|
||||
b'\x07\x81\x15\x00\x00\x00\x00\xF0',
|
||||
b'\x7A\x82\x15\x00\x00\x00\x00\xF0',
|
||||
b'\x26\x83\x15\x00\x00\x00\x00\xF0',
|
||||
b'\xBE\x84\x15\x00\x00\x00\x00\xF0',
|
||||
b'\x5A\x85\x15\x00\x00\x00\x00\xF0',
|
||||
b'\xFC\x86\x15\x00\x00\x00\x00\xF0',
|
||||
b'\x9E\x87\x15\x00\x00\x00\x00\xF0',
|
||||
b'\xAF\x88\x15\x00\x00\x00\x00\xF0',
|
||||
b'\x35\x89\x15\x00\x00\x00\x00\xF0',
|
||||
b'\xC5\x8A\x15\x00\x00\x00\x00\xF0',
|
||||
b'\x11\x8B\x15\x00\x00\x00\x00\xF0',
|
||||
b'\xD0\x8C\x15\x00\x00\x00\x00\xF0',
|
||||
b'\xE8\x8D\x15\x00\x00\x00\x00\xF0',
|
||||
b'\xF5\x8E\x15\x00\x00\x00\x00\xF0',
|
||||
b'\x00\x8F\x15\x00\x00\x00\x00\xF0',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_esp_33(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "ESP_33", 0x1AB, [
|
||||
b'\x64\x00\x80\x02\x00\x00\x00\x00',
|
||||
b'\x19\x01\x00\x00\x00\x00\x00\x00',
|
||||
b'\xfc\x02\x00\x10\x01\x00\x00\x00',
|
||||
b'\x8b\x03\x80\x02\x00\x00\x00\x00',
|
||||
b'\xa4\x04\x00\x10\x01\x00\x00\x00',
|
||||
b'\x97\x05\x00\x02\x00\x00\x01\x00',
|
||||
b'\xd5\x06\x80\x02\x00\x00\x01\x00',
|
||||
b'\xa0\x07\x80\x02\x00\x00\x01\x00',
|
||||
b'\x89\x08\x00\x00\x00\x00\x00\x00',
|
||||
b'\xe3\x09\x00\x00\x00\x00\x00\x00',
|
||||
b'\x0e\x0a\x00\x00\x00\x00\x00\x00',
|
||||
b'\x90\x0b\x00\x00\x00\x00\x00\x00',
|
||||
b'\x32\x0c\x00\x10\x01\x00\x00\x00',
|
||||
b'\x30\x0d\x00\x00\x00\x00\x00\x00',
|
||||
b'\xc2\x0e\x00\x10\x01\x00\x00\x00',
|
||||
b'\x68\x0f\x80\x02\x00\x00\x00\x00',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_acc_02(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "ACC_02", 0x30C, [
|
||||
b'\x82\xf0\x3f\x00\x40\x30\x00\x40',
|
||||
b'\xe6\xf1\x3f\x00\x40\x30\x00\x40',
|
||||
b'\x4a\xf2\x3f\x00\x40\x30\x00\x40',
|
||||
b'\x2e\xf3\x3f\x00\x40\x30\x00\x40',
|
||||
b'\x3d\xf4\x3f\x00\x40\x30\x00\x40',
|
||||
b'\x59\xf5\x3f\x00\x40\x30\x00\x40',
|
||||
b'\xf5\xf6\x3f\x00\x40\x30\x00\x40',
|
||||
b'\x91\xf7\x3f\x00\x40\x30\x00\x40',
|
||||
b'\xd3\xf8\x3f\x00\x40\x30\x00\x40',
|
||||
b'\xb7\xf9\x3f\x00\x40\x30\x00\x40',
|
||||
b'\x1b\xfa\x3f\x00\x40\x30\x00\x40',
|
||||
b'\x7f\xfb\x3f\x00\x40\x30\x00\x40',
|
||||
b'\x6c\xfc\x3f\x00\x40\x30\x00\x40',
|
||||
b'\x08\xfd\x3f\x00\x40\x30\x00\x40',
|
||||
b'\xa4\xfe\x3f\x00\x40\x30\x00\x40',
|
||||
b'\xc0\xff\x3f\x00\x40\x30\x00\x40',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_swa_01(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "SWA_01", 0x30F, [
|
||||
b'\x10\x00\x10\x00\x00\x00\x00\x00',
|
||||
b'\x74\x01\x10\x00\x00\x00\x00\x00',
|
||||
b'\xD8\x02\x10\x00\x00\x00\x00\x00',
|
||||
b'\xBC\x03\x10\x00\x00\x00\x00\x00',
|
||||
b'\xAF\x04\x10\x00\x00\x00\x00\x00',
|
||||
b'\xCB\x05\x10\x00\x00\x00\x00\x00',
|
||||
b'\x67\x06\x10\x00\x00\x00\x00\x00',
|
||||
b'\x03\x07\x10\x00\x00\x00\x00\x00',
|
||||
b'\x41\x08\x10\x00\x00\x00\x00\x00',
|
||||
b'\x25\x09\x10\x00\x00\x00\x00\x00',
|
||||
b'\x89\x0A\x10\x00\x00\x00\x00\x00',
|
||||
b'\xED\x0B\x10\x00\x00\x00\x00\x00',
|
||||
b'\xFE\x0C\x10\x00\x00\x00\x00\x00',
|
||||
b'\x9A\x0D\x10\x00\x00\x00\x00\x00',
|
||||
b'\x36\x0E\x10\x00\x00\x00\x00\x00',
|
||||
b'\x52\x0F\x10\x00\x00\x00\x00\x00',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_acc_04(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "ACC_04", 0x324, [
|
||||
b'\xba\x00\x00\x00\x00\x00\x00\x10',
|
||||
b'\xde\x01\x00\x00\x00\x00\x00\x10',
|
||||
b'\x72\x02\x00\x00\x00\x00\x00\x10',
|
||||
b'\x16\x03\x00\x00\x00\x00\x00\x10',
|
||||
b'\x05\x04\x00\x00\x00\x00\x00\x10',
|
||||
b'\x44\x05\x00\x00\x00\x00\x00\x00',
|
||||
b'\xe8\x06\x00\x00\x00\x00\x00\x00',
|
||||
b'\xa9\x07\x00\x00\x00\x00\x00\x10',
|
||||
b'\xeb\x08\x00\x00\x00\x00\x00\x10',
|
||||
b'\x8f\x09\x00\x00\x00\x00\x00\x10',
|
||||
b'\x06\x0a\x00\x00\x00\x00\x00\x00',
|
||||
b'\x47\x0b\x00\x00\x00\x00\x00\x10',
|
||||
b'\x71\x0c\x00\x00\x00\x00\x00\x00',
|
||||
b'\x15\x0d\x00\x00\x00\x00\x00\x00',
|
||||
b'\xb9\x0e\x00\x00\x00\x00\x00\x00',
|
||||
b'\xdd\x0f\x00\x00\x00\x00\x00\x00',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_klemmen_status_01(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "Klemmen_Status_01", 0x3C0, [
|
||||
b'\x74\x00\x03\x00',
|
||||
b'\xc1\x01\x03\x00',
|
||||
b'\x31\x02\x03\x00',
|
||||
b'\x84\x03\x03\x00',
|
||||
b'\xfe\x04\x03\x00',
|
||||
b'\x4b\x05\x03\x00',
|
||||
b'\xbb\x06\x03\x00',
|
||||
b'\x0e\x07\x03\x00',
|
||||
b'\x4f\x08\x03\x00',
|
||||
b'\xfa\x09\x03\x00',
|
||||
b'\x0a\x0a\x03\x00',
|
||||
b'\xbf\x0b\x03\x00',
|
||||
b'\xc5\x0c\x03\x00',
|
||||
b'\x70\x0d\x03\x00',
|
||||
b'\x80\x0e\x03\x00',
|
||||
b'\x35\x0f\x03\x00',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_licht_anf_01(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "Licht_Anf_01", 0x3D5, [
|
||||
b'\xc8\x00\x00\x04\x00\x00\x00\x00',
|
||||
b'\x9f\x01\x00\x04\x00\x00\x00\x00',
|
||||
b'\x5e\x02\x00\x04\x00\x00\x00\x00',
|
||||
b'\x52\x03\x00\x04\x00\x00\x00\x00',
|
||||
b'\xf2\x04\x00\x04\x00\x00\x00\x00',
|
||||
b'\x79\x05\x00\x04\x00\x00\x00\x00',
|
||||
b'\xe6\x06\x00\x04\x00\x00\x00\x00',
|
||||
b'\xfd\x07\x00\x04\x00\x00\x00\x00',
|
||||
b'\xf8\x08\x00\x04\x00\x00\x00\x00',
|
||||
b'\xc6\x09\x00\x04\x00\x00\x00\x00',
|
||||
b'\xf5\x0a\x00\x04\x00\x00\x00\x00',
|
||||
b'\x1a\x0b\x00\x04\x00\x00\x00\x00',
|
||||
b'\x65\x0c\x00\x04\x00\x00\x00\x00',
|
||||
b'\x41\x0d\x00\x04\x00\x00\x00\x00',
|
||||
b'\x7f\x0e\x00\x04\x00\x00\x00\x00',
|
||||
b'\x98\x0f\x00\x04\x00\x00\x00\x00',
|
||||
])
|
||||
|
||||
def test_volkswagen_mqb_crc_esp_20(self, subtests):
|
||||
self.verify_volkswagen_mqb_crc(subtests, "ESP_20", 0x65D, [
|
||||
b'\x98\x30\x2b\x10\x00\x00\x22\x81',
|
||||
b'\xc8\x31\x2b\x10\x00\x00\x22\x81',
|
||||
b'\x9d\x32\x2b\x10\x00\x00\x22\x81',
|
||||
b'\x1f\x33\x2b\x10\x00\x00\x22\x81',
|
||||
b'\x6e\x34\x2b\x10\x00\x00\x22\x81',
|
||||
b'\x61\x35\x2b\x10\x00\x00\x22\x81',
|
||||
b'\x6f\x36\x2b\x10\x00\x00\x22\x81',
|
||||
b'\xe5\x37\x2b\x10\x00\x00\x22\x81',
|
||||
b'\xf8\x38\x2b\x10\x00\x00\x22\x81',
|
||||
b'\xe1\x39\x2b\x10\x00\x00\x22\x81',
|
||||
b'\xaa\x3a\x2b\x10\x00\x00\x22\x81',
|
||||
b'\xe6\x3b\x2b\x10\x00\x00\x22\x81',
|
||||
b'\xef\x3c\x2b\x10\x00\x00\x22\x81',
|
||||
b'\xbb\x3d\x2b\x10\x00\x00\x22\x81',
|
||||
b'\x9b\x3e\x2b\x10\x00\x00\x22\x81',
|
||||
b'\x72\x3f\x2b\x10\x00\x00\x22\x81',
|
||||
])
|
||||
@@ -0,0 +1,29 @@
|
||||
import pytest
|
||||
|
||||
from iqdbc.can import CANDefine, CANPacker, CANParser
|
||||
from iqdbc.can.tests import TEST_DBC
|
||||
|
||||
|
||||
class TestCanParserPackerExceptions:
|
||||
def test_civic_exceptions(self):
|
||||
dbc_file = "honda_civic_touring_2016_can_generated"
|
||||
dbc_invalid = dbc_file + "abcdef"
|
||||
msgs = [("STEERING_CONTROL", 50)]
|
||||
with pytest.raises(FileNotFoundError):
|
||||
CANParser(dbc_invalid, msgs, 0)
|
||||
with pytest.raises(FileNotFoundError):
|
||||
CANPacker(dbc_invalid)
|
||||
with pytest.raises(FileNotFoundError):
|
||||
CANDefine(dbc_invalid)
|
||||
with pytest.raises(KeyError):
|
||||
CANDefine(TEST_DBC)
|
||||
|
||||
parser = CANParser(dbc_file, msgs, 0)
|
||||
with pytest.raises(IndexError):
|
||||
parser.update([b''])
|
||||
|
||||
# Everything is supposed to work below
|
||||
CANParser(dbc_file, msgs, 0)
|
||||
CANParser(dbc_file, [], 0)
|
||||
CANPacker(dbc_file)
|
||||
CANDefine(dbc_file)
|
||||
@@ -0,0 +1,21 @@
|
||||
from iqdbc.can import CANParser
|
||||
from iqdbc.can.tests import ALL_DBCS
|
||||
|
||||
|
||||
class TestDBCParser:
|
||||
def test_enough_dbcs(self):
|
||||
# sanity check that we're running on the real DBCs
|
||||
assert len(ALL_DBCS) > 20
|
||||
|
||||
def test_parse_all_dbcs(self, subtests):
|
||||
"""
|
||||
Dynamic DBC parser checks:
|
||||
- Checksum and counter length, start bit, endianness
|
||||
- Duplicate message addresses and names
|
||||
- Signal out of bounds
|
||||
- All BO_, SG_, VAL_ lines for syntax errors
|
||||
"""
|
||||
|
||||
for dbc in ALL_DBCS:
|
||||
with subtests.test(dbc=dbc):
|
||||
CANParser(dbc, [], 0)
|
||||
@@ -0,0 +1,27 @@
|
||||
from iqdbc.can import CANDefine
|
||||
from iqdbc.can.tests import ALL_DBCS
|
||||
|
||||
|
||||
class TestCANDefine:
|
||||
def test_civic(self):
|
||||
|
||||
dbc_file = "honda_civic_touring_2016_can_generated"
|
||||
defs = CANDefine(dbc_file)
|
||||
|
||||
assert defs.dv[399] == defs.dv['STEER_STATUS']
|
||||
assert defs.dv[399] == {'STEER_STATUS':
|
||||
{7: 'PERMANENT_FAULT',
|
||||
6: 'TMP_FAULT',
|
||||
5: 'FAULT_1',
|
||||
4: 'NO_TORQUE_ALERT_2',
|
||||
3: 'LOW_SPEED_LOCKOUT',
|
||||
2: 'NO_TORQUE_ALERT_1',
|
||||
1: 'DRIVER_STEERING',
|
||||
0: 'NORMAL'}
|
||||
}
|
||||
|
||||
def test_all_dbcs(self, subtests):
|
||||
# Asserts no exceptions on all DBCs
|
||||
for dbc in ALL_DBCS:
|
||||
with subtests.test(dbc=dbc):
|
||||
CANDefine(dbc)
|
||||
@@ -0,0 +1,407 @@
|
||||
import pytest
|
||||
import random
|
||||
|
||||
from iqdbc.can import CANPacker, CANParser
|
||||
from iqdbc.can.tests import TEST_DBC
|
||||
|
||||
MAX_BAD_COUNTER = 5
|
||||
|
||||
|
||||
class TestCanParserPacker:
|
||||
def test_seen_addresses(self):
|
||||
parser = CANParser(TEST_DBC, [], 0)
|
||||
parser.update([0, [(0x123, b'\x00', 0), (0x124, b'\x00', 1)]])
|
||||
assert parser.seen_addresses == {0x123}
|
||||
parser.enable_capture = False
|
||||
parser.update([1, [(0x125, b'\x00', 0)]])
|
||||
assert parser.seen_addresses == {0x123}
|
||||
|
||||
def test_packer(self):
|
||||
packer = CANPacker(TEST_DBC)
|
||||
|
||||
for b in range(6):
|
||||
for i in range(256):
|
||||
values = {"COUNTER": i}
|
||||
addr, dat, bus = packer.make_can_msg("CAN_FD_MESSAGE", b, values)
|
||||
assert addr == 245
|
||||
assert bus == b
|
||||
assert dat[0] == i
|
||||
|
||||
def test_packer_counter(self):
|
||||
msgs = [("CAN_FD_MESSAGE", 0), ]
|
||||
packer = CANPacker(TEST_DBC)
|
||||
parser = CANParser(TEST_DBC, msgs, 0)
|
||||
|
||||
# packer should increment the counter
|
||||
for i in range(1000):
|
||||
msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {})
|
||||
parser.update([0, [msg]])
|
||||
assert parser.vl["CAN_FD_MESSAGE"]["COUNTER"] == (i % 256)
|
||||
|
||||
# setting COUNTER should override
|
||||
for _ in range(100):
|
||||
cnt = random.randint(0, 255)
|
||||
msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {
|
||||
"COUNTER": cnt,
|
||||
"SIGNED": 0
|
||||
})
|
||||
parser.update([0, [msg]])
|
||||
assert parser.vl["CAN_FD_MESSAGE"]["COUNTER"] == cnt
|
||||
|
||||
cnt = random.randint(0, 255)
|
||||
msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {"SIGNED": 0}, rx_counter=cnt)
|
||||
parser.update([0, [msg]])
|
||||
assert parser.vl["CAN_FD_MESSAGE"]["COUNTER"] == cnt
|
||||
|
||||
# then, should resume counting from the override value
|
||||
cnt = parser.vl["CAN_FD_MESSAGE"]["COUNTER"]
|
||||
for i in range(100):
|
||||
msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {})
|
||||
parser.update([0, [msg]])
|
||||
assert parser.vl["CAN_FD_MESSAGE"]["COUNTER"] == ((cnt + i) % 256)
|
||||
|
||||
def test_parser_can_valid(self):
|
||||
msgs = [("CAN_FD_MESSAGE", 10), ]
|
||||
packer = CANPacker(TEST_DBC)
|
||||
parser = CANParser(TEST_DBC, msgs, 0)
|
||||
|
||||
# shouldn't be valid initially
|
||||
assert not parser.can_valid
|
||||
|
||||
# not valid until the message is seen
|
||||
for _ in range(100):
|
||||
parser.update([0, []])
|
||||
assert not parser.can_valid
|
||||
|
||||
# valid once seen
|
||||
for i in range(1, 100):
|
||||
t = int(0.01 * i * 1e9)
|
||||
msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {})
|
||||
parser.update([t, [msg]])
|
||||
assert parser.can_valid
|
||||
|
||||
def test_lazy_add_not_ignore_alive(self):
|
||||
"""
|
||||
Accessing an undeclared message via parser.vl[...] lazily adds it via
|
||||
_add_message(key) with the default freq=None, which is NOT the same as
|
||||
declaring it with math.nan (ignore_alive=True). It's treated as "assume
|
||||
~1Hz, must be seen within ~10s" — so if that message is never fed, the
|
||||
parser is permanently invalid. Declaring an optional/rarely-sent message
|
||||
with math.nan (or gating the .vl[...] read entirely) is required to avoid
|
||||
this; see iqdbc/car/volkswagen/carstate.py's Diagnose_1/EPB_1 bugs.
|
||||
"""
|
||||
parser = CANParser(TEST_DBC, [], 0)
|
||||
assert parser.can_valid
|
||||
|
||||
# lazily add STEERING_CONTROL by reading it, without ever declaring it
|
||||
# or feeding any CAN data for it
|
||||
_ = parser.vl["STEERING_CONTROL"]
|
||||
state = parser.message_states[parser.dbc.name_to_msg["STEERING_CONTROL"].address]
|
||||
assert not state.ignore_alive
|
||||
|
||||
# never becomes valid again, no matter how many times it's checked
|
||||
# (can_valid debounces over MAX_BAD_COUNTER reads before flipping false)
|
||||
for _ in range(MAX_BAD_COUNTER):
|
||||
_ = parser.can_valid
|
||||
for _ in range(20):
|
||||
assert not parser.can_valid
|
||||
|
||||
def test_parser_updated_list(self):
|
||||
msgs = [("CAN_FD_MESSAGE", 10), ]
|
||||
parser = CANParser(TEST_DBC, msgs, 0)
|
||||
packer = CANPacker(TEST_DBC)
|
||||
|
||||
msg = packer.make_can_msg("CAN_FD_MESSAGE", 0, {})
|
||||
ret = parser.update([0, [msg]])
|
||||
assert ret == {245}
|
||||
|
||||
ret = parser.update([])
|
||||
assert len(ret) == 0
|
||||
|
||||
def test_parser_counter_can_valid(self):
|
||||
"""
|
||||
Tests number of allowed bad counters + ensures CAN stays invalid
|
||||
while receiving invalid messages + that we can recover
|
||||
"""
|
||||
msgs = [
|
||||
("STEERING_CONTROL", 0),
|
||||
]
|
||||
packer = CANPacker("honda_civic_touring_2016_can_generated")
|
||||
parser = CANParser("honda_civic_touring_2016_can_generated", msgs, 0)
|
||||
|
||||
msg = packer.make_can_msg("STEERING_CONTROL", 0, {"COUNTER": 0})
|
||||
|
||||
# bad static counter, invalid once it's seen MAX_BAD_COUNTER messages
|
||||
for idx in range(0x1000):
|
||||
parser.update([0, [msg]])
|
||||
assert ((idx + 1) < MAX_BAD_COUNTER) == parser.can_valid
|
||||
|
||||
# one to recover
|
||||
msg = packer.make_can_msg("STEERING_CONTROL", 0, {"COUNTER": 1})
|
||||
parser.update([0, [msg]])
|
||||
assert parser.can_valid
|
||||
|
||||
def test_parser_no_partial_update(self):
|
||||
"""
|
||||
Ensure that the CANParser doesn't partially update messages with invalid signals (COUNTER/CHECKSUM).
|
||||
Previously, the signal update loop would only break once it got to one of these invalid signals,
|
||||
after already updating most/all of the signals.
|
||||
"""
|
||||
msgs = [
|
||||
("STEERING_CONTROL", 0),
|
||||
]
|
||||
packer = CANPacker("honda_civic_touring_2016_can_generated")
|
||||
parser = CANParser("honda_civic_touring_2016_can_generated", msgs, 0)
|
||||
|
||||
def rx_steering_msg(values, bad_checksum=False):
|
||||
msg = packer.make_can_msg("STEERING_CONTROL", 0, values)
|
||||
if bad_checksum:
|
||||
# add 1 to checksum
|
||||
dat = bytearray(msg[1])
|
||||
dat[4] = (dat[4] & 0xF0) | ((dat[4] & 0x0F) + 1)
|
||||
msg = (msg[0], bytes(dat), msg[2])
|
||||
|
||||
parser.update([0, [msg]])
|
||||
|
||||
rx_steering_msg({"STEER_TORQUE": 100}, bad_checksum=False)
|
||||
assert parser.vl["STEERING_CONTROL"]["STEER_TORQUE"] == 100
|
||||
assert parser.vl_all["STEERING_CONTROL"]["STEER_TORQUE"] == [100]
|
||||
|
||||
for _ in range(5):
|
||||
rx_steering_msg({"STEER_TORQUE": 200}, bad_checksum=True)
|
||||
assert parser.vl["STEERING_CONTROL"]["STEER_TORQUE"] == 100
|
||||
assert parser.vl_all["STEERING_CONTROL"]["STEER_TORQUE"] == []
|
||||
|
||||
# Even if CANParser doesn't update instantaneous vl, make sure it didn't add invalid values to vl_all
|
||||
rx_steering_msg({"STEER_TORQUE": 300}, bad_checksum=False)
|
||||
assert parser.vl["STEERING_CONTROL"]["STEER_TORQUE"] == 300
|
||||
assert parser.vl_all["STEERING_CONTROL"]["STEER_TORQUE"] == [300]
|
||||
|
||||
def test_packer_parser(self):
|
||||
msgs = [
|
||||
("Brake_Status", 0),
|
||||
("CAN_FD_MESSAGE", 0),
|
||||
("STEERING_CONTROL", 0),
|
||||
]
|
||||
packer = CANPacker(TEST_DBC)
|
||||
parser = CANParser(TEST_DBC, msgs, 0)
|
||||
|
||||
for steer in range(-256, 255):
|
||||
for active in (1, 0):
|
||||
values = {
|
||||
"STEERING_CONTROL": {
|
||||
"STEER_TORQUE": steer,
|
||||
"STEER_TORQUE_REQUEST": active,
|
||||
},
|
||||
"Brake_Status": {
|
||||
"Signal1": 61042322657536.0,
|
||||
},
|
||||
"CAN_FD_MESSAGE": {
|
||||
"SIGNED": steer,
|
||||
"64_BIT_LE": random.randint(0, 100),
|
||||
"64_BIT_BE": random.randint(0, 100),
|
||||
},
|
||||
}
|
||||
|
||||
msgs = [packer.make_can_msg(k, 0, v) for k, v in values.items()]
|
||||
parser.update([0, msgs])
|
||||
|
||||
for k, v in values.items():
|
||||
for key, val in v.items():
|
||||
assert parser.vl[k][key] == pytest.approx(val)
|
||||
|
||||
# also check address
|
||||
for sig in ("STEER_TORQUE", "STEER_TORQUE_REQUEST", "COUNTER", "CHECKSUM"):
|
||||
assert parser.vl["STEERING_CONTROL"][sig] == parser.vl[228][sig]
|
||||
|
||||
def test_scale_offset(self):
|
||||
"""Test that both scale and offset are correctly preserved"""
|
||||
dbc_file = "honda_civic_touring_2016_can_generated"
|
||||
msgs = [("VSA_STATUS", 50)]
|
||||
parser = CANParser(dbc_file, msgs, 0)
|
||||
packer = CANPacker(dbc_file)
|
||||
|
||||
for brake in range(100):
|
||||
values = {"USER_BRAKE": brake}
|
||||
msgs = packer.make_can_msg("VSA_STATUS", 0, values)
|
||||
parser.update([0, [msgs]])
|
||||
|
||||
assert parser.vl["VSA_STATUS"]["USER_BRAKE"] == pytest.approx(brake)
|
||||
|
||||
def test_subaru(self):
|
||||
# Subaru is little endian
|
||||
|
||||
dbc_file = "subaru_global_2017_generated"
|
||||
|
||||
msgs = [("ES_LKAS", 50)]
|
||||
|
||||
parser = CANParser(dbc_file, msgs, 0)
|
||||
packer = CANPacker(dbc_file)
|
||||
|
||||
idx = 0
|
||||
for steer in range(-256, 255):
|
||||
for active in [1, 0]:
|
||||
values = {
|
||||
"LKAS_Output": steer,
|
||||
"LKAS_Request": active,
|
||||
"SET_1": 1
|
||||
}
|
||||
|
||||
msgs = packer.make_can_msg("ES_LKAS", 0, values)
|
||||
parser.update([0, [msgs]])
|
||||
|
||||
assert parser.vl["ES_LKAS"]["LKAS_Output"] == pytest.approx(steer)
|
||||
assert parser.vl["ES_LKAS"]["LKAS_Request"] == pytest.approx(active)
|
||||
assert parser.vl["ES_LKAS"]["SET_1"] == pytest.approx(1)
|
||||
assert parser.vl["ES_LKAS"]["COUNTER"] == pytest.approx(idx % 16)
|
||||
idx += 1
|
||||
|
||||
def test_bus_timeout(self):
|
||||
"""Test CAN bus timeout detection"""
|
||||
dbc_file = "honda_civic_touring_2016_can_generated"
|
||||
|
||||
freq = 100
|
||||
msgs = [("VSA_STATUS", freq), ("STEER_MOTOR_TORQUE", freq/2)]
|
||||
|
||||
parser = CANParser(dbc_file, msgs, 0)
|
||||
packer = CANPacker(dbc_file)
|
||||
|
||||
i = 0
|
||||
|
||||
def send_msg(blank=False):
|
||||
nonlocal i
|
||||
i += 1
|
||||
t = i*((1 / freq) * 1e9)
|
||||
|
||||
if blank:
|
||||
msgs = []
|
||||
else:
|
||||
msgs = [packer.make_can_msg("VSA_STATUS", 0, {}), ]
|
||||
|
||||
parser.update([t, msgs])
|
||||
|
||||
# all good, no timeout
|
||||
for _ in range(1000):
|
||||
send_msg()
|
||||
assert not parser.bus_timeout, str(_)
|
||||
|
||||
# timeout after 10 blank msgs
|
||||
for n in range(200):
|
||||
send_msg(blank=True)
|
||||
assert (n >= 10) == parser.bus_timeout
|
||||
|
||||
# no timeout immediately after seen again
|
||||
send_msg()
|
||||
assert not parser.bus_timeout
|
||||
|
||||
def test_updated(self):
|
||||
"""Test updated value dict"""
|
||||
dbc_file = "honda_civic_touring_2016_can_generated"
|
||||
msgs = [("VSA_STATUS", 50)]
|
||||
parser = CANParser(dbc_file, msgs, 0)
|
||||
packer = CANPacker(dbc_file)
|
||||
|
||||
# Make sure nothing is updated
|
||||
assert len(parser.vl_all["VSA_STATUS"]["USER_BRAKE"]) == 0
|
||||
|
||||
idx = 0
|
||||
for _ in range(10):
|
||||
# Ensure CANParser holds the values of any duplicate messages over multiple frames
|
||||
user_brake_vals = [random.randrange(100) for _ in range(random.randrange(5, 10))]
|
||||
half_idx = len(user_brake_vals) // 2
|
||||
can_msgs = [[], []]
|
||||
for frame, brake_vals in enumerate((user_brake_vals[:half_idx], user_brake_vals[half_idx:])):
|
||||
for user_brake in brake_vals:
|
||||
values = {"USER_BRAKE": user_brake}
|
||||
can_msgs[frame].append(packer.make_can_msg("VSA_STATUS", 0, values))
|
||||
idx += 1
|
||||
|
||||
parser.update([[0, m] for m in can_msgs])
|
||||
vl_all = parser.vl_all["VSA_STATUS"]["USER_BRAKE"]
|
||||
|
||||
assert vl_all == user_brake_vals
|
||||
if len(user_brake_vals):
|
||||
assert vl_all[-1] == parser.vl["VSA_STATUS"]["USER_BRAKE"]
|
||||
|
||||
def test_timestamp_nanos(self):
|
||||
"""Test message timestamp dict"""
|
||||
dbc_file = "honda_civic_touring_2016_can_generated"
|
||||
|
||||
msgs = [
|
||||
("VSA_STATUS", 50),
|
||||
("POWERTRAIN_DATA", 100),
|
||||
]
|
||||
|
||||
parser = CANParser(dbc_file, msgs, 0)
|
||||
packer = CANPacker(dbc_file)
|
||||
|
||||
# Check the default timestamp is zero
|
||||
for msg in ("VSA_STATUS", "POWERTRAIN_DATA"):
|
||||
ts_nanos = parser.ts_nanos[msg].values()
|
||||
assert set(ts_nanos) == {0}
|
||||
|
||||
# Check:
|
||||
# - timestamp is only updated for correct messages
|
||||
# - timestamp is correct for multiple runs
|
||||
# - timestamp is from the latest message if updating multiple strings
|
||||
for _ in range(10):
|
||||
can_strings = []
|
||||
log_mono_time = 0
|
||||
for i in range(10):
|
||||
log_mono_time = int(0.01 * i * 1e+9)
|
||||
can_msg = packer.make_can_msg("VSA_STATUS", 0, {})
|
||||
can_strings.append((log_mono_time, [can_msg]))
|
||||
parser.update(can_strings)
|
||||
|
||||
ts_nanos = parser.ts_nanos["VSA_STATUS"].values()
|
||||
assert set(ts_nanos) == {log_mono_time}
|
||||
ts_nanos = parser.ts_nanos["POWERTRAIN_DATA"].values()
|
||||
assert set(ts_nanos) == {0}
|
||||
|
||||
def test_nonexistent_messages(self):
|
||||
# Ensure we don't allow messages not in the DBC
|
||||
existing_messages = ("STEERING_CONTROL", 228, "CAN_FD_MESSAGE", 245)
|
||||
|
||||
for msg in existing_messages:
|
||||
CANParser(TEST_DBC, [(msg, 0)], 0)
|
||||
with pytest.raises(RuntimeError):
|
||||
new_msg = msg + "1" if isinstance(msg, str) else msg + 1
|
||||
CANParser(TEST_DBC, [(new_msg, 0)], 0)
|
||||
|
||||
def test_track_all_signals(self):
|
||||
parser = CANParser("toyota_nodsu_pt_generated", [("ACC_CONTROL", 0)], 0)
|
||||
assert parser.vl["ACC_CONTROL"] == {
|
||||
"ACCEL_CMD": 0,
|
||||
"ALLOW_LONG_PRESS": 0,
|
||||
"ACC_MALFUNCTION": 0,
|
||||
"RADAR_DIRTY": 0,
|
||||
"DISTANCE": 0,
|
||||
"MINI_CAR": 0,
|
||||
"ACC_TYPE": 0,
|
||||
"CANCEL_REQ": 0,
|
||||
"ACC_CUT_IN": 0,
|
||||
"LEAD_VEHICLE_STOPPED": 0,
|
||||
"PERMIT_BRAKING": 0,
|
||||
"RELEASE_STANDSTILL": 0,
|
||||
"ITS_CONNECT_LEAD": 0,
|
||||
"ACCEL_CMD_ALT": 0,
|
||||
"CHECKSUM": 0,
|
||||
}
|
||||
|
||||
def test_disallow_duplicate_messages(self):
|
||||
CANParser("toyota_nodsu_pt_generated", [("ACC_CONTROL", 5)], 0)
|
||||
|
||||
with pytest.raises(RuntimeError):
|
||||
CANParser("toyota_nodsu_pt_generated", [("ACC_CONTROL", 5), ("ACC_CONTROL", 10)], 0)
|
||||
|
||||
with pytest.raises(RuntimeError):
|
||||
CANParser("toyota_nodsu_pt_generated", [("ACC_CONTROL", 10), ("ACC_CONTROL", 10)], 0)
|
||||
|
||||
def test_allow_undefined_msgs(self):
|
||||
# TODO: we should throw an exception for these, but we need good
|
||||
# discovery tests in openpilot first
|
||||
packer = CANPacker("toyota_nodsu_pt_generated")
|
||||
|
||||
assert packer.make_can_msg("ACC_CONTROL", 0, {"UNKNOWN_SIGNAL": 0}) == (835, b'\x00\x00\x00\x00\x00\x00\x00N', 0)
|
||||
assert packer.make_can_msg("UNKNOWN_MESSAGE", 0, {"UNKNOWN_SIGNAL": 0}) == (0, b'', 0)
|
||||
assert packer.make_can_msg(0, 0, {"UNKNOWN_SIGNAL": 0}) == (0, b'', 0)
|
||||
252
artifacts/package_sources/iqdbc/iqdbc/car/__init__.py
Normal file
252
artifacts/package_sources/iqdbc/iqdbc/car/__init__.py
Normal file
@@ -0,0 +1,252 @@
|
||||
# functions common among cars
|
||||
import numpy as np
|
||||
from dataclasses import dataclass, field
|
||||
from enum import IntFlag, ReprEnum, StrEnum, EnumType, auto
|
||||
from dataclasses import replace
|
||||
|
||||
from iqdbc.car import structs, uds
|
||||
from iqdbc.car.can_definitions import CanData
|
||||
from iqdbc.car.docs_definitions import CarDocs, ExtraCarDocs
|
||||
|
||||
DT_CTRL = 0.01 # car state and control loop timestep (s)
|
||||
|
||||
# kg of standard extra cargo to count for drive, gas, etc...
|
||||
STD_CARGO_KG = 136.
|
||||
|
||||
ACCELERATION_DUE_TO_GRAVITY = 9.81 # m/s^2
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
def apply_hysteresis(val: float, val_steady: float, hyst_gap: float) -> float:
|
||||
if val > val_steady + hyst_gap:
|
||||
val_steady = val - hyst_gap
|
||||
elif val < val_steady - hyst_gap:
|
||||
val_steady = val + hyst_gap
|
||||
return val_steady
|
||||
|
||||
|
||||
def create_button_events(cur_btn: int, prev_btn: int, buttons_dict: dict[int, structs.CarState.ButtonEvent.Type],
|
||||
unpressed_btn: int = 0) -> list[structs.CarState.ButtonEvent]:
|
||||
events: list[structs.CarState.ButtonEvent] = []
|
||||
|
||||
if cur_btn == prev_btn:
|
||||
return events
|
||||
|
||||
# Add events for button presses, multiple when a button switches without going to unpressed
|
||||
for pressed, btn in ((False, prev_btn), (True, cur_btn)):
|
||||
if btn != unpressed_btn:
|
||||
events.append(structs.CarState.ButtonEvent(pressed=pressed,
|
||||
type=buttons_dict.get(btn, ButtonType.unknown)))
|
||||
return events
|
||||
|
||||
|
||||
def gen_empty_fingerprint():
|
||||
return {i: {} for i in range(8)}
|
||||
|
||||
|
||||
# these params were derived for the Civic and used to calculate params for other cars
|
||||
class VehicleDynamicsParams:
|
||||
MASS = 1326. + STD_CARGO_KG
|
||||
WHEELBASE = 2.70
|
||||
CENTER_TO_FRONT = WHEELBASE * 0.4
|
||||
CENTER_TO_REAR = WHEELBASE - CENTER_TO_FRONT
|
||||
ROTATIONAL_INERTIA = 2500
|
||||
TIRE_STIFFNESS_FRONT = 192150
|
||||
TIRE_STIFFNESS_REAR = 202500
|
||||
|
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for
|
||||
# civic and scaling by mass and wheelbase
|
||||
def scale_rot_inertia(mass, wheelbase):
|
||||
return VehicleDynamicsParams.ROTATIONAL_INERTIA * mass * wheelbase ** 2 / (VehicleDynamicsParams.MASS * VehicleDynamicsParams.WHEELBASE ** 2)
|
||||
|
||||
|
||||
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by
|
||||
# mass and CG position, so all cars will have approximately similar dyn behaviors
|
||||
def scale_tire_stiffness(mass, wheelbase, center_to_front, tire_stiffness_factor):
|
||||
center_to_rear = wheelbase - center_to_front
|
||||
tire_stiffness_front = (VehicleDynamicsParams.TIRE_STIFFNESS_FRONT * tire_stiffness_factor) * mass / VehicleDynamicsParams.MASS * \
|
||||
(center_to_rear / wheelbase) / (VehicleDynamicsParams.CENTER_TO_REAR / VehicleDynamicsParams.WHEELBASE)
|
||||
|
||||
tire_stiffness_rear = (VehicleDynamicsParams.TIRE_STIFFNESS_REAR * tire_stiffness_factor) * mass / VehicleDynamicsParams.MASS * \
|
||||
(center_to_front / wheelbase) / (VehicleDynamicsParams.CENTER_TO_FRONT / VehicleDynamicsParams.WHEELBASE)
|
||||
|
||||
return tire_stiffness_front, tire_stiffness_rear
|
||||
|
||||
|
||||
DbcDict = dict[StrEnum, str]
|
||||
|
||||
|
||||
class Bus(StrEnum):
|
||||
pt = auto()
|
||||
aux = auto()
|
||||
cam = auto()
|
||||
radar = auto()
|
||||
adas = auto()
|
||||
alt = auto()
|
||||
body = auto()
|
||||
chassis = auto()
|
||||
loopback = auto()
|
||||
main = auto()
|
||||
party = auto()
|
||||
ap_party = auto()
|
||||
|
||||
|
||||
def rate_limit(new_value, last_value, dw_step, up_step):
|
||||
return float(np.clip(new_value, last_value + dw_step, last_value + up_step))
|
||||
|
||||
|
||||
def make_tester_present_msg(addr, bus, subaddr=None, suppress_response=False):
|
||||
dat = [0x02, uds.SERVICE_TYPE.TESTER_PRESENT]
|
||||
if subaddr is not None:
|
||||
dat.insert(0, subaddr)
|
||||
dat.append(0x80 if suppress_response else 0x0) # sub-function
|
||||
|
||||
dat.extend([0x0] * (8 - len(dat)))
|
||||
return CanData(addr, bytes(dat), bus)
|
||||
|
||||
|
||||
def get_safety_config(safety_model: structs.CarParams.SafetyModel, safety_param: int | None = None) -> structs.CarParams.SafetyConfig:
|
||||
ret = structs.CarParams.SafetyConfig()
|
||||
ret.safetyModel = safety_model
|
||||
if safety_param is not None:
|
||||
ret.safetyParam = safety_param
|
||||
return ret
|
||||
|
||||
|
||||
class CanBusBase:
|
||||
offset: int
|
||||
|
||||
def __init__(self, CP, fingerprint: dict[int, dict[int, int]] | None) -> None:
|
||||
if CP is None:
|
||||
assert fingerprint is not None
|
||||
num = max([k for k, v in fingerprint.items() if len(v)], default=0) // 4 + 1
|
||||
else:
|
||||
num = len(CP.safetyConfigs)
|
||||
self.offset = 4 * (num - 1)
|
||||
|
||||
|
||||
class CanSignalRateCalculator:
|
||||
"""
|
||||
Calculates the instantaneous rate of a CAN signal by using the counter
|
||||
variable and the known frequency of the CAN message that contains it.
|
||||
"""
|
||||
def __init__(self, frequency: int):
|
||||
self.frequency = frequency
|
||||
self.previous_value = 0
|
||||
self.rate = 0
|
||||
|
||||
def update(self, current_value: float, updated: bool):
|
||||
if updated:
|
||||
self.rate = (current_value - self.previous_value) * self.frequency
|
||||
|
||||
self.previous_value = current_value
|
||||
|
||||
return self.rate
|
||||
|
||||
|
||||
@dataclass(frozen=True, kw_only=True)
|
||||
class CarSpecs:
|
||||
mass: float # kg, curb weight
|
||||
wheelbase: float # meters
|
||||
steerRatio: float
|
||||
centerToFrontRatio: float = 0.5
|
||||
minSteerSpeed: float = 0.0 # m/s
|
||||
minEnableSpeed: float = -1.0 # m/s
|
||||
tireStiffnessFactor: float = 1.0
|
||||
|
||||
def override(self, **kwargs):
|
||||
return replace(self, **kwargs)
|
||||
|
||||
|
||||
class Freezable:
|
||||
_frozen: bool = False
|
||||
|
||||
def freeze(self):
|
||||
if not self._frozen:
|
||||
self._frozen = True
|
||||
|
||||
def __setattr__(self, *args, **kwargs):
|
||||
if self._frozen:
|
||||
raise Exception("cannot modify frozen object")
|
||||
super().__setattr__(*args, **kwargs)
|
||||
|
||||
|
||||
@dataclass(order=True)
|
||||
class PlatformConfigBase(Freezable):
|
||||
car_docs: list[CarDocs] | list[ExtraCarDocs]
|
||||
specs: CarSpecs
|
||||
|
||||
dbc_dict: DbcDict
|
||||
|
||||
flags: int = 0
|
||||
iq_flags: int = 0
|
||||
|
||||
platform_str: str | None = None
|
||||
|
||||
origin_car_docs: list[CarDocs] | list[ExtraCarDocs] = field(init=False)
|
||||
|
||||
def __hash__(self) -> int:
|
||||
return hash(self.platform_str)
|
||||
|
||||
def override(self, **kwargs):
|
||||
return replace(self, **kwargs)
|
||||
|
||||
def init(self):
|
||||
pass
|
||||
|
||||
def __post_init__(self):
|
||||
self.origin_car_docs = self.car_docs
|
||||
self.init()
|
||||
|
||||
def get_all_docs(self):
|
||||
return self.origin_car_docs
|
||||
|
||||
|
||||
@dataclass(order=True)
|
||||
class PlatformConfig(PlatformConfigBase):
|
||||
car_docs: list[CarDocs]
|
||||
specs: CarSpecs
|
||||
dbc_dict: DbcDict
|
||||
|
||||
|
||||
@dataclass(order=True)
|
||||
class ExtraPlatformConfig(PlatformConfigBase):
|
||||
car_docs: list[ExtraCarDocs]
|
||||
specs: CarSpecs = CarSpecs(mass=0., wheelbase=0., steerRatio=0.)
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: dict())
|
||||
|
||||
|
||||
class PlatformsType(EnumType):
|
||||
def __new__(metacls, cls, bases, classdict, *, boundary=None, _simple=False, **kwds):
|
||||
for key in classdict._member_names.keys():
|
||||
cfg: PlatformConfig = classdict[key]
|
||||
cfg.platform_str = key
|
||||
cfg.freeze()
|
||||
return super().__new__(metacls, cls, bases, classdict, boundary=boundary, _simple=_simple, **kwds)
|
||||
|
||||
|
||||
class Platforms(str, ReprEnum, metaclass=PlatformsType):
|
||||
config: PlatformConfigBase
|
||||
|
||||
def __new__(cls, platform_config: PlatformConfig):
|
||||
member = str.__new__(cls, platform_config.platform_str)
|
||||
member.config = platform_config
|
||||
member._value_ = platform_config.platform_str
|
||||
return member
|
||||
|
||||
def __repr__(self):
|
||||
return f"<{self.__class__.__name__}.{self.name}>"
|
||||
|
||||
@classmethod
|
||||
def create_dbc_map(cls) -> dict[str, DbcDict]:
|
||||
return {p: p.config.dbc_dict for p in cls}
|
||||
|
||||
@classmethod
|
||||
def with_flags(cls, flags: IntFlag) -> set['Platforms']:
|
||||
return {p for p in cls if p.config.flags & flags}
|
||||
|
||||
@classmethod
|
||||
def with_iq_flags(cls, iq_flags: IntFlag) -> set['Platforms']:
|
||||
return {p for p in cls if p.config.iq_flags & iq_flags}
|
||||
17
artifacts/package_sources/iqdbc/iqdbc/car/body/bodycan.py
Normal file
17
artifacts/package_sources/iqdbc/iqdbc/car/body/bodycan.py
Normal file
@@ -0,0 +1,17 @@
|
||||
from iqdbc.car.crc import CRC8BODY
|
||||
|
||||
|
||||
def create_control(packer, torque_l, torque_r):
|
||||
values = {
|
||||
"TORQUE_L": torque_l,
|
||||
"TORQUE_R": torque_r,
|
||||
}
|
||||
|
||||
return packer.make_can_msg("TORQUE_CMD", 0, values)
|
||||
|
||||
|
||||
def body_checksum(address: int, sig, d: bytearray) -> int:
|
||||
crc = 0xFF
|
||||
for i in range(len(d) - 2, -1, -1):
|
||||
crc = CRC8BODY[crc ^ d[i]]
|
||||
return crc
|
||||
@@ -0,0 +1,82 @@
|
||||
import numpy as np
|
||||
|
||||
from iqdbc.can import CANPacker
|
||||
from iqdbc.car import Bus, DT_CTRL
|
||||
from iqdbc.car.common.pid import PIDController
|
||||
from iqdbc.car.body import bodycan
|
||||
from iqdbc.car.body.values import SPEED_FROM_RPM
|
||||
from iqdbc.car.interfaces import CarControllerBase
|
||||
|
||||
MAX_TORQUE = 500
|
||||
MAX_TORQUE_RATE = 50
|
||||
MAX_ANGLE_ERROR = np.radians(7)
|
||||
MAX_POS_INTEGRATOR = 0.2 # meters
|
||||
MAX_TURN_INTEGRATOR = 0.1 # meters
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_names, CP, CP_IQ):
|
||||
super().__init__(dbc_names, CP, CP_IQ)
|
||||
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.torque_r_filtered = 0.
|
||||
self.torque_l_filtered = 0.
|
||||
|
||||
@staticmethod
|
||||
def deadband_filter(torque, deadband):
|
||||
if torque > 0:
|
||||
torque += deadband
|
||||
else:
|
||||
torque -= deadband
|
||||
return torque
|
||||
|
||||
def update(self, CC, CC_IQ, CS, now_nanos):
|
||||
|
||||
torque_l = 0
|
||||
torque_r = 0
|
||||
|
||||
if CC.enabled:
|
||||
# Read these from the joystick
|
||||
# TODO: this isn't acceleration, okay?
|
||||
speed_desired = CC.actuators.accel / 5.
|
||||
speed_diff_desired = -CC.actuators.torque / 2.
|
||||
|
||||
speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2.
|
||||
speed_error = speed_desired - speed_measured
|
||||
|
||||
torque = self.wheeled_speed_pid.update(speed_error, freeze_integrator=False)
|
||||
|
||||
speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr)
|
||||
turn_error = speed_diff_measured - speed_diff_desired
|
||||
freeze_integrator = ((turn_error < 0 and self.turn_pid.error_integral <= -MAX_TURN_INTEGRATOR) or
|
||||
(turn_error > 0 and self.turn_pid.error_integral >= MAX_TURN_INTEGRATOR))
|
||||
torque_diff = self.turn_pid.update(turn_error, freeze_integrator=freeze_integrator)
|
||||
|
||||
# Combine 2 PIDs outputs
|
||||
torque_r = torque + torque_diff
|
||||
torque_l = torque - torque_diff
|
||||
|
||||
# Torque rate limits
|
||||
self.torque_r_filtered = np.clip(self.deadband_filter(torque_r, 10),
|
||||
self.torque_r_filtered - MAX_TORQUE_RATE,
|
||||
self.torque_r_filtered + MAX_TORQUE_RATE)
|
||||
self.torque_l_filtered = np.clip(self.deadband_filter(torque_l, 10),
|
||||
self.torque_l_filtered - MAX_TORQUE_RATE,
|
||||
self.torque_l_filtered + MAX_TORQUE_RATE)
|
||||
torque_r = int(np.clip(self.torque_r_filtered, -MAX_TORQUE, MAX_TORQUE))
|
||||
torque_l = int(np.clip(self.torque_l_filtered, -MAX_TORQUE, MAX_TORQUE))
|
||||
|
||||
can_sends = []
|
||||
can_sends.append(bodycan.create_control(self.packer, torque_l, torque_r))
|
||||
|
||||
new_actuators = CC.actuators.as_builder()
|
||||
new_actuators.accel = torque_l
|
||||
new_actuators.torque = torque_r
|
||||
new_actuators.torqueOutputCan = torque_r
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
36
artifacts/package_sources/iqdbc/iqdbc/car/body/carstate.py
Normal file
36
artifacts/package_sources/iqdbc/iqdbc/car/body/carstate.py
Normal file
@@ -0,0 +1,36 @@
|
||||
from iqdbc.can import CANParser
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.car.interfaces import CarStateBase
|
||||
from iqdbc.car.body.values import DBC
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
|
||||
cp = can_parsers[Bus.main]
|
||||
ret = structs.CarState()
|
||||
ret_iq = structs.IQCarState()
|
||||
|
||||
ret.wheelSpeeds.fl = cp.vl['MOTORS_DATA']['SPEED_L']
|
||||
ret.wheelSpeeds.fr = cp.vl['MOTORS_DATA']['SPEED_R']
|
||||
|
||||
ret.vEgoRaw = ((ret.wheelSpeeds.fl + ret.wheelSpeeds.fr) / 2.) * self.CP.wheelSpeedFactor
|
||||
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = False
|
||||
|
||||
ret.steerFaultPermanent = any([cp.vl['VAR_VALUES']['MOTOR_ERR_L'], cp.vl['VAR_VALUES']['MOTOR_ERR_R'],
|
||||
cp.vl['VAR_VALUES']['FAULT']])
|
||||
|
||||
ret.charging = cp.vl["BODY_DATA"]["CHARGER_CONNECTED"] == 1
|
||||
ret.fuelGauge = cp.vl["BODY_DATA"]["BATT_PERCENTAGE"] / 100
|
||||
|
||||
# irrelevant for non-car
|
||||
ret.gearShifter = structs.CarState.GearShifter.drive
|
||||
ret.cruiseState.enabled = True
|
||||
ret.cruiseState.available = True
|
||||
|
||||
return ret, ret_iq
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers(CP, CP_IQ):
|
||||
return {Bus.main: CANParser(DBC[CP.carFingerprint][Bus.main], [], 0)}
|
||||
@@ -0,0 +1,28 @@
|
||||
""" AUTO-FORMATTED USING iqdbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.car.body.values import CAR
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
# debug ecu fw version is the git hash of the firmware
|
||||
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.COMMA_BODY: [{
|
||||
513: 8, 516: 8, 514: 3, 515: 4
|
||||
}],
|
||||
}
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.COMMA_BODY: {
|
||||
(Ecu.engine, 0x720, None): [
|
||||
b'0.0.01',
|
||||
b'0.3.00a',
|
||||
b'02/27/2022',
|
||||
],
|
||||
(Ecu.debug, 0x721, None): [
|
||||
b'166bd860',
|
||||
b'dc780f85',
|
||||
],
|
||||
},
|
||||
}
|
||||
30
artifacts/package_sources/iqdbc/iqdbc/car/body/interface.py
Normal file
30
artifacts/package_sources/iqdbc/iqdbc/car/body/interface.py
Normal file
@@ -0,0 +1,30 @@
|
||||
import math
|
||||
from iqdbc.car import get_safety_config, structs
|
||||
from iqdbc.car.body.carcontroller import CarController
|
||||
from iqdbc.car.body.carstate import CarState
|
||||
from iqdbc.car.body.values import SPEED_FROM_RPM
|
||||
from iqdbc.car.interfaces import CarInterfaceBase
|
||||
|
||||
|
||||
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.notCar = True
|
||||
ret.brand = "body"
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.body)]
|
||||
|
||||
ret.minSteerSpeed = -math.inf
|
||||
ret.maxLateralAccel = math.inf # TODO: set to a reasonable value
|
||||
ret.steerLimitTimer = 1.0
|
||||
ret.steerActuatorDelay = 0.
|
||||
|
||||
ret.wheelSpeedFactor = SPEED_FROM_RPM
|
||||
|
||||
ret.radarUnavailable = True
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.steerControlType = structs.CarParams.SteerControlType.angle
|
||||
|
||||
return ret
|
||||
40
artifacts/package_sources/iqdbc/iqdbc/car/body/values.py
Normal file
40
artifacts/package_sources/iqdbc/iqdbc/car/body/values.py
Normal file
@@ -0,0 +1,40 @@
|
||||
from iqdbc.car import Bus, CarSpecs, PlatformConfig, Platforms
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.car.docs_definitions import CarDocs
|
||||
from iqdbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
SPEED_FROM_RPM = 0.008587
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
ANGLE_DELTA_BP = [0., 5., 15.]
|
||||
ANGLE_DELTA_V = [5., .8, .15] # windup limit
|
||||
ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit
|
||||
LKAS_MAX_TORQUE = 1 # A value of 1 is easy to overpower
|
||||
STEER_THRESHOLD = 1.0
|
||||
|
||||
def __init__(self, CP):
|
||||
pass
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
COMMA_BODY = PlatformConfig(
|
||||
[CarDocs("comma body", package="All", video="https://youtu.be/VT-i3yRsX2s?t=2736")],
|
||||
CarSpecs(mass=9, wheelbase=0.406, steerRatio=0.5, centerToFrontRatio=0.44),
|
||||
{Bus.main: 'comma_body'},
|
||||
)
|
||||
|
||||
|
||||
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()
|
||||
128
artifacts/package_sources/iqdbc/iqdbc/car/byd/bydcan.py
Normal file
128
artifacts/package_sources/iqdbc/iqdbc/car/byd/bydcan.py
Normal file
@@ -0,0 +1,128 @@
|
||||
# iqdbc/can/dbc.py imports byd_checksum from here, so this module must not import from
|
||||
# iqdbc.car (circular import at DBC parse time).
|
||||
|
||||
# Measured off the stock camera while it was actively steering (route 0000007b--88dd577c32):
|
||||
# every engaged 0x1E2 carries +500/-500 and byte5 = 0x64. 251/-252/0xFF came from Atto 3 notes.
|
||||
ANGLE_RATE_LIMIT_UPPER = 500
|
||||
ANGLE_RATE_LIMIT_LOWER = -500
|
||||
SET_ME_FF_VALUE = 0x64
|
||||
|
||||
# 0x316 LKAS_STATE, in the order the stock camera walks them on an engage
|
||||
LKAS_STATE_IDLE = 1
|
||||
LKAS_STATE_SUSPENDED = 2
|
||||
LKAS_STATE_ACTIVE = 3
|
||||
LKAS_STATE_PREPARING = 5
|
||||
|
||||
# COUNTER and CHECKSUM are filled in by the packer from the DBC signal types, so they are
|
||||
# stripped from any stock frame we pass through rather than inherited.
|
||||
_GENERATED = ("COUNTER", "CHECKSUM")
|
||||
|
||||
|
||||
def byd_checksum(address: int, sig, d: bytearray) -> int:
|
||||
return (~sum(d[:7])) & 0xFF
|
||||
|
||||
|
||||
def _passthrough(stock: dict) -> dict:
|
||||
return {k: v for k, v in stock.items() if k not in _GENERATED}
|
||||
|
||||
|
||||
def create_steering_control(packer, apply_angle: float, steer_req: bool):
|
||||
# The rate limits and STEER_REQ_ACTIVE_LOW do NOT track STEER_REQ. Measured on the stock
|
||||
# camera's idle frame (f4 31 c8 .. .. 64, 47625 samples): it holds +500/-500 and keeps
|
||||
# STEER_REQ_ACTIVE_LOW at 0 with STEER_REQ=0. Zeroing the limits and asserting ACTIVE_LOW while
|
||||
# idle parks the EPS in nibble 11 (LKS_PREPARED+CRUISE_ACTIVATED) permanently, a state the stock
|
||||
# camera never produces while steering.
|
||||
values = {
|
||||
"STEER_REQ": 1 if steer_req else 0,
|
||||
"STEER_REQ_ACTIVE_LOW": 0,
|
||||
"STEER_ANGLE": apply_angle,
|
||||
"ANGLE_RATE_LIMIT_UPPER": ANGLE_RATE_LIMIT_UPPER,
|
||||
"ANGLE_RATE_LIMIT_LOWER": ANGLE_RATE_LIMIT_LOWER,
|
||||
"E2E_ALIVE_1": 1,
|
||||
"E2E_ALIVE_2": 1,
|
||||
"SET_ME_FF": SET_ME_FF_VALUE,
|
||||
"SET_ME_F": 0xF,
|
||||
}
|
||||
return packer.make_can_msg("STEERING_MODULE_ADAS", 0, values)
|
||||
|
||||
|
||||
def create_lkas_hud(packer, lkas_state: int, lkas_active: bool, stock_lkas_hud: dict, hud_control):
|
||||
# The ADAS modules cross-check this frame's exact bit pattern every cycle and fail-safe on a
|
||||
# mismatch, so this reproduces the stock camera's frames field for field rather than
|
||||
# synthesizing them. Reference (dashcam-mode capture, camera driving ICC natively):
|
||||
# idle ACTIVE=0 PREPARE=0 LKAS_STATE=1 L=0 R=0 cf 84 00 e0 13 ff 71 49
|
||||
# preparing ACTIVE=0 PREPARE=0 LKAS_STATE=5 L=1 R=1 df 84 00 e0 57 ff 11 55
|
||||
# engaged ACTIVE=1 PREPARE=0 LKAS_STATE=3 L=1 R=1 df 84 00 f0 37 ff 61 15
|
||||
# suspended ACTIVE=0 PREPARE=1 LKAS_STATE=2 L=1 R=1 df 84 00 e8 27 ff 41 4d
|
||||
values = _passthrough(stock_lkas_hud)
|
||||
|
||||
# The camera's health fields are NOT passed through: with its 0x1E2 blocked by the relay it
|
||||
# can never actuate, so it permanently reports its own failure (TJA_ICA_STATE/MPCErr=2 with
|
||||
# LKAS_STATE=4) and forwarding that painted standing ADAS errors on the cluster.
|
||||
values["TJA_ICA_STATE"] = 0
|
||||
|
||||
values["LKAS_STATE"] = lkas_state
|
||||
values["LKAS_ACTIVE"] = 1 if lkas_active else 0
|
||||
# PREPARE accompanies exactly the suspend state: 9/9 stock suspend runs have it, 0 frames
|
||||
# anywhere else
|
||||
values["LKAS_REQ_PREPARE"] = 1 if lkas_state == LKAS_STATE_SUSPENDED else 0
|
||||
|
||||
# stock lights the lanes in every non-idle state, including preparing and suspend
|
||||
if lkas_state != LKAS_STATE_IDLE:
|
||||
values["LEFT_LANE_STATE"] = 1
|
||||
values["RIGHT_LANE_STATE"] = 1
|
||||
|
||||
if hud_control is not None:
|
||||
if hud_control.leftLaneDepart:
|
||||
values["LEFT_LANE_STATE"] = 2
|
||||
if hud_control.rightLaneDepart:
|
||||
values["RIGHT_LANE_STATE"] = 2
|
||||
|
||||
return packer.make_can_msg("LKAS_HUD_ADAS", 0, values)
|
||||
|
||||
|
||||
def create_acc_cmd(packer, accel: float, long_active: bool, stock_acc_cmd: dict,
|
||||
standstill: bool = False, resume: bool = False):
|
||||
# ACCEL_FACTOR/DECEL_FACTOR select the IPB gain profile: coast, soft accel, soft decel,
|
||||
# sustained brake. Pairs are stock's modal values per accel band.
|
||||
holding = long_active and standstill and not resume
|
||||
|
||||
if not long_active or abs(accel) < 0.1:
|
||||
accel_fac, decel_fac = 0, 0
|
||||
elif accel > 0:
|
||||
accel_fac, decel_fac = 12, 5
|
||||
elif accel > -1.5:
|
||||
accel_fac, decel_fac = 13, 1
|
||||
else:
|
||||
accel_fac, decel_fac = 1, 1
|
||||
|
||||
values = {
|
||||
**_passthrough(stock_acc_cmd),
|
||||
"ACCEL_CMD": accel if long_active else 0.0,
|
||||
"ACC_ON_1": 1 if long_active else 0,
|
||||
"ACC_ON_2": 1 if long_active else 0,
|
||||
"ACC_CONTROLLABLE_AND_ON": 1 if long_active else 0,
|
||||
"ACC_REQ_NOT_STANDSTILL": 0 if holding else (1 if long_active else 0),
|
||||
"CMD_REQ_ACTIVE_LOW": 0 if long_active else 1,
|
||||
"ACC_OVERRIDE_OR_STANDSTILL": 1 if holding else 0,
|
||||
"STANDSTILL_RESUME": 1 if (long_active and resume) else 0,
|
||||
"STANDSTILL_STATE": 1 if holding else 0,
|
||||
"ACCEL_FACTOR": accel_fac,
|
||||
"DECEL_FACTOR": decel_fac,
|
||||
"SET_ME_25_1": 25,
|
||||
"SET_ME_25_2": 25,
|
||||
"SET_ME_1": 1,
|
||||
"SET_ME_X8": 8,
|
||||
"SET_ME_XF": 15,
|
||||
}
|
||||
return packer.make_can_msg("ACC_CMD", 0, values)
|
||||
|
||||
|
||||
def create_buttons(packer, stock_buttons: dict, cancel: bool):
|
||||
values = {
|
||||
**_passthrough(stock_buttons),
|
||||
"SET_ME_1_1": 1,
|
||||
"SET_ME_1_2": 1,
|
||||
"ACC_ON_BTN": 1 if cancel else 0,
|
||||
}
|
||||
return packer.make_can_msg("PCM_BUTTONS", 0, values)
|
||||
113
artifacts/package_sources/iqdbc/iqdbc/car/byd/carcontroller.py
Normal file
113
artifacts/package_sources/iqdbc/iqdbc/car/byd/carcontroller.py
Normal file
@@ -0,0 +1,113 @@
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
from iqdbc.can.packer import CANPacker
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.car.lateral import apply_std_steer_angle_limits
|
||||
from iqdbc.car.interfaces import CarControllerBase
|
||||
from iqdbc.car.byd import bydcan
|
||||
from iqdbc.car.byd.carstate import EPS_STATE_LATCHED_FAULT
|
||||
from iqdbc.car.byd.values import CarControllerParams
|
||||
from iqdbc.car.vehicle_model import VehicleModel
|
||||
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
ACC_STEP = 3 # ~33 Hz
|
||||
ACC_DT = ACC_STEP * 0.01
|
||||
STEER_DT = 0.02 # STEER_STEP (2) at the 100 Hz base loop = 50 Hz command
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_names, CP, CP_IQ):
|
||||
super().__init__(dbc_names, CP, CP_IQ)
|
||||
self.packer = CANPacker(dbc_names[Bus.pt])
|
||||
self.apply_angle_last = 0.0
|
||||
self.accel_last = 0.0
|
||||
self.VM = VehicleModel(CP)
|
||||
self.hands_on_frames = 0
|
||||
self.resume_frames = 0
|
||||
self.angle_cmd_filtered = 0.0
|
||||
self.low_speed_latch = True
|
||||
self.tight_turn_latch = False
|
||||
|
||||
def update(self, CC, CC_IQ, CS, now_nanos):
|
||||
can_sends = []
|
||||
actuators = CC.actuators
|
||||
|
||||
# 0x1E2/0x316 go out unconditionally, gated only by STEER_REQ: the safety blocks the
|
||||
# camera's copies, and the EPS latches a fault if the stream stops while it is actuating.
|
||||
if self.frame % CarControllerParams.STEER_STEP == 0:
|
||||
# MINIMAL steering shape (bark bisect baseline). Only three things:
|
||||
# 1. std angle-rate limiter (flat speed-interpolated curve in ANGLE_LIMITS, max 85 deg so the
|
||||
# command never reaches the ~90 deg EPS fault),
|
||||
# 2. soft anti-windup clamp (command never sits more than MAX_ANGLE_ERROR from the wheel - this
|
||||
# IS the driver override: openpilot can never fight the driver by more than that), and
|
||||
# 3. fault-bail (drop REQ the instant the EPS trips to nibble 11 so a transient never latches).
|
||||
# Everything else (low-speed gate, tight-turn hand-off, divergence/lane-change yield) is
|
||||
# removed for now; re-add one at a time with a drive between each to find what caused the bark.
|
||||
faulting = CS.eps_state == EPS_STATE_LATCHED_FAULT
|
||||
steer_req = CC.latActive and not faulting
|
||||
|
||||
# 2 Hz 1-pole low-pass on the model's desired angle, filtered against its OWN state (not the
|
||||
# rate-limited output). Filtering against the output is fine only when the rate limiter never
|
||||
# binds; at our EPS-safe rate it does bind, and feeding the rate-limited value back into the
|
||||
# filter creates a feedback oscillation - the bark. Decoupling the filter from that feedback,
|
||||
# plus the actuator-delay anticipation so the rate-limited command arrives on time, is what
|
||||
# smooths the stronger turns. Snap to the wheel while not actuating so re-engage is clean.
|
||||
if steer_req:
|
||||
alpha_lp = math.exp(-2.0 * math.pi * CarControllerParams.STEER_LOWPASS_HZ * STEER_DT)
|
||||
self.angle_cmd_filtered = alpha_lp * self.angle_cmd_filtered + (1.0 - alpha_lp) * actuators.steeringAngleDeg
|
||||
else:
|
||||
self.angle_cmd_filtered = CS.out.steeringAngleDeg
|
||||
|
||||
apply_angle = apply_std_steer_angle_limits(self.angle_cmd_filtered, self.apply_angle_last,
|
||||
CS.out.vEgoRaw, CS.out.steeringAngleDeg,
|
||||
CC.latActive, CarControllerParams.ANGLE_LIMITS)
|
||||
if steer_req:
|
||||
apply_angle = float(np.clip(apply_angle, CS.out.steeringAngleDeg - CarControllerParams.MAX_ANGLE_ERROR,
|
||||
CS.out.steeringAngleDeg + CarControllerParams.MAX_ANGLE_ERROR))
|
||||
else:
|
||||
apply_angle = CS.out.steeringAngleDeg
|
||||
self.apply_angle_last = apply_angle
|
||||
|
||||
lkas_state = bydcan.LKAS_STATE_ACTIVE if steer_req else bydcan.LKAS_STATE_IDLE
|
||||
can_sends.append(bydcan.create_steering_control(self.packer, self.apply_angle_last, steer_req))
|
||||
can_sends.append(bydcan.create_lkas_hud(self.packer, lkas_state, steer_req,
|
||||
CS.lkas_hud, CC.hudControl))
|
||||
|
||||
accel = 0.0
|
||||
if self.CP.openpilotLongitudinalControl and self.frame % ACC_STEP == 0:
|
||||
if CC.longActive:
|
||||
accel = self._apply_long_limits(actuators, CS, CC)
|
||||
else:
|
||||
self.accel_last = float(np.clip(CS.out.aEgo, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
|
||||
lcs = actuators.longControlState
|
||||
stopping = (lcs == LongCtrlState.stopping) or (CS.out.standstill and accel <= 0.0)
|
||||
resume = (lcs == LongCtrlState.starting) or CC.cruiseControl.resume
|
||||
can_sends.append(bydcan.create_acc_cmd(self.packer, accel, CC.longActive, CS.acc_cmd,
|
||||
standstill=stopping and CS.out.standstill, resume=resume))
|
||||
|
||||
new_actuators = actuators.as_builder()
|
||||
new_actuators.steeringAngleDeg = float(self.apply_angle_last)
|
||||
new_actuators.accel = accel
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
||||
def _apply_long_limits(self, actuators, CS, CC) -> float:
|
||||
target = float(np.clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
|
||||
launch = CS.out.vEgo < 2.0 and target > 0.0
|
||||
up = (CarControllerParams.JERK_UP_LAUNCH if launch else CarControllerParams.JERK_UP) * ACC_DT
|
||||
down = CarControllerParams.JERK_DOWN * ACC_DT
|
||||
|
||||
# the hold parks the ramp at the stopping brake; snap to 0 so the launch kick applies
|
||||
# immediately instead of ramping back through the negative band while ESC-held
|
||||
resume = (actuators.longControlState == LongCtrlState.starting) or CC.cruiseControl.resume
|
||||
if resume and CS.out.standstill and self.accel_last < 0.0:
|
||||
self.accel_last = 0.0
|
||||
|
||||
accel = float(np.clip(target, self.accel_last - down, self.accel_last + up))
|
||||
self.accel_last = accel
|
||||
return accel
|
||||
152
artifacts/package_sources/iqdbc/iqdbc/car/byd/carstate.py
Normal file
152
artifacts/package_sources/iqdbc/iqdbc/car/byd/carstate.py
Normal file
@@ -0,0 +1,152 @@
|
||||
import copy
|
||||
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.can.parser import CANParser
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.byd.values import DBC, CarControllerParams as CCP
|
||||
from iqdbc.car.interfaces import CarStateBase
|
||||
|
||||
GearShifter = structs.CarState.GearShifter
|
||||
|
||||
GEAR_MAP = {
|
||||
1: GearShifter.park,
|
||||
2: GearShifter.reverse,
|
||||
3: GearShifter.neutral,
|
||||
4: GearShifter.drive,
|
||||
}
|
||||
|
||||
# STEERING_TORQUE low nibble, rebuilt from LKS_PREPARED + CRUISE_ACTIVATED
|
||||
EPS_STATE_OFF = 8
|
||||
EPS_STATE_PREPARED = 9
|
||||
EPS_STATE_ACTUATING = 10
|
||||
EPS_STATE_LATCHED_FAULT = 11
|
||||
|
||||
# ACC_HUD_ADAS.CRUISE_STATE
|
||||
CRUISE_STATE_OFF = 0
|
||||
CRUISE_STATE_AVAILABLE = 1
|
||||
CRUISE_STATE_ENGAGED = 2
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP, CP_IQ):
|
||||
super().__init__(CP, CP_IQ)
|
||||
self.lkas_hud = {}
|
||||
self.acc_cmd = {}
|
||||
self.buttons = {}
|
||||
self.eps_state = EPS_STATE_OFF
|
||||
self.eps_actuating = False
|
||||
self.override_latched = False
|
||||
self.lkas_btn_prev = False
|
||||
self.override_frames = 0
|
||||
self.release_frames = 0
|
||||
|
||||
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
ret = structs.CarState()
|
||||
ret_iq = structs.IQCarState()
|
||||
|
||||
ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["FL"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["FR"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["RL"] * CV.KPH_TO_MS
|
||||
ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["RR"] * CV.KPH_TO_MS
|
||||
self.parse_wheel_speeds(ret,
|
||||
cp.vl["WHEEL_SPEEDS"]["FL"],
|
||||
cp.vl["WHEEL_SPEEDS"]["FR"],
|
||||
cp.vl["WHEEL_SPEEDS"]["RL"],
|
||||
cp.vl["WHEEL_SPEEDS"]["RR"],
|
||||
)
|
||||
ret.standstill = ret.vEgoRaw < 0.01
|
||||
ret.vEgoCluster = ret.vEgo
|
||||
|
||||
ret.steeringAngleDeg = cp.vl["STEER_MODULE_2"]["STEER_ANGLE_2"]
|
||||
# DRIVER_EPS_TORQUE (STEER_MODULE_2 byte 2, raw 0-255) is the column torque sensor = clean
|
||||
# driver input. Verified on route ff: 0 hands-off even while openpilot steers, rising only on
|
||||
# real wheel input (route max 79). Unlike STEERING_TORQUE.DRIVER_TORQUE it does NOT read the tire
|
||||
# self-aligning load (which hit 85 Nm hands-off and made the torque override chatter at every
|
||||
# threshold). Threshold 80: normal driver turns on this sensor peak ~52, so 80 clears them.
|
||||
ret.steeringTorque = cp.vl["STEER_MODULE_2"]["DRIVER_EPS_TORQUE"]
|
||||
ret.steeringTorqueEps = cp.vl["STEERING_TORQUE"]["MAIN_TORQUE"]
|
||||
ret.steeringPressed = self.update_steering_pressed(ret.steeringTorque > CCP.STEER_DRIVER_OVERRIDE, 5)
|
||||
# Disengagement on override is handled by the latch below, which also decides when
|
||||
# re-engagement is allowed, so no separate hard-disengage threshold here.
|
||||
ret.steeringDisengage = False
|
||||
|
||||
# state 11 is a latched dropout: the command stream stopped while the EPS was actuating.
|
||||
# It clears only on a STEER_REQ rising edge over a continuous stream.
|
||||
lks_prepared = bool(cp.vl["STEERING_TORQUE"]["LKS_PREPARED"])
|
||||
cruise_activated = bool(cp.vl["STEERING_TORQUE"]["CRUISE_ACTIVATED"])
|
||||
self.eps_state = EPS_STATE_OFF + int(lks_prepared) + 2 * int(cruise_activated)
|
||||
# The EPS drives the wheel whenever CRUISE_ACTIVATED is set - that is nibble 10 AND 11.
|
||||
# Testing for nibble == 10 excluded the most common state and pinned the command to the
|
||||
# wheel there, leaving almost no steering authority.
|
||||
self.eps_actuating = cruise_activated
|
||||
|
||||
# Use the EPS's own fault bits. Neither of the conditions this used to test is a fault:
|
||||
# - nibble 11 (LKS_PREPARED + CRUISE_ACTIVATED) is the MOST COMMON operating state
|
||||
# (14714 samples, TORQUE_FAILED=0 in every one, CRUISE_STATE=2, openpilot active).
|
||||
# "state 11 = latched dropout" came from the Atto 3 notes and does not hold here.
|
||||
# - LKAS_STATE 4 is the camera's state when IT is not commanding LKAS, which is normal
|
||||
# while openpilot steers (1778 samples with openpilot active).
|
||||
# Reporting either as a fault produced constant phantom "Steering Assist Unavailable"
|
||||
# and, because we then refused to steer, blocked engagement.
|
||||
ret.steerFaultTemporary = bool(cp.vl["STEERING_TORQUE"]["TORQUE_TEMP_FAILED"])
|
||||
ret.steerFaultPermanent = bool(cp.vl["STEERING_TORQUE"]["TORQUE_FAILED"])
|
||||
|
||||
# DRIVE_STATE.RAW_THROTTLE is powertrain torque demand, not the pedal
|
||||
ret.gasPressed = cp.vl["PEDAL"]["GAS_PEDAL"] > 0.10
|
||||
ret.brake = cp.vl["PEDAL"]["BRAKE_PEDAL"]
|
||||
# must stay the same bit byd_rx_hook reads, or the two engage latches desync on a light
|
||||
# brake graze and controlsd raises "Controls Mismatch"
|
||||
ret.brakePressed = bool(cp.vl["DRIVE_STATE"]["BRAKE_PRESSED"])
|
||||
|
||||
ret.gearShifter = GEAR_MAP.get(int(cp.vl["DRIVE_STATE"]["GEAR"]), GearShifter.unknown)
|
||||
|
||||
ret.leftBlinker = bool(cp.vl["STALKS"]["LEFT_BLINKER"])
|
||||
ret.rightBlinker = bool(cp.vl["STALKS"]["RIGHT_BLINKER"])
|
||||
|
||||
ret.leftBlindspot = cp.vl["BSD_RADAR"]["LEFT_APPROACH"] != 0
|
||||
ret.rightBlindspot = cp.vl["BSD_RADAR"]["RIGHT_APPROACH"] != 0
|
||||
|
||||
ret.doorOpen = any((
|
||||
cp.vl["METER_CLUSTER"]["FRONT_LEFT_DOOR"],
|
||||
cp.vl["METER_CLUSTER"]["FRONT_RIGHT_DOOR"],
|
||||
cp.vl["METER_CLUSTER"]["BACK_LEFT_DOOR"],
|
||||
cp.vl["METER_CLUSTER"]["BACK_RIGHT_DOOR"],
|
||||
))
|
||||
ret.seatbeltUnlatched = not bool(cp.vl["METER_CLUSTER"]["SEATBELT_DRIVER"])
|
||||
|
||||
# The ADAS/ACC ECU is on the chassis bus, not behind the camera relay, so these come off
|
||||
# bus 0. Bus 2 carries only the camera's own frames (0x1E2, 0x316, ...). This differs from
|
||||
# the Atto 3, where PR #3337 reads both from the camera bus.
|
||||
# CRUISE_STATE: 0=off, 1=available, 2=engaged, 3=engaged and commanding accel.
|
||||
# Do NOT use PR #3337/#3352's ACC_STATE (19|3) - byte 2 is a constant 0x3c on this car, so
|
||||
# it reads 7 (ERROR) forever and engagement can never happen.
|
||||
ret.cruiseState.speed = cp.vl["ACC_HUD_ADAS"]["SET_SPEED"] * CV.KPH_TO_MS
|
||||
cruise_state = int(cp.vl["ACC_HUD_ADAS"]["CRUISE_STATE"])
|
||||
ret.cruiseState.available = cruise_state >= CRUISE_STATE_AVAILABLE
|
||||
|
||||
# cruiseState.enabled tracks the car's ACC engage bit directly (plain pcm_cruise), matching
|
||||
# the panda's controls_allowed. The old torque-based override latch that suppressed this was
|
||||
# the controlsMismatch source: the driver holding the wheel at ~15-19 Nm straddled the 18 Nm
|
||||
# threshold, so the latch flickered and dragged cruiseState.enabled True<->False while the
|
||||
# raw CRUISE_STATE sat steady at 2 - and python's and the panda's latches read the torque a
|
||||
# few frames apart, so they disagreed. Removed here AND in byd.h (acc_on = cruise_state>=2).
|
||||
# Driver override is now the standard path: openpilot yields on steeringPressed and the
|
||||
# carcontroller's TORQUE_BAIL drops REQ, without ever desyncing the engage state.
|
||||
self.override_latched = False
|
||||
ret.cruiseState.enabled = cruise_state >= CRUISE_STATE_ENGAGED
|
||||
ret.cruiseState.standstill = bool(cp.vl["ACC_CMD"]["STANDSTILL_STATE"])
|
||||
|
||||
self.lkas_hud = copy.copy(cp_cam.vl["LKAS_HUD_ADAS"])
|
||||
self.acc_cmd = copy.copy(cp.vl["ACC_CMD"])
|
||||
self.buttons = copy.copy(cp.vl["PCM_BUTTONS"])
|
||||
|
||||
return ret, ret_iq
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers(CP, CP_IQ):
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 0),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
|
||||
}
|
||||
@@ -0,0 +1,23 @@
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.car.byd.values import CAR
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
# Placeholder until a real car is captured in bring-up B2. Ecu.engine is essential, so a version
|
||||
# no car reports keeps the platform from ever exact-matching.
|
||||
#
|
||||
# Do NOT replace this with an empty ECU dict: match_fw_to_car_exact only invalidates a platform
|
||||
# when an expected version fails to match, so an empty dict leaves the platform a candidate for
|
||||
# every car on the road.
|
||||
#
|
||||
# Until populated, fingerprint explicitly: FINGERPRINT=BYD_SEALION_7
|
||||
#
|
||||
# ECUs answering 0xF195 on this platform: 0x704 MPC camera (non-essential), 0x782 brake/IPB,
|
||||
# 0x783 EPS, 0x7f2 forward radar, 0x7e0 drive unit, 0x7f1 SRS airbag.
|
||||
FW_VERSIONS = {
|
||||
CAR.BYD_SEALION_7: {
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'PLACEHOLDER_UNTIL_CAPTURED',
|
||||
],
|
||||
},
|
||||
}
|
||||
79
artifacts/package_sources/iqdbc/iqdbc/car/byd/interface.py
Normal file
79
artifacts/package_sources/iqdbc/iqdbc/car/byd/interface.py
Normal file
@@ -0,0 +1,79 @@
|
||||
from iqdbc.car import get_safety_config, structs
|
||||
from iqdbc.car.interfaces import CarInterfaceBase
|
||||
from iqdbc.car.byd.carcontroller import CarController
|
||||
from iqdbc.car.byd.carstate import CarState
|
||||
from iqdbc.car.byd.values import BydFlags, BydSafetyFlags
|
||||
|
||||
|
||||
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 = "byd"
|
||||
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.byd)]
|
||||
|
||||
ret.steerControlType = structs.CarParams.SteerControlType.angle
|
||||
# Near-zero actuator delay, paired with the fast rate curve and the 2 Hz command low-pass in the
|
||||
# carcontroller. This is a self-consistent regime for this EPS: react to NOW (no over-
|
||||
# anticipation), let the command move fast, and smooth it firmly. A high delay (0.35) with a
|
||||
# moderate rate was the bad middle that oscillated ("barking") on stronger turns - the model
|
||||
# anticipated but the rate could not keep up, so the phase never matched. Do not raise this
|
||||
# without also slowing the rate; the two are a set.
|
||||
ret.steerActuatorDelay = 0.35
|
||||
ret.steerLimitTimer = 0.4
|
||||
|
||||
# the Veoneer tracks live on a private CAN-FD pair that the BYD-6 harness jumpers straight
|
||||
# through, so the panda never sees them
|
||||
ret.radarUnavailable = True
|
||||
|
||||
# Two harness types exist for this car, and they differ in what can be filtered:
|
||||
#
|
||||
# camera harness - the relay only intercepts the MPC camera. 0x1E2/0x316 are camera
|
||||
# frames so lateral works, but the ADAS/ACC ECU sits on the chassis bus
|
||||
# in front of the relay: its 0x32E cannot be blocked and openpilot would
|
||||
# contend with the stock ACC on the same address. Stock long only.
|
||||
# gateway harness - the ACC ECU is behind the relay, so 0x32E is filterable and openpilot
|
||||
# longitudinal is possible.
|
||||
#
|
||||
# These CANNOT be told apart from the fingerprint: fingerprinting runs with the relay
|
||||
# closed, which ties bus 2 to bus 0, so bus 2 shows the whole car on either harness. The
|
||||
# difference is only observable once the relay opens, which is after CarParams is fixed.
|
||||
# Measured on a camera harness with the relay open: bus 2 carries 11 camera addresses and
|
||||
# neither 0x32D nor 0x32E is among them.
|
||||
#
|
||||
# So default to the camera harness and keep longitudinal off. Setting GATEWAY_HARNESS is an
|
||||
# explicit opt-in that must not be inferred - see BYD_SEALION7_PORT_PLAN.md.
|
||||
gateway_harness = bool(ret.flags & BydFlags.GATEWAY_HARNESS)
|
||||
|
||||
ret.alphaLongitudinalAvailable = gateway_harness
|
||||
if alpha_long and gateway_harness:
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.safetyConfigs[0].safetyParam |= BydSafetyFlags.LONG_CONTROL.value
|
||||
|
||||
ret.longitudinalActuatorDelay = 0.5 # the IPB closes its own loop
|
||||
ret.vEgoStarting = 0.3
|
||||
ret.stopAccel = -0.5
|
||||
ret.startAccel = 1.5
|
||||
# without this longcontrol goes stopping -> pid directly and neither the startAccel kick
|
||||
# nor the STANDSTILL_RESUME pulse ever fires
|
||||
ret.startingState = True
|
||||
|
||||
# ACCEL_CMD is feedforward; high feedback gain on a 0.5s-lag actuator winds up the
|
||||
# integrator before the car responds
|
||||
ret.longitudinalTuning.kpBP = [0.0, 5.0, 35.0]
|
||||
ret.longitudinalTuning.kpV = [0.5, 0.4, 0.3]
|
||||
ret.longitudinalTuning.kiBP = [0.0, 35.0]
|
||||
ret.longitudinalTuning.kiV = [0.03, 0.02]
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def _get_params_iq(stock_cp: structs.CarParams, ret: structs.IQCarParams, candidate, fingerprint,
|
||||
car_fw, alpha_long: bool, is_release_iq: bool, docs: bool) -> structs.IQCarParams:
|
||||
if stock_cp.openpilotLongitudinalControl:
|
||||
ret.longitudinalStoppingSpeedOverride = 0.3
|
||||
|
||||
return ret
|
||||
536
artifacts/package_sources/iqdbc/iqdbc/car/byd/tests/test_byd.py
Normal file
536
artifacts/package_sources/iqdbc/iqdbc/car/byd/tests/test_byd.py
Normal file
@@ -0,0 +1,536 @@
|
||||
#!/usr/bin/env python3
|
||||
import unittest
|
||||
|
||||
import numpy as np
|
||||
|
||||
from iqdbc.can.packer import CANPacker
|
||||
from iqdbc.can.parser import CANParser
|
||||
from iqdbc.car.byd import bydcan
|
||||
from iqdbc.car.byd.carstate import (EPS_STATE_OFF, EPS_STATE_PREPARED, EPS_STATE_ACTUATING,
|
||||
EPS_STATE_LATCHED_FAULT)
|
||||
from iqdbc.car.byd.fingerprints import FW_VERSIONS
|
||||
from iqdbc.car.byd.interface import CarInterface
|
||||
from iqdbc.car.byd.values import CAR, DBC, BydFlags, BydSafetyFlags, CarControllerParams
|
||||
from iqdbc.car.fw_versions import match_fw_to_car_exact, build_fw_dict
|
||||
from iqdbc.car import structs
|
||||
from iqdbc.car.structs import CarParams
|
||||
|
||||
DBC_NAME = DBC[CAR.BYD_SEALION_7]['pt']
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
|
||||
def _unpack(dbc_name, msg_name, dat):
|
||||
"""Decode one frame with the DBC, bypassing the parser's liveness tracking."""
|
||||
dbc = CANParser(dbc_name, [], 0).dbc
|
||||
msg = dbc.name_to_msg[msg_name]
|
||||
out = {}
|
||||
for sig in msg.sigs.values():
|
||||
val = 0
|
||||
if sig.is_little_endian:
|
||||
for i in range(sig.size):
|
||||
bit = sig.lsb + i
|
||||
val |= ((dat[bit // 8] >> (bit % 8)) & 1) << i
|
||||
else:
|
||||
be_bits = [j + i * 8 for i in range(64) for j in range(7, -1, -1)]
|
||||
idx = be_bits.index(sig.start_bit)
|
||||
for i in range(sig.size):
|
||||
bit = be_bits[idx + i]
|
||||
val = (val << 1) | ((dat[bit // 8] >> (bit % 8)) & 1)
|
||||
if sig.is_signed and (val & (1 << (sig.size - 1))):
|
||||
val -= (1 << sig.size)
|
||||
out[sig.name] = val * sig.factor + sig.offset
|
||||
return out
|
||||
|
||||
|
||||
class TestBydChecksum(unittest.TestCase):
|
||||
def test_checksum_is_inverted_sum(self):
|
||||
for dat in (bytearray(8), bytearray(b'\x01' * 8), bytearray(b'\xff' * 8),
|
||||
bytearray(b'\x12\x34\x56\x78\x9a\xbc\xde\x00')):
|
||||
self.assertEqual(bydcan.byd_checksum(0, None, dat), (~sum(dat[:7])) & 0xFF)
|
||||
|
||||
def test_packer_fills_checksum_and_counter(self):
|
||||
packer = CANPacker(DBC_NAME)
|
||||
seen = []
|
||||
for _ in range(18):
|
||||
_, dat, _ = packer.make_can_msg("STEERING_MODULE_ADAS", 0, {"STEER_REQ": 1})
|
||||
self.assertEqual(dat[7], (~sum(dat[:7])) & 0xFF, "checksum not filled by the DBC layer")
|
||||
seen.append(dat[6] >> 4) # COUNTER is 55|4@0
|
||||
# rolls 0..15 and wraps, never repeating within a cycle
|
||||
self.assertEqual(seen[:16], list(range(16)))
|
||||
self.assertEqual(seen[16:], [0, 1])
|
||||
|
||||
|
||||
class TestBydSteeringControl(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.packer = CANPacker(DBC_NAME)
|
||||
|
||||
def test_steer_req_and_angle_round_trip(self):
|
||||
for angle in (-390.0, -100.5, 0.0, 12.3, 390.0):
|
||||
for lat_active in (True, False):
|
||||
_, dat, _ = bydcan.create_steering_control(self.packer, angle, lat_active)
|
||||
vals = _unpack(DBC_NAME, "STEERING_MODULE_ADAS", dat)
|
||||
self.assertAlmostEqual(vals["STEER_ANGLE"], angle, places=4)
|
||||
self.assertEqual(vals["STEER_REQ"], 1 if lat_active else 0)
|
||||
# despite the name, the stock camera never inverts this - 0 in every engaged frame and
|
||||
# in 47625 of 47660 idle frames
|
||||
self.assertEqual(vals["STEER_REQ_ACTIVE_LOW"], 0)
|
||||
self.assertEqual(vals["E2E_ALIVE_1"], 1)
|
||||
self.assertEqual(vals["E2E_ALIVE_2"], 1)
|
||||
|
||||
def test_rate_limits_held_when_inactive(self):
|
||||
# The camera holds +500/-500 with STEER_REQ=0 (idle frame f4 31 c8 .. .. 64). Zeroing them
|
||||
# while idle parked the EPS in nibble 11 permanently.
|
||||
for lat_active in (True, False):
|
||||
_, dat, _ = bydcan.create_steering_control(self.packer, 0.0, lat_active)
|
||||
vals = _unpack(DBC_NAME, "STEERING_MODULE_ADAS", dat)
|
||||
self.assertEqual(vals["ANGLE_RATE_LIMIT_UPPER"], bydcan.ANGLE_RATE_LIMIT_UPPER)
|
||||
self.assertEqual(vals["ANGLE_RATE_LIMIT_LOWER"], bydcan.ANGLE_RATE_LIMIT_LOWER)
|
||||
|
||||
def test_matches_stock_camera_idle_frame(self):
|
||||
# stock idle: f4 31 c8 .. .. 64 (47625 samples); only the angle bytes differ
|
||||
_, dat, _ = bydcan.create_steering_control(self.packer, 0.0, False)
|
||||
self.assertEqual(bytes(dat[:3]), bytes.fromhex("f431c8"),
|
||||
f"idle 0x1E2 diverges from stock: {bytes(dat[:3]).hex(' ')} != f4 31 c8")
|
||||
self.assertEqual(dat[5], 0x64)
|
||||
|
||||
def test_matches_stock_camera_engaged_frame(self):
|
||||
# Byte-for-byte against a frame the stock camera actually sent while steering, captured in
|
||||
# dashcam mode (route 0000007b--88dd577c32): f4 31 e8 01 00 64 9f ee at +0.1 deg.
|
||||
# Only COUNTER/CHECKSUM (byte 6 high nibble, byte 7) may differ.
|
||||
OEM = bytes.fromhex("f431e8010064")
|
||||
_, dat, _ = bydcan.create_steering_control(self.packer, 0.1, True)
|
||||
self.assertEqual(bytes(dat[:6]), OEM,
|
||||
f"0x1E2 diverges from stock: {bytes(dat[:6]).hex(' ')} != {OEM.hex(' ')}")
|
||||
self.assertEqual(dat[6] & 0x0F, 0x0F)
|
||||
|
||||
def test_rate_limits_and_set_me_match_stock(self):
|
||||
# 251/-252/0xFF came from the Atto 3 notes; this car's camera sends 500/-500/0x64
|
||||
self.assertEqual(bydcan.ANGLE_RATE_LIMIT_UPPER, 500)
|
||||
self.assertEqual(bydcan.ANGLE_RATE_LIMIT_LOWER, -500)
|
||||
self.assertEqual(bydcan.SET_ME_FF_VALUE, 0x64)
|
||||
_, dat, _ = bydcan.create_steering_control(self.packer, 0.0, True)
|
||||
self.assertEqual(_unpack(DBC_NAME, "STEERING_MODULE_ADAS", dat)["SET_ME_FF"], 0x64)
|
||||
|
||||
|
||||
class TestBydAngleEnvelope(unittest.TestCase):
|
||||
"""Guards the command envelope against the stock camera's measured behaviour."""
|
||||
|
||||
def test_clamp_is_below_bail(self):
|
||||
# The clamp holds the command within MAX_ANGLE_ERROR of the wheel; the bail drops STEER_REQ
|
||||
# above ANGLE_ERROR_BAIL. If the clamp permits what the bail punishes, every firm turn
|
||||
# cuts steering and chatters - that shipped as 6.0/5.0 and broke cornering on-car.
|
||||
self.assertLess(CarControllerParams.MAX_ANGLE_ERROR, CarControllerParams.ANGLE_ERROR_BAIL)
|
||||
self.assertLess(CarControllerParams.MAX_ANGLE_ERROR_INACTIVE,
|
||||
CarControllerParams.MAX_ANGLE_ERROR)
|
||||
|
||||
def test_envelope_covers_stock_tracking_error(self):
|
||||
# stock ICC engaged: p99 3.31, p99.9 10.61, max 19.70 deg of |commanded - measured|
|
||||
self.assertGreater(CarControllerParams.MAX_ANGLE_ERROR, 10.61)
|
||||
self.assertGreater(CarControllerParams.ANGLE_ERROR_BAIL, 19.70)
|
||||
|
||||
def test_command_rate_stays_within_stock(self):
|
||||
# stock never steps its own command more than 1.30 deg/frame
|
||||
self.assertLessEqual(max(CarControllerParams.ANGLE_RATE_V),
|
||||
CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE)
|
||||
self.assertLess(CarControllerParams.ANGLE_LIMITS.MAX_ANGLE_RATE, 3)
|
||||
|
||||
|
||||
class TestBydLkasHud(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.packer = CANPacker(DBC_NAME)
|
||||
# a stock frame with bits set in every field we touch and several we must not
|
||||
self.stock = {
|
||||
"HMA_STATE": 3, "LEFT_LANE_STATE": 1, "LKS_MODE": 2, "HANDS_ON_WHEEL_REQ": 1,
|
||||
"TJA_ICA_STATE": 5, "HMA_ON_OFF": 1, "LKAS_OUTPUT": -20, "LKAS_REQ_PREPARE": 1,
|
||||
"LKAS_ACTIVE": 1, "SLA_STATE": 3, "RIGHT_LANE_STATE": 1, "LKAS_STATE": 0b1000,
|
||||
"SPEED_LIMIT_VALUE": 100, "LDSW_TYPE": 2, "COUNTER": 9, "CHECKSUM": 0x11,
|
||||
}
|
||||
|
||||
def test_passes_stock_bits_through(self):
|
||||
# The ADAS modules cross-check this frame; every bit we do not own must survive.
|
||||
_, dat, _ = bydcan.create_lkas_hud(self.packer, bydcan.LKAS_STATE_IDLE, False, self.stock, None)
|
||||
vals = _unpack(DBC_NAME, "LKAS_HUD_ADAS", dat)
|
||||
for name in ("HMA_STATE", "LKS_MODE", "HANDS_ON_WHEEL_REQ", "HMA_ON_OFF",
|
||||
"LKAS_OUTPUT", "SLA_STATE",
|
||||
"SPEED_LIMIT_VALUE", "LDSW_TYPE"):
|
||||
self.assertEqual(vals[name], self.stock[name], f"{name} was modified")
|
||||
|
||||
def test_hands_on_wheel_req_never_cleared(self):
|
||||
for state, active in ((bydcan.LKAS_STATE_ACTIVE, True), (bydcan.LKAS_STATE_IDLE, False)):
|
||||
_, dat, _ = bydcan.create_lkas_hud(self.packer, state, active, self.stock, None)
|
||||
vals = _unpack(DBC_NAME, "LKAS_HUD_ADAS", dat)
|
||||
self.assertEqual(vals["HANDS_ON_WHEEL_REQ"], 1)
|
||||
|
||||
def test_camera_error_state_never_forwarded(self):
|
||||
# camera reports its own failure (TJA_ICA_STATE=2, LKAS_STATE=4) because its 0x1E2 is
|
||||
# blocked; forwarding that paints standing ADAS errors on the cluster
|
||||
err_stock = dict(self.stock, TJA_ICA_STATE=2, LKAS_STATE=4)
|
||||
for state, active in ((bydcan.LKAS_STATE_ACTIVE, True), (bydcan.LKAS_STATE_PREPARING, False),
|
||||
(bydcan.LKAS_STATE_IDLE, False)):
|
||||
_, dat, _ = bydcan.create_lkas_hud(self.packer, state, active, err_stock, None)
|
||||
vals = _unpack(DBC_NAME, "LKAS_HUD_ADAS", dat)
|
||||
self.assertEqual(int(vals["TJA_ICA_STATE"]), 0)
|
||||
self.assertEqual(int(vals["LKAS_STATE"]), state)
|
||||
|
||||
def test_matches_stock_camera_frames(self):
|
||||
# Byte-for-byte against the camera's own frames (route 0000007b--88dd577c32).
|
||||
# SPEED_LIMIT_VALUE is (5, -5), so the stock 0xff raw byte is 1270 kph (no limit / SNA)
|
||||
stock = dict(self.stock, HMA_STATE=15, LKS_MODE=3, HANDS_ON_WHEEL_REQ=1, HMA_ON_OFF=1,
|
||||
LKAS_OUTPUT=0, SLA_STATE=7, SPEED_LIMIT_VALUE=1270, LDSW_TYPE=1, SET_ME_3=3)
|
||||
for state, active, oem in (
|
||||
(bydcan.LKAS_STATE_ACTIVE, True, "df8400f037ff"), # engaged
|
||||
(bydcan.LKAS_STATE_PREPARING, False, "df8400e057ff"), # preparing
|
||||
(bydcan.LKAS_STATE_SUSPENDED, False, "df8400e827ff"), # suspend after override
|
||||
):
|
||||
_, dat, _ = bydcan.create_lkas_hud(self.packer, state, active, stock, None)
|
||||
self.assertEqual(bytes(dat[:6]), bytes.fromhex(oem),
|
||||
f"state {state} 0x316 diverges from stock: {bytes(dat[:6]).hex(' ')} != {oem}")
|
||||
# byte 6 low nibble is LDSW_TYPE (stock 1); the high nibble is our own COUNTER
|
||||
self.assertEqual(dat[6] & 0x0F, 0x01)
|
||||
|
||||
def test_prepare_bit_accompanies_exactly_the_suspend_state(self):
|
||||
# 9/9 stock suspend runs have PREPARE=1; 0 frames anywhere else (stock passes it as 1 here
|
||||
# to prove we do not inherit it outside suspend)
|
||||
for state, active, expected in ((bydcan.LKAS_STATE_IDLE, False, 0),
|
||||
(bydcan.LKAS_STATE_PREPARING, False, 0),
|
||||
(bydcan.LKAS_STATE_ACTIVE, True, 0),
|
||||
(bydcan.LKAS_STATE_SUSPENDED, False, 1)):
|
||||
_, dat, _ = bydcan.create_lkas_hud(self.packer, state, active, self.stock, None)
|
||||
vals = _unpack(DBC_NAME, "LKAS_HUD_ADAS", dat)
|
||||
self.assertEqual(int(vals["LKAS_REQ_PREPARE"]), expected)
|
||||
|
||||
def test_lane_bits_lit_in_every_non_idle_state(self):
|
||||
stock = dict(self.stock, LEFT_LANE_STATE=0, RIGHT_LANE_STATE=0)
|
||||
for state, active in ((bydcan.LKAS_STATE_PREPARING, False), (bydcan.LKAS_STATE_ACTIVE, True),
|
||||
(bydcan.LKAS_STATE_SUSPENDED, False)):
|
||||
_, dat, _ = bydcan.create_lkas_hud(self.packer, state, active, stock, None)
|
||||
vals = _unpack(DBC_NAME, "LKAS_HUD_ADAS", dat)
|
||||
# stock uses 1, not a bitmask - 2 and 3 are never observed on an engaged frame
|
||||
self.assertEqual(int(vals["LEFT_LANE_STATE"]), 1)
|
||||
self.assertEqual(int(vals["RIGHT_LANE_STATE"]), 1)
|
||||
|
||||
def test_counter_not_inherited_from_stock(self):
|
||||
# inheriting the camera's counter would make our 50 Hz stream non-monotonic
|
||||
counters = []
|
||||
for _ in range(4):
|
||||
_, dat, _ = bydcan.create_lkas_hud(self.packer, bydcan.LKAS_STATE_ACTIVE, True, self.stock, None)
|
||||
counters.append(int(_unpack(DBC_NAME, "LKAS_HUD_ADAS", dat)["COUNTER"]))
|
||||
self.assertNotEqual(counters, [self.stock["COUNTER"]] * 4)
|
||||
self.assertEqual(counters, [(counters[0] + i) % 16 for i in range(4)])
|
||||
|
||||
def test_checksum_recomputed_not_inherited(self):
|
||||
_, dat, _ = bydcan.create_lkas_hud(self.packer, bydcan.LKAS_STATE_ACTIVE, True, self.stock, None)
|
||||
self.assertEqual(dat[7], (~sum(dat[:7])) & 0xFF)
|
||||
|
||||
|
||||
class TestBydAccCmd(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.packer = CANPacker(DBC_NAME)
|
||||
self.stock = {"ACCEL_CMD": 0.0, "COUNTER": 7, "CHECKSUM": 0x22}
|
||||
|
||||
def test_accel_scale_is_physical(self):
|
||||
# raw x 0.05 - 5 m/s^2, so 0 m/s^2 is raw 100
|
||||
for accel in (-3.0, -1.5, 0.0, 0.5, 1.5):
|
||||
_, dat, _ = bydcan.create_acc_cmd(self.packer, accel, True, self.stock)
|
||||
self.assertEqual(dat[0], round((accel + 5.0) / 0.05))
|
||||
vals = _unpack(DBC_NAME, "ACC_CMD", dat)
|
||||
self.assertAlmostEqual(vals["ACCEL_CMD"], accel, places=6)
|
||||
|
||||
def test_inactive_commands_zero_accel(self):
|
||||
_, dat, _ = bydcan.create_acc_cmd(self.packer, -2.0, False, self.stock)
|
||||
vals = _unpack(DBC_NAME, "ACC_CMD", dat)
|
||||
self.assertEqual(vals["ACCEL_CMD"], 0.0)
|
||||
self.assertEqual(dat[0], 100)
|
||||
self.assertEqual(vals["ACC_ON_1"], 0)
|
||||
self.assertEqual(vals["ACC_ON_2"], 0)
|
||||
self.assertEqual(vals["ACC_CONTROLLABLE_AND_ON"], 0)
|
||||
self.assertEqual(vals["CMD_REQ_ACTIVE_LOW"], 1)
|
||||
|
||||
def test_standstill_hold_and_resume(self):
|
||||
_, dat, _ = bydcan.create_acc_cmd(self.packer, -0.5, True, self.stock, standstill=True)
|
||||
vals = _unpack(DBC_NAME, "ACC_CMD", dat)
|
||||
self.assertEqual(vals["STANDSTILL_STATE"], 1)
|
||||
self.assertEqual(vals["ACC_OVERRIDE_OR_STANDSTILL"], 1)
|
||||
self.assertEqual(vals["ACC_REQ_NOT_STANDSTILL"], 0)
|
||||
self.assertEqual(vals["STANDSTILL_RESUME"], 0)
|
||||
|
||||
_, dat, _ = bydcan.create_acc_cmd(self.packer, 0.5, True, self.stock, standstill=True, resume=True)
|
||||
vals = _unpack(DBC_NAME, "ACC_CMD", dat)
|
||||
self.assertEqual(vals["STANDSTILL_RESUME"], 1)
|
||||
self.assertEqual(vals["STANDSTILL_STATE"], 0)
|
||||
self.assertEqual(vals["ACC_REQ_NOT_STANDSTILL"], 1)
|
||||
|
||||
def test_regime_pairs(self):
|
||||
for accel, expected in ((0.0, (0, 0)), (0.05, (0, 0)), (0.8, (12, 5)),
|
||||
(-1.0, (13, 1)), (-2.5, (1, 1))):
|
||||
_, dat, _ = bydcan.create_acc_cmd(self.packer, accel, True, self.stock)
|
||||
vals = _unpack(DBC_NAME, "ACC_CMD", dat)
|
||||
self.assertEqual((int(vals["ACCEL_FACTOR"]), int(vals["DECEL_FACTOR"])), expected, f"{accel=}")
|
||||
|
||||
def test_accel_within_safety_bounds(self):
|
||||
# the comfort envelope must stay inside what byd.h allows (-3.5 .. +2.0)
|
||||
self.assertGreaterEqual(CarControllerParams.ACCEL_MIN, -3.5)
|
||||
self.assertLessEqual(CarControllerParams.ACCEL_MAX, 2.0)
|
||||
|
||||
|
||||
class TestBydEpsState(unittest.TestCase):
|
||||
"""The 0x1FC decode is the core fix over the Sealion 7 PR, which inherited a stub that
|
||||
packed these status bits into a fake 16-bit torque value."""
|
||||
|
||||
def test_state_nibble_table(self):
|
||||
for prepared, activated, expected in (
|
||||
(0, 0, EPS_STATE_OFF),
|
||||
(1, 0, EPS_STATE_PREPARED),
|
||||
(0, 1, EPS_STATE_ACTUATING),
|
||||
(1, 1, EPS_STATE_LATCHED_FAULT),
|
||||
):
|
||||
self.assertEqual(EPS_STATE_OFF + prepared + 2 * activated, expected)
|
||||
|
||||
def test_steering_torque_signals_exist_and_are_signed(self):
|
||||
dbc = CANParser(DBC_NAME, [], 0).dbc
|
||||
sigs = dbc.name_to_msg["STEERING_TORQUE"].sigs
|
||||
for name in ("LKS_PREPARED", "CRUISE_ACTIVATED", "TORQUE_FAILED", "DRIVER_TORQUE",
|
||||
"TARGET_ANGLE", "MAIN_TORQUE"):
|
||||
self.assertIn(name, sigs, f"{name} missing from STEERING_TORQUE")
|
||||
# driver torque must be signed or override detection cannot see direction
|
||||
self.assertTrue(sigs["DRIVER_TORQUE"].is_signed)
|
||||
self.assertTrue(sigs["MAIN_TORQUE"].is_signed)
|
||||
self.assertEqual(sigs["DRIVER_TORQUE"].start_bit, 4)
|
||||
self.assertEqual(sigs["DRIVER_TORQUE"].size, 12)
|
||||
self.assertEqual(sigs["MAIN_TORQUE"].start_bit, 32)
|
||||
self.assertEqual(sigs["MAIN_TORQUE"].size, 12)
|
||||
|
||||
def test_driver_torque_decodes_negative(self):
|
||||
packer = CANPacker(DBC_NAME)
|
||||
for torque in (-20.0, -0.5, 0.0, 0.5, 20.0):
|
||||
_, dat, _ = packer.make_can_msg("STEERING_TORQUE", 0, {"DRIVER_TORQUE": torque})
|
||||
vals = _unpack(DBC_NAME, "STEERING_TORQUE", dat)
|
||||
self.assertAlmostEqual(vals["DRIVER_TORQUE"], torque, places=4)
|
||||
|
||||
|
||||
class TestBydWheelSpeeds(unittest.TestCase):
|
||||
def test_four_independent_wheels(self):
|
||||
dbc = CANParser(DBC_NAME, [], 0).dbc
|
||||
sigs = dbc.name_to_msg["WHEEL_SPEEDS"].sigs
|
||||
for name, start in (("FL", 0), ("FR", 16), ("RL", 28), ("RR", 40)):
|
||||
self.assertEqual(sigs[name].start_bit, start)
|
||||
self.assertEqual(sigs[name].size, 12)
|
||||
self.assertAlmostEqual(sigs[name].factor, 0.0725)
|
||||
|
||||
def test_wheel_speeds_round_trip(self):
|
||||
packer = CANPacker(DBC_NAME)
|
||||
_, dat, _ = packer.make_can_msg("WHEEL_SPEEDS", 0, {"FL": 50.0, "FR": 51.0, "RL": 52.0, "RR": 53.0})
|
||||
vals = _unpack(DBC_NAME, "WHEEL_SPEEDS", dat)
|
||||
for name, expected in (("FL", 50.0), ("FR", 51.0), ("RL", 52.0), ("RR", 53.0)):
|
||||
self.assertAlmostEqual(vals[name], expected, delta=0.0725)
|
||||
|
||||
|
||||
class TestBydFingerprint(unittest.TestCase):
|
||||
def test_placeholder_never_matches_a_real_car(self):
|
||||
# a platform whose ECU dict is empty survives as a candidate for EVERY car, so the
|
||||
# placeholder must be a version no car reports rather than an empty dict
|
||||
self.assertTrue(FW_VERSIONS[CAR.BYD_SEALION_7], "empty ECU dict would match every car")
|
||||
|
||||
live = build_fw_dict([CarParams.CarFw(ecu=Ecu.engine, fwVersion=b'REAL_CAR_FW', brand='byd',
|
||||
address=0x7e0, subAddress=0)])
|
||||
self.assertNotIn(str(CAR.BYD_SEALION_7), match_fw_to_car_exact(live, 'byd'))
|
||||
|
||||
def test_fuzzy_match_requires_vds(self):
|
||||
# WMI + model year alone would claim every BYD of that year
|
||||
from iqdbc.car.byd.values import match_fw_to_car_fuzzy
|
||||
self.assertEqual(CAR.BYD_SEALION_7.config.vds_prefixes, set())
|
||||
vin = "LGX" + "A" * 6 + "R" + "A" * 7 # LGX, 2024 model year
|
||||
self.assertEqual(match_fw_to_car_fuzzy({}, vin, {}), set())
|
||||
|
||||
|
||||
class TestBydCarController(unittest.TestCase):
|
||||
"""The EPS latches a fault (state 11) if the 0x1E2 stream stops while it is actuating, and
|
||||
re-arms only on a STEER_REQ rising edge over a continuous stream. The safety also statically
|
||||
blocks the camera's own 0x1E2/0x316, so openpilot is the only source of both."""
|
||||
|
||||
def _run(self, lat_active, long_active=False, frames=20):
|
||||
CP = CarInterface.get_non_essential_params("BYD_SEALION_7")
|
||||
CP_IQ = CarInterface.get_non_essential_params_iq(CP, "BYD_SEALION_7")
|
||||
CC_obj = structs.CarControl()
|
||||
CC_obj.enabled = lat_active
|
||||
CC_obj.latActive = lat_active
|
||||
CC_obj.longActive = long_active
|
||||
CC = CC_obj.as_reader()
|
||||
CC_IQ = structs.IQCarControl()
|
||||
|
||||
carcontroller = CarInterface.CarController({'pt': DBC_NAME}, CP, CP_IQ)
|
||||
carstate = CarInterface.CarState(CP, CP_IQ)
|
||||
parsers = CarInterface.CarState.get_can_parsers(CP, CP_IQ)
|
||||
cs_out, _ = carstate.update(parsers)
|
||||
|
||||
class _CS:
|
||||
pass
|
||||
cs = _CS()
|
||||
cs.out = cs_out
|
||||
cs.eps_state = EPS_STATE_ACTUATING
|
||||
cs.eps_actuating = True
|
||||
cs.override_latched = False
|
||||
cs.lkas_hud = carstate.lkas_hud
|
||||
cs.acc_cmd = carstate.acc_cmd
|
||||
cs.buttons = carstate.buttons
|
||||
|
||||
sent = []
|
||||
for i in range(frames):
|
||||
_, can_sends = carcontroller.update(CC, CC_IQ, cs, i * 10_000_000)
|
||||
sent.append([addr for addr, _, _ in can_sends])
|
||||
return sent
|
||||
|
||||
def test_steering_stream_is_continuous_when_inactive(self):
|
||||
for lat_active in (True, False):
|
||||
sent = self._run(lat_active)
|
||||
steering = [i for i, addrs in enumerate(sent) if 0x1E2 in addrs]
|
||||
hud = [i for i, addrs in enumerate(sent) if 0x316 in addrs]
|
||||
# every other frame, whether or not lateral is active
|
||||
self.assertEqual(steering, list(range(0, 20, 2)), f"{lat_active=}")
|
||||
self.assertEqual(hud, list(range(0, 20, 2)), f"{lat_active=}")
|
||||
|
||||
def test_steer_req_gates_actuation_not_transmission(self):
|
||||
CP = CarInterface.get_non_essential_params("BYD_SEALION_7")
|
||||
CP_IQ = CarInterface.get_non_essential_params_iq(CP, "BYD_SEALION_7")
|
||||
carcontroller = CarInterface.CarController({'pt': DBC_NAME}, CP, CP_IQ)
|
||||
for lat_active in (False, True):
|
||||
_, dat, _ = bydcan.create_steering_control(carcontroller.packer, 0.0, lat_active)
|
||||
vals = _unpack(DBC_NAME, "STEERING_MODULE_ADAS", dat)
|
||||
self.assertEqual(vals["STEER_REQ"], 1 if lat_active else 0)
|
||||
|
||||
def test_no_acc_cmd_without_openpilot_longitudinal(self):
|
||||
sent = self._run(True, long_active=True)
|
||||
self.assertFalse(any(0x32E in addrs for addrs in sent),
|
||||
"0x32E sent while openpilotLongitudinalControl is off")
|
||||
|
||||
|
||||
class TestBydSteerReqGating(unittest.TestCase):
|
||||
"""STEER_REQ is the only actuation gate. The frames stream continuously either way, because the
|
||||
EPS latches a fault if the 0x1E2 stream stops while it is actuating."""
|
||||
|
||||
def _controller(self):
|
||||
CP = CarInterface.get_non_essential_params("BYD_SEALION_7")
|
||||
CP_IQ = CarInterface.get_non_essential_params_iq(CP, "BYD_SEALION_7")
|
||||
return CarInterface.CarController({'pt': DBC_NAME}, CP, CP_IQ), CP, CP_IQ
|
||||
|
||||
def _step(self, cc, CP, CP_IQ, lat_active, eps_state, angle=0.0, frames=4):
|
||||
CC_obj = structs.CarControl()
|
||||
CC_obj.enabled = lat_active
|
||||
CC_obj.latActive = lat_active
|
||||
CC = CC_obj.as_reader()
|
||||
carstate = CarInterface.CarState(CP, CP_IQ)
|
||||
cs_out, _ = carstate.update(CarInterface.CarState.get_can_parsers(CP, CP_IQ))
|
||||
|
||||
class _CS:
|
||||
pass
|
||||
cs = _CS()
|
||||
cs.out = cs_out
|
||||
cs.eps_state = eps_state
|
||||
cs.eps_actuating = eps_state == EPS_STATE_ACTUATING
|
||||
cs.override_latched = False
|
||||
cs.lkas_hud = carstate.lkas_hud
|
||||
cs.acc_cmd = carstate.acc_cmd
|
||||
cs.buttons = carstate.buttons
|
||||
|
||||
out = []
|
||||
for i in range(frames):
|
||||
_, can_sends = cc.update(CC, structs.IQCarControl(), cs, i * 10_000_000)
|
||||
for addr, dat, _bus in can_sends:
|
||||
if addr == 0x1E2:
|
||||
out.append(_unpack(DBC_NAME, "STEERING_MODULE_ADAS", dat))
|
||||
return out
|
||||
|
||||
def test_req_follows_lat_active(self):
|
||||
cc, CP, CP_IQ = self._controller()
|
||||
for lat_active in (True, False):
|
||||
frames = self._step(cc, CP, CP_IQ, lat_active, EPS_STATE_ACTUATING)
|
||||
self.assertTrue(frames)
|
||||
for vals in frames:
|
||||
self.assertEqual(int(vals["STEER_REQ"]), 1 if lat_active else 0)
|
||||
|
||||
def test_latched_fault_drops_req(self):
|
||||
# A transient nibble 11 must drop STEER_REQ within a frame; holding it there latches the ADAS
|
||||
# for the rest of the drive.
|
||||
cc, CP, CP_IQ = self._controller()
|
||||
frames = self._step(cc, CP, CP_IQ, True, EPS_STATE_LATCHED_FAULT)
|
||||
self.assertTrue(frames)
|
||||
for vals in frames:
|
||||
self.assertEqual(int(vals["STEER_REQ"]), 0)
|
||||
|
||||
def test_command_never_exceeds_eps_fault_angle(self):
|
||||
# The EPS faults out past ~90 deg of commanded wheel angle.
|
||||
self.assertLessEqual(CarControllerParams.ANGLE_LIMITS.STEER_ANGLE_MAX, 90.)
|
||||
|
||||
|
||||
class TestBydLowSpeedAngleRate(unittest.TestCase):
|
||||
"""Regression for the 2026-08-05 EPS latch. At 0.29 m/s the planner oscillated and the command
|
||||
swung -5.9 to +2.4 deg against a stationary wheel in 220 ms; the EPS went from state 9 straight
|
||||
to a latched 11. The rate curve is tightest at a standstill so the command cannot run away from
|
||||
a wheel that is not moving."""
|
||||
|
||||
def _cap(self, v):
|
||||
bp, vals = CarControllerParams.ANGLE_LIMITS.ANGLE_RATE_LIMIT_UP
|
||||
return float(np.interp(v, bp, vals))
|
||||
|
||||
def test_standstill_slew_is_bounded(self):
|
||||
self.assertLessEqual(self._cap(0.0), 0.5)
|
||||
|
||||
def test_rate_cap_scales_with_speed(self):
|
||||
self.assertLess(self._cap(0.0), self._cap(5.0), "low-speed cap must be tighter than at speed")
|
||||
|
||||
def test_rate_stays_within_stock_command_step(self):
|
||||
# the stock camera never steps its own command more than 1.30 deg/frame
|
||||
_bp, vals = CarControllerParams.ANGLE_LIMITS.ANGLE_RATE_LIMIT_UP
|
||||
self.assertLessEqual(max(vals), 1.30)
|
||||
|
||||
|
||||
class TestBydHarnessType(unittest.TestCase):
|
||||
"""Longitudinal requires the ACC ECU to sit behind the relay so 0x32E is filterable. That is
|
||||
a property of the harness, and it cannot be inferred from the fingerprint: fingerprinting
|
||||
runs with the relay closed, which ties bus 2 to bus 0, so bus 2 shows the whole car either
|
||||
way. Default must therefore be the camera harness (lateral only)."""
|
||||
|
||||
@staticmethod
|
||||
def _params(cam_bus_addrs, alpha_long=True):
|
||||
fp = {0: {0x1FC: 8, 0x1F0: 8}, 1: {}, 2: dict.fromkeys(cam_bus_addrs, 8)}
|
||||
return CarInterface.get_params("BYD_SEALION_7", fp, [], alpha_long, False, False)
|
||||
|
||||
def test_defaults_to_camera_harness_lateral_only(self):
|
||||
CP = self._params([0x1E2, 0x316])
|
||||
self.assertFalse(CP.flags & BydFlags.GATEWAY_HARNESS)
|
||||
self.assertFalse(CP.alphaLongitudinalAvailable)
|
||||
self.assertFalse(CP.openpilotLongitudinalControl)
|
||||
self.assertFalse(CP.safetyConfigs[0].safetyParam & BydSafetyFlags.LONG_CONTROL)
|
||||
|
||||
def test_acc_cmd_on_fingerprint_bus2_does_not_imply_gateway(self):
|
||||
# the relay is closed while fingerprinting, so bus 2 sees the chassis bus too. Seeing
|
||||
# 0x32E there must NOT unlock longitudinal.
|
||||
CP = self._params([0x1E2, 0x316, 0x32E, 0x32D, 0x1FC])
|
||||
self.assertFalse(CP.flags & BydFlags.GATEWAY_HARNESS)
|
||||
self.assertFalse(CP.alphaLongitudinalAvailable)
|
||||
self.assertFalse(CP.openpilotLongitudinalControl)
|
||||
|
||||
def test_lateral_still_available_on_camera_harness(self):
|
||||
CP = self._params([0x1E2, 0x316])
|
||||
self.assertFalse(CP.dashcamOnly)
|
||||
self.assertEqual(CP.steerControlType, CarParams.SteerControlType.angle)
|
||||
|
||||
|
||||
class TestBydCarParams(unittest.TestCase):
|
||||
def test_angle_control_and_no_radar(self):
|
||||
CP = CarInterface.get_non_essential_params("BYD_SEALION_7")
|
||||
self.assertEqual(CP.brand, "byd")
|
||||
self.assertEqual(CP.steerControlType, CarParams.SteerControlType.angle)
|
||||
self.assertEqual(CP.safetyConfigs[0].safetyModel, CarParams.SafetyModel.byd)
|
||||
# the BYD-6 harness jumpers the Veoneer private CAN-FD pair straight through
|
||||
self.assertTrue(CP.radarUnavailable)
|
||||
self.assertFalse(CP.dashcamOnly)
|
||||
|
||||
def test_steer_step_matches_safety_frequency(self):
|
||||
# byd.h declares .frequency = 50U for the angle limiter
|
||||
self.assertEqual(CarControllerParams.STEER_STEP, 2)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
226
artifacts/package_sources/iqdbc/iqdbc/car/byd/values.py
Normal file
226
artifacts/package_sources/iqdbc/iqdbc/car/byd/values.py
Normal file
@@ -0,0 +1,226 @@
|
||||
from dataclasses import dataclass, field
|
||||
from enum import IntFlag, StrEnum
|
||||
|
||||
from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, structs
|
||||
from iqdbc.car.lateral import AngleSteeringLimits, AVERAGE_ROAD_ROLL, ISO_LATERAL_ACCEL
|
||||
from iqdbc.car.docs_definitions import CarDocs, CarHarness, CarParts
|
||||
from iqdbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
|
||||
from iqdbc.car.vin import Vin
|
||||
|
||||
Ecu = structs.CarParams.Ecu
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
STEER_STEP = 2 # 50 Hz
|
||||
|
||||
ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
|
||||
# 85 deg max command. The EPS faults out past ~90 deg (route 113: on a tight 11 km/h turn the
|
||||
# model wanted 360+ deg, openpilot drove the wheel past 90 and the ADAS latched for the rest of
|
||||
# the drive). 85 keeps openpilot's own command below the fault; tighter corners (5% of steering
|
||||
# frames want >90, all near full lock at 11-12 km/h) are handed to the driver. Was 390 (never
|
||||
# clamped).
|
||||
85., # deg
|
||||
# ANGLE_RATE_LIMIT_UP / DOWN: fast speed-interpolated rate curve (deg per 20 ms frame). Paired
|
||||
# with near-zero actuator delay and the 2 Hz command low-pass, the fast rate lets the command
|
||||
# track without the phase lag that oscillated the wheel; the low-pass keeps it smooth.
|
||||
([0., 2., 5., 15.], [0.3, 1.0, 1.3, 1.0]),
|
||||
([0., 2., 5., 15.], [0.5, 1.2, 1.5, 1.2]),
|
||||
|
||||
# MUST mirror safety/lateral.h, which the panda enforces for every car; a tighter value here
|
||||
# only desyncs the two and makes the safety envelope untestable. Command smoothness is set by
|
||||
# the rate curve above, not by this.
|
||||
MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
|
||||
MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
|
||||
|
||||
# deg/20ms. The stock camera's own command never steps more than 1.30 deg/frame
|
||||
# (4783 engaged samples: p99 0.40, p99.9 0.80, max 1.30). At 2 we sat at the cap constantly
|
||||
# (route db: p90 = 2.0 deg/frame = 100 deg/s) and every disturbance produced a 100 deg/s
|
||||
# whip-crack that the driver felt as a sharp jerk and that de-actuated the EPS (17 of the
|
||||
# 10->9 drops landed right after a max-rate step). 1.0 keeps us under the stock max and above
|
||||
# its p99.9 (0.80), so turns still track but corrections are smooth.
|
||||
MAX_ANGLE_RATE=1.5,
|
||||
)
|
||||
# 1-pole low-pass on the steering command (Hz). This is the primary smoothing in the fast-rate /
|
||||
# near-zero-delay regime - it takes the residual oscillation ("barking") off stronger turns.
|
||||
STEER_LOWPASS_HZ = 2.0
|
||||
|
||||
# Low-speed taper on the angle rate, in deg per STEER_STEP frame.
|
||||
#
|
||||
# The vehicle-model jerk limit scales as 1/v^2, so below a few m/s it stops binding and only
|
||||
# the flat MAX_ANGLE_RATE is left. The lateral planner is ill-conditioned at a standstill and
|
||||
# oscillates, and slewing the command at the full rate while the wheel is not moving walks the
|
||||
# EPS straight from state 9 to a latched 11. Measured 2026-08-05 at 0.29 m/s: the command swung
|
||||
# -5.9 to +2.4 deg in 220 ms against a stationary wheel and the EPS latched, taking LKAS with
|
||||
# it. A healthy engagement at 0.9 m/s held the command within 1.1 deg of measured.
|
||||
ANGLE_RATE_BP = [0.0, 2.0, 5.0] # m/s
|
||||
ANGLE_RATE_V = [0.3, 1.0, 1.5] # deg/frame, tops out under MAX_ANGLE_RATE
|
||||
|
||||
# Below this the lateral planner is ill-conditioned and demands garbage angles (measured route
|
||||
# e3: 35 deg desired at 3 km/h with yawRate 0, car going straight). Rate-tapering only slowed the
|
||||
# swing - the command still ramped to 40+ deg. Below MIN_STEER_SPEED we do not chase the planner
|
||||
# at all; we hold the command on the measured wheel so it neither swings nor fights, and pick up
|
||||
# normal steering once past it. 2.0 m/s = 7.2 km/h, the speed where the VM jerk limit re-binds.
|
||||
# Speed gate with hysteresis: below MIN_STEER_SPEED openpilot does not actuate; it only resumes
|
||||
# once back above MIN_STEER_SPEED_RESUME. The band stops the gate chattering at ~7-8 km/h - a
|
||||
# single hard threshold at 2.0 m/s flipped on every 1 km/h speed wobble and toggled REQ (route
|
||||
# 116: 153 toggles = the "parkinsons" tremor).
|
||||
MIN_STEER_SPEED = 2.0 # m/s (7.2 km/h) - drop
|
||||
MIN_STEER_SPEED_RESUME = 2.8 # m/s (10 km/h) - resume
|
||||
|
||||
# Angle gate with hysteresis: hand off (drop STEER_REQ) once the wheel is past MAX_STEER_ANGLE - a
|
||||
# tight turn heading for the ~90 deg EPS fault - and only re-engage once it comes back under
|
||||
# MAX_STEER_ANGLE_RESUME. Below the 85 deg command clamp so openpilot relaxes and the driver
|
||||
# finishes the corner; the resume band keeps it from chattering at the 80 deg edge.
|
||||
MAX_STEER_ANGLE = 80.0 # deg - drop
|
||||
MAX_STEER_ANGLE_RESUME = 65.0 # deg - resume
|
||||
|
||||
# First-order low-pass on the commanded steering angle. DISABLED (1.0 = passthrough): it added
|
||||
# ~45 ms of loop delay that fed the high-speed weave without fixing the felt stutter (the stutter
|
||||
# is that weave, not the +-0.5 deg jitter this was chasing). The weave is addressed by
|
||||
# steerActuatorDelay instead. Re-enable a light value only if a real HF buzz remains after.
|
||||
ANGLE_FILTER_ALPHA = 0.5
|
||||
|
||||
# STEERING_TORQUE.DRIVER_TORQUE thresholds, derived from a drive where openpilot actually
|
||||
# steered (route 0000000f, EPS state 10):
|
||||
# |torque| while openpilot steered: p50 1.2 p90 2.7 p95 3.3 p99 5.8 max 9.8
|
||||
# |torque| while the human drove: p50 0.2 p90 8.1 p95 17.2 p99 24.5 max 35.5
|
||||
# The old 3.0 sat below what openpilot generates while steering, so it tripped its own
|
||||
# override and dropped out within a few frames of every engage.
|
||||
# 12.0 sat inside the overlap between openpilot's own steering (max 9.8) and ordinary human
|
||||
# driving (p90 8.1, p95 17.2), so it tripped constantly on 13-15 Nm corrections. 18 is above
|
||||
# p95 and still well under a deliberate grab.
|
||||
# Override / grey-border threshold on DRIVER_EPS_TORQUE (STEER_MODULE_2 byte 2, raw 0-255, the
|
||||
# clean column sensor - see carstate). Normal openpilot steering keeps it near 0 and a route max
|
||||
# was 79; normal driver turns peak ~52. So 80 catches a real
|
||||
# takeover without tripping on openpilot's own steering. This finally lets the yield ride
|
||||
# steeringPressed (border-linked, as Gutek wanted) without the load-driven chatter of DRIVER_TORQUE.
|
||||
STEER_DRIVER_OVERRIDE = 80
|
||||
# Consecutive 0x1FC samples over the threshold before the override latches. Driver torque is
|
||||
# spiky: single frames touch 12+ with hands resting, and latching on one of them disengaged
|
||||
# constantly. MUST equal BYD_OVERRIDE_FRAMES in byd.h or the two latches desync.
|
||||
STEER_DRIVER_OVERRIDE_FRAMES = 5
|
||||
# Frames continuously back under the threshold before the override releases. Without this the
|
||||
# latch is permanent in practice: the car cycles CRUISE_STATE 1<->2 on its own and never
|
||||
# reaches 0, so normal manual driving pinned it on and engagement became impossible.
|
||||
# 100 frames at the 50 Hz 0x1FC rate = 2 s hands-settled.
|
||||
STEER_DRIVER_RELEASE_FRAMES = 100
|
||||
|
||||
# |commanded - measured| that the stock camera runs while ICC steers (route
|
||||
# 0000007b--88dd577c32, 4796 engaged frames):
|
||||
# p50 0.20 p95 1.00 p99 3.31 p99.9 10.61 max 19.70
|
||||
# So the EPS tolerates ~20 deg of transient tracking error. The old 6.0/5.0 pair was built on
|
||||
# the "EPS latches state 11 on divergence" model, which the stock capture refutes, and it was
|
||||
# self-defeating: the clamp parked the command in a 5-6 deg band that the bail then punished,
|
||||
# so every firm turn dropped STEER_REQ and chattered.
|
||||
#
|
||||
# INVARIANT: MAX_ANGLE_ERROR < ANGLE_ERROR_BAIL, or the clamp manufactures bail conditions.
|
||||
MAX_ANGLE_ERROR = 12.0 # deg, once actuating; just above stock p99.9
|
||||
|
||||
# Hard bail: drop STEER_REQ once the wheel has genuinely run away from the command. Above the
|
||||
# stock maximum so it only catches a real runaway, never normal cornering. Deasserting REQ
|
||||
# walks the EPS 10 -> 9 cleanly, which is the safe exit; it re-arms on the next rising edge.
|
||||
ANGLE_ERROR_BAIL = 25.0 # deg
|
||||
|
||||
# Drop STEER_REQ the instant driver torque spikes, no debounce. A violent jerk moves the
|
||||
# wheel faster than the angle-divergence bail can react (the EPS latched at ~12 deg before
|
||||
# 50 Hz control even saw 8), but torque spikes a few frames BEFORE the angle diverges.
|
||||
# Dropping REQ is safe and reversible (EPS 10 -> 9, re-arms on the next rising edge), so
|
||||
# unlike the disengage latch it needs no debounce. Also makes the wheel yield immediately
|
||||
# instead of fighting the driver until the override latch fires.
|
||||
TORQUE_BAIL = 20.0 # Nm, instantaneous
|
||||
|
||||
# While the EPS is armed but NOT actuating (state 9) the wheel does not follow, so any command
|
||||
# offset just sits there as error until the EPS gives up and latches 11. Measured 2026-08-05:
|
||||
# 4.2 deg of standing error at 0.9 m/s in state 9 was enough. Keep the command on the wheel
|
||||
# until the EPS reports state 10, then let it depart normally.
|
||||
MAX_ANGLE_ERROR_INACTIVE = 1.0 # deg
|
||||
|
||||
# comfort envelope, inside the safety cap of -3.5..+2.0
|
||||
ACCEL_MIN = -3.0
|
||||
ACCEL_MAX = 1.5
|
||||
|
||||
JERK_UP = 2.5
|
||||
JERK_UP_LAUNCH = 4.0 # below 2 m/s, to beat the ~0.5s IPB lag off the line
|
||||
JERK_DOWN = 5.0
|
||||
|
||||
|
||||
class BydSafetyFlags(IntFlag):
|
||||
LONG_CONTROL = 1
|
||||
|
||||
|
||||
class BydFlags(IntFlag):
|
||||
# The ADAS/ACC ECU is behind the relay, so its 0x32E ACC_CMD can be blocked and replaced.
|
||||
# Set when ACC_CMD is fingerprinted on the camera-side bus.
|
||||
GATEWAY_HARNESS = 1
|
||||
|
||||
|
||||
# addresses used to tell the two harness types apart
|
||||
ACC_CMD_ADDR = 0x32E
|
||||
|
||||
|
||||
class WMI(StrEnum):
|
||||
BYD_AUTO = "LGX"
|
||||
|
||||
|
||||
class ModelYear(StrEnum):
|
||||
R_2024 = "R"
|
||||
S_2025 = "S"
|
||||
T_2026 = "T"
|
||||
|
||||
|
||||
@dataclass
|
||||
class BydCarDocs(CarDocs):
|
||||
package: str = "All"
|
||||
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.custom]))
|
||||
|
||||
|
||||
@dataclass
|
||||
class BydPlatformConfig(PlatformConfig):
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'byd_sealion_7'})
|
||||
wmis: set[WMI] = field(default_factory=set)
|
||||
years: set[ModelYear] = field(default_factory=set)
|
||||
vds_prefixes: set[str] = field(default_factory=set)
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
BYD_SEALION_7 = BydPlatformConfig(
|
||||
[BydCarDocs("BYD Sealion 7 2024-25")],
|
||||
CarSpecs(mass=2090., wheelbase=2.93, steerRatio=16.0, centerToFrontRatio=0.44),
|
||||
wmis={WMI.BYD_AUTO},
|
||||
years={ModelYear.R_2024, ModelYear.S_2025, ModelYear.T_2026},
|
||||
)
|
||||
|
||||
|
||||
def match_fw_to_car_fuzzy(live_fw_versions, vin, offline_fw_versions) -> set[str]:
|
||||
# VIN: LGX (WMI) + <VDS> + <year><plant><seq> (VIS). Matching on WMI + year alone would claim
|
||||
# every BYD of that year, so a platform only matches once its VDS prefix is known.
|
||||
vin_obj = Vin(vin)
|
||||
year = vin_obj.vis[:1]
|
||||
|
||||
candidates = set()
|
||||
for platform in CAR:
|
||||
cfg = platform.config
|
||||
if not cfg.vds_prefixes or vin_obj.wmi not in cfg.wmis or year not in cfg.years:
|
||||
continue
|
||||
if any(vin_obj.vds.startswith(p) for p in cfg.vds_prefixes):
|
||||
candidates.add(platform)
|
||||
|
||||
return {str(c) for c in candidates}
|
||||
|
||||
|
||||
FW_QUERY_CONFIG = FwQueryConfig(
|
||||
# BYD ECUs NRC 0xF188 (openpilot's default) but answer 0xF195
|
||||
requests=[
|
||||
Request(
|
||||
[StdQueries.SUPPLIER_SOFTWARE_VERSION_REQUEST],
|
||||
[StdQueries.SUPPLIER_SOFTWARE_VERSION_RESPONSE],
|
||||
bus=0,
|
||||
),
|
||||
],
|
||||
# the MPC camera answers OBD DTC scans but not the bus-0 DID sweep
|
||||
non_essential_ecus={Ecu.fwdCamera: list(CAR)},
|
||||
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
|
||||
)
|
||||
|
||||
|
||||
DBC = CAR.create_dbc_map()
|
||||
15
artifacts/package_sources/iqdbc/iqdbc/car/can_definitions.py
Normal file
15
artifacts/package_sources/iqdbc/iqdbc/car/can_definitions.py
Normal file
@@ -0,0 +1,15 @@
|
||||
from collections.abc import Callable
|
||||
from typing import NamedTuple, Protocol
|
||||
|
||||
|
||||
class CanData(NamedTuple):
|
||||
address: int
|
||||
dat: bytes
|
||||
src: int
|
||||
|
||||
|
||||
CanSendCallable = Callable[[list[CanData]], None]
|
||||
|
||||
|
||||
class CanRecvCallable(Protocol):
|
||||
def __call__(self, wait_for_one: bool = False) -> list[list[CanData]]: ...
|
||||
840
artifacts/package_sources/iqdbc/iqdbc/car/car.capnp
Normal file
840
artifacts/package_sources/iqdbc/iqdbc/car/car.capnp
Normal file
@@ -0,0 +1,840 @@
|
||||
using Cxx = import "./include/c++.capnp";
|
||||
$Cxx.namespace("cereal");
|
||||
|
||||
@0x8e2af1e708af8b8d;
|
||||
|
||||
# ******* events causing controls state machine transition *******
|
||||
|
||||
# IMPORTANT: This struct is to not be modified so old logs can be parsed
|
||||
struct OnroadEventDEPRECATED @0x9b1657f34caf3ad3 {
|
||||
name @0 :EventName;
|
||||
|
||||
# event types
|
||||
enable @1 :Bool;
|
||||
noEntry @2 :Bool;
|
||||
warning @3 :Bool; # alerts presented only when enabled or soft disabling
|
||||
userDisable @4 :Bool;
|
||||
softDisable @5 :Bool;
|
||||
immediateDisable @6 :Bool;
|
||||
preEnable @7 :Bool;
|
||||
permanent @8 :Bool; # alerts presented regardless of openpilot state
|
||||
overrideLateral @10 :Bool;
|
||||
overrideLongitudinal @9 :Bool;
|
||||
|
||||
enum EventName @0xbaa8c5d505f727de {
|
||||
canError @0;
|
||||
steerUnavailable @1;
|
||||
wrongGear @4;
|
||||
doorOpen @5;
|
||||
seatbeltNotLatched @6;
|
||||
espDisabled @7;
|
||||
wrongCarMode @8;
|
||||
steerTempUnavailable @9;
|
||||
reverseGear @10;
|
||||
buttonCancel @11;
|
||||
buttonEnable @12;
|
||||
pedalPressed @13; # exits active state
|
||||
preEnableStandstill @73; # added during pre-enable state with brake
|
||||
gasPressedOverride @108; # added when user is pressing gas with no disengage on gas
|
||||
steerOverride @114;
|
||||
cruiseDisabled @14;
|
||||
speedTooLow @17;
|
||||
outOfSpace @18;
|
||||
overheat @19;
|
||||
calibrationIncomplete @20;
|
||||
calibrationInvalid @21;
|
||||
calibrationRecalibrating @117;
|
||||
controlsMismatch @22;
|
||||
pcmEnable @23;
|
||||
pcmDisable @24;
|
||||
radarFault @26;
|
||||
brakeHold @28;
|
||||
parkBrake @29;
|
||||
manualRestart @30;
|
||||
joystickDebug @34;
|
||||
longitudinalManeuver @124;
|
||||
steerTempUnavailableSilent @35;
|
||||
resumeRequired @36;
|
||||
preDriverDistracted @37;
|
||||
promptDriverDistracted @38;
|
||||
driverDistracted @39;
|
||||
preDriverUnresponsive @43;
|
||||
promptDriverUnresponsive @44;
|
||||
driverUnresponsive @45;
|
||||
belowSteerSpeed @46;
|
||||
lowBattery @48;
|
||||
accFaulted @51;
|
||||
sensorDataInvalid @52;
|
||||
commIssue @53;
|
||||
commIssueAvgFreq @109;
|
||||
tooDistracted @54;
|
||||
posenetInvalid @55;
|
||||
preLaneChangeLeft @57;
|
||||
preLaneChangeRight @58;
|
||||
laneChange @59;
|
||||
lowMemory @63;
|
||||
stockAeb @64;
|
||||
ldw @65;
|
||||
carUnrecognized @66;
|
||||
invalidLkasSetting @69;
|
||||
speedTooHigh @70;
|
||||
laneChangeBlocked @71;
|
||||
relayMalfunction @72;
|
||||
stockFcw @74;
|
||||
startup @75;
|
||||
startupNoCar @76;
|
||||
startupNoControl @77;
|
||||
startupNoSecOcKey @125;
|
||||
startupMaster @78;
|
||||
fcw @79;
|
||||
steerSaturated @80;
|
||||
belowEngageSpeed @84;
|
||||
noGps @85;
|
||||
wrongCruiseMode @87;
|
||||
modeldLagging @89;
|
||||
deviceFalling @90;
|
||||
fanMalfunction @91;
|
||||
cameraMalfunction @92;
|
||||
cameraFrameRate @110;
|
||||
processNotRunning @95;
|
||||
dashcamMode @96;
|
||||
selfdriveInitializing @98;
|
||||
usbError @99;
|
||||
cruiseMismatch @106;
|
||||
canBusMissing @111;
|
||||
selfdrivedLagging @112;
|
||||
resumeBlocked @113;
|
||||
steerTimeLimit @115;
|
||||
vehicleSensorsInvalid @116;
|
||||
locationdTemporaryError @103;
|
||||
locationdPermanentError @118;
|
||||
paramsdTemporaryError @50;
|
||||
paramsdPermanentError @119;
|
||||
actuatorsApiUnavailable @120;
|
||||
espActive @121;
|
||||
personalityChanged @122;
|
||||
aeb @123;
|
||||
|
||||
radarCanErrorDEPRECATED @15;
|
||||
communityFeatureDisallowedDEPRECATED @62;
|
||||
radarCommIssueDEPRECATED @67;
|
||||
driverMonitorLowAccDEPRECATED @68;
|
||||
gasUnavailableDEPRECATED @3;
|
||||
dataNeededDEPRECATED @16;
|
||||
modelCommIssueDEPRECATED @27;
|
||||
ipasOverrideDEPRECATED @33;
|
||||
geofenceDEPRECATED @40;
|
||||
driverMonitorOnDEPRECATED @41;
|
||||
driverMonitorOffDEPRECATED @42;
|
||||
calibrationProgressDEPRECATED @47;
|
||||
invalidGiraffeHondaDEPRECATED @49;
|
||||
invalidGiraffeToyotaDEPRECATED @60;
|
||||
internetConnectivityNeededDEPRECATED @61;
|
||||
whitePandaUnsupportedDEPRECATED @81;
|
||||
commIssueWarningDEPRECATED @83;
|
||||
focusRecoverActiveDEPRECATED @86;
|
||||
neosUpdateRequiredDEPRECATED @88;
|
||||
modelLagWarningDEPRECATED @93;
|
||||
startupOneplusDEPRECATED @82;
|
||||
startupFuzzyFingerprintDEPRECATED @97;
|
||||
noTargetDEPRECATED @25;
|
||||
brakeUnavailableDEPRECATED @2;
|
||||
plannerErrorDEPRECATED @32;
|
||||
gpsMalfunctionDEPRECATED @94;
|
||||
roadCameraErrorDEPRECATED @100;
|
||||
driverCameraErrorDEPRECATED @101;
|
||||
wideRoadCameraErrorDEPRECATED @102;
|
||||
highCpuUsageDEPRECATED @105;
|
||||
startupNoFwDEPRECATED @104;
|
||||
lowSpeedLockoutDEPRECATED @31;
|
||||
lkasDisabledDEPRECATED @107;
|
||||
soundsUnavailableDEPRECATED @56;
|
||||
}
|
||||
}
|
||||
|
||||
struct CarState {
|
||||
# CAN health
|
||||
canValid @26 :Bool; # invalid counter/checksums
|
||||
canTimeout @40 :Bool; # CAN bus dropped out
|
||||
canErrorCounter @48 :UInt32;
|
||||
|
||||
# process meta
|
||||
cumLagMs @50 :Float32;
|
||||
|
||||
# car speed
|
||||
vEgo @1 :Float32; # best estimate of speed
|
||||
aEgo @16 :Float32; # best estimate of aCAN cceleration
|
||||
vEgoRaw @17 :Float32; # unfiltered speed from wheel speed sensors
|
||||
vEgoCluster @44 :Float32; # best estimate of speed shown on car's instrument cluster, used for UI
|
||||
|
||||
vCruise @53 :Float32; # actual set speed
|
||||
vCruiseCluster @54 :Float32; # set speed to display in the UI
|
||||
|
||||
yawRate @22 :Float32; # best estimate of yaw rate
|
||||
standstill @18 :Bool;
|
||||
wheelSpeeds @2 :WheelSpeeds;
|
||||
|
||||
gasPressed @4 :Bool; # this is user pedal only
|
||||
|
||||
# brake pedal, 0.0-1.0
|
||||
brake @5 :Float32; # this is user pedal only
|
||||
brakePressed @6 :Bool; # this is user pedal only
|
||||
regenBraking @45 :Bool; # this is user pedal only
|
||||
parkingBrake @39 :Bool;
|
||||
brakeHoldActive @38 :Bool;
|
||||
|
||||
# steering wheel
|
||||
steeringAngleDeg @7 :Float32;
|
||||
steeringAngleOffsetDeg @37 :Float32; # Offset between sensors in case there multiple
|
||||
steeringRateDeg @15 :Float32; # optional
|
||||
steeringTorque @8 :Float32; # Native CAN units, only needed on cars where it's used for control
|
||||
steeringTorqueEps @27 :Float32; # Native CAN units, only needed on cars where it's used for control
|
||||
steeringPressed @9 :Bool; # is the user overring the steering wheel?
|
||||
steeringDisengage @58 :Bool; # more force than steeringPressed, disengages for applicable brands
|
||||
steerFaultTemporary @35 :Bool;
|
||||
steerFaultPermanent @36 :Bool;
|
||||
steeringCurvature @64 :Float32;
|
||||
|
||||
invalidLkasSetting @55 :Bool; # stock LKAS is incorrectly configured (i.e. on or off)
|
||||
stockAeb @30 :Bool;
|
||||
stockLkas @59 :Bool;
|
||||
stockFcw @31 :Bool;
|
||||
espDisabled @32 :Bool;
|
||||
accFaulted @42 :Bool;
|
||||
carFaultedNonCritical @47 :Bool; # some ECU is faulted, but car remains controllable
|
||||
espActive @51 :Bool;
|
||||
vehicleSensorsInvalid @52 :Bool; # invalid steering angle readings, etc.
|
||||
lowSpeedAlert @56 :Bool; # lost steering control due to a dynamic min steering speed
|
||||
blockPcmEnable @60 :Bool; # whether to allow PCM to enable this frame
|
||||
lateralAvailable @61 :Bool; # lateral control is available even if cruise is faulted
|
||||
cruiseFaultLateralMode @62 :Bool; # cruise is faulted but lateral control is still active
|
||||
carNotReady @95 :Bool; # car is transiently refusing engagement, not a fault
|
||||
radarDisableFailed @66 :Bool;
|
||||
# Physical vehicle odometer in kilometers. Zero means unavailable on this platform.
|
||||
odometer @67 :Float64;
|
||||
|
||||
# cruise state
|
||||
cruiseState @10 :CruiseState;
|
||||
|
||||
# gear
|
||||
gearShifter @14 :GearShifter;
|
||||
|
||||
# button presses
|
||||
buttonEvents @11 :List(ButtonEvent);
|
||||
buttonEnable @57 :Bool; # user is requesting enable, usually one frame. set if pcmCruise=False
|
||||
leftBlinker @20 :Bool;
|
||||
rightBlinker @21 :Bool;
|
||||
genericToggle @23 :Bool;
|
||||
|
||||
# lock info
|
||||
doorOpen @24 :Bool; # ideally includes all doors
|
||||
seatbeltUnlatched @25 :Bool; # driver seatbelt
|
||||
|
||||
# blindspot sensors
|
||||
leftBlindspot @33 :Bool; # Is there something blocking the left lane change
|
||||
rightBlindspot @34 :Bool; # Is there something blocking the right lane change
|
||||
|
||||
fuelGauge @41 :Float32; # battery or fuel tank level from [0.0, 1.0]
|
||||
charging @43 :Bool;
|
||||
fuelTankLevelL @63 :Float32; # raw fuel tank level in liters (konn3kt: VW PQ Kombi_1.Tankinhalt)
|
||||
batteryDetails @65 :BatteryDetails;
|
||||
|
||||
# carrotpilot HKG extension state
|
||||
vCluRatio @68 :Float32;
|
||||
logCarrot @69 :Text;
|
||||
softHoldActive @70 :Int16;
|
||||
activateCruise @71 :Int16;
|
||||
latEnabled @72 :Bool;
|
||||
pcmCruiseGap @73 :Int16;
|
||||
speedLimit @74 :Float32;
|
||||
speedLimitDistance @75 :Float32;
|
||||
gearStep @76 :Int16;
|
||||
tpms @77 :Tpms;
|
||||
useLaneLineSpeed @78 :Float32;
|
||||
leftLatDist @79 :Float32;
|
||||
rightLatDist @80 :Float32;
|
||||
leftLongDist @81 :Float32;
|
||||
rightLongDist @82 :Float32;
|
||||
carrotCruise @83 :Int16;
|
||||
leftLaneLine @84 :Int16;
|
||||
rightLaneLine @85 :Int16;
|
||||
datetime @86 :UInt64;
|
||||
leftRearLongDist @87 :Float32;
|
||||
rightRearLongDist @88 :Float32;
|
||||
leftRearLatDist @89 :Float32;
|
||||
rightRearLatDist @90 :Float32;
|
||||
trailerConnected @91 :Bool;
|
||||
ureaGauge @92 :Float32;
|
||||
evModeActive @93 :Bool;
|
||||
evModeValid @94 :Bool;
|
||||
|
||||
struct Tpms {
|
||||
fl @0 :Float32;
|
||||
fr @1 :Float32;
|
||||
rl @2 :Float32;
|
||||
rr @3 :Float32;
|
||||
}
|
||||
|
||||
struct BatteryDetails {
|
||||
capacity @0 :Float32;
|
||||
charge @1 :Float32;
|
||||
soc @2 :Float32;
|
||||
temperature @3 :Float32;
|
||||
heaterActive @4 :Bool;
|
||||
voltage @5 :Float32;
|
||||
current @6 :Float32;
|
||||
power @7 :Float32;
|
||||
chargingMode @8 :UInt8;
|
||||
}
|
||||
|
||||
struct WheelSpeeds {
|
||||
# optional wheel speeds
|
||||
fl @0 :Float32;
|
||||
fr @1 :Float32;
|
||||
rl @2 :Float32;
|
||||
rr @3 :Float32;
|
||||
}
|
||||
|
||||
struct CruiseState {
|
||||
enabled @0 :Bool;
|
||||
speed @1 :Float32;
|
||||
speedCluster @6 :Float32; # Set speed as shown on instrument cluster
|
||||
available @2 :Bool;
|
||||
standstill @4 :Bool;
|
||||
nonAdaptive @5 :Bool;
|
||||
speedLimit @7 :Float32;
|
||||
speedLimitPredicative @8 :Float32;
|
||||
|
||||
speedOffsetDEPRECATED @3 :Float32;
|
||||
}
|
||||
|
||||
enum GearShifter {
|
||||
unknown @0;
|
||||
park @1;
|
||||
drive @2;
|
||||
neutral @3;
|
||||
reverse @4;
|
||||
sport @5;
|
||||
low @6;
|
||||
brake @7;
|
||||
eco @8;
|
||||
manumatic @9;
|
||||
}
|
||||
|
||||
# send on change
|
||||
struct ButtonEvent {
|
||||
pressed @0 :Bool;
|
||||
type @1 :Type;
|
||||
|
||||
enum Type {
|
||||
unknown @0;
|
||||
leftBlinker @1;
|
||||
rightBlinker @2;
|
||||
accelCruise @3;
|
||||
decelCruise @4;
|
||||
cancel @5;
|
||||
lkas @6;
|
||||
altButton2 @7;
|
||||
mainCruise @8;
|
||||
setCruise @9;
|
||||
resumeCruise @10;
|
||||
gapAdjustCruise @11;
|
||||
lfaButton @12;
|
||||
paddleLeft @13;
|
||||
paddleRight @14;
|
||||
}
|
||||
}
|
||||
|
||||
# deprecated
|
||||
errorsDEPRECATED @0 :List(OnroadEventDEPRECATED.EventName);
|
||||
gas @3 :Float32; # this is user pedal only
|
||||
brakeLights @19 :Bool;
|
||||
steeringRateLimitedDEPRECATED @29 :Bool;
|
||||
canMonoTimesDEPRECATED @12: List(UInt64);
|
||||
canRcvTimeoutDEPRECATED @49 :Bool;
|
||||
eventsDEPRECATED @13 :List(OnroadEventDEPRECATED);
|
||||
clutchPressedDEPRECATED @28 :Bool;
|
||||
engineRpmDEPRECATED @46 :Float32;
|
||||
}
|
||||
|
||||
# ******* radar state @ 20hz *******
|
||||
|
||||
struct RadarData @0x888ad6581cf0aacb {
|
||||
errors @3 :Error;
|
||||
points @1 :List(RadarPoint);
|
||||
|
||||
struct Error {
|
||||
canError @0 :Bool;
|
||||
radarFault @1 :Bool;
|
||||
wrongConfig @2 :Bool;
|
||||
radarUnavailableTemporary @3 :Bool; # radar data is temporarily unavailable due to conditions the car sets
|
||||
}
|
||||
|
||||
# similar to LiveTracks
|
||||
# is one timestamp valid for all? I think so
|
||||
struct RadarPoint {
|
||||
trackId @0 :UInt64; # no trackId reuse
|
||||
|
||||
# these 3 are the minimum required
|
||||
dRel @1 :Float32; # m from the front bumper of the car
|
||||
yRel @2 :Float32; # m
|
||||
vRel @3 :Float32; # m/s
|
||||
|
||||
# these are optional and valid if they are not NaN
|
||||
aRel @4 :Float32; # m/s^2
|
||||
yvRel @5 :Float32; # m/s
|
||||
|
||||
# some radars flag measurements VS estimates
|
||||
measured @6 :Bool;
|
||||
|
||||
vLead @7 :Float32; # m/s
|
||||
aLead @8 :Float32; # m/s^2
|
||||
jLead @9 :Float32; # m/s^3
|
||||
radarSource @10 :RadarSource;
|
||||
|
||||
enum RadarSource {
|
||||
frontRadar @0;
|
||||
scc @1;
|
||||
corner235 @2;
|
||||
corner180 @3;
|
||||
}
|
||||
}
|
||||
|
||||
enum ErrorDEPRECATED {
|
||||
canError @0;
|
||||
fault @1;
|
||||
wrongConfig @2;
|
||||
}
|
||||
|
||||
# deprecated
|
||||
canMonoTimesDEPRECATED @2 :List(UInt64);
|
||||
errorsDEPRECATED @0 :List(ErrorDEPRECATED);
|
||||
}
|
||||
|
||||
# ******* car controls @ 100hz *******
|
||||
|
||||
struct CarControl {
|
||||
# must be true for any actuator commands to work
|
||||
enabled @0 :Bool;
|
||||
latActive @11: Bool;
|
||||
longActive @12: Bool;
|
||||
|
||||
# Final actuator commands
|
||||
actuators @6 :Actuators;
|
||||
|
||||
# Blinker controls
|
||||
leftBlinker @15: Bool;
|
||||
rightBlinker @16: Bool;
|
||||
|
||||
orientationNED @13 :List(Float32);
|
||||
angularVelocity @14 :List(Float32);
|
||||
currentCurvature @17 :Float32; # From vehicle model
|
||||
curvatureControllerActive @18: Bool;
|
||||
rollCompensation @19 :Float32;
|
||||
steerLimited @20: Bool;
|
||||
forceRHDForBSM @21: Bool;
|
||||
longComfortMode @22: Bool;
|
||||
|
||||
cruiseControl @4 :CruiseControl;
|
||||
hudControl @5 :HUDControl;
|
||||
|
||||
struct Actuators {
|
||||
# lateral commands, mutually exclusive
|
||||
torque @2: Float32; # [0.0, 1.0]
|
||||
steeringAngleDeg @3: Float32;
|
||||
curvature @7: Float32;
|
||||
|
||||
# longitudinal commands
|
||||
accel @4: Float32; # m/s^2
|
||||
longControlState @5: LongControlState;
|
||||
|
||||
# these are only for logging the actual values sent to the car over CAN
|
||||
gas @0: Float32; # [0.0, 1.0]
|
||||
brake @1: Float32; # [0.0, 1.0]
|
||||
torqueOutputCan @8: Float32; # value sent over can to the car
|
||||
speed @6: Float32; # m/s
|
||||
jerk @9: Float32; # m/s^3
|
||||
aTarget @10: Float32; # m/s^2
|
||||
|
||||
enum LongControlState @0xe40f3a917d908282{
|
||||
off @0;
|
||||
pid @1;
|
||||
stopping @2;
|
||||
starting @3;
|
||||
}
|
||||
}
|
||||
|
||||
struct CruiseControl {
|
||||
cancel @0: Bool;
|
||||
resume @1: Bool;
|
||||
override @4: Bool;
|
||||
speedLimit @5: Bool;
|
||||
speedLimitPredicative @6: Bool;
|
||||
speedLimitPredReactToSL @7: Bool;
|
||||
speedLimitPredReactToCurves @8: Bool;
|
||||
speedOverrideDEPRECATED @2: Float32;
|
||||
accelOverrideDEPRECATED @3: Float32;
|
||||
}
|
||||
|
||||
struct HUDControl {
|
||||
speedVisible @0: Bool;
|
||||
setSpeed @1: Float32;
|
||||
lanesVisible @2: Bool;
|
||||
leadVisible @3: Bool;
|
||||
visualAlert @4: VisualAlert;
|
||||
rightLaneVisible @6: Bool;
|
||||
leftLaneVisible @7: Bool;
|
||||
rightLaneDepart @8: Bool;
|
||||
leftLaneDepart @9: Bool;
|
||||
leadDistanceBars @10: Int8; # 1-3: 1 is closest, 3 is farthest. some ports may utilize 2-4 bars instead
|
||||
leadFollowTime @11: Float32;
|
||||
leadDistance @12: Float32;
|
||||
driverUnresponsive @13: Bool;
|
||||
activeCarrot @14: Int16;
|
||||
leadRelSpeed @15: Float32;
|
||||
leadDPath @16: Float32;
|
||||
leadRadar @17: Int16;
|
||||
modelDesire @18: Int16;
|
||||
atcDistance @19: Float32;
|
||||
|
||||
# not used with the dash, TODO: separate structs for dash UI and device UI
|
||||
audibleAlert @5: AudibleAlert;
|
||||
|
||||
enum VisualAlert {
|
||||
# these are the choices from the Honda
|
||||
# map as good as you can for your car
|
||||
none @0;
|
||||
fcw @1;
|
||||
steerRequired @2;
|
||||
brakePressed @3;
|
||||
wrongGear @4;
|
||||
seatbeltUnbuckled @5;
|
||||
speedTooHigh @6;
|
||||
ldw @7;
|
||||
}
|
||||
|
||||
enum AudibleAlert {
|
||||
none @0;
|
||||
|
||||
engage @1;
|
||||
disengage @2;
|
||||
refuse @3;
|
||||
|
||||
warningSoft @4;
|
||||
warningImmediate @5;
|
||||
|
||||
prompt @6;
|
||||
promptRepeat @7;
|
||||
promptDistracted @8;
|
||||
preAlert @9;
|
||||
}
|
||||
}
|
||||
|
||||
gasDEPRECATED @1 :Float32;
|
||||
brakeDEPRECATED @2 :Float32;
|
||||
steeringTorqueDEPRECATED @3 :Float32;
|
||||
activeDEPRECATED @7 :Bool;
|
||||
rollDEPRECATED @8 :Float32;
|
||||
pitchDEPRECATED @9 :Float32;
|
||||
actuatorsOutputDEPRECATED @10 :Actuators;
|
||||
}
|
||||
|
||||
struct CarOutput {
|
||||
# Any car specific rate limits or quirks applied by
|
||||
# the CarController are reflected in actuatorsOutput
|
||||
# and matches what is sent to the car
|
||||
actuatorsOutput @0 :CarControl.Actuators;
|
||||
}
|
||||
|
||||
# ****** car param ******
|
||||
|
||||
struct CarParams {
|
||||
brand @0 :Text; # Designates which group a platform falls under. Each folder in iqdbc/car is assigned one brand string
|
||||
carFingerprint @1 :Text;
|
||||
fuzzyFingerprint @55 :Bool;
|
||||
|
||||
notCar @66 :Bool; # flag for non-car robotics platforms
|
||||
|
||||
pcmCruise @3 :Bool; # is openpilot's state tied to the PCM's cruise state?
|
||||
enableBsm @56 :Bool; # blind spot monitoring
|
||||
flags @64 :UInt32; # flags for car specific quirks
|
||||
alphaLongitudinalAvailable @71 :Bool;
|
||||
extFlags @79 :UInt32; # carrotpilot HKG extension flags
|
||||
|
||||
minEnableSpeed @7 :Float32;
|
||||
minSteerSpeed @8 :Float32;
|
||||
steerAtStandstill @77 :Bool; # is steering available at standstill? just check if it faults
|
||||
safetyConfigs @62 :List(SafetyConfig);
|
||||
alternativeExperience @65 :Int16; # panda flag for features like no disengage on gas
|
||||
|
||||
# Car docs fields, not used for control
|
||||
maxLateralAccel @68 :Float32;
|
||||
autoResumeSng @69 :Bool; # describes whether car can resume from a stop automatically
|
||||
|
||||
# things about the car in the manual
|
||||
mass @17 :Float32; # [kg] curb weight: all fluids no cargo
|
||||
wheelbase @18 :Float32; # [m] distance from rear axle to front axle
|
||||
centerToFront @19 :Float32; # [m] distance from center of mass to front axle
|
||||
steerRatio @20 :Float32; # [] ratio of steering wheel angle to front wheel angle
|
||||
steerRatioRear @21 :Float32; # [] ratio of steering wheel angle to rear wheel angle (usually 0)
|
||||
|
||||
# things we can derive
|
||||
rotationalInertia @22 :Float32; # [kg*m2] body rotational inertia
|
||||
tireStiffnessFactor @72 :Float32; # scaling factor used in calculating tireStiffness[Front,Rear]
|
||||
tireStiffnessFront @23 :Float32; # [N/rad] front tire coeff of stiff
|
||||
tireStiffnessRear @24 :Float32; # [N/rad] rear tire coeff of stiff
|
||||
|
||||
longitudinalTuning @25 :LongitudinalPIDTuning;
|
||||
lateralParams @48 :LateralParams;
|
||||
lateralTuning :union {
|
||||
pid @26 :LateralPIDTuning;
|
||||
indiDEPRECATED @27 :LateralINDITuning;
|
||||
lqrDEPRECATED @40 :LateralLQRTuning;
|
||||
torque @67 :LateralTorqueTuning;
|
||||
}
|
||||
|
||||
steerLimitAlert @28 :Bool;
|
||||
steerLimitTimer @47 :Float32; # time before steerLimitAlert is issued
|
||||
|
||||
steerControlType @34 :SteerControlType;
|
||||
radarUnavailable @35 :Bool; # True when radar objects aren't visible on CAN or aren't parsed out
|
||||
stopAccel @60 :Float32; # Required acceleration to keep vehicle stationary
|
||||
|
||||
steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds
|
||||
longitudinalActuatorDelay @58 :Float32; # Gas/Brake actuator delay in seconds
|
||||
openpilotLongitudinalControl @37 :Bool; # is openpilot doing the longitudinal control?
|
||||
carVin @38 :Text; # VIN number queried during fingerprinting
|
||||
dashcamOnly @41: Bool;
|
||||
dashcamOnlyReason @78 :DashcamOnlyReason;
|
||||
passive @73: Bool; # is openpilot in control?
|
||||
transmissionType @43 :TransmissionType;
|
||||
carFw @44 :List(CarFw);
|
||||
|
||||
radarDelay @74 :Float32;
|
||||
fingerprintSource @49: FingerprintSource;
|
||||
networkLocation @50 :NetworkLocation; # Where Panda/C2 is integrated into the car's CAN network
|
||||
|
||||
wheelSpeedFactor @63 :Float32; # Multiplier on wheels speeds to computer actual speeds
|
||||
|
||||
secOcRequired @75 :Bool; # Car requires SecOC message authentication to operate
|
||||
secOcKeyAvailable @76 :Bool; # Stored SecOC key loaded from params
|
||||
|
||||
struct SafetyConfig {
|
||||
safetyModel @0 :SafetyModel;
|
||||
safetyParam @3 :UInt16;
|
||||
safetyParamDEPRECATED @1 :Int16;
|
||||
safetyParam2DEPRECATED @2 :UInt32;
|
||||
}
|
||||
|
||||
struct LateralParams {
|
||||
torqueBP @0 :List(Int32);
|
||||
torqueV @1 :List(Int32);
|
||||
}
|
||||
|
||||
struct LateralPIDTuning {
|
||||
kpBP @0 :List(Float32);
|
||||
kpV @1 :List(Float32);
|
||||
kiBP @2 :List(Float32);
|
||||
kiV @3 :List(Float32);
|
||||
kf @4 :Float32;
|
||||
}
|
||||
|
||||
struct LateralTorqueTuning {
|
||||
friction @3 :Float32;
|
||||
steeringAngleDeadzoneDeg @5 :Float32;
|
||||
latAccelFactor @6 :Float32;
|
||||
latAccelOffset @7 :Float32;
|
||||
useSteeringAngleDEPRECATED @0 :Bool;
|
||||
kpDEPRECATED @1 :Float32;
|
||||
kiDEPRECATED @2 :Float32;
|
||||
kfDEPRECATED @4 :Float32;
|
||||
kdDEPRECATED @8 : Float32;
|
||||
}
|
||||
|
||||
struct LongitudinalPIDTuning {
|
||||
kpBP @0 :List(Float32);
|
||||
kpV @1 :List(Float32);
|
||||
kiBP @2 :List(Float32);
|
||||
kiV @3 :List(Float32);
|
||||
kf @6 :Float32;
|
||||
deadzoneBPDEPRECATED @4 :List(Float32);
|
||||
deadzoneVDEPRECATED @5 :List(Float32);
|
||||
}
|
||||
|
||||
struct LateralINDITuning {
|
||||
outerLoopGainBP @4 :List(Float32);
|
||||
outerLoopGainV @5 :List(Float32);
|
||||
innerLoopGainBP @6 :List(Float32);
|
||||
innerLoopGainV @7 :List(Float32);
|
||||
timeConstantBP @8 :List(Float32);
|
||||
timeConstantV @9 :List(Float32);
|
||||
actuatorEffectivenessBP @10 :List(Float32);
|
||||
actuatorEffectivenessV @11 :List(Float32);
|
||||
|
||||
outerLoopGainDEPRECATED @0 :Float32;
|
||||
innerLoopGainDEPRECATED @1 :Float32;
|
||||
timeConstantDEPRECATED @2 :Float32;
|
||||
actuatorEffectivenessDEPRECATED @3 :Float32;
|
||||
}
|
||||
|
||||
struct LateralLQRTuning {
|
||||
scale @0 :Float32;
|
||||
ki @1 :Float32;
|
||||
dcGain @2 :Float32;
|
||||
|
||||
# State space system
|
||||
a @3 :List(Float32);
|
||||
b @4 :List(Float32);
|
||||
c @5 :List(Float32);
|
||||
|
||||
k @6 :List(Float32); # LQR gain
|
||||
l @7 :List(Float32); # Kalman gain
|
||||
}
|
||||
|
||||
enum SafetyModel {
|
||||
silent @0;
|
||||
hondaNidec @1;
|
||||
toyota @2;
|
||||
elm327 @3;
|
||||
gm @4;
|
||||
hondaBoschGiraffe @5;
|
||||
ford @6;
|
||||
cadillac @7;
|
||||
hyundai @8;
|
||||
chrysler @9;
|
||||
tesla @10;
|
||||
subaru @11;
|
||||
gmPassive @12;
|
||||
mazda @13;
|
||||
nissan @14;
|
||||
volkswagen @15;
|
||||
toyotaIpas @16;
|
||||
allOutput @17;
|
||||
gmAscm @18;
|
||||
noOutput @19; # like silent but without silent CAN TXs
|
||||
hondaBosch @20;
|
||||
volkswagenPq @21;
|
||||
subaruPreglobal @22; # pre-Global platform
|
||||
hyundaiLegacy @23;
|
||||
hyundaiCommunity @24;
|
||||
volkswagenMlb @25;
|
||||
hongqi @26;
|
||||
body @27;
|
||||
hyundaiCanfd @28;
|
||||
volkswagenMqbEvo @29;
|
||||
chryslerCusw @30;
|
||||
psa @31;
|
||||
fcaGiorgio @32;
|
||||
rivian @33;
|
||||
volkswagenMeb @34;
|
||||
byd @35;
|
||||
}
|
||||
|
||||
enum SteerControlType {
|
||||
torque @0;
|
||||
angle @1;
|
||||
|
||||
curvatureDEPRECATED @2;
|
||||
}
|
||||
|
||||
enum TransmissionType {
|
||||
unknown @0;
|
||||
automatic @1; # Traditional auto, including DSG
|
||||
manual @2; # True "stick shift" only
|
||||
direct @3; # Electric vehicle or other direct drive
|
||||
cvt @4;
|
||||
}
|
||||
|
||||
enum DashcamOnlyReason {
|
||||
unknown @0;
|
||||
radarDisableEngineOn @1;
|
||||
}
|
||||
|
||||
struct CarFw {
|
||||
ecu @0 :Ecu;
|
||||
fwVersion @1 :Data;
|
||||
address @2 :UInt32;
|
||||
subAddress @3 :UInt8;
|
||||
responseAddress @4 :UInt32;
|
||||
request @5 :List(Data);
|
||||
brand @6 :Text;
|
||||
bus @7 :UInt8;
|
||||
logging @8 :Bool;
|
||||
obdMultiplexing @9 :Bool;
|
||||
}
|
||||
|
||||
enum Ecu {
|
||||
eps @0;
|
||||
abs @1;
|
||||
fwdRadar @2;
|
||||
fwdCamera @3;
|
||||
engine @4;
|
||||
unknown @5;
|
||||
transmission @8; # Transmission Control Module
|
||||
hybrid @18; # hybrid control unit, e.g. Chrysler's HCP, Honda's IMA Control Unit, Toyota's hybrid control computer
|
||||
inverter @25; # inverter for electric engine
|
||||
srs @9; # airbag
|
||||
gateway @10; # can gateway
|
||||
hud @11; # heads up display
|
||||
combinationMeter @12; # instrument cluster
|
||||
electricBrakeBooster @15;
|
||||
shiftByWire @16;
|
||||
adas @19;
|
||||
cornerRadar @21;
|
||||
hvac @20;
|
||||
parkingAdas @7; # parking assist system ECU, e.g. Toyota's IPAS, Hyundai's RSPA, etc.
|
||||
epb @22; # electronic parking brake
|
||||
telematics @23;
|
||||
body @24; # body control module
|
||||
|
||||
# Toyota only
|
||||
dsu @6;
|
||||
|
||||
# Honda only
|
||||
vsa @13; # Vehicle Stability Assist
|
||||
programmedFuelInjection @14;
|
||||
|
||||
debug @17;
|
||||
}
|
||||
|
||||
enum FingerprintSource {
|
||||
can @0;
|
||||
fw @1;
|
||||
fixed @2;
|
||||
}
|
||||
|
||||
enum NetworkLocation {
|
||||
fwdCamera @0; # Standard/default integration at LKAS camera
|
||||
gateway @1; # Integration at vehicle's CAN gateway
|
||||
}
|
||||
|
||||
enableGasInterceptorDEPRECATED @2 :Bool;
|
||||
enableCameraDEPRECATED @4 :Bool;
|
||||
enableApgsDEPRECATED @6 :Bool;
|
||||
steerRateCostDEPRECATED @33 :Float32;
|
||||
isPandaBlackDEPRECATED @39 :Bool;
|
||||
hasStockCameraDEPRECATED @57 :Bool;
|
||||
safetyParamDEPRECATED @10 :Int16;
|
||||
safetyModelDEPRECATED @9 :SafetyModel;
|
||||
safetyModelPassiveDEPRECATED @42 :SafetyModel = silent;
|
||||
minSpeedCanDEPRECATED @51 :Float32;
|
||||
communityFeatureDEPRECATED @46: Bool;
|
||||
startingAccelRateDEPRECATED @53 :Float32;
|
||||
steerMaxBPDEPRECATED @11 :List(Float32);
|
||||
steerMaxVDEPRECATED @12 :List(Float32);
|
||||
gasMaxBPDEPRECATED @13 :List(Float32);
|
||||
gasMaxVDEPRECATED @14 :List(Float32);
|
||||
brakeMaxBPDEPRECATED @15 :List(Float32);
|
||||
brakeMaxVDEPRECATED @16 :List(Float32);
|
||||
directAccelControlDEPRECATED @30 :Bool;
|
||||
maxSteeringAngleDegDEPRECATED @54 :Float32;
|
||||
longitudinalActuatorDelayLowerBoundDEPRECATED @61 :Float32;
|
||||
stoppingControlDEPRECATED @31 :Bool; # Does the car allow full control even at lows speeds when stopping
|
||||
radarTimeStep @45: Float32 = 0.05; # time delta between radar updates, 20Hz is very standard
|
||||
enableDsuDEPRECATED @5 :Bool; # driving support unit
|
||||
vEgoStarting @59 :Float32;
|
||||
startAccel @32 :Float32;
|
||||
startingState @70 :Bool;
|
||||
vEgoStoppingDEPRECATED @29 :Float32;
|
||||
stoppingDecelRateDEPRECATED @52 :Float32;
|
||||
}
|
||||
181
artifacts/package_sources/iqdbc/iqdbc/car/car_helpers.py
Normal file
181
artifacts/package_sources/iqdbc/iqdbc/car/car_helpers.py
Normal file
@@ -0,0 +1,181 @@
|
||||
import os
|
||||
import time
|
||||
|
||||
from iqdbc.car import gen_empty_fingerprint
|
||||
from iqdbc.car.can_definitions import CanRecvCallable, CanSendCallable
|
||||
from iqdbc.car.carlog import carlog
|
||||
from iqdbc.car.structs import CarParams, CarParamsT
|
||||
from iqdbc.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
|
||||
from iqdbc.car.fw_versions import ObdCallback, get_fw_versions_ordered, get_present_ecus, match_fw_to_car
|
||||
from iqdbc.car.mock.values import CAR as MOCK
|
||||
from iqdbc.car.values import BRANDS
|
||||
from iqdbc.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN
|
||||
|
||||
from iqdbc.lvbs.car.interfaces import apply_iq_car_config as iqpilot_interfaces
|
||||
|
||||
FRAME_FINGERPRINT = 100 # 1s
|
||||
|
||||
|
||||
def load_interfaces(brand_names):
|
||||
ret = {}
|
||||
for brand_name in brand_names:
|
||||
path = f'iqdbc.car.{brand_name}'
|
||||
CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface
|
||||
for model_name in brand_names[brand_name]:
|
||||
ret[model_name] = CarInterface
|
||||
return ret
|
||||
|
||||
|
||||
def _get_interface_names() -> dict[str, list[str]]:
|
||||
# returns a dict of brand name and its respective models
|
||||
brand_names = {}
|
||||
for brand in BRANDS:
|
||||
brand_name = brand.__module__.split('.')[-2]
|
||||
brand_names[brand_name] = [model.value for model in brand]
|
||||
|
||||
return brand_names
|
||||
|
||||
|
||||
# imports from directory iqdbc/car/<name>/
|
||||
interface_names = _get_interface_names()
|
||||
interfaces = load_interfaces(interface_names)
|
||||
|
||||
|
||||
def can_fingerprint(can_recv: CanRecvCallable) -> tuple[str | None, dict[int, dict]]:
|
||||
finger = gen_empty_fingerprint()
|
||||
candidate_cars = {i: all_legacy_fingerprint_cars() for i in [0, 1]} # attempt fingerprint on both bus 0 and 1
|
||||
frame = 0
|
||||
car_fingerprint = None
|
||||
done = False
|
||||
|
||||
while not done:
|
||||
# can_recv(wait_for_one=True) may return zero or multiple packets, so we increment frame for each one we receive
|
||||
can_packets = can_recv(wait_for_one=True)
|
||||
for can_packet in can_packets:
|
||||
for can in can_packet:
|
||||
# The fingerprint dict is generated for all buses, this way the car interface
|
||||
# can use it to detect a (valid) multipanda setup and initialize accordingly
|
||||
if can.src < 128:
|
||||
if can.src not in finger:
|
||||
finger[can.src] = {}
|
||||
finger[can.src][can.address] = len(can.dat)
|
||||
|
||||
for b in candidate_cars:
|
||||
# Ignore extended messages and VIN query response.
|
||||
if can.src == b and can.address < 0x800 and can.address not in (0x7df, 0x7e0, 0x7e8):
|
||||
candidate_cars[b] = eliminate_incompatible_cars(can, candidate_cars[b])
|
||||
|
||||
# if we only have one car choice and the time since we got our first
|
||||
# message has elapsed, exit
|
||||
for b in candidate_cars:
|
||||
if len(candidate_cars[b]) == 1 and frame > FRAME_FINGERPRINT:
|
||||
# fingerprint done
|
||||
car_fingerprint = candidate_cars[b][0]
|
||||
|
||||
# bail if no cars left or we've been waiting for more than 2s
|
||||
failed = (all(len(cc) == 0 for cc in candidate_cars.values()) and frame > FRAME_FINGERPRINT) or frame > 200
|
||||
succeeded = car_fingerprint is not None
|
||||
done = failed or succeeded
|
||||
|
||||
frame += 1
|
||||
|
||||
return car_fingerprint, finger
|
||||
|
||||
|
||||
# **** for use live only ****
|
||||
def fingerprint(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, num_pandas: int,
|
||||
cached_params: CarParamsT | None,
|
||||
fixed_fingerprint: str | None) -> tuple[str | None, dict, str, list[CarParams.CarFw], CarParams.FingerprintSource, bool]:
|
||||
fixed_fingerprint = fixed_fingerprint or os.environ.get('FINGERPRINT', "")
|
||||
skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) or bool(fixed_fingerprint)
|
||||
disable_fw_cache = os.environ.get('DISABLE_FW_CACHE', False)
|
||||
ecu_rx_addrs = set()
|
||||
|
||||
start_time = time.monotonic()
|
||||
if not skip_fw_query:
|
||||
if cached_params is not None and cached_params.brand != "mock" and len(cached_params.carFw) > 0 and \
|
||||
cached_params.carVin is not VIN_UNKNOWN and not disable_fw_cache:
|
||||
carlog.warning("Using cached CarParams")
|
||||
vin_rx_addr, vin_rx_bus, vin = -1, -1, cached_params.carVin
|
||||
car_fw = list(cached_params.carFw)
|
||||
cached = True
|
||||
else:
|
||||
carlog.warning("Getting VIN & FW versions")
|
||||
# enable OBD multiplexing for VIN query
|
||||
# NOTE: this takes ~0.1s and is relied on to allow sendcan subscriber to connect in time
|
||||
set_obd_multiplexing(True)
|
||||
# VIN query only reliably works through OBDII
|
||||
vin_rx_addr, vin_rx_bus, vin = get_vin(can_recv, can_send, (0, 1))
|
||||
ecu_rx_addrs = get_present_ecus(can_recv, can_send, set_obd_multiplexing, num_pandas=num_pandas)
|
||||
car_fw = get_fw_versions_ordered(can_recv, can_send, set_obd_multiplexing, vin, ecu_rx_addrs, num_pandas=num_pandas)
|
||||
cached = False
|
||||
|
||||
exact_fw_match, fw_candidates = match_fw_to_car(car_fw, vin)
|
||||
else:
|
||||
vin_rx_addr, vin_rx_bus, vin = -1, -1, VIN_UNKNOWN
|
||||
exact_fw_match, fw_candidates, car_fw = True, set(), []
|
||||
cached = False
|
||||
|
||||
if not is_valid_vin(vin):
|
||||
carlog.error({"event": "Malformed VIN", "vin": vin})
|
||||
vin = VIN_UNKNOWN
|
||||
carlog.warning("VIN %s", vin)
|
||||
|
||||
# disable OBD multiplexing for CAN fingerprinting and potential ECU knockouts
|
||||
set_obd_multiplexing(False)
|
||||
|
||||
fw_query_time = time.monotonic() - start_time
|
||||
|
||||
# CAN fingerprint
|
||||
# drain CAN socket so we get the latest messages
|
||||
can_recv()
|
||||
car_fingerprint, finger = can_fingerprint(can_recv)
|
||||
|
||||
exact_match = True
|
||||
source = CarParams.FingerprintSource.can
|
||||
|
||||
# If FW query returns exactly 1 candidate, use it
|
||||
if len(fw_candidates) == 1:
|
||||
car_fingerprint = list(fw_candidates)[0]
|
||||
source = CarParams.FingerprintSource.fw
|
||||
exact_match = exact_fw_match
|
||||
|
||||
if fixed_fingerprint:
|
||||
car_fingerprint = fixed_fingerprint
|
||||
source = CarParams.FingerprintSource.fixed
|
||||
|
||||
carlog.error({"event": "fingerprinted", "car_fingerprint": str(car_fingerprint), "source": source, "fuzzy": not exact_match,
|
||||
"cached": cached, "fw_count": len(car_fw), "ecu_responses": list(ecu_rx_addrs), "vin_rx_addr": vin_rx_addr,
|
||||
"vin_rx_bus": vin_rx_bus, "fingerprints": repr(finger), "fw_query_time": fw_query_time})
|
||||
|
||||
return car_fingerprint, finger, vin, car_fw, source, exact_match
|
||||
|
||||
|
||||
def get_car(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, alpha_long_allowed: bool,
|
||||
is_release: bool, num_pandas: int = 1, cached_params: CarParamsT | None = None,
|
||||
fixed_fingerprint: str | None = None, init_params_list_iq: list[dict[str, str]] | None = None, is_release_iq: bool = False):
|
||||
candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(can_recv, can_send, set_obd_multiplexing, num_pandas, cached_params,
|
||||
fixed_fingerprint)
|
||||
|
||||
if candidate is None:
|
||||
carlog.error({"event": "car doesn't match any fingerprints", "fingerprints": repr(fingerprints)})
|
||||
candidate = "MOCK"
|
||||
|
||||
CarInterface = interfaces[candidate]
|
||||
CP: CarParams = CarInterface.get_params(candidate, fingerprints, car_fw, alpha_long_allowed, is_release, docs=False)
|
||||
CP.carVin = vin
|
||||
CP.carFw = car_fw
|
||||
CP.fingerprintSource = source
|
||||
CP.fuzzyFingerprint = not exact_match
|
||||
CP_IQ = CarInterface.get_params_iq(CP, candidate, fingerprints, car_fw, alpha_long_allowed, is_release_iq, docs=False)
|
||||
|
||||
iqpilot_interfaces(CarInterface, CP, CP_IQ, init_params_list_iq, can_recv, can_send)
|
||||
|
||||
return interfaces[CP.carFingerprint](CP, CP_IQ)
|
||||
|
||||
|
||||
def get_demo_car_params():
|
||||
platform = MOCK.MOCK
|
||||
CarInterface = interfaces[platform]
|
||||
CP = CarInterface.get_non_essential_params(platform)
|
||||
return CP
|
||||
12
artifacts/package_sources/iqdbc/iqdbc/car/carlog.py
Normal file
12
artifacts/package_sources/iqdbc/iqdbc/car/carlog.py
Normal file
@@ -0,0 +1,12 @@
|
||||
import os
|
||||
import logging
|
||||
|
||||
# set up logging
|
||||
LOGPRINT = os.environ.get('LOGPRINT', 'INFO').upper()
|
||||
carlog = logging.getLogger('carlog')
|
||||
carlog.setLevel(LOGPRINT)
|
||||
carlog.propagate = False
|
||||
|
||||
handler = logging.StreamHandler()
|
||||
handler.setFormatter(logging.Formatter('%(message)s'))
|
||||
carlog.addHandler(handler)
|
||||
385
artifacts/package_sources/iqdbc/iqdbc/car/ccp.py
Normal file
385
artifacts/package_sources/iqdbc/iqdbc/car/ccp.py
Normal file
@@ -0,0 +1,385 @@
|
||||
import sys
|
||||
import time
|
||||
import struct
|
||||
from enum import IntEnum, Enum
|
||||
from dataclasses import dataclass
|
||||
|
||||
|
||||
@dataclass
|
||||
class ExchangeStationIdsReturn:
|
||||
id_length: int
|
||||
data_type: int
|
||||
available: int
|
||||
protected: int
|
||||
|
||||
|
||||
@dataclass
|
||||
class GetDaqListSizeReturn:
|
||||
list_size: int
|
||||
first_pid: int
|
||||
|
||||
|
||||
@dataclass
|
||||
class GetSessionStatusReturn:
|
||||
status: int
|
||||
info: int | None
|
||||
|
||||
|
||||
@dataclass
|
||||
class DiagnosticServiceReturn:
|
||||
length: int
|
||||
type: int
|
||||
|
||||
|
||||
@dataclass
|
||||
class ActionServiceReturn:
|
||||
length: int
|
||||
type: int
|
||||
|
||||
|
||||
class COMMAND_CODE(IntEnum):
|
||||
CONNECT = 0x01
|
||||
SET_MTA = 0x02
|
||||
DNLOAD = 0x03
|
||||
UPLOAD = 0x04
|
||||
TEST = 0x05
|
||||
START_STOP = 0x06
|
||||
DISCONNECT = 0x07
|
||||
START_STOP_ALL = 0x08
|
||||
GET_ACTIVE_CAL_PAGE = 0x09
|
||||
SET_S_STATUS = 0x0C
|
||||
GET_S_STATUS = 0x0D
|
||||
BUILD_CHKSUM = 0x0E
|
||||
SHORT_UP = 0x0F
|
||||
CLEAR_MEMORY = 0x10
|
||||
SELECT_CAL_PAGE = 0x11
|
||||
GET_SEED = 0x12
|
||||
UNLOCK = 0x13
|
||||
GET_DAQ_SIZE = 0x14
|
||||
SET_DAQ_PTR = 0x15
|
||||
WRITE_DAQ = 0x16
|
||||
EXCHANGE_ID = 0x17
|
||||
PROGRAM = 0x18
|
||||
MOVE = 0x19
|
||||
GET_CCP_VERSION = 0x1B
|
||||
DIAG_SERVICE = 0x20
|
||||
ACTION_SERVICE = 0x21
|
||||
PROGRAM_6 = 0x22
|
||||
DNLOAD_6 = 0x23
|
||||
|
||||
|
||||
COMMAND_RETURN_CODES = {
|
||||
0x00: "acknowledge / no error",
|
||||
0x01: "DAQ processor overload",
|
||||
0x10: "command processor busy",
|
||||
0x11: "DAQ processor busy",
|
||||
0x12: "internal timeout",
|
||||
0x18: "key request",
|
||||
0x19: "session status request",
|
||||
0x20: "cold start request",
|
||||
0x21: "cal. data init. request",
|
||||
0x22: "DAQ list init. request",
|
||||
0x23: "code update request",
|
||||
0x30: "unknown command",
|
||||
0x31: "command syntax",
|
||||
0x32: "parameter(s) out of range",
|
||||
0x33: "access denied",
|
||||
0x34: "overload",
|
||||
0x35: "access locked",
|
||||
0x36: "resource/function not available",
|
||||
}
|
||||
|
||||
|
||||
class BYTE_ORDER(Enum):
|
||||
LITTLE_ENDIAN = '<'
|
||||
BIG_ENDIAN = '>'
|
||||
|
||||
|
||||
class CommandTimeoutError(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class CommandCounterError(Exception):
|
||||
pass
|
||||
|
||||
|
||||
class CommandResponseError(Exception):
|
||||
def __init__(self, message, return_code):
|
||||
super().__init__()
|
||||
self.message = message
|
||||
self.return_code = return_code
|
||||
|
||||
def __str__(self):
|
||||
return self.message
|
||||
|
||||
|
||||
class CcpClient:
|
||||
def __init__(self, panda, tx_addr: int, rx_addr: int, bus: int=0, byte_order: BYTE_ORDER=BYTE_ORDER.BIG_ENDIAN, debug=False):
|
||||
self.tx_addr = tx_addr
|
||||
self.rx_addr = rx_addr
|
||||
self.can_bus = bus
|
||||
self.byte_order = byte_order
|
||||
self.debug = debug
|
||||
self._panda = panda
|
||||
self._command_counter = -1
|
||||
|
||||
def _send_cro(self, cmd: int, dat: bytes = b"") -> None:
|
||||
self._command_counter = (self._command_counter + 1) & 0xFF
|
||||
tx_data = (bytes([cmd, self._command_counter]) + dat).ljust(8, b"\x00")
|
||||
if self.debug:
|
||||
print(f"CAN-TX: {hex(self.tx_addr)} - 0x{bytes.hex(tx_data)}")
|
||||
assert len(tx_data) == 8, "data is not 8 bytes"
|
||||
self._panda.can_clear(self.can_bus)
|
||||
self._panda.can_clear(0xFFFF)
|
||||
self._panda.can_send(self.tx_addr, tx_data, self.can_bus)
|
||||
|
||||
def _recv_dto(self, timeout: float) -> bytes:
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < timeout:
|
||||
msgs = self._panda.can_recv() or []
|
||||
if len(msgs) >= 256:
|
||||
print("CAN RX buffer overflow!!!", file=sys.stderr)
|
||||
for rx_addr, rx_data_bytearray, rx_bus in msgs:
|
||||
if rx_bus == self.can_bus and rx_addr == self.rx_addr:
|
||||
rx_data = bytes(rx_data_bytearray)
|
||||
if self.debug:
|
||||
print(f"CAN-RX: {hex(rx_addr)} - 0x{bytes.hex(rx_data)}")
|
||||
assert len(rx_data) == 8, f"message length not 8: {len(rx_data)}"
|
||||
|
||||
pid = rx_data[0]
|
||||
if pid == 0xFF or pid == 0xFE:
|
||||
err = rx_data[1]
|
||||
err_desc = COMMAND_RETURN_CODES.get(err, "unknown error")
|
||||
ctr = rx_data[2]
|
||||
dat = rx_data[3:]
|
||||
|
||||
if pid == 0xFF and self._command_counter != ctr:
|
||||
raise CommandCounterError(f"counter invalid: {ctr} != {self._command_counter}")
|
||||
|
||||
if err >= 0x10 and err <= 0x12:
|
||||
if self.debug:
|
||||
print(f"CCP-WAIT: {hex(err)} - {err_desc}")
|
||||
start_time = time.time()
|
||||
continue
|
||||
|
||||
if err >= 0x30:
|
||||
raise CommandResponseError(f"{hex(err)} - {err_desc}", err)
|
||||
else:
|
||||
dat = rx_data[1:]
|
||||
|
||||
return dat
|
||||
time.sleep(0.001)
|
||||
|
||||
raise CommandTimeoutError("timeout waiting for response")
|
||||
|
||||
# commands
|
||||
def connect(self, station_addr: int) -> None:
|
||||
if station_addr > 65535:
|
||||
raise ValueError("station address must be less than 65536")
|
||||
# NOTE: station address is always little endian
|
||||
self._send_cro(COMMAND_CODE.CONNECT, struct.pack("<H", station_addr))
|
||||
self._recv_dto(0.025)
|
||||
|
||||
def exchange_station_ids(self, device_id_info: bytes = b"") -> ExchangeStationIdsReturn:
|
||||
self._send_cro(COMMAND_CODE.EXCHANGE_ID, device_id_info)
|
||||
resp = self._recv_dto(0.025)
|
||||
return ExchangeStationIdsReturn(id_length=resp[0], data_type=resp[1], available=resp[2], protected=resp[3])
|
||||
|
||||
def get_seed(self, resource_mask: int) -> bytes:
|
||||
if resource_mask > 255:
|
||||
raise ValueError("resource mask must be less than 256")
|
||||
self._send_cro(COMMAND_CODE.GET_SEED, bytes([resource_mask]))
|
||||
resp = self._recv_dto(0.025)
|
||||
# protected = resp[0] == 0
|
||||
seed = resp[1:]
|
||||
return seed
|
||||
|
||||
def unlock(self, key: bytes) -> int:
|
||||
if len(key) > 6:
|
||||
raise ValueError("max key size is 6 bytes")
|
||||
self._send_cro(COMMAND_CODE.UNLOCK, key)
|
||||
resp = self._recv_dto(0.025)
|
||||
status = resp[0]
|
||||
return status
|
||||
|
||||
def set_memory_transfer_address(self, mta_num: int, addr_ext: int, addr: int) -> None:
|
||||
if mta_num > 255:
|
||||
raise ValueError("MTA number must be less than 256")
|
||||
if addr_ext > 255:
|
||||
raise ValueError("address extension must be less than 256")
|
||||
self._send_cro(COMMAND_CODE.SET_MTA, bytes([mta_num, addr_ext]) + struct.pack(f"{self.byte_order.value}I", addr))
|
||||
self._recv_dto(0.025)
|
||||
|
||||
def download(self, data: bytes) -> int:
|
||||
if len(data) > 5:
|
||||
raise ValueError("max data size is 5 bytes")
|
||||
self._send_cro(COMMAND_CODE.DNLOAD, bytes([len(data)]) + data)
|
||||
resp = self._recv_dto(0.025)
|
||||
# mta_addr_ext = resp[0]
|
||||
mta_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
|
||||
return mta_addr
|
||||
|
||||
def download_6_bytes(self, data: bytes) -> int:
|
||||
if len(data) != 6:
|
||||
raise ValueError("data size must be 6 bytes")
|
||||
self._send_cro(COMMAND_CODE.DNLOAD_6, data)
|
||||
resp = self._recv_dto(0.025)
|
||||
# mta_addr_ext = resp[0]
|
||||
mta_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
|
||||
return mta_addr
|
||||
|
||||
def upload(self, size: int) -> bytes:
|
||||
if size > 5:
|
||||
raise ValueError("size must be less than 6")
|
||||
self._send_cro(COMMAND_CODE.UPLOAD, bytes([size]))
|
||||
return self._recv_dto(0.025)[:size]
|
||||
|
||||
def short_upload(self, size: int, addr_ext: int, addr: int) -> bytes:
|
||||
if size > 5:
|
||||
raise ValueError("size must be less than 6")
|
||||
if addr_ext > 255:
|
||||
raise ValueError("address extension must be less than 256")
|
||||
self._send_cro(COMMAND_CODE.SHORT_UP, bytes([size, addr_ext]) + struct.pack(f"{self.byte_order.value}I", addr))
|
||||
return self._recv_dto(0.025)[:size]
|
||||
|
||||
def select_calibration_page(self) -> None:
|
||||
self._send_cro(COMMAND_CODE.SELECT_CAL_PAGE)
|
||||
self._recv_dto(0.025)
|
||||
|
||||
def get_daq_list_size(self, list_num: int, can_id: int = 0) -> GetDaqListSizeReturn:
|
||||
if list_num > 255:
|
||||
raise ValueError("list number must be less than 256")
|
||||
self._send_cro(COMMAND_CODE.GET_DAQ_SIZE, bytes([list_num, 0]) + struct.pack(f"{self.byte_order.value}I", can_id))
|
||||
resp = self._recv_dto(0.025)
|
||||
return GetDaqListSizeReturn(list_size=resp[0], first_pid=resp[1])
|
||||
|
||||
def set_daq_list_pointer(self, list_num: int, odt_num: int, element_num: int) -> None:
|
||||
if list_num > 255:
|
||||
raise ValueError("list number must be less than 256")
|
||||
if odt_num > 255:
|
||||
raise ValueError("ODT number must be less than 256")
|
||||
if element_num > 255:
|
||||
raise ValueError("element number must be less than 256")
|
||||
self._send_cro(COMMAND_CODE.SET_DAQ_PTR, bytes([list_num, odt_num, element_num]))
|
||||
self._recv_dto(0.025)
|
||||
|
||||
def write_daq_list_entry(self, size: int, addr_ext: int, addr: int) -> None:
|
||||
if size > 255:
|
||||
raise ValueError("size must be less than 256")
|
||||
if addr_ext > 255:
|
||||
raise ValueError("address extension must be less than 256")
|
||||
self._send_cro(COMMAND_CODE.WRITE_DAQ, bytes([size, addr_ext]) + struct.pack(f"{self.byte_order.value}I", addr))
|
||||
self._recv_dto(0.025)
|
||||
|
||||
def start_stop_transmission(self, mode: int, list_num: int, odt_num: int, channel_num: int, rate_prescaler: int = 0) -> None:
|
||||
if mode > 255:
|
||||
raise ValueError("mode must be less than 256")
|
||||
if list_num > 255:
|
||||
raise ValueError("list number must be less than 256")
|
||||
if odt_num > 255:
|
||||
raise ValueError("ODT number must be less than 256")
|
||||
if channel_num > 255:
|
||||
raise ValueError("channel number must be less than 256")
|
||||
if rate_prescaler > 65535:
|
||||
raise ValueError("rate prescaler must be less than 65536")
|
||||
self._send_cro(COMMAND_CODE.START_STOP, bytes([mode, list_num, odt_num, channel_num]) + struct.pack(f"{self.byte_order.value}H", rate_prescaler))
|
||||
self._recv_dto(0.025)
|
||||
|
||||
def disconnect(self, station_addr: int, temporary: bool = False) -> None:
|
||||
if station_addr > 65535:
|
||||
raise ValueError("station address must be less than 65536")
|
||||
# NOTE: station address is always little endian
|
||||
self._send_cro(COMMAND_CODE.DISCONNECT, bytes([int(not temporary), 0x00]) + struct.pack("<H", station_addr))
|
||||
self._recv_dto(0.025)
|
||||
|
||||
def set_session_status(self, status: int) -> None:
|
||||
if status > 255:
|
||||
raise ValueError("status must be less than 256")
|
||||
self._send_cro(COMMAND_CODE.SET_S_STATUS, bytes([status]))
|
||||
self._recv_dto(0.025)
|
||||
|
||||
def get_session_status(self) -> GetSessionStatusReturn:
|
||||
self._send_cro(COMMAND_CODE.GET_S_STATUS)
|
||||
resp = self._recv_dto(0.025)
|
||||
info = resp[2] if resp[1] else None
|
||||
return GetSessionStatusReturn(status=resp[0], info=info)
|
||||
|
||||
def build_checksum(self, size: int) -> bytes:
|
||||
self._send_cro(COMMAND_CODE.BUILD_CHKSUM, struct.pack(f"{self.byte_order.value}I", size))
|
||||
resp = self._recv_dto(30.0)
|
||||
chksum_size = resp[0]
|
||||
assert chksum_size <= 4, "checksum more than 4 bytes"
|
||||
chksum = resp[1:1+chksum_size]
|
||||
return chksum
|
||||
|
||||
def clear_memory(self, size: int) -> None:
|
||||
self._send_cro(COMMAND_CODE.CLEAR_MEMORY, struct.pack(f"{self.byte_order.value}I", size))
|
||||
self._recv_dto(30.0)
|
||||
|
||||
def program(self, size: int, data: bytes) -> int:
|
||||
if size > 5:
|
||||
raise ValueError("size must be less than 6")
|
||||
if len(data) > 5:
|
||||
raise ValueError("max data size is 5 bytes")
|
||||
self._send_cro(COMMAND_CODE.PROGRAM, bytes([size]) + data)
|
||||
resp = self._recv_dto(0.1)
|
||||
# mta_addr_ext = resp[0]
|
||||
mta_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
|
||||
return mta_addr
|
||||
|
||||
def program_6_bytes(self, data: bytes) -> int:
|
||||
if len(data) != 6:
|
||||
raise ValueError("data size must be 6 bytes")
|
||||
self._send_cro(COMMAND_CODE.PROGRAM_6, data)
|
||||
resp = self._recv_dto(0.1)
|
||||
# mta_addr_ext = resp[0]
|
||||
mta_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
|
||||
return mta_addr
|
||||
|
||||
def move_memory_block(self, size: int) -> None:
|
||||
self._send_cro(COMMAND_CODE.MOVE, struct.pack(f"{self.byte_order.value}I", size))
|
||||
self._recv_dto(0.025)
|
||||
|
||||
def diagnostic_service(self, service_num: int, data: bytes = b"") -> DiagnosticServiceReturn:
|
||||
if service_num > 65535:
|
||||
raise ValueError("service number must be less than 65536")
|
||||
if len(data) > 4:
|
||||
raise ValueError("max data size is 4 bytes")
|
||||
self._send_cro(COMMAND_CODE.DIAG_SERVICE, struct.pack(f"{self.byte_order.value}H", service_num) + data)
|
||||
resp = self._recv_dto(0.025)
|
||||
return DiagnosticServiceReturn(length=resp[0], type=resp[1])
|
||||
|
||||
def action_service(self, service_num: int, data: bytes = b"") -> ActionServiceReturn:
|
||||
if service_num > 65535:
|
||||
raise ValueError("service number must be less than 65536")
|
||||
if len(data) > 4:
|
||||
raise ValueError("max data size is 4 bytes")
|
||||
self._send_cro(COMMAND_CODE.ACTION_SERVICE, struct.pack(f"{self.byte_order.value}H", service_num) + data)
|
||||
resp = self._recv_dto(0.025)
|
||||
return ActionServiceReturn(length=resp[0], type=resp[1])
|
||||
|
||||
def test_availability(self, station_addr: int) -> None:
|
||||
if station_addr > 65535:
|
||||
raise ValueError("station address must be less than 65536")
|
||||
# NOTE: station address is always little endian
|
||||
self._send_cro(COMMAND_CODE.TEST, struct.pack("<H", station_addr))
|
||||
self._recv_dto(0.025)
|
||||
|
||||
def start_stop_synchronised_transmission(self, mode: int) -> None:
|
||||
if mode > 255:
|
||||
raise ValueError("mode must be less than 256")
|
||||
self._send_cro(COMMAND_CODE.START_STOP_ALL, bytes([mode]))
|
||||
self._recv_dto(0.025)
|
||||
|
||||
def get_active_calibration_page(self):
|
||||
self._send_cro(COMMAND_CODE.GET_ACTIVE_CAL_PAGE)
|
||||
resp = self._recv_dto(0.025)
|
||||
# cal_addr_ext = resp[0]
|
||||
cal_addr = struct.unpack(f"{self.byte_order.value}I", resp[1:5])[0]
|
||||
return cal_addr
|
||||
|
||||
def get_version(self, desired_version: float = 2.1) -> float:
|
||||
major, minor = map(int, str(desired_version).split("."))
|
||||
self._send_cro(COMMAND_CODE.GET_CCP_VERSION, bytes([major, minor]))
|
||||
resp = self._recv_dto(0.025)
|
||||
return float(f"{resp[0]}.{resp[1]}")
|
||||
@@ -0,0 +1,96 @@
|
||||
from iqdbc.can import CANPacker
|
||||
from iqdbc.car import Bus, DT_CTRL
|
||||
from iqdbc.car.lateral import apply_meas_steer_torque_limits
|
||||
from iqdbc.car.chrysler import chryslercan
|
||||
from iqdbc.car.chrysler.values import RAM_CARS, CarControllerParams, ChryslerFlags, RAM_DT
|
||||
from iqdbc.car.interfaces import CarControllerBase
|
||||
|
||||
from iqdbc.lvbs.car.chrysler.iq_carcontroller import IQCarController
|
||||
from iqdbc.lvbs.car.chrysler.aol import AolCarController
|
||||
from iqdbc.lvbs.car.chrysler.iq_values import ChryslerFlagsIQ
|
||||
|
||||
|
||||
class CarController(CarControllerBase, AolCarController, IQCarController):
|
||||
def __init__(self, dbc_names, CP, CP_IQ):
|
||||
CarControllerBase.__init__(self, dbc_names, CP, CP_IQ)
|
||||
AolCarController.__init__(self)
|
||||
IQCarController.__init__(self, CP, CP_IQ)
|
||||
self.apply_torque_last = 0
|
||||
|
||||
self.hud_count = 0
|
||||
self.last_lkas_falling_edge = 0
|
||||
self.lkas_control_bit_prev = False
|
||||
self.last_button_frame = 0
|
||||
|
||||
self.packer = CANPacker(dbc_names[Bus.pt])
|
||||
self.params = CarControllerParams(CP)
|
||||
|
||||
def update(self, CC, CC_IQ, CS, now_nanos):
|
||||
AolCarController.update(self, CC, CC_IQ, CS)
|
||||
can_sends = []
|
||||
|
||||
lkas_active = CC.latActive and self.lkas_control_bit_prev
|
||||
|
||||
# cruise buttons
|
||||
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
|
||||
if CC.cruiseControl.cancel:
|
||||
self.last_button_frame = self.frame
|
||||
can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, cancel=True))
|
||||
|
||||
# ACC resume from standstill
|
||||
elif CC.cruiseControl.resume:
|
||||
self.last_button_frame = self.frame
|
||||
can_sends.append(chryslercan.create_cruise_buttons(self.packer, CS.button_counter + 1, das_bus, resume=True))
|
||||
|
||||
# HUD alerts
|
||||
if self.frame % 25 == 0:
|
||||
if CS.lkas_car_model != -1:
|
||||
can_sends.append(chryslercan.create_lkas_hud(self.packer, self.CP, lkas_active, CC.hudControl.visualAlert,
|
||||
self.hud_count, CS.lkas_car_model, CS.auto_high_beam, self.aol))
|
||||
self.hud_count += 1
|
||||
|
||||
# steering
|
||||
if self.frame % self.params.STEER_STEP == 0:
|
||||
|
||||
# TODO: can we make this more sane? why is it different for all the cars?
|
||||
lkas_control_bit = self.lkas_control_bit_prev
|
||||
if self.CP_IQ.flags & ChryslerFlagsIQ.NO_MIN_STEERING_SPEED or self.CP.carFingerprint in RAM_DT:
|
||||
lkas_control_bit = IQCarController.get_lkas_control_bit(self, CS, CC, lkas_control_bit)
|
||||
elif CS.out.vEgo > self.CP.minSteerSpeed:
|
||||
lkas_control_bit = True
|
||||
elif self.CP.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED:
|
||||
if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0):
|
||||
lkas_control_bit = False
|
||||
elif self.CP.carFingerprint in RAM_CARS:
|
||||
if CS.out.vEgo < (self.CP.minSteerSpeed - 0.5):
|
||||
lkas_control_bit = False
|
||||
|
||||
# EPS faults if LKAS re-enables too quickly
|
||||
lkas_control_bit = lkas_control_bit and (self.frame - self.last_lkas_falling_edge > 200)
|
||||
|
||||
if not lkas_control_bit and self.lkas_control_bit_prev:
|
||||
self.last_lkas_falling_edge = self.frame
|
||||
self.lkas_control_bit_prev = lkas_control_bit
|
||||
|
||||
# steer torque
|
||||
new_torque = int(round(CC.actuators.torque * self.params.STEER_MAX))
|
||||
apply_torque = apply_meas_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorqueEps, self.params)
|
||||
if not lkas_active or not lkas_control_bit:
|
||||
apply_torque = 0
|
||||
self.apply_torque_last = apply_torque
|
||||
|
||||
can_sends.append(chryslercan.create_lkas_command(self.packer, self.CP, int(apply_torque), lkas_control_bit))
|
||||
|
||||
if self.frame % 10 == 0 and self.CP.carFingerprint not in RAM_CARS:
|
||||
can_sends.append(AolCarController.create_lkas_heartbit(self.packer, CS.lkas_heartbit, self.aol))
|
||||
|
||||
self.frame += 1
|
||||
|
||||
new_actuators = CC.actuators.as_builder()
|
||||
new_actuators.torque = self.apply_torque_last / self.params.STEER_MAX
|
||||
new_actuators.torqueOutputCan = self.apply_torque_last
|
||||
|
||||
return new_actuators, can_sends
|
||||
123
artifacts/package_sources/iqdbc/iqdbc/car/chrysler/carstate.py
Normal file
123
artifacts/package_sources/iqdbc/iqdbc/car/chrysler/carstate.py
Normal file
@@ -0,0 +1,123 @@
|
||||
from iqdbc.can import CANDefine, CANParser
|
||||
from iqdbc.car import Bus, create_button_events, structs
|
||||
from iqdbc.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.interfaces import CarStateBase
|
||||
|
||||
from iqdbc.lvbs.car.chrysler.aol import AolCarState
|
||||
from iqdbc.lvbs.car.chrysler.iq_carstate import IQCarState
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
|
||||
class CarState(CarStateBase, AolCarState, IQCarState):
|
||||
def __init__(self, CP, CP_IQ):
|
||||
CarStateBase.__init__(self, CP, CP_IQ)
|
||||
AolCarState.__init__(self, CP, CP_IQ)
|
||||
IQCarState.__init__(self, CP, CP_IQ)
|
||||
self.CP = CP
|
||||
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
|
||||
|
||||
self.auto_high_beam = 0
|
||||
self.button_counter = 0
|
||||
self.lkas_car_model = -1
|
||||
|
||||
if CP.carFingerprint in RAM_CARS:
|
||||
self.shifter_values = can_define.dv["Transmission_Status"]["Gear_State"]
|
||||
else:
|
||||
self.shifter_values = can_define.dv["GEAR"]["PRNDL"]
|
||||
|
||||
self.distance_button = 0
|
||||
|
||||
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
|
||||
ret = structs.CarState()
|
||||
ret_iq = structs.IQCarState()
|
||||
|
||||
prev_distance_button = self.distance_button
|
||||
self.distance_button = cp.vl["CRUISE_BUTTONS"]["ACC_Distance_Dec"]
|
||||
|
||||
# lock info
|
||||
ret.doorOpen = any([cp.vl["BCM_1"]["DOOR_OPEN_FL"],
|
||||
cp.vl["BCM_1"]["DOOR_OPEN_FR"],
|
||||
cp.vl["BCM_1"]["DOOR_OPEN_RL"],
|
||||
cp.vl["BCM_1"]["DOOR_OPEN_RR"]])
|
||||
ret.seatbeltUnlatched = cp.vl["ORC_1"]["SEATBELT_DRIVER_UNLATCHED"] == 1
|
||||
|
||||
# brake pedal
|
||||
ret.brake = 0
|
||||
ret.brakePressed = cp.vl["ESP_1"]['Brake_Pedal_State'] == 1 # Physical brake pedal switch
|
||||
|
||||
# gas pedal
|
||||
ret.gasPressed = cp.vl["ECM_5"]["Accelerator_Position"] > 1e-5
|
||||
|
||||
# car speed
|
||||
if self.CP.carFingerprint in RAM_CARS:
|
||||
ret.vEgoRaw = cp.vl["ESP_8"]["Vehicle_Speed"] * CV.KPH_TO_MS
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["Transmission_Status"]["Gear_State"], None))
|
||||
else:
|
||||
ret.vEgoRaw = (cp.vl["SPEED_1"]["SPEED_LEFT"] + cp.vl["SPEED_1"]["SPEED_RIGHT"]) / 2.
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl["GEAR"]["PRNDL"], None))
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = not ret.vEgoRaw > 0.001
|
||||
|
||||
# button presses
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(200, cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 1,
|
||||
cp.vl["STEERING_LEVERS"]["TURN_SIGNALS"] == 2)
|
||||
ret.genericToggle = cp.vl["STEERING_LEVERS"]["HIGH_BEAM_PRESSED"] == 1
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngleDeg = cp.vl["STEERING"]["STEERING_ANGLE"] + cp.vl["STEERING"]["STEERING_ANGLE_HP"]
|
||||
ret.steeringRateDeg = cp.vl["STEERING"]["STEERING_RATE"]
|
||||
ret.steeringTorque = cp.vl["EPS_2"]["COLUMN_TORQUE"]
|
||||
ret.steeringTorqueEps = cp.vl["EPS_2"]["EPS_TORQUE_MOTOR"]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
|
||||
|
||||
# cruise state
|
||||
cp_cruise = cp_cam if self.CP.carFingerprint in RAM_CARS else cp
|
||||
|
||||
ret.cruiseState.available = cp_cruise.vl["DAS_3"]["ACC_AVAILABLE"] == 1
|
||||
ret.cruiseState.enabled = cp_cruise.vl["DAS_3"]["ACC_ACTIVE"] == 1
|
||||
ret.cruiseState.speed = cp_cruise.vl["DAS_4"]["ACC_SET_SPEED_KPH"] * CV.KPH_TO_MS
|
||||
ret.cruiseState.nonAdaptive = cp_cruise.vl["DAS_4"]["ACC_STATE"] in (1, 2) # 1 NormalCCOn and 2 NormalCCSet
|
||||
ret.cruiseState.standstill = cp_cruise.vl["DAS_3"]["ACC_STANDSTILL"] == 1
|
||||
ret.accFaulted = cp_cruise.vl["DAS_3"]["ACC_FAULTED"] != 0
|
||||
|
||||
if self.CP.carFingerprint in RAM_CARS:
|
||||
# Auto High Beam isn't Located in this message on chrysler or jeep currently located in 729 message
|
||||
self.auto_high_beam = cp_cam.vl["DAS_6"]['AUTO_HIGH_BEAM_ON']
|
||||
ret.steerFaultTemporary = cp.vl["EPS_3"]["DASM_FAULT"] == 1
|
||||
else:
|
||||
ret.steerFaultTemporary = cp.vl["EPS_2"]["LKAS_TEMPORARY_FAULT"] == 1
|
||||
ret.steerFaultPermanent = cp.vl["EPS_2"]["LKAS_STATE"] == 4
|
||||
|
||||
# blindspot sensors
|
||||
if self.CP.enableBsm:
|
||||
ret.leftBlindspot = cp.vl["BSM_1"]["LEFT_STATUS"] == 1
|
||||
ret.rightBlindspot = cp.vl["BSM_1"]["RIGHT_STATUS"] == 1
|
||||
|
||||
self.lkas_car_model = cp_cam.vl["DAS_6"]["CAR_MODEL"]
|
||||
self.button_counter = cp.vl["CRUISE_BUTTONS"]["COUNTER"]
|
||||
|
||||
AolCarState.update_aol(self, ret, can_parsers)
|
||||
IQCarState.update(self, ret, ret_iq, can_parsers)
|
||||
|
||||
ret.buttonEvents = [
|
||||
*create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}),
|
||||
*create_button_events(self.lkas_button, self.prev_lkas_button, {1: ButtonType.lkas}),
|
||||
*self.button_events,
|
||||
]
|
||||
|
||||
return ret, ret_iq
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers(CP, CP_IQ):
|
||||
pt_messages: list = []
|
||||
cam_messages: list = []
|
||||
AolCarState.get_parser(CP, pt_messages, cam_messages)
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, 2),
|
||||
}
|
||||
@@ -0,0 +1,118 @@
|
||||
from iqdbc.car import structs
|
||||
from iqdbc.car.crc import CRC8J1850
|
||||
from iqdbc.car.chrysler.values import RAM_CARS
|
||||
|
||||
GearShifter = structs.CarState.GearShifter
|
||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
def create_lkas_hud(packer, CP, lkas_active, hud_alert, hud_count, car_model, auto_high_beam, aol):
|
||||
# LKAS_HUD - Controls what lane-keeping icon is displayed
|
||||
|
||||
# == Color ==
|
||||
# 0 hidden?
|
||||
# 1 white
|
||||
# 2 green
|
||||
# 3 ldw
|
||||
|
||||
# == Lines ==
|
||||
# 03 white Lines
|
||||
# 04 grey lines
|
||||
# 09 left lane close
|
||||
# 0A right lane close
|
||||
# 0B left Lane very close
|
||||
# 0C right Lane very close
|
||||
# 0D left cross cross
|
||||
# 0E right lane cross
|
||||
|
||||
# == Alerts ==
|
||||
# 7 Normal
|
||||
# 6 lane departure place hands on wheel
|
||||
|
||||
if aol.enable_aol:
|
||||
color = 2 if lkas_active else 1 if aol.paused else 0
|
||||
else:
|
||||
color = 2 if lkas_active else 1
|
||||
lines = 3 if lkas_active else 0
|
||||
alerts = 7 if lkas_active else 0
|
||||
|
||||
if hud_count < (1 * 4): # first 3 seconds, 4Hz
|
||||
alerts = 1
|
||||
|
||||
if hud_alert in (VisualAlert.ldw, VisualAlert.steerRequired):
|
||||
color = 4
|
||||
lines = 0
|
||||
alerts = 6
|
||||
|
||||
values = {
|
||||
"LKAS_ICON_COLOR": color,
|
||||
"CAR_MODEL": car_model,
|
||||
"LKAS_LANE_LINES": lines,
|
||||
"LKAS_ALERTS": alerts,
|
||||
}
|
||||
|
||||
if CP.carFingerprint in RAM_CARS:
|
||||
values['AUTO_HIGH_BEAM_ON'] = auto_high_beam
|
||||
|
||||
return packer.make_can_msg("DAS_6", 0, values)
|
||||
|
||||
|
||||
def create_lkas_command(packer, CP, apply_torque, lkas_control_bit):
|
||||
# LKAS_COMMAND Lane-keeping signal to turn the wheel
|
||||
enabled_val = 2 if CP.carFingerprint in RAM_CARS else 1
|
||||
values = {
|
||||
"STEERING_TORQUE": apply_torque,
|
||||
"LKAS_CONTROL_BIT": enabled_val if lkas_control_bit else 0,
|
||||
}
|
||||
return packer.make_can_msg("LKAS_COMMAND", 0, values)
|
||||
|
||||
|
||||
def create_cruise_buttons(packer, frame, bus, cancel=False, resume=False, accel=False, decel=False):
|
||||
values = {
|
||||
"ACC_Cancel": cancel,
|
||||
"ACC_Resume": resume,
|
||||
"ACC_Accel": accel,
|
||||
"ACC_Decel": decel,
|
||||
"COUNTER": frame % 0x10,
|
||||
}
|
||||
return packer.make_can_msg("CRUISE_BUTTONS", bus, values)
|
||||
|
||||
|
||||
def chrysler_checksum(address: int, sig, d: bytearray) -> int:
|
||||
checksum = 0xFF
|
||||
for j in range(len(d) - 1):
|
||||
curr = d[j]
|
||||
shift = 0x80
|
||||
for _ in range(8):
|
||||
bit_sum = curr & shift
|
||||
temp_chk = checksum & 0x80
|
||||
if bit_sum:
|
||||
bit_sum = 0x1C
|
||||
if temp_chk:
|
||||
bit_sum = 1
|
||||
checksum = (checksum << 1) & 0xFF
|
||||
temp_chk = checksum | 1
|
||||
bit_sum ^= temp_chk
|
||||
else:
|
||||
if temp_chk:
|
||||
bit_sum = 0x1D
|
||||
checksum = (checksum << 1) & 0xFF
|
||||
bit_sum ^= checksum
|
||||
checksum = bit_sum & 0xFF
|
||||
shift >>= 1
|
||||
return (~checksum) & 0xFF
|
||||
|
||||
|
||||
def fca_giorgio_checksum(address: int, sig, d: bytearray) -> int:
|
||||
crc = 0
|
||||
for i in range(len(d) - 1):
|
||||
crc ^= d[i]
|
||||
crc = CRC8J1850[crc]
|
||||
if address == 0xDE:
|
||||
return crc ^ 0x10
|
||||
elif address == 0x106:
|
||||
return crc ^ 0xF6
|
||||
elif address == 0x122:
|
||||
return crc ^ 0xF1
|
||||
else:
|
||||
return crc ^ 0x0A
|
||||
@@ -0,0 +1,794 @@
|
||||
""" AUTO-FORMATTED USING iqdbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.car.chrysler.values import CAR
|
||||
from iqdbc.lvbs.car.iq_fingerprints import extend_fw_versions
|
||||
from iqdbc.lvbs.car.chrysler.iq_fingerprints import FW_VERSIONS_EXT
|
||||
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.CHRYSLER_PACIFICA_2018: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68227902AF',
|
||||
b'68227902AG',
|
||||
b'68227902AH',
|
||||
b'68227905AG',
|
||||
b'68360252AC',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68211617AF',
|
||||
b'68211617AG',
|
||||
b'68358974AC',
|
||||
b'68405937AA',
|
||||
],
|
||||
(Ecu.abs, 0x747, None): [
|
||||
b'68222747AG',
|
||||
b'68330876AA',
|
||||
b'68330876AB',
|
||||
b'68352227AA',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'04672758AA',
|
||||
b'04672758AB',
|
||||
b'68226356AF',
|
||||
b'68226356AH',
|
||||
b'68226356AI',
|
||||
],
|
||||
(Ecu.eps, 0x75a, None): [
|
||||
b'68288891AE',
|
||||
b'68378884AA',
|
||||
b'68525338AA',
|
||||
b'68525338AB',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'68267018AO ',
|
||||
b'68267020AJ ',
|
||||
b'68303534AG ',
|
||||
b'68303534AJ ',
|
||||
b'68340762AD ',
|
||||
b'68340764AD ',
|
||||
b'68352652AE ',
|
||||
b'68352654AE ',
|
||||
b'68366851AH ',
|
||||
b'68366853AE ',
|
||||
b'68366853AG ',
|
||||
b'68372861AF ',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'68277370AJ',
|
||||
b'68277370AM',
|
||||
b'68277372AD',
|
||||
b'68277372AE',
|
||||
b'68277372AN',
|
||||
b'68277374AA',
|
||||
b'68277374AB',
|
||||
b'68277374AD',
|
||||
b'68277374AN',
|
||||
b'68367471AC',
|
||||
b'68367471AD',
|
||||
b'68380571AB',
|
||||
],
|
||||
},
|
||||
CAR.CHRYSLER_PACIFICA_2020: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68405327AC',
|
||||
b'68436233AB',
|
||||
b'68436233AC',
|
||||
b'68436234AB',
|
||||
b'68436250AE',
|
||||
b'68529067AA',
|
||||
b'68594993AB',
|
||||
b'68594994AB',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68405565AB',
|
||||
b'68405565AC',
|
||||
b'68444299AC',
|
||||
b'68480707AC',
|
||||
b'68480708AC',
|
||||
b'68526663AB',
|
||||
],
|
||||
(Ecu.abs, 0x747, None): [
|
||||
b'68397394AA',
|
||||
b'68433480AB',
|
||||
b'68453575AF',
|
||||
b'68577676AA',
|
||||
b'68593395AA',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'04672758AA',
|
||||
b'04672758AB',
|
||||
b'68417813AF',
|
||||
b'68540436AA',
|
||||
b'68540436AB',
|
||||
b'68540436AC',
|
||||
b'68540436AD',
|
||||
b'68598670AB',
|
||||
b'68598670AC',
|
||||
],
|
||||
(Ecu.eps, 0x75a, None): [
|
||||
b'68416742AA',
|
||||
b'68460393AA',
|
||||
b'68460393AB',
|
||||
b'68494461AB',
|
||||
b'68494461AC',
|
||||
b'68524936AA',
|
||||
b'68524936AB',
|
||||
b'68525338AB',
|
||||
b'68594337AB',
|
||||
b'68594340AB',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'68413871AD ',
|
||||
b'68413871AE ',
|
||||
b'68413871AH ',
|
||||
b'68413871AI ',
|
||||
b'68413871AJ ',
|
||||
b'68413873AH ',
|
||||
b'68413873AI ',
|
||||
b'68443120AE ',
|
||||
b'68443123AC ',
|
||||
b'68443125AC ',
|
||||
b'68496647AI ',
|
||||
b'68496647AJ ',
|
||||
b'68496650AH ',
|
||||
b'68496650AI ',
|
||||
b'68496650AL ',
|
||||
b'68496652AH ',
|
||||
b'68526752AD ',
|
||||
b'68526752AE ',
|
||||
b'68526754AD ',
|
||||
b'68526754AE ',
|
||||
b'68536264AE ',
|
||||
b'68700304AB ',
|
||||
b'68700306AB ',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'68414271AC',
|
||||
b'68414271AD',
|
||||
b'68414275AC',
|
||||
b'68414275AD',
|
||||
b'68443154AB',
|
||||
b'68443154AC',
|
||||
b'68443155AC',
|
||||
b'68443158AB',
|
||||
b'68501050AD',
|
||||
b'68501051AD',
|
||||
b'68501055AD',
|
||||
b'68527221AB',
|
||||
b'68527223AB',
|
||||
b'68586231AD',
|
||||
b'68586233AD',
|
||||
],
|
||||
},
|
||||
CAR.CHRYSLER_PACIFICA_2018_HYBRID: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68239262AH',
|
||||
b'68239262AI',
|
||||
b'68239262AJ',
|
||||
b'68239263AH',
|
||||
b'68239263AJ',
|
||||
b'68358439AE',
|
||||
b'68358439AG',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68238840AH',
|
||||
b'68358990AC',
|
||||
b'68405939AA',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'04672758AA',
|
||||
b'68226356AI',
|
||||
],
|
||||
(Ecu.eps, 0x75a, None): [
|
||||
b'68288309AC',
|
||||
b'68288309AD',
|
||||
b'68525339AA',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'68277480AV ',
|
||||
b'68277480AX ',
|
||||
b'68277480AZ ',
|
||||
b'68366580AI ',
|
||||
b'68366580AK ',
|
||||
b'68366580AM ',
|
||||
],
|
||||
(Ecu.hybrid, 0x7e2, None): [
|
||||
b'05190175BF',
|
||||
b'05190175BH',
|
||||
b'05190226AI',
|
||||
b'05190226AK',
|
||||
b'05190226AM',
|
||||
],
|
||||
},
|
||||
CAR.CHRYSLER_PACIFICA_2019_HYBRID: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68405292AC',
|
||||
b'68434956AC',
|
||||
b'68434956AD',
|
||||
b'68434960AE',
|
||||
b'68434960AF',
|
||||
b'68529064AB',
|
||||
b'68594990AB',
|
||||
b'68594990AD',
|
||||
b'68594990AE',
|
||||
b'68594991AB',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68405567AB',
|
||||
b'68405567AC',
|
||||
b'68453076AD',
|
||||
b'68480710AC',
|
||||
b'68526665AB',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'04672758AB',
|
||||
b'68417813AF',
|
||||
b'68540436AA',
|
||||
b'68540436AB',
|
||||
b'68540436AC',
|
||||
b'68540436AD',
|
||||
b'68598670AB',
|
||||
b'68598670AC',
|
||||
b'68645752AA',
|
||||
],
|
||||
(Ecu.eps, 0x75a, None): [
|
||||
b'68416741AA',
|
||||
b'68460392AA',
|
||||
b'68525339AA',
|
||||
b'68525339AB',
|
||||
b'68594341AB',
|
||||
b'68594341AC',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'05190392AB ',
|
||||
b'68416680AD ',
|
||||
b'68416680AE ',
|
||||
b'68416680AF ',
|
||||
b'68416680AG ',
|
||||
b'68444228AC ',
|
||||
b'68444228AD ',
|
||||
b'68444228AE ',
|
||||
b'68444228AF ',
|
||||
b'68499122AD ',
|
||||
b'68499122AE ',
|
||||
b'68499122AF ',
|
||||
b'68526772AD ',
|
||||
b'68526772AH ',
|
||||
b'68599493AC ',
|
||||
b'68657433AA ',
|
||||
b'68700317AC ',
|
||||
],
|
||||
(Ecu.hybrid, 0x7e2, None): [
|
||||
b'05185116AF',
|
||||
b'05185116AJ',
|
||||
b'05185116AK',
|
||||
b'05185116AL',
|
||||
b'05190240AP',
|
||||
b'05190240AQ',
|
||||
b'05190240AR',
|
||||
b'05190265AG',
|
||||
b'05190265AH',
|
||||
b'05190289AE',
|
||||
b'68540977AH',
|
||||
b'68540977AK',
|
||||
b'68540977AL',
|
||||
b'68597647AE',
|
||||
b'68597647AF',
|
||||
b'68632416AB',
|
||||
b'68632416AC',
|
||||
b'68676877AB',
|
||||
],
|
||||
},
|
||||
CAR.JEEP_GRAND_CHEROKEE: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68243549AG',
|
||||
b'68302211AC',
|
||||
b'68302212AD',
|
||||
b'68302214AC',
|
||||
b'68302223AC',
|
||||
b'68302246AC',
|
||||
b'68331511AC',
|
||||
b'68331574AC',
|
||||
b'68331687AC',
|
||||
b'68331690AC',
|
||||
b'68340272AD',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68309533AA',
|
||||
b'68316742AB',
|
||||
b'68355363AB',
|
||||
],
|
||||
(Ecu.abs, 0x747, None): [
|
||||
b'68252642AG',
|
||||
b'68306178AD',
|
||||
b'68336275AB',
|
||||
b'68336276AB',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'04672627AB',
|
||||
b'68251506AF',
|
||||
b'68332015AB',
|
||||
],
|
||||
(Ecu.eps, 0x75a, None): [
|
||||
b'68276201AG',
|
||||
b'68321644AB',
|
||||
b'68321644AC',
|
||||
b'68321646AC',
|
||||
b'68321648AC',
|
||||
b'68321650AC',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'05035920AE ',
|
||||
b'68252272AG ',
|
||||
b'68284455AI ',
|
||||
b'68284456AI ',
|
||||
b'68284456AJ ',
|
||||
b'68284477AF ',
|
||||
b'68325564AH ',
|
||||
b'68325564AI ',
|
||||
b'68325565AH ',
|
||||
b'68325565AI ',
|
||||
b'68325565AJ ',
|
||||
b'68325618AD ',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'05035517AH',
|
||||
b'68253222AF',
|
||||
b'68311218AC',
|
||||
b'68311218AD',
|
||||
b'68311223AF',
|
||||
b'68311223AG',
|
||||
b'68361911AE',
|
||||
b'68361911AF',
|
||||
b'68361911AH',
|
||||
b'68361914AE',
|
||||
b'68361916AD',
|
||||
],
|
||||
},
|
||||
CAR.JEEP_GRAND_CHEROKEE_2019: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68402703AB',
|
||||
b'68402704AB',
|
||||
b'68402707AB',
|
||||
b'68402708AB',
|
||||
b'68402714AB',
|
||||
b'68402736AB',
|
||||
b'68402971AD',
|
||||
b'68454144AD',
|
||||
b'68454145AB',
|
||||
b'68454152AB',
|
||||
b'68454156AB',
|
||||
b'68516650AB',
|
||||
b'68516651AB',
|
||||
b'68516669AB',
|
||||
b'68516671AB',
|
||||
b'68516683AB',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68355363AB',
|
||||
b'68355364AB',
|
||||
],
|
||||
(Ecu.abs, 0x747, None): [
|
||||
b'68408639AC',
|
||||
b'68408639AD',
|
||||
b'68499978AB',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'04672788AA',
|
||||
b'68456722AC',
|
||||
],
|
||||
(Ecu.eps, 0x75a, None): [
|
||||
b'68417279AA',
|
||||
b'68417280AA',
|
||||
b'68417281AA',
|
||||
b'68417283AA',
|
||||
b'68453431AA',
|
||||
b'68453433AA',
|
||||
b'68453435AA',
|
||||
b'68499171AA',
|
||||
b'68499171AB',
|
||||
b'68501183AA',
|
||||
b'68501186AA',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'05035674AB ',
|
||||
b'68412635AE ',
|
||||
b'68412635AG ',
|
||||
b'68412635AH ',
|
||||
b'68412660AD ',
|
||||
b'68412660AF ',
|
||||
b'68422860AB',
|
||||
b'68449435AE ',
|
||||
b'68496223AA ',
|
||||
b'68504959AD ',
|
||||
b'68504959AE ',
|
||||
b'68504960AD ',
|
||||
b'68504993AC ',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'05035707AA',
|
||||
b'68419672AC',
|
||||
b'68419675AC',
|
||||
b'68419678AB',
|
||||
b'68423905AB',
|
||||
b'68449258AC',
|
||||
b'68495807AA',
|
||||
b'68495807AB',
|
||||
b'68503641AC',
|
||||
b'68503644AC',
|
||||
b'68503664AC',
|
||||
],
|
||||
},
|
||||
CAR.RAM_1500_5TH_GEN: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68294051AG',
|
||||
b'68294051AI',
|
||||
b'68294052AG',
|
||||
b'68294052AH',
|
||||
b'68294059AI',
|
||||
b'68294063AG',
|
||||
b'68294063AH',
|
||||
b'68294063AI',
|
||||
b'68434846AC',
|
||||
b'68434847AC',
|
||||
b'68434849AC',
|
||||
b'68434850AC',
|
||||
b'68434855AC',
|
||||
b'68434856AC',
|
||||
b'68434858AC',
|
||||
b'68434859AC',
|
||||
b'68434860AC',
|
||||
b'68453471AD',
|
||||
b'68453483AC',
|
||||
b'68453483AD',
|
||||
b'68453487AD',
|
||||
b'68453491AC',
|
||||
b'68453491AD',
|
||||
b'68453499AD',
|
||||
b'68453502AC',
|
||||
b'68453503AC',
|
||||
b'68453503AD',
|
||||
b'68453505AC',
|
||||
b'68453505AD',
|
||||
b'68453511AC',
|
||||
b'68453513AC',
|
||||
b'68453513AD',
|
||||
b'68453514AD',
|
||||
b'68505633AB',
|
||||
b'68510277AG',
|
||||
b'68510277AH',
|
||||
b'68510280AG',
|
||||
b'68510280AH',
|
||||
b'68510282AG',
|
||||
b'68510282AH',
|
||||
b'68510283AG',
|
||||
b'68527346AE',
|
||||
b'68527361AD',
|
||||
b'68527375AD',
|
||||
b'68527381AD',
|
||||
b'68527381AE',
|
||||
b'68527382AE',
|
||||
b'68527383AD',
|
||||
b'68527383AE',
|
||||
b'68527387AE',
|
||||
b'68527397AD',
|
||||
b'68527403AC',
|
||||
b'68527403AD',
|
||||
b'68527404AD',
|
||||
b'68546047AF',
|
||||
b'68631938AA',
|
||||
b'68631939AA',
|
||||
b'68631940AA',
|
||||
b'68631940AB',
|
||||
b'68631941AB',
|
||||
b'68631942AA',
|
||||
b'68631943AB',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68428609AB',
|
||||
b'68441329AA',
|
||||
b'68441329AB',
|
||||
b'68473844AB',
|
||||
b'68490898AA',
|
||||
b'68500728AA',
|
||||
b'68615033AA',
|
||||
b'68615034AA',
|
||||
],
|
||||
(Ecu.abs, 0x747, None): [
|
||||
b'68292406AG',
|
||||
b'68292406AH',
|
||||
b'68432418AB',
|
||||
b'68432418AC',
|
||||
b'68432418AD',
|
||||
b'68436004AD',
|
||||
b'68436004AE',
|
||||
b'68438454AC',
|
||||
b'68438454AD',
|
||||
b'68438456AE',
|
||||
b'68438456AF',
|
||||
b'68535469AB',
|
||||
b'68535470AC',
|
||||
b'68548900AB',
|
||||
b'68548900AC',
|
||||
b'68586307AB',
|
||||
b'68586307AC',
|
||||
b'68728724AA',
|
||||
b'68728727AA',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'04672892AB',
|
||||
b'04672932AB',
|
||||
b'04672932AC',
|
||||
b'22DTRHD_AA',
|
||||
b'68320950AH',
|
||||
b'68320950AI',
|
||||
b'68320950AJ',
|
||||
b'68320950AL',
|
||||
b'68320950AM',
|
||||
b'68454268AB',
|
||||
b'68454268AC',
|
||||
b'68475160AE',
|
||||
b'68475160AF',
|
||||
b'68475160AG',
|
||||
],
|
||||
(Ecu.eps, 0x75a, None): [
|
||||
b'21590101AA',
|
||||
b'21590101AB',
|
||||
b'22490101AB',
|
||||
b'68273275AF',
|
||||
b'68273275AG',
|
||||
b'68273275AH',
|
||||
b'68312176AE',
|
||||
b'68312176AF',
|
||||
b'68312176AG',
|
||||
b'68440789AC',
|
||||
b'68466110AA',
|
||||
b'68466110AB',
|
||||
b'68466113AA',
|
||||
b'68466116AA',
|
||||
b'68469901AA',
|
||||
b'68469904AA',
|
||||
b'68469907AA',
|
||||
b'68522583AA',
|
||||
b'68522583AB',
|
||||
b'68522584AA',
|
||||
b'68522585AB',
|
||||
b'68552788AA',
|
||||
b'68552789AA',
|
||||
b'68552790AA',
|
||||
b'68552791AB',
|
||||
b'68552794AA',
|
||||
b'68552794AD',
|
||||
b'68585106AB',
|
||||
b'68585107AB',
|
||||
b'68585108AB',
|
||||
b'68585109AB',
|
||||
b'68585112AB',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'05035699AG ',
|
||||
b'05035841AC ',
|
||||
b'05035841AD ',
|
||||
b'05036026AB ',
|
||||
b'05036030AC ',
|
||||
b'05036065AE ',
|
||||
b'05036066AE ',
|
||||
b'05036067AE ',
|
||||
b'05036193AA ',
|
||||
b'05149368AA ',
|
||||
b'05149374AA ',
|
||||
b'05149591AD ',
|
||||
b'05149591AE ',
|
||||
b'05149592AE ',
|
||||
b'05149599AE ',
|
||||
b'05149600AD ',
|
||||
b'05149600AE ',
|
||||
b'05149605AE ',
|
||||
b'05149846AA ',
|
||||
b'05149848AA ',
|
||||
b'05149848AC ',
|
||||
b'05190341AD',
|
||||
b'05190346AD',
|
||||
b'68378695AI ',
|
||||
b'68378695AJ ',
|
||||
b'68378695AK ',
|
||||
b'68378696AJ ',
|
||||
b'68378696AK ',
|
||||
b'68378701AI ',
|
||||
b'68378702AI ',
|
||||
b'68378710AL ',
|
||||
b'68378742AI ',
|
||||
b'68378742AK ',
|
||||
b'68378743AI ',
|
||||
b'68378743AM ',
|
||||
b'68378748AL ',
|
||||
b'68378758AM ',
|
||||
b'68378759AM ',
|
||||
b'68448163AJ',
|
||||
b'68448163AK',
|
||||
b'68448163AL',
|
||||
b'68448165AG',
|
||||
b'68448165AK',
|
||||
b'68455111AC ',
|
||||
b'68455119AC ',
|
||||
b'68455137AC ',
|
||||
b'68455142AC ',
|
||||
b'68455142AE ',
|
||||
b'68455145AC ',
|
||||
b'68455145AE ',
|
||||
b'68455146AC ',
|
||||
b'68460927AA ',
|
||||
b'68467909AB ',
|
||||
b'68467909AC ',
|
||||
b'68467915AC ',
|
||||
b'68467916AC ',
|
||||
b'68467936AC ',
|
||||
b'68500630AD',
|
||||
b'68500630AE',
|
||||
b'68500630AF',
|
||||
b'68500631AE',
|
||||
b'68502719AC ',
|
||||
b'68502722AC ',
|
||||
b'68502733AC ',
|
||||
b'68502734AF ',
|
||||
b'68502737AF ',
|
||||
b'68502740AF ',
|
||||
b'68502741AF ',
|
||||
b'68502742AC ',
|
||||
b'68502742AF ',
|
||||
b'68539650AD',
|
||||
b'68539650AF',
|
||||
b'68539651AD',
|
||||
b'68586101AA ',
|
||||
b'68586102AA ',
|
||||
b'68586105AB ',
|
||||
b'68629917AC ',
|
||||
b'68629919AC ',
|
||||
b'68629919AD ',
|
||||
b'68629922AC ',
|
||||
b'68629925AC ',
|
||||
b'68629926AC ',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'05035706AD',
|
||||
b'05035842AB',
|
||||
b'05036069AA',
|
||||
b'05036181AA',
|
||||
b'05149536AC',
|
||||
b'05149537AC',
|
||||
b'05149543AC',
|
||||
b'68360078AL',
|
||||
b'68360080AL',
|
||||
b'68360080AM',
|
||||
b'68360081AM',
|
||||
b'68360081AN',
|
||||
b'68360085AH',
|
||||
b'68360085AJ',
|
||||
b'68360085AK',
|
||||
b'68360085AL',
|
||||
b'68360085AO',
|
||||
b'68360086AH',
|
||||
b'68360086AK',
|
||||
b'68360086AN',
|
||||
b'68384328AD',
|
||||
b'68384332AD',
|
||||
b'68445531AC',
|
||||
b'68445532AB',
|
||||
b'68445533AB',
|
||||
b'68445536AB',
|
||||
b'68445537AB',
|
||||
b'68466081AB',
|
||||
b'68466086AB',
|
||||
b'68466087AB',
|
||||
b'68484466AC',
|
||||
b'68484467AC',
|
||||
b'68484471AC',
|
||||
b'68502994AC',
|
||||
b'68502994AD',
|
||||
b'68502996AD',
|
||||
b'68520867AE',
|
||||
b'68520867AF',
|
||||
b'68520870AC',
|
||||
b'68520871AC',
|
||||
b'68528325AE',
|
||||
b'68540431AB',
|
||||
b'68540433AB',
|
||||
b'68551676AA',
|
||||
b'68629935AB',
|
||||
b'68629936AC',
|
||||
],
|
||||
},
|
||||
CAR.RAM_HD_5TH_GEN: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68361606AH',
|
||||
b'68437735AC',
|
||||
b'68492693AD',
|
||||
b'68525485AB',
|
||||
b'68525487AB',
|
||||
b'68525498AB',
|
||||
b'68528791AF',
|
||||
b'68628474AB',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68399794AC',
|
||||
b'68428503AA',
|
||||
b'68428505AA',
|
||||
b'68428507AA',
|
||||
],
|
||||
(Ecu.abs, 0x747, None): [
|
||||
b'68334977AH',
|
||||
b'68455481AC',
|
||||
b'68504022AA',
|
||||
b'68504022AB',
|
||||
b'68504022AC',
|
||||
b'68530686AB',
|
||||
b'68530686AC',
|
||||
b'68544596AC',
|
||||
b'68641704AA',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'04672895AB',
|
||||
b'04672934AB',
|
||||
b'56029827AG',
|
||||
b'56029827AH',
|
||||
b'68462657AE',
|
||||
b'68484694AD',
|
||||
b'68484694AE',
|
||||
b'68615489AB',
|
||||
],
|
||||
(Ecu.eps, 0x761, None): [
|
||||
b'68421036AC',
|
||||
b'68507906AB',
|
||||
b'68534023AC',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'52370131AF',
|
||||
b'52370231AF',
|
||||
b'52370231AG',
|
||||
b'52370491AA',
|
||||
b'52370931CT',
|
||||
b'52401032AE',
|
||||
b'52421132AF',
|
||||
b'52421332AF',
|
||||
b'68527616AD ',
|
||||
b'M2370131MB',
|
||||
b'M2421132MB',
|
||||
],
|
||||
},
|
||||
CAR.DODGE_DURANGO: {
|
||||
(Ecu.combinationMeter, 0x742, None): [
|
||||
b'68454261AD',
|
||||
b'68471535AE',
|
||||
],
|
||||
(Ecu.srs, 0x744, None): [
|
||||
b'68355362AB',
|
||||
b'68492238AD',
|
||||
],
|
||||
(Ecu.abs, 0x747, None): [
|
||||
b'68408639AD',
|
||||
b'68499978AB',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x753, None): [
|
||||
b'68440581AE',
|
||||
b'68456722AC',
|
||||
],
|
||||
(Ecu.eps, 0x75a, None): [
|
||||
b'68453435AA',
|
||||
b'68498477AA',
|
||||
],
|
||||
(Ecu.engine, 0x7e0, None): [
|
||||
b'05035786AE ',
|
||||
b'68449476AE ',
|
||||
],
|
||||
(Ecu.transmission, 0x7e1, None): [
|
||||
b'05035826AC',
|
||||
b'68449265AC',
|
||||
],
|
||||
},
|
||||
}
|
||||
|
||||
|
||||
|
||||
FW_VERSIONS = extend_fw_versions(FW_VERSIONS, FW_VERSIONS_EXT)
|
||||
109
artifacts/package_sources/iqdbc/iqdbc/car/chrysler/interface.py
Executable file
109
artifacts/package_sources/iqdbc/iqdbc/car/chrysler/interface.py
Executable file
@@ -0,0 +1,109 @@
|
||||
#!/usr/bin/env python3
|
||||
from iqdbc.car import get_safety_config, structs
|
||||
from iqdbc.car.chrysler.carcontroller import CarController
|
||||
from iqdbc.car.chrysler.carstate import CarState
|
||||
from iqdbc.car.chrysler.radar_interface import RadarInterface
|
||||
from iqdbc.car.chrysler.values import CAR, RAM_HD, RAM_DT, RAM_CARS, ChryslerFlags, ChryslerSafetyFlags
|
||||
from iqdbc.car.interfaces import CarInterfaceBase
|
||||
from iqdbc.lvbs.car.chrysler.iq_values import ChryslerFlagsIQ
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
CarState = CarState
|
||||
CarController = CarController
|
||||
RadarInterface = RadarInterface
|
||||
|
||||
DRIVABLE_GEARS = (structs.CarState.GearShifter.low,)
|
||||
|
||||
@staticmethod
|
||||
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
|
||||
ret.brand = "chrysler"
|
||||
ret.dashcamOnly = candidate in RAM_HD
|
||||
|
||||
# radar parsing needs some work, see https://github.com/commaai/openpilot/issues/26842
|
||||
ret.radarUnavailable = True # Bus.radar not in DBC[candidate][Bus.radar]
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerLimitTimer = 0.4
|
||||
|
||||
# safety config
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.chrysler)]
|
||||
if candidate in RAM_HD:
|
||||
ret.safetyConfigs[0].safetyParam |= ChryslerSafetyFlags.RAM_HD.value
|
||||
elif candidate in RAM_DT:
|
||||
ret.safetyConfigs[0].safetyParam |= ChryslerSafetyFlags.RAM_DT.value
|
||||
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
if candidate not in RAM_CARS:
|
||||
# Newer FW versions standard on the following platforms, or flashed by a dealer onto older platforms have a higher minimum steering speed.
|
||||
new_eps_platform = candidate in (CAR.CHRYSLER_PACIFICA_2019_HYBRID, CAR.CHRYSLER_PACIFICA_2020, CAR.JEEP_GRAND_CHEROKEE_2019, CAR.DODGE_DURANGO)
|
||||
new_eps_firmware = any(fw.ecu == 'eps' and fw.fwVersion[:4] >= b"6841" for fw in car_fw)
|
||||
if new_eps_platform or new_eps_firmware:
|
||||
ret.flags |= ChryslerFlags.HIGHER_MIN_STEERING_SPEED.value
|
||||
|
||||
# Chrysler
|
||||
if candidate in (CAR.CHRYSLER_PACIFICA_2018, CAR.CHRYSLER_PACIFICA_2018_HYBRID, CAR.CHRYSLER_PACIFICA_2019_HYBRID,
|
||||
CAR.CHRYSLER_PACIFICA_2020, CAR.DODGE_DURANGO):
|
||||
ret.lateralTuning.init('pid')
|
||||
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
|
||||
# Jeep
|
||||
elif candidate in (CAR.JEEP_GRAND_CHEROKEE, CAR.JEEP_GRAND_CHEROKEE_2019):
|
||||
ret.steerActuatorDelay = 0.2
|
||||
|
||||
ret.lateralTuning.init('pid')
|
||||
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kiBP = [[9., 20.], [9., 20.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.15, 0.30], [0.03, 0.05]]
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
|
||||
# Ram
|
||||
elif candidate == CAR.RAM_1500_5TH_GEN:
|
||||
ret.steerActuatorDelay = 0.2
|
||||
ret.wheelbase = 3.88
|
||||
# Older EPS FW allow steer to zero
|
||||
if any(fw.ecu == 'eps' and b"68" < fw.fwVersion[:4] <= b"6831" for fw in car_fw):
|
||||
ret.minSteerSpeed = 0.
|
||||
|
||||
elif candidate == CAR.RAM_HD_5TH_GEN:
|
||||
ret.steerActuatorDelay = 0.2
|
||||
|
||||
else:
|
||||
raise ValueError(f"Unsupported car: {candidate}")
|
||||
|
||||
if ret.flags & ChryslerFlags.HIGHER_MIN_STEERING_SPEED:
|
||||
# TODO: allow these cars to steer down to 13 m/s if already engaged.
|
||||
# TODO: Durango 2020 may be able to steer to zero once above 38 kph
|
||||
ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged.
|
||||
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
ret.enableBsm = 720 in fingerprint[0]
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def _get_params_iq(stock_cp: structs.CarParams, ret: structs.IQCarParams, candidate, fingerprint: dict[int, dict[int, int]],
|
||||
car_fw: list[structs.CarParams.CarFw], alpha_long: bool, is_release_iq: bool, docs: bool) -> structs.IQCarParams:
|
||||
if candidate == CAR.RAM_1500_5TH_GEN:
|
||||
if stock_cp.minSteerSpeed != 0.:
|
||||
stock_cp.minSteerSpeed = 0.5
|
||||
stock_cp.minEnableSpeed = 14.5
|
||||
if any(fw.ecu == 'eps' and fw.fwVersion in (b"68273275AF", b"68273275AG", b"68312176AE", b"68312176AG",) for fw in car_fw):
|
||||
stock_cp.minEnableSpeed = 0.
|
||||
|
||||
if candidate == CAR.RAM_HD_5TH_GEN:
|
||||
stock_cp.dashcamOnly = False
|
||||
# https://github.com/commaai/openpilot/issues/25389
|
||||
stock_cp.tireStiffnessFactor = 1.0
|
||||
stock_cp.tireStiffnessFront = 65155.
|
||||
stock_cp.tireStiffnessRear = 80926.
|
||||
stock_cp.wheelbase = 3.79
|
||||
stock_cp.steerRatio = 19.
|
||||
|
||||
# LKAS heartbeat on bus 0 (msg 0x4FF) means the camera is on the ADAS bus and
|
||||
# IQ.Pilot can steer down to a standstill.
|
||||
if 0x4FF in fingerprint[0]:
|
||||
ret.flags |= ChryslerFlagsIQ.NO_MIN_STEERING_SPEED.value
|
||||
stock_cp.minSteerSpeed = 0.
|
||||
|
||||
return ret
|
||||
85
artifacts/package_sources/iqdbc/iqdbc/car/chrysler/radar_interface.py
Executable file
85
artifacts/package_sources/iqdbc/iqdbc/car/chrysler/radar_interface.py
Executable file
@@ -0,0 +1,85 @@
|
||||
#!/usr/bin/env python3
|
||||
from iqdbc.can import CANParser
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.car.interfaces import RadarInterfaceBase
|
||||
from iqdbc.car.chrysler.values import DBC
|
||||
|
||||
RADAR_MSGS_C = list(range(0x2c2, 0x2d4+2, 2)) # c_ messages 706,...,724
|
||||
RADAR_MSGS_D = list(range(0x2a2, 0x2b4+2, 2)) # d_ messages
|
||||
LAST_MSG = max(RADAR_MSGS_C + RADAR_MSGS_D)
|
||||
NUMBER_MSGS = len(RADAR_MSGS_C) + len(RADAR_MSGS_D)
|
||||
|
||||
|
||||
def _create_radar_can_parser(car_fingerprint):
|
||||
if Bus.radar not in DBC[car_fingerprint]:
|
||||
return None
|
||||
|
||||
msg_n = len(RADAR_MSGS_C)
|
||||
# list of [(signal name, message name or number), (...)]
|
||||
# [('RADAR_STATE', 1024),
|
||||
# ('LONG_DIST', 1072),
|
||||
# ('LONG_DIST', 1073),
|
||||
# ('LONG_DIST', 1074),
|
||||
# ('LONG_DIST', 1075),
|
||||
|
||||
messages = list(zip(RADAR_MSGS_C +
|
||||
RADAR_MSGS_D,
|
||||
[20] * msg_n + # 20Hz (0.05s)
|
||||
[20] * msg_n, strict=True)) # 20Hz (0.05s)
|
||||
|
||||
return CANParser(DBC[car_fingerprint][Bus.radar], messages, 1)
|
||||
|
||||
|
||||
def _address_to_track(address):
|
||||
if address in RADAR_MSGS_C:
|
||||
return (address - RADAR_MSGS_C[0]) // 2
|
||||
if address in RADAR_MSGS_D:
|
||||
return (address - RADAR_MSGS_D[0]) // 2
|
||||
raise ValueError("radar received unexpected address %d" % address)
|
||||
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP, CP_IQ):
|
||||
super().__init__(CP, CP_IQ)
|
||||
self.rcp = _create_radar_can_parser(CP.carFingerprint)
|
||||
self.updated_messages = set()
|
||||
self.trigger_msg = LAST_MSG
|
||||
|
||||
def update(self, can_strings):
|
||||
if self.rcp is None or self.CP.radarUnavailable:
|
||||
return super().update(None)
|
||||
|
||||
vls = self.rcp.update(can_strings)
|
||||
self.updated_messages.update(vls)
|
||||
|
||||
if self.trigger_msg not in self.updated_messages:
|
||||
return None
|
||||
|
||||
ret = structs.RadarData()
|
||||
if not self.rcp.can_valid:
|
||||
ret.errors.canError = True
|
||||
|
||||
for ii in self.updated_messages: # ii should be the message ID as a number
|
||||
cpt = self.rcp.vl[ii]
|
||||
trackId = _address_to_track(ii)
|
||||
|
||||
if trackId not in self.pts:
|
||||
self.pts[trackId] = structs.RadarData.RadarPoint()
|
||||
self.pts[trackId].trackId = trackId
|
||||
self.pts[trackId].aRel = float('nan')
|
||||
self.pts[trackId].yvRel = float('nan')
|
||||
self.pts[trackId].measured = True
|
||||
|
||||
if 'LONG_DIST' in cpt: # c_* message
|
||||
self.pts[trackId].dRel = cpt['LONG_DIST'] # from front of car
|
||||
# our lat_dist is positive to the right in car's frame.
|
||||
# LAT_DIST is right-positive, yRel is left-positive
|
||||
self.pts[trackId].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
|
||||
else: # d_* message
|
||||
self.pts[trackId].vRel = cpt['REL_SPEED']
|
||||
|
||||
# We want a list, not a dictionary. Filter out LONG_DIST==0 because that means it's not valid.
|
||||
ret.points = [x for x in self.pts.values() if x.dRel != 0]
|
||||
|
||||
self.updated_messages.clear()
|
||||
return ret
|
||||
160
artifacts/package_sources/iqdbc/iqdbc/car/chrysler/values.py
Normal file
160
artifacts/package_sources/iqdbc/iqdbc/car/chrysler/values.py
Normal file
@@ -0,0 +1,160 @@
|
||||
from enum import IntFlag
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from iqdbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.car.docs_definitions import CarHarness, CarDocs, CarParts
|
||||
from iqdbc.car.fw_query_definitions import FwQueryConfig, Request, p16
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
|
||||
class ChryslerSafetyFlags(IntFlag):
|
||||
RAM_DT = 1
|
||||
RAM_HD = 2
|
||||
|
||||
|
||||
class ChryslerFlags(IntFlag):
|
||||
# Detected flags
|
||||
HIGHER_MIN_STEERING_SPEED = 1
|
||||
|
||||
|
||||
@dataclass
|
||||
class ChryslerCarDocs(CarDocs):
|
||||
package: str = "Adaptive Cruise Control (ACC)"
|
||||
car_parts: CarParts = field(default_factory=CarParts.common([CarHarness.fca]))
|
||||
|
||||
|
||||
@dataclass
|
||||
class ChryslerPlatformConfig(PlatformConfig):
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: {
|
||||
Bus.pt: 'chrysler_pacifica_2017_hybrid_generated',
|
||||
Bus.radar: 'chrysler_pacifica_2017_hybrid_private_fusion',
|
||||
})
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class ChryslerCarSpecs(CarSpecs):
|
||||
minSteerSpeed: float = 3.8 # m/s
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
# Chrysler
|
||||
CHRYSLER_PACIFICA_2018_HYBRID = ChryslerPlatformConfig(
|
||||
[ChryslerCarDocs("Chrysler Pacifica Hybrid 2017-18")],
|
||||
ChryslerCarSpecs(mass=2242., wheelbase=3.089, steerRatio=16.2),
|
||||
)
|
||||
CHRYSLER_PACIFICA_2019_HYBRID = ChryslerPlatformConfig(
|
||||
[ChryslerCarDocs("Chrysler Pacifica Hybrid 2019-25")],
|
||||
CHRYSLER_PACIFICA_2018_HYBRID.specs,
|
||||
)
|
||||
CHRYSLER_PACIFICA_2018 = ChryslerPlatformConfig(
|
||||
[ChryslerCarDocs("Chrysler Pacifica 2017-18")],
|
||||
CHRYSLER_PACIFICA_2018_HYBRID.specs,
|
||||
)
|
||||
CHRYSLER_PACIFICA_2020 = ChryslerPlatformConfig(
|
||||
[
|
||||
ChryslerCarDocs("Chrysler Pacifica 2019-20"),
|
||||
ChryslerCarDocs("Chrysler Pacifica 2021-23", package="All"),
|
||||
],
|
||||
CHRYSLER_PACIFICA_2018_HYBRID.specs,
|
||||
)
|
||||
|
||||
# Dodge
|
||||
DODGE_DURANGO = ChryslerPlatformConfig(
|
||||
[ChryslerCarDocs("Dodge Durango 2020-21")],
|
||||
CHRYSLER_PACIFICA_2018_HYBRID.specs,
|
||||
)
|
||||
|
||||
# Jeep
|
||||
JEEP_GRAND_CHEROKEE = ChryslerPlatformConfig( # includes 2017 Trailhawk
|
||||
[ChryslerCarDocs("Jeep Grand Cherokee 2016-18", video="https://www.youtube.com/watch?v=eLR9o2JkuRk")],
|
||||
ChryslerCarSpecs(mass=1778., wheelbase=2.71, steerRatio=16.7),
|
||||
)
|
||||
|
||||
JEEP_GRAND_CHEROKEE_2019 = ChryslerPlatformConfig( # includes 2020 Trailhawk
|
||||
[ChryslerCarDocs("Jeep Grand Cherokee 2019-21", video="https://www.youtube.com/watch?v=jBe4lWnRSu4")],
|
||||
JEEP_GRAND_CHEROKEE.specs,
|
||||
)
|
||||
|
||||
# Ram
|
||||
RAM_1500_5TH_GEN = ChryslerPlatformConfig(
|
||||
[ChryslerCarDocs("Ram 1500 2019-24", car_parts=CarParts.common([CarHarness.ram]))],
|
||||
ChryslerCarSpecs(mass=2493., wheelbase=3.88, steerRatio=16.3, minSteerSpeed=14.5),
|
||||
{Bus.pt: 'chrysler_ram_dt_generated'},
|
||||
)
|
||||
RAM_HD_5TH_GEN = ChryslerPlatformConfig(
|
||||
[
|
||||
ChryslerCarDocs("Ram 2500 2020-24", car_parts=CarParts.common([CarHarness.ram])),
|
||||
ChryslerCarDocs("Ram 3500 2019-22", car_parts=CarParts.common([CarHarness.ram])),
|
||||
],
|
||||
ChryslerCarSpecs(mass=3405., wheelbase=3.785, steerRatio=15.61, minSteerSpeed=16.),
|
||||
{Bus.pt: 'chrysler_ram_hd_generated'},
|
||||
)
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
def __init__(self, CP):
|
||||
self.STEER_STEP = 2 # 50 Hz
|
||||
self.STEER_ERROR_MAX = 80
|
||||
if CP.carFingerprint in RAM_HD:
|
||||
self.STEER_DELTA_UP = 14
|
||||
self.STEER_DELTA_DOWN = 14
|
||||
self.STEER_MAX = 361 # higher than this faults the EPS
|
||||
elif CP.carFingerprint in RAM_DT:
|
||||
self.STEER_DELTA_UP = 6
|
||||
self.STEER_DELTA_DOWN = 6
|
||||
self.STEER_MAX = 350 # EPS allows more, up to 350?
|
||||
else:
|
||||
self.STEER_DELTA_UP = 3
|
||||
self.STEER_DELTA_DOWN = 3
|
||||
self.STEER_MAX = 261 # higher than this faults the EPS
|
||||
|
||||
|
||||
STEER_THRESHOLD = 120
|
||||
|
||||
RAM_DT = {CAR.RAM_1500_5TH_GEN, }
|
||||
RAM_HD = {CAR.RAM_HD_5TH_GEN, }
|
||||
RAM_CARS = RAM_DT | RAM_HD
|
||||
|
||||
|
||||
CHRYSLER_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(0xf132)
|
||||
CHRYSLER_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
|
||||
p16(0xf132)
|
||||
|
||||
CHRYSLER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_NUMBER)
|
||||
CHRYSLER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_NUMBER)
|
||||
|
||||
CHRYSLER_RX_OFFSET = -0x280
|
||||
|
||||
FW_QUERY_CONFIG = FwQueryConfig(
|
||||
requests=[
|
||||
Request(
|
||||
[CHRYSLER_VERSION_REQUEST],
|
||||
[CHRYSLER_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.abs, Ecu.eps, Ecu.srs, Ecu.fwdRadar, Ecu.combinationMeter],
|
||||
rx_offset=CHRYSLER_RX_OFFSET,
|
||||
bus=0,
|
||||
),
|
||||
Request(
|
||||
[CHRYSLER_VERSION_REQUEST],
|
||||
[CHRYSLER_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.abs, Ecu.hybrid, Ecu.engine, Ecu.transmission],
|
||||
bus=0,
|
||||
),
|
||||
Request(
|
||||
[CHRYSLER_SOFTWARE_VERSION_REQUEST],
|
||||
[CHRYSLER_SOFTWARE_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.engine, Ecu.transmission],
|
||||
bus=0,
|
||||
),
|
||||
],
|
||||
extra_ecus=[
|
||||
(Ecu.abs, 0x7e4, None), # alt address for abs on hybrids, NOTE: not on all hybrid platforms
|
||||
],
|
||||
)
|
||||
|
||||
DBC = CAR.create_dbc_map()
|
||||
@@ -0,0 +1,4 @@
|
||||
import os
|
||||
|
||||
|
||||
BASEDIR = os.path.abspath(os.path.join(os.path.dirname(os.path.realpath(__file__)), "../"))
|
||||
@@ -0,0 +1,20 @@
|
||||
import numpy as np
|
||||
|
||||
|
||||
class Conversions:
|
||||
# Speed
|
||||
MPH_TO_KPH = 1.609344
|
||||
KPH_TO_MPH = 1. / MPH_TO_KPH
|
||||
MS_TO_KPH = 3.6
|
||||
KPH_TO_MS = 1. / MS_TO_KPH
|
||||
MS_TO_MPH = MS_TO_KPH * KPH_TO_MPH
|
||||
MPH_TO_MS = MPH_TO_KPH * KPH_TO_MS
|
||||
MS_TO_KNOTS = 1.9438
|
||||
KNOTS_TO_MS = 1. / MS_TO_KNOTS
|
||||
|
||||
# Angle
|
||||
DEG_TO_RAD = np.pi / 180.
|
||||
RAD_TO_DEG = 1. / DEG_TO_RAD
|
||||
|
||||
# Mass
|
||||
LB_TO_KG = 0.453592
|
||||
@@ -0,0 +1,44 @@
|
||||
class FirstOrderFilter:
|
||||
# first order filter
|
||||
def __init__(self, x0, rc, dt, initialized=True):
|
||||
self.x = x0
|
||||
self._dt = dt
|
||||
self.update_alpha(rc)
|
||||
self.initialized = initialized
|
||||
|
||||
def update_dt(self, dt):
|
||||
self._dt = dt
|
||||
self.update_alpha(self._rc)
|
||||
|
||||
def update_alpha(self, rc):
|
||||
self._rc = rc
|
||||
self._alpha = self._dt / (self._rc + self._dt)
|
||||
|
||||
def update(self, x):
|
||||
if self.initialized:
|
||||
self.x = (1. - self._alpha) * self.x + self._alpha * x
|
||||
else:
|
||||
self.initialized = True
|
||||
self.x = x
|
||||
return self.x
|
||||
|
||||
|
||||
class HighPassFilter:
|
||||
# technically a band-pass filter
|
||||
def __init__(self, x0, rc1, rc2, dt, initialized=True):
|
||||
self.x = x0
|
||||
self._f1 = FirstOrderFilter(x0, rc1, dt, initialized)
|
||||
self._f2 = FirstOrderFilter(x0, rc2, dt, initialized)
|
||||
assert rc2 > rc1, "rc2 must be greater than rc1"
|
||||
|
||||
def update_dt(self, dt):
|
||||
self._f1.update_dt(dt)
|
||||
self._f2.update_dt(dt)
|
||||
|
||||
def update_alpha(self, rc1, rc2):
|
||||
self._f1.update_alpha(rc1)
|
||||
self._f2.update_alpha(rc2)
|
||||
|
||||
def update(self, x):
|
||||
self.x = self._f1.update(x) - self._f2.update(x)
|
||||
return self.x
|
||||
@@ -0,0 +1,20 @@
|
||||
def clip(x, lo, hi):
|
||||
return max(lo, min(hi, x))
|
||||
|
||||
def interp(x, xp, fp):
|
||||
N = len(xp)
|
||||
|
||||
def get_interp(xv):
|
||||
hi = 0
|
||||
while hi < N and xv > xp[hi]:
|
||||
hi += 1
|
||||
low = hi - 1
|
||||
return fp[-1] if hi == N and xv > xp[low] else (
|
||||
fp[0] if hi == 0 else
|
||||
(xv - xp[low]) * (fp[hi] - fp[low]) / (xp[hi] - xp[low]) + fp[low])
|
||||
|
||||
return [get_interp(v) for v in x] if hasattr(x, '__iter__') else get_interp(x)
|
||||
|
||||
def mean(x):
|
||||
return sum(x) / len(x)
|
||||
|
||||
71
artifacts/package_sources/iqdbc/iqdbc/car/common/pid.py
Normal file
71
artifacts/package_sources/iqdbc/iqdbc/car/common/pid.py
Normal file
@@ -0,0 +1,71 @@
|
||||
import numpy as np
|
||||
from numbers import Number
|
||||
|
||||
|
||||
class PIDController:
|
||||
def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
|
||||
self._k_p = k_p
|
||||
self._k_i = k_i
|
||||
self._k_d = k_d
|
||||
self.k_f = k_f # feedforward gain
|
||||
if isinstance(self._k_p, Number):
|
||||
self._k_p = [[0], [self._k_p]]
|
||||
if isinstance(self._k_i, Number):
|
||||
self._k_i = [[0], [self._k_i]]
|
||||
if isinstance(self._k_d, Number):
|
||||
self._k_d = [[0], [self._k_d]]
|
||||
|
||||
self.pos_limit = pos_limit
|
||||
self.neg_limit = neg_limit
|
||||
|
||||
self.i_unwind_rate = 0.3 / rate
|
||||
self.i_rate = 1.0 / rate
|
||||
self.speed = 0.0
|
||||
|
||||
self.reset()
|
||||
|
||||
@property
|
||||
def k_p(self):
|
||||
return np.interp(self.speed, self._k_p[0], self._k_p[1])
|
||||
|
||||
@property
|
||||
def k_i(self):
|
||||
return np.interp(self.speed, self._k_i[0], self._k_i[1])
|
||||
|
||||
@property
|
||||
def k_d(self):
|
||||
return np.interp(self.speed, self._k_d[0], self._k_d[1])
|
||||
|
||||
@property
|
||||
def error_integral(self):
|
||||
return self.i/self.k_i
|
||||
|
||||
def reset(self):
|
||||
self.p = 0.0
|
||||
self.i = 0.0
|
||||
self.d = 0.0
|
||||
self.f = 0.0
|
||||
self.control = 0
|
||||
|
||||
def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False):
|
||||
self.speed = speed
|
||||
|
||||
self.p = float(error) * self.k_p
|
||||
self.f = feedforward * self.k_f
|
||||
self.d = error_rate * self.k_d
|
||||
|
||||
if override:
|
||||
self.i -= self.i_unwind_rate * float(np.sign(self.i))
|
||||
else:
|
||||
if not freeze_integrator:
|
||||
self.i = self.i + error * self.k_i * self.i_rate
|
||||
|
||||
# Clip i to prevent exceeding control limits
|
||||
control_no_i = self.p + self.d + self.f
|
||||
control_no_i = np.clip(control_no_i, self.neg_limit, self.pos_limit)
|
||||
self.i = np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
|
||||
|
||||
control = self.p + self.i + self.d + self.f
|
||||
|
||||
self.control = np.clip(control, self.neg_limit, self.pos_limit)
|
||||
return self.control
|
||||
@@ -0,0 +1,54 @@
|
||||
import numpy as np
|
||||
|
||||
|
||||
def get_kalman_gain(dt, A, C, Q, R, iterations=100):
|
||||
P = np.zeros_like(Q)
|
||||
for _ in range(iterations):
|
||||
P = A.dot(P).dot(A.T) + dt * Q
|
||||
S = C.dot(P).dot(C.T) + R
|
||||
K = P.dot(C.T).dot(np.linalg.inv(S))
|
||||
P = (np.eye(len(P)) - K.dot(C)).dot(P)
|
||||
return K
|
||||
|
||||
|
||||
class KF1D:
|
||||
# this EKF assumes constant covariance matrix, so calculations are much simpler
|
||||
# the Kalman gain also needs to be precomputed using the control module
|
||||
|
||||
def __init__(self, x0, A, C, K):
|
||||
self.x0_0 = x0[0][0]
|
||||
self.x1_0 = x0[1][0]
|
||||
self.A0_0 = A[0][0]
|
||||
self.A0_1 = A[0][1]
|
||||
self.A1_0 = A[1][0]
|
||||
self.A1_1 = A[1][1]
|
||||
self.C0_0 = C[0]
|
||||
self.C0_1 = C[1]
|
||||
self.K0_0 = K[0][0]
|
||||
self.K1_0 = K[1][0]
|
||||
|
||||
self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0
|
||||
self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1
|
||||
self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0
|
||||
self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1
|
||||
|
||||
# K matrix needs to be pre-computed as follow:
|
||||
# import control
|
||||
# (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R)
|
||||
# self.K = np.transpose(K)
|
||||
|
||||
def update(self, meas):
|
||||
#self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas)
|
||||
x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas
|
||||
x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas
|
||||
self.x0_0 = x0_0
|
||||
self.x1_0 = x1_0
|
||||
return [self.x0_0, self.x1_0]
|
||||
|
||||
@property
|
||||
def x(self):
|
||||
return [[self.x0_0], [self.x1_0]]
|
||||
|
||||
def set_x(self, x):
|
||||
self.x0_0 = x[0][0]
|
||||
self.x1_0 = x[1][0]
|
||||
31
artifacts/package_sources/iqdbc/iqdbc/car/crc.py
Normal file
31
artifacts/package_sources/iqdbc/iqdbc/car/crc.py
Normal file
@@ -0,0 +1,31 @@
|
||||
|
||||
def _gen_crc8_table(poly: int) -> list[int]:
|
||||
table = []
|
||||
for i in range(256):
|
||||
crc = i
|
||||
for _ in range(8):
|
||||
if crc & 0x80:
|
||||
crc = ((crc << 1) ^ poly) & 0xFF
|
||||
else:
|
||||
crc = (crc << 1) & 0xFF
|
||||
table.append(crc)
|
||||
return table
|
||||
|
||||
|
||||
def _gen_crc16_table(poly: int) -> list[int]:
|
||||
table = []
|
||||
for i in range(256):
|
||||
crc = i << 8
|
||||
for _ in range(8):
|
||||
if crc & 0x8000:
|
||||
crc = ((crc << 1) ^ poly) & 0xFFFF
|
||||
else:
|
||||
crc = (crc << 1) & 0xFFFF
|
||||
table.append(crc)
|
||||
return table
|
||||
|
||||
|
||||
CRC8H2F = _gen_crc8_table(0x2F)
|
||||
CRC8J1850 = _gen_crc8_table(0x1D)
|
||||
CRC8BODY = _gen_crc8_table(0xD5)
|
||||
CRC16_XMODEM = _gen_crc16_table(0x1021)
|
||||
82
artifacts/package_sources/iqdbc/iqdbc/car/debug/format_fingerprints.py
Executable file
82
artifacts/package_sources/iqdbc/iqdbc/car/debug/format_fingerprints.py
Executable file
@@ -0,0 +1,82 @@
|
||||
#!/usr/bin/env python3
|
||||
import jinja2
|
||||
import os
|
||||
|
||||
from iqdbc.car.common.basedir import BASEDIR
|
||||
from iqdbc.car.interfaces import get_interface_attr
|
||||
from iqdbc.car.structs import CarParams
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
CARS = get_interface_attr('CAR')
|
||||
FW_VERSIONS = get_interface_attr('FW_VERSIONS')
|
||||
FINGERPRINTS = get_interface_attr('FINGERPRINTS')
|
||||
ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()}
|
||||
|
||||
FINGERPRINTS_PY_TEMPLATE = jinja2.Template("""
|
||||
{%- if FINGERPRINTS[brand] and brand != 'body' %}
|
||||
# ruff: noqa: E501
|
||||
{% endif %}
|
||||
\"\"\" AUTO-FORMATTED USING iqdbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE.\"\"\"
|
||||
{% if FW_VERSIONS[brand] %}
|
||||
from iqdbc.car.structs import CarParams
|
||||
{% endif %}
|
||||
from iqdbc.car.{{brand}}.values import CAR
|
||||
{% if FW_VERSIONS[brand] %}
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
{% endif %}
|
||||
{% if comments +%}
|
||||
{{ comments | join() }}
|
||||
{% endif %}
|
||||
{% if FINGERPRINTS[brand] %}
|
||||
|
||||
FINGERPRINTS = {
|
||||
{% for car, fingerprints in FINGERPRINTS[brand].items() %}
|
||||
CAR.{{car.name}}: [{
|
||||
{% for fingerprint in fingerprints %}
|
||||
{% if not loop.first %}
|
||||
{{ "{" }}
|
||||
{% endif %}
|
||||
{% for key, value in fingerprint.items() %}{{key}}: {{value}}{% if not loop.last %}, {% endif %}{% endfor %}
|
||||
|
||||
}{% if loop.last %}]{% endif %},
|
||||
{% endfor %}
|
||||
{% endfor %}
|
||||
}
|
||||
{% endif %}
|
||||
|
||||
FW_VERSIONS{% if not FW_VERSIONS[brand] %}: dict[str, dict[tuple, list[bytes]]]{% endif %} = {
|
||||
{% for car, _ in FW_VERSIONS[brand].items() %}
|
||||
CAR.{{car.name}}: {
|
||||
{% for key, fw_versions in FW_VERSIONS[brand][car].items() %}
|
||||
(Ecu.{{ECU_NAME[key[0]]}}, 0x{{"%0x" | format(key[1] | int)}}, \
|
||||
{% if key[2] %}0x{{"%0x" | format(key[2] | int)}}{% else %}{{key[2]}}{% endif %}): [
|
||||
{% for fw_version in (fw_versions + extra_fw_versions.get(car, {}).get(key, [])) | unique | sort %}
|
||||
{{fw_version}},
|
||||
{% endfor %}
|
||||
],
|
||||
{% endfor %}
|
||||
},
|
||||
{% endfor %}
|
||||
}
|
||||
|
||||
""", trim_blocks=True)
|
||||
|
||||
|
||||
def format_brand_fw_versions(brand, extra_fw_versions: None | dict[str, dict[tuple, list[bytes]]] = None):
|
||||
extra_fw_versions = extra_fw_versions or {}
|
||||
|
||||
fingerprints_file = os.path.join(BASEDIR, f"{brand}/fingerprints.py")
|
||||
with open(fingerprints_file) as f:
|
||||
comments = [line for line in f.readlines() if line.startswith("#") and "noqa" not in line]
|
||||
|
||||
with open(fingerprints_file, "w") as f:
|
||||
f.write(FINGERPRINTS_PY_TEMPLATE.render(brand=brand, comments=comments, ECU_NAME=ECU_NAME,
|
||||
FINGERPRINTS=FINGERPRINTS, FW_VERSIONS=FW_VERSIONS,
|
||||
extra_fw_versions=extra_fw_versions))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
for brand in FW_VERSIONS.keys():
|
||||
format_brand_fw_versions(brand)
|
||||
80
artifacts/package_sources/iqdbc/iqdbc/car/disable_ecu.py
Normal file
80
artifacts/package_sources/iqdbc/iqdbc/car/disable_ecu.py
Normal file
@@ -0,0 +1,80 @@
|
||||
from iqdbc.car.can_definitions import CanData
|
||||
from iqdbc.car.carlog import carlog
|
||||
from iqdbc.car.isotp_parallel_query import IsoTpParallelQuery
|
||||
|
||||
EXT_DIAG_REQUEST = b'\x10\x03'
|
||||
EXT_DIAG_RESPONSE = b'\x50\x03'
|
||||
|
||||
COM_CONT_RESPONSE = b''
|
||||
|
||||
CLEAR_DTC_REQUEST = b'\x14\xff\xff\xff'
|
||||
CLEAR_DTC_RESPONSE = b'\x54'
|
||||
|
||||
FUNCTIONAL_ADDR_29BIT = 0x18DB33F1
|
||||
CLEAR_DTC_ISOTP_SF = bytes([len(CLEAR_DTC_REQUEST)]) + CLEAR_DTC_REQUEST + b'\x00' * (7 - len(CLEAR_DTC_REQUEST))
|
||||
|
||||
|
||||
def clear_all_dtcs(can_send, buses, functional_addr=FUNCTIONAL_ADDR_29BIT):
|
||||
# broadcast clears stored DTCs on every ECU on the bus, including safety-relevant modules
|
||||
for bus in buses:
|
||||
carlog.warning(f"clear all DTCs (functional) on bus {bus} ...")
|
||||
can_send([CanData(functional_addr, CLEAR_DTC_ISOTP_SF, bus)])
|
||||
|
||||
|
||||
def clear_ecu_dtcs(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, timeout=0.1, retry=10, response_offset: int = 0x8):
|
||||
carlog.warning(f"ecu clear DTCs {hex(addr), sub_addr} ...")
|
||||
|
||||
for i in range(retry):
|
||||
try:
|
||||
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], response_offset)
|
||||
|
||||
for _, _ in query.get_data(timeout).items():
|
||||
carlog.warning("clear diagnostic information ...")
|
||||
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [CLEAR_DTC_REQUEST], [CLEAR_DTC_RESPONSE], response_offset)
|
||||
query.get_data(timeout)
|
||||
|
||||
carlog.warning("ecu DTCs cleared")
|
||||
return True
|
||||
|
||||
except Exception:
|
||||
carlog.exception("ecu clear DTCs exception")
|
||||
|
||||
carlog.error(f"ecu clear DTCs retry ({i + 1}) ...")
|
||||
carlog.error("ecu clear DTCs failed")
|
||||
return False
|
||||
|
||||
|
||||
def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_req=b'\x28\x83\x01',
|
||||
timeout=0.1, retry=10, response_offset: int = 0x8, clear_dtc=False):
|
||||
"""Silence an ECU by disabling sending and receiving messages using UDS 0x28.
|
||||
The ECU will stay silent as long as openpilot keeps sending Tester Present.
|
||||
|
||||
This is used to disable the radar in some cars. Openpilot will emulate the radar.
|
||||
WARNING: THIS DISABLES AEB!"""
|
||||
carlog.warning(f"ecu disable {hex(addr), sub_addr} ...")
|
||||
|
||||
for i in range(retry):
|
||||
try:
|
||||
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], response_offset)
|
||||
|
||||
for _, _ in query.get_data(timeout).items():
|
||||
# a DTC clear can take the ECU several hundred ms, so it must complete before comms go down
|
||||
if clear_dtc:
|
||||
carlog.warning("clear diagnostic information ...")
|
||||
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [CLEAR_DTC_REQUEST], [CLEAR_DTC_RESPONSE], response_offset)
|
||||
query.get_data(timeout)
|
||||
|
||||
carlog.warning("communication control disable tx/rx ...")
|
||||
|
||||
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [com_cont_req], [COM_CONT_RESPONSE], response_offset)
|
||||
query.get_data(0)
|
||||
|
||||
carlog.warning("ecu disabled")
|
||||
return True
|
||||
|
||||
except Exception:
|
||||
carlog.exception("ecu disable exception")
|
||||
|
||||
carlog.error(f"ecu disable retry ({i + 1}) ...")
|
||||
carlog.error("ecu disable failed")
|
||||
return False
|
||||
113
artifacts/package_sources/iqdbc/iqdbc/car/docs.py
Executable file
113
artifacts/package_sources/iqdbc/iqdbc/car/docs.py
Executable file
@@ -0,0 +1,113 @@
|
||||
#!/usr/bin/env python3
|
||||
import re
|
||||
import os
|
||||
import jinja2
|
||||
import argparse
|
||||
import unicodedata
|
||||
from typing import get_args
|
||||
|
||||
from enum import Enum
|
||||
from collections import defaultdict
|
||||
|
||||
from iqdbc.car.common.basedir import BASEDIR
|
||||
from iqdbc.car import gen_empty_fingerprint
|
||||
from iqdbc.car.structs import CarParams, IQCarParams
|
||||
from iqdbc.car.docs_definitions import BaseCarHarness, CarDocs, Device, ExtraCarDocs, Column, ExtraCarsColumn, CommonFootnote, PartType, SupportType
|
||||
from iqdbc.car.car_helpers import interfaces
|
||||
from iqdbc.car.interfaces import get_interface_attr
|
||||
from iqdbc.car.values import Platform
|
||||
from iqdbc.car.mock.values import CAR as MOCK
|
||||
from iqdbc.car.extra_cars import CAR as EXTRA
|
||||
|
||||
|
||||
EXTRA_CARS_MD_OUT = os.path.join(BASEDIR, "../", "../", "docs", "CARS.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(platform) -> tuple[CarParams, IQCarParams]:
|
||||
cp_platform = platform if platform in interfaces else MOCK.MOCK
|
||||
CP: CarParams = interfaces[cp_platform].get_params(cp_platform, fingerprint=gen_empty_fingerprint(),
|
||||
car_fw=[CarParams.CarFw(ecu=CarParams.Ecu.unknown)],
|
||||
alpha_long=True, is_release=True, docs=True)
|
||||
|
||||
CP_IQ: IQCarParams = interfaces[cp_platform].get_params_iq(CP, cp_platform, fingerprint=gen_empty_fingerprint(),
|
||||
car_fw=[CarParams.CarFw(ecu=CarParams.Ecu.unknown)],
|
||||
alpha_long=True, is_release_iq=True, docs=True)
|
||||
return CP, CP_IQ
|
||||
|
||||
|
||||
def get_all_footnotes() -> dict[Enum, int]:
|
||||
all_footnotes = list(CommonFootnote)
|
||||
for footnotes in get_interface_attr("Footnote", ignore_none=True).values():
|
||||
all_footnotes.extend(footnotes)
|
||||
return {fn: idx + 1 for idx, fn in enumerate(all_footnotes)}
|
||||
|
||||
|
||||
def _natural_sort_key(s):
|
||||
# NFKD normalization ensures accented characters sort with their base letter (e.g., Š sorts with S)
|
||||
normalized = unicodedata.normalize('NFKD', s)
|
||||
return [int(t) if t.isdigit() else t.lower() for t in re.split(r'(\d+)', normalized) if t]
|
||||
|
||||
|
||||
def build_sorted_car_docs_list(platforms, footnotes=None):
|
||||
collected_car_docs: list[CarDocs | ExtraCarDocs] = []
|
||||
for platform in platforms.values():
|
||||
car_docs = platform.config.car_docs
|
||||
CP, CP_IQ = get_params_for_docs(platform)
|
||||
|
||||
if not len(car_docs):
|
||||
continue
|
||||
|
||||
# A platform can include multiple car models
|
||||
for _car_docs in car_docs:
|
||||
if not hasattr(_car_docs, "row"):
|
||||
_car_docs.init_make(CP)
|
||||
_car_docs.init(CP, footnotes)
|
||||
collected_car_docs.append(_car_docs)
|
||||
|
||||
# Sort cars by make and model + year
|
||||
sorted_cars = sorted(collected_car_docs, key=lambda car: _natural_sort_key(car.name))
|
||||
return sorted_cars
|
||||
|
||||
|
||||
# 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(EXTRA_PLATFORMS, footnotes=collected_footnotes)
|
||||
return sorted_list
|
||||
|
||||
|
||||
def group_by_make(all_car_docs: list[CarDocs]) -> dict[str, list[CarDocs]]:
|
||||
sorted_car_docs = defaultdict(list)
|
||||
for car_docs in all_car_docs:
|
||||
sorted_car_docs[car_docs.make].append(car_docs)
|
||||
return dict(sorted_car_docs)
|
||||
|
||||
|
||||
def generate_cars_md(all_car_docs: list[CarDocs], template_fn: str, **kwargs) -> str:
|
||||
with open(template_fn) as f:
|
||||
template = jinja2.Template(f.read(), trim_blocks=True, lstrip_blocks=True)
|
||||
|
||||
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, ExtraCarsColumn=ExtraCarsColumn,
|
||||
BaseCarHarness=BaseCarHarness, SupportType=SupportType,
|
||||
**kwargs)
|
||||
return cars_md
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Auto generates supportability info docs for all known cars",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
|
||||
parser.add_argument("--out", default=EXTRA_CARS_MD_OUT, help="Override default generated filename")
|
||||
args = parser.parse_args()
|
||||
|
||||
with open(args.out, 'w') as f:
|
||||
f.write(generate_cars_md(get_all_car_docs(), args.template))
|
||||
print(f"Generated and written to {args.out}")
|
||||
406
artifacts/package_sources/iqdbc/iqdbc/car/docs_definitions.py
Normal file
406
artifacts/package_sources/iqdbc/iqdbc/car/docs_definitions.py
Normal file
@@ -0,0 +1,406 @@
|
||||
import re
|
||||
from collections import namedtuple
|
||||
import copy
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum
|
||||
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.structs import CarParams
|
||||
|
||||
GOOD_TORQUE_THRESHOLD = 1.0 # m/s^2
|
||||
MODEL_YEARS_RE = r"(?<= )((\d{4}-\d{2})|(\d{4}))(,|$)"
|
||||
|
||||
|
||||
class Column(Enum):
|
||||
MAKE = "Make"
|
||||
MODEL = "Model"
|
||||
PACKAGE = "Supported Package"
|
||||
LONGITUDINAL = "ACC"
|
||||
FSR_LONGITUDINAL = "No ACC accel below"
|
||||
FSR_STEERING = "No ALC below"
|
||||
STEERING_TORQUE = "Steering Torque"
|
||||
AUTO_RESUME = "Resume from stop"
|
||||
HARDWARE = "Hardware Needed"
|
||||
VIDEO = "Video"
|
||||
SETUP_VIDEO = "Setup Video"
|
||||
|
||||
|
||||
class ExtraCarsColumn(Enum):
|
||||
MAKE = "Make"
|
||||
MODEL = "Model"
|
||||
PACKAGE = "Package"
|
||||
SUPPORT = "Support Level"
|
||||
|
||||
|
||||
class SupportType(Enum):
|
||||
UPSTREAM = "Upstream" # Actively maintained by comma, plug-and-play in release versions of openpilot
|
||||
REVIEW = "Under review" # Dashcam, but planned for official support after safety validation
|
||||
DASHCAM = "Dashcam mode" # Dashcam, but may be drivable in a community fork
|
||||
COMMUNITY = "Community" # Not upstream, but available in a custom community fork, not validated by comma
|
||||
CUSTOM = "Custom" # Upstream, but don't have a harness available or need an unusual custom install
|
||||
INCOMPATIBLE = "Not compatible" # Known fundamental incompatibility such as Flexray or hydraulic power steering
|
||||
|
||||
|
||||
class Star(Enum):
|
||||
FULL = "full"
|
||||
HALF = "half"
|
||||
EMPTY = "empty"
|
||||
|
||||
|
||||
# A part + its comprised parts
|
||||
@dataclass
|
||||
class BasePart:
|
||||
name: str
|
||||
parts: list[Enum] = field(default_factory=list)
|
||||
|
||||
def all_parts(self):
|
||||
# Recursively get all parts
|
||||
_parts = 'parts'
|
||||
parts = []
|
||||
parts.extend(getattr(self, _parts))
|
||||
for part in getattr(self, _parts):
|
||||
parts.extend(part.value.all_parts())
|
||||
|
||||
return parts
|
||||
|
||||
|
||||
class EnumBase(Enum):
|
||||
@property
|
||||
def part_type(self):
|
||||
return PartType(self.__class__)
|
||||
|
||||
|
||||
class Mount(EnumBase):
|
||||
mount = BasePart("mount")
|
||||
|
||||
|
||||
class Cable(EnumBase):
|
||||
long_obdc_cable = BasePart("long OBD-C cable (9.5 ft)")
|
||||
usb_a_2_a_cable = BasePart("USB A-A cable")
|
||||
usbc_otg_cable = BasePart("USB C OTG cable")
|
||||
obd_c_cable_2ft = BasePart("OBD-C cable (2 ft)")
|
||||
|
||||
|
||||
class Accessory(EnumBase):
|
||||
harness_box = BasePart("harness box")
|
||||
comma_power = BasePart("comma power v3")
|
||||
|
||||
|
||||
class Tool(EnumBase):
|
||||
socket_8mm_deep = BasePart("Socket Wrench 8mm or 5/16\" (deep)")
|
||||
pry_tool = BasePart("Pry Tool")
|
||||
|
||||
|
||||
@dataclass
|
||||
class BaseCarHarness(BasePart):
|
||||
parts: list[Enum] = field(default_factory=lambda: [Accessory.harness_box, Accessory.comma_power])
|
||||
has_connector: bool = True # without are hidden on the harness connector page
|
||||
|
||||
|
||||
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", parts=[Accessory.harness_box, Accessory.comma_power, Tool.socket_8mm_deep, Tool.pry_tool])
|
||||
subaru_b = BaseCarHarness("Subaru B connector", parts=[Accessory.harness_box, Accessory.comma_power, Tool.socket_8mm_deep, Tool.pry_tool])
|
||||
subaru_c = BaseCarHarness("Subaru C connector", parts=[Accessory.harness_box, Accessory.comma_power, Tool.socket_8mm_deep, Tool.pry_tool])
|
||||
subaru_d = BaseCarHarness("Subaru D connector", parts=[Accessory.harness_box, Accessory.comma_power, Tool.socket_8mm_deep, Tool.pry_tool])
|
||||
fca = BaseCarHarness("FCA connector")
|
||||
ram = BaseCarHarness("Ram connector")
|
||||
vw_a = BaseCarHarness("VW A connector")
|
||||
vw_j533 = BaseCarHarness("VW J533 connector", parts=[Accessory.harness_box, Cable.long_obdc_cable])
|
||||
hyundai_a = BaseCarHarness("Hyundai A connector")
|
||||
hyundai_b = BaseCarHarness("Hyundai B connector")
|
||||
hyundai_c = BaseCarHarness("Hyundai C connector")
|
||||
hyundai_d = BaseCarHarness("Hyundai D connector")
|
||||
hyundai_e = BaseCarHarness("Hyundai E connector")
|
||||
hyundai_f = BaseCarHarness("Hyundai F connector")
|
||||
hyundai_g = BaseCarHarness("Hyundai G connector")
|
||||
hyundai_h = BaseCarHarness("Hyundai H connector")
|
||||
hyundai_i = BaseCarHarness("Hyundai I connector")
|
||||
hyundai_j = BaseCarHarness("Hyundai J connector")
|
||||
hyundai_k = BaseCarHarness("Hyundai K connector")
|
||||
hyundai_l = BaseCarHarness("Hyundai L connector")
|
||||
hyundai_m = BaseCarHarness("Hyundai M connector")
|
||||
hyundai_n = BaseCarHarness("Hyundai N connector")
|
||||
hyundai_o = BaseCarHarness("Hyundai O connector")
|
||||
hyundai_p = BaseCarHarness("Hyundai P connector")
|
||||
hyundai_q = BaseCarHarness("Hyundai Q connector")
|
||||
hyundai_r = BaseCarHarness("Hyundai R connector")
|
||||
custom = BaseCarHarness("Developer connector")
|
||||
obd_ii = BaseCarHarness("OBD-II connector", parts=[Cable.long_obdc_cable], has_connector=False)
|
||||
gm = BaseCarHarness("GM connector", parts=[Accessory.harness_box])
|
||||
gmsdgm = BaseCarHarness("GM SDGM connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable])
|
||||
nissan_a = BaseCarHarness("Nissan A connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable])
|
||||
nissan_b = BaseCarHarness("Nissan B connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable])
|
||||
mazda = BaseCarHarness("Mazda connector")
|
||||
ford_q3 = BaseCarHarness("Ford Q3 connector")
|
||||
ford_q4 = BaseCarHarness("Ford Q4 connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable])
|
||||
rivian = BaseCarHarness("Rivian A connector", parts=[Accessory.harness_box, Accessory.comma_power, Cable.long_obdc_cable])
|
||||
tesla_a = BaseCarHarness("Tesla A connector", parts=[Accessory.harness_box, Cable.long_obdc_cable])
|
||||
tesla_b = BaseCarHarness("Tesla B connector", parts=[Accessory.harness_box, Cable.long_obdc_cable])
|
||||
psa_a = BaseCarHarness("PSA A connector", parts=[Accessory.harness_box, Cable.long_obdc_cable])
|
||||
|
||||
# custom harness
|
||||
honda_clarity = BaseCarHarness("Honda Nidec connector + Honda Clarity Proxy Board")
|
||||
|
||||
|
||||
class Device(EnumBase):
|
||||
four = BasePart("comma four", parts=[Mount.mount, Cable.obd_c_cable_2ft])
|
||||
|
||||
|
||||
class PartType(Enum):
|
||||
accessory = Accessory
|
||||
cable = Cable
|
||||
connector = CarHarness
|
||||
device = Device
|
||||
mount = Mount
|
||||
tool = Tool
|
||||
|
||||
|
||||
DEFAULT_CAR_PARTS: list[EnumBase] = [Device.four]
|
||||
|
||||
|
||||
@dataclass
|
||||
class CarParts:
|
||||
parts: list[EnumBase] = field(default_factory=list)
|
||||
custom_parts_url: str | None = None
|
||||
|
||||
def __call__(self):
|
||||
return copy.deepcopy(self)
|
||||
|
||||
@classmethod
|
||||
def common(cls, add: list[EnumBase] | None = None, remove: list[EnumBase] | None = None):
|
||||
p = [part for part in (add or []) + DEFAULT_CAR_PARTS if part not in (remove or [])]
|
||||
return cls(p)
|
||||
|
||||
def all_parts(self):
|
||||
parts = []
|
||||
for part in self.parts:
|
||||
parts.extend(part.value.all_parts())
|
||||
return self.parts + parts
|
||||
|
||||
|
||||
CarFootnote = namedtuple("CarFootnote", ["text", "column", "docs_only", "setup_note"], defaults=(False, False))
|
||||
|
||||
|
||||
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 `nightly-dev`.",
|
||||
Column.LONGITUDINAL, docs_only=True)
|
||||
|
||||
|
||||
def get_footnotes(footnotes: list[Enum], column: Column) -> list[Enum]:
|
||||
# Returns applicable footnotes given current column
|
||||
return [fn for fn in footnotes if fn.value.column == column]
|
||||
|
||||
|
||||
# TODO: store years as a list
|
||||
def get_year_list(years):
|
||||
years_list = []
|
||||
if len(years) == 0:
|
||||
return years_list
|
||||
|
||||
for year in years.split(','):
|
||||
year = year.strip()
|
||||
if len(year) == 4:
|
||||
years_list.append(str(year))
|
||||
elif "-" in year and len(year) == 7:
|
||||
start, end = year.split("-")
|
||||
years_list.extend(map(str, range(int(start), int(f"20{end}") + 1)))
|
||||
else:
|
||||
raise Exception(f"Malformed year string: {years}")
|
||||
return years_list
|
||||
|
||||
|
||||
def split_name(name: str) -> tuple[str, str, str]:
|
||||
make, model = name.split(" ", 1)
|
||||
years = ""
|
||||
match = re.search(MODEL_YEARS_RE, model)
|
||||
if match is not None:
|
||||
years = model[match.start():]
|
||||
model = model[:match.start() - 1]
|
||||
return make, model, years
|
||||
|
||||
|
||||
@dataclass
|
||||
class CarDocs:
|
||||
# make + model + model years
|
||||
name: str
|
||||
|
||||
# the simplest description of the requirements for the US market
|
||||
package: str
|
||||
|
||||
video: str | None = None
|
||||
setup_video: str | None = None
|
||||
footnotes: list[Enum] = field(default_factory=list)
|
||||
min_steer_speed: float | None = None
|
||||
min_enable_speed: float | None = None
|
||||
auto_resume: bool | None = None
|
||||
|
||||
# all the parts needed for the supported car
|
||||
car_parts: CarParts = field(default_factory=CarParts)
|
||||
|
||||
merged: bool = True
|
||||
support_type: SupportType = SupportType.UPSTREAM
|
||||
support_link: str | None = "#upstream"
|
||||
|
||||
def __post_init__(self):
|
||||
self.make, self.model, self.years = split_name(self.name)
|
||||
self.year_list = get_year_list(self.years)
|
||||
|
||||
def init(self, CP: CarParams, all_footnotes=None):
|
||||
self.brand = CP.brand
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.longitudinal_control = CP.openpilotLongitudinalControl and not CP.alphaLongitudinalAvailable
|
||||
|
||||
if self.merged and CP.dashcamOnly:
|
||||
if self.support_type not in (SupportType.CUSTOM, SupportType.REVIEW):
|
||||
self.support_type = SupportType.DASHCAM
|
||||
self.support_link = "#dashcam"
|
||||
|
||||
# longitudinal column
|
||||
op_long = "Stock"
|
||||
if CP.alphaLongitudinalAvailable:
|
||||
op_long = "openpilot available"
|
||||
self.footnotes.append(CommonFootnote.EXP_LONG_AVAIL)
|
||||
elif CP.openpilotLongitudinalControl:
|
||||
op_long = "openpilot"
|
||||
|
||||
# min steer & enable speed columns
|
||||
# TODO: set all the min steer speeds in carParams and remove this
|
||||
if self.min_steer_speed is not None:
|
||||
assert CP.minSteerSpeed < 0.5, f"{CP.carFingerprint}: Minimum steer speed set in both CarDocs and CarParams"
|
||||
else:
|
||||
self.min_steer_speed = CP.minSteerSpeed
|
||||
|
||||
# TODO: set all the min enable speeds in carParams correctly and remove this
|
||||
if self.min_enable_speed is None:
|
||||
self.min_enable_speed = CP.minEnableSpeed
|
||||
|
||||
if self.auto_resume is None:
|
||||
self.auto_resume = CP.autoResumeSng and self.min_enable_speed <= 0
|
||||
|
||||
# hardware column
|
||||
hardware_col = "None"
|
||||
if self.car_parts.parts:
|
||||
if self.car_parts.custom_parts_url is not None:
|
||||
buy_link = f'<a href="{self.car_parts.custom_parts_url}">Buy Here</a>'
|
||||
else:
|
||||
buy_link = f'<a href="https://comma.ai/shop/comma-3x?harness={self.name}">Buy Here</a>'
|
||||
|
||||
tools_docs = [part for part in self.car_parts.all_parts() if isinstance(part, Tool)]
|
||||
parts_docs = [part for part in self.car_parts.all_parts() if not isinstance(part, Tool)]
|
||||
|
||||
def display_func(parts):
|
||||
return '<br>'.join([f"- {parts.count(part)} {part.value.name}" for part in sorted(set(parts), key=lambda part: str(part.value.name))])
|
||||
|
||||
hardware_col = f'<details><summary>Parts</summary><sub>{display_func(parts_docs)}<br>{buy_link}</sub></details>'
|
||||
if len(tools_docs):
|
||||
hardware_col += f'<details><summary>Tools</summary><sub>{display_func(tools_docs)}</sub></details>'
|
||||
|
||||
self.row: dict[Enum, str | Star] = {
|
||||
Column.MAKE: self.make,
|
||||
Column.MODEL: self.model,
|
||||
Column.PACKAGE: self.package,
|
||||
Column.LONGITUDINAL: op_long,
|
||||
Column.FSR_LONGITUDINAL: f"{max(self.min_enable_speed * CV.MS_TO_MPH, 0):.0f} mph",
|
||||
Column.FSR_STEERING: f"{max(self.min_steer_speed * CV.MS_TO_MPH, 0):.0f} mph",
|
||||
Column.STEERING_TORQUE: Star.EMPTY,
|
||||
Column.AUTO_RESUME: Star.FULL if self.auto_resume else Star.EMPTY,
|
||||
Column.HARDWARE: hardware_col,
|
||||
Column.VIDEO: self.video or "", # replaced with an image and link from template in get_column
|
||||
Column.SETUP_VIDEO: self.setup_video or "", # replaced with an image and link from template in get_column
|
||||
}
|
||||
|
||||
if self.support_link is not None:
|
||||
support_info = f"[{self.support_type.value}]({self.support_link})"
|
||||
else:
|
||||
support_info = self.support_type.value
|
||||
|
||||
self.extra_cars_row: dict[Enum, str] = {
|
||||
ExtraCarsColumn.MAKE: self.make,
|
||||
ExtraCarsColumn.MODEL: self.model,
|
||||
ExtraCarsColumn.PACKAGE: self.package,
|
||||
ExtraCarsColumn.SUPPORT: support_info,
|
||||
}
|
||||
|
||||
# Set steering torque star from max lateral acceleration
|
||||
assert CP.maxLateralAccel > 0.1
|
||||
if CP.maxLateralAccel >= GOOD_TORQUE_THRESHOLD:
|
||||
self.row[Column.STEERING_TORQUE] = Star.FULL
|
||||
|
||||
self.all_footnotes = all_footnotes
|
||||
self.detail_sentence = self.get_detail_sentence(CP)
|
||||
|
||||
return self
|
||||
|
||||
def init_make(self, CP: CarParams):
|
||||
"""CarDocs subclasses can add make-specific logic for harness selection, footnotes, etc."""
|
||||
|
||||
def get_detail_sentence(self, CP):
|
||||
if not CP.notCar:
|
||||
sentence_builder = "openpilot upgrades your <strong>{car_model}</strong> with automated lane centering{alc} and adaptive cruise control{acc}."
|
||||
|
||||
if self.min_steer_speed > self.min_enable_speed:
|
||||
alc = f" <strong>above {self.min_steer_speed * CV.MS_TO_MPH:.0f} mph</strong>," if self.min_steer_speed > 0 else " <strong>at all speeds</strong>,"
|
||||
else:
|
||||
alc = ""
|
||||
|
||||
# Exception for cars which do not auto-resume yet
|
||||
acc = ""
|
||||
if self.min_enable_speed > 0:
|
||||
acc = f" <strong>while driving above {self.min_enable_speed * CV.MS_TO_MPH:.0f} mph</strong>"
|
||||
elif self.auto_resume:
|
||||
acc = " <strong>that automatically resumes from a stop</strong>"
|
||||
|
||||
if self.row[Column.STEERING_TORQUE] != Star.FULL:
|
||||
sentence_builder += " This car may not be able to take tight turns on its own."
|
||||
|
||||
# experimental mode
|
||||
exp_link = "<a href='https://blog.comma.ai/090release/#experimental-mode' target='_blank' class='highlight'>Experimental mode</a>"
|
||||
if CP.openpilotLongitudinalControl and not CP.alphaLongitudinalAvailable:
|
||||
sentence_builder += f" Traffic light and stop sign handling is also available in {exp_link}."
|
||||
|
||||
return sentence_builder.format(car_model=f"{self.make} {self.model}", alc=alc, acc=acc)
|
||||
|
||||
else:
|
||||
if CP.carFingerprint == "COMMA_BODY":
|
||||
return "The body is a robotics dev kit that can run openpilot. <a href='https://www.commabody.com' target='_blank' class='highlight'>Learn more.</a>"
|
||||
else:
|
||||
raise Exception(f"This notCar does not have a detail sentence: {CP.carFingerprint}")
|
||||
|
||||
def get_column(self, column: Column, star_icon: str, video_icon: str, footnote_tag: str) -> str:
|
||||
item: str | Star = self.row[column]
|
||||
if isinstance(item, Star):
|
||||
item = star_icon.format(item.value)
|
||||
elif column == Column.MODEL and len(self.years):
|
||||
item += f" {self.years}"
|
||||
elif column in (Column.VIDEO, Column.SETUP_VIDEO) and len(item) > 0:
|
||||
item = video_icon.format(item)
|
||||
|
||||
footnotes = get_footnotes(self.footnotes, column)
|
||||
if len(footnotes):
|
||||
sups = sorted([self.all_footnotes[fn] for fn in footnotes])
|
||||
item += footnote_tag.format(f'{",".join(map(str, sups))}')
|
||||
|
||||
return item
|
||||
|
||||
def get_extra_cars_column(self, column: ExtraCarsColumn) -> str:
|
||||
item: str = self.extra_cars_row[column]
|
||||
if column == ExtraCarsColumn.MODEL and len(self.years):
|
||||
item += f" {self.years}"
|
||||
|
||||
return item
|
||||
|
||||
|
||||
@dataclass
|
||||
class ExtraCarDocs(CarDocs):
|
||||
package: str = "All"
|
||||
merged: bool = False
|
||||
support_type: SupportType = SupportType.INCOMPATIBLE
|
||||
support_link: str | None = "#incompatible"
|
||||
56
artifacts/package_sources/iqdbc/iqdbc/car/ecu_addrs.py
Normal file
56
artifacts/package_sources/iqdbc/iqdbc/car/ecu_addrs.py
Normal file
@@ -0,0 +1,56 @@
|
||||
import time
|
||||
|
||||
from iqdbc.car import make_tester_present_msg, uds
|
||||
from iqdbc.car.can_definitions import CanData, CanRecvCallable, CanSendCallable
|
||||
from iqdbc.car.carlog import carlog
|
||||
from iqdbc.car.fw_query_definitions import EcuAddrBusType
|
||||
|
||||
|
||||
def _is_tester_present_response(msg: CanData, subaddr: int | None = None) -> bool:
|
||||
# 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
|
||||
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
|
||||
# error response
|
||||
if msg.dat[dat_offset + 1] == 0x7F and msg.dat[dat_offset + 2] == uds.SERVICE_TYPE.TESTER_PRESENT:
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def get_all_ecu_addrs(can_recv: CanRecvCallable, can_send: CanSendCallable, bus: int, timeout: float = 1) -> set[EcuAddrBusType]:
|
||||
addr_list = [0x700 + i for i in range(256)] + [0x18da00f1 + (i << 8) for i in range(256)]
|
||||
queries: set[EcuAddrBusType] = {(addr, None, bus) for addr in addr_list}
|
||||
responses = queries
|
||||
return get_ecu_addrs(can_recv, can_send, queries, responses, timeout=timeout)
|
||||
|
||||
|
||||
def get_ecu_addrs(can_recv: CanRecvCallable, can_send: CanSendCallable, queries: set[EcuAddrBusType],
|
||||
responses: set[EcuAddrBusType], timeout: float = 1) -> set[EcuAddrBusType]:
|
||||
ecu_responses: set[EcuAddrBusType] = set() # set((addr, subaddr, bus),)
|
||||
try:
|
||||
msgs = [make_tester_present_msg(addr, bus, subaddr) for addr, subaddr, bus in queries]
|
||||
|
||||
can_recv()
|
||||
can_send(msgs)
|
||||
start_time = time.monotonic()
|
||||
while time.monotonic() - start_time < timeout:
|
||||
can_packets = can_recv(wait_for_one=True)
|
||||
for packet in can_packets:
|
||||
for msg in packet:
|
||||
if not len(msg.dat):
|
||||
carlog.warning("ECU addr scan: skipping empty remote frame")
|
||||
continue
|
||||
|
||||
subaddr = None if (msg.address, None, msg.src) in responses else msg.dat[0]
|
||||
if (msg.address, subaddr, msg.src) in responses and _is_tester_present_response(msg, subaddr):
|
||||
carlog.debug(f"CAN-RX: {hex(msg.address)} - 0x{bytes.hex(msg.dat)}")
|
||||
if (msg.address, subaddr, msg.src) in ecu_responses:
|
||||
carlog.debug(f"Duplicate ECU address: {hex(msg.address)}")
|
||||
ecu_responses.add((msg.address, subaddr, msg.src))
|
||||
except Exception:
|
||||
carlog.exception("ECU addr scan exception")
|
||||
return ecu_responses
|
||||
88
artifacts/package_sources/iqdbc/iqdbc/car/extra_cars.py
Normal file
88
artifacts/package_sources/iqdbc/iqdbc/car/extra_cars.py
Normal file
@@ -0,0 +1,88 @@
|
||||
from dataclasses import dataclass
|
||||
|
||||
from iqdbc.car import structs, Platforms, ExtraPlatformConfig
|
||||
from iqdbc.car.docs_definitions import ExtraCarDocs, SupportType
|
||||
|
||||
|
||||
@dataclass
|
||||
class CommunityCarDocs(ExtraCarDocs):
|
||||
def init_make(self, CP: structs.CarParams):
|
||||
self.support_type = SupportType.COMMUNITY
|
||||
self.support_link = "#community"
|
||||
|
||||
|
||||
@dataclass
|
||||
class ToyotaSecurityCarDocs(ExtraCarDocs):
|
||||
def init_make(self, CP: structs.CarParams):
|
||||
self.support_type = SupportType.INCOMPATIBLE
|
||||
self.support_link = "#can-bus-security"
|
||||
|
||||
|
||||
@dataclass
|
||||
class GMSecurityCarDocs(ExtraCarDocs):
|
||||
def init_make(self, CP: structs.CarParams):
|
||||
self.support_type = SupportType.INCOMPATIBLE
|
||||
self.support_link = "#can-bus-security"
|
||||
|
||||
|
||||
@dataclass
|
||||
class FlexRayCarDocs(ExtraCarDocs):
|
||||
def init_make(self, CP: structs.CarParams):
|
||||
self.support_type = SupportType.INCOMPATIBLE
|
||||
self.support_link = "#flexray"
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
config: ExtraPlatformConfig
|
||||
|
||||
EXTRA_HONDA = ExtraPlatformConfig(
|
||||
[
|
||||
CommunityCarDocs("Acura ADX 2025-26"),
|
||||
CommunityCarDocs("Acura Integra 2023-25"),
|
||||
CommunityCarDocs("Acura MDX 2015-16", "Advance Package"),
|
||||
CommunityCarDocs("Acura MDX 2017-20"),
|
||||
CommunityCarDocs("Acura MDX Hybrid 2017-20"),
|
||||
CommunityCarDocs("Acura MDX 2022-24"),
|
||||
CommunityCarDocs("Acura RDX 2022-25"),
|
||||
CommunityCarDocs("Acura RLX 2017", "Advance Package or Technology Package"),
|
||||
CommunityCarDocs("Acura TLX 2015-17", "Advance Package"),
|
||||
CommunityCarDocs("Acura TLX 2018-20"),
|
||||
CommunityCarDocs("Acura TLX 2022-23"),
|
||||
GMSecurityCarDocs("Acura ZDX 2024"),
|
||||
CommunityCarDocs("Honda Accord 2016-17", "Honda Sensing"),
|
||||
CommunityCarDocs("Honda Accord Hybrid 2017"),
|
||||
CommunityCarDocs("Honda Clarity 2018-21"),
|
||||
GMSecurityCarDocs("Honda Prologue 2024-25"),
|
||||
],
|
||||
)
|
||||
|
||||
EXTRA_HYUNDAI = ExtraPlatformConfig(
|
||||
[
|
||||
CommunityCarDocs("Hyundai Palisade 2023-24", "Highway Driving Assist II"),
|
||||
CommunityCarDocs("Kia Telluride 2023-24", "Highway Driving Assist II"),
|
||||
],
|
||||
)
|
||||
|
||||
EXTRA_TOYOTA = ExtraPlatformConfig(
|
||||
[
|
||||
ToyotaSecurityCarDocs("Subaru Solterra 2023-25"),
|
||||
ToyotaSecurityCarDocs("Lexus NS 2022-25"),
|
||||
ToyotaSecurityCarDocs("Toyota bZ4x 2023-25"),
|
||||
ToyotaSecurityCarDocs("Toyota Camry 2025"),
|
||||
ToyotaSecurityCarDocs("Toyota Corolla Cross 2022-25"),
|
||||
ToyotaSecurityCarDocs("Toyota Highlander 2025"),
|
||||
ToyotaSecurityCarDocs("Toyota RAV4 Prime 2024-25"),
|
||||
ToyotaSecurityCarDocs("Toyota Sequoia 2023-25"),
|
||||
ToyotaSecurityCarDocs("Toyota Sienna 2024-25"),
|
||||
ToyotaSecurityCarDocs("Toyota Tundra 2022-25"),
|
||||
ToyotaSecurityCarDocs("Toyota Venza 2021-25"),
|
||||
],
|
||||
)
|
||||
|
||||
EXTRA_VOLKSWAGEN = ExtraPlatformConfig(
|
||||
[
|
||||
FlexRayCarDocs("Audi A4 2016-24"),
|
||||
FlexRayCarDocs("Audi A5 2016-24"),
|
||||
FlexRayCarDocs("Audi Q5 2017-24"),
|
||||
],
|
||||
)
|
||||
341
artifacts/package_sources/iqdbc/iqdbc/car/fingerprints.py
Normal file
341
artifacts/package_sources/iqdbc/iqdbc/car/fingerprints.py
Normal file
@@ -0,0 +1,341 @@
|
||||
from iqdbc.car.interfaces import get_interface_attr
|
||||
from iqdbc.car.body.values import CAR as BODY
|
||||
from iqdbc.car.chrysler.values import CAR as CHRYSLER
|
||||
from iqdbc.car.ford.values import CAR as FORD
|
||||
from iqdbc.car.gm.values import CAR as GM
|
||||
from iqdbc.car.honda.values import CAR as HONDA
|
||||
from iqdbc.car.hyundai.values import CAR as HYUNDAI
|
||||
from iqdbc.car.mazda.values import CAR as MAZDA
|
||||
from iqdbc.car.mock.values import CAR as MOCK
|
||||
from iqdbc.car.nissan.values import CAR as NISSAN
|
||||
from iqdbc.car.subaru.values import CAR as SUBARU
|
||||
from iqdbc.car.toyota.values import CAR as TOYOTA
|
||||
from iqdbc.car.volkswagen.values import CAR as VW
|
||||
|
||||
FW_VERSIONS = get_interface_attr('FW_VERSIONS', combine_brands=True, ignore_none=True)
|
||||
_FINGERPRINTS = get_interface_attr('FINGERPRINTS', combine_brands=True, ignore_none=True)
|
||||
|
||||
_DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes
|
||||
|
||||
|
||||
def is_valid_for_fingerprint(msg, car_fingerprint: dict[int, int]):
|
||||
adr = msg.address
|
||||
# ignore addresses that are more than 11 bits
|
||||
return (adr in car_fingerprint and car_fingerprint[adr] == len(msg.dat)) or adr >= 0x800
|
||||
|
||||
|
||||
def eliminate_incompatible_cars(msg, candidate_cars):
|
||||
"""Removes cars that could not have sent msg.
|
||||
|
||||
Inputs:
|
||||
msg: A cereal/log CanData message from the car.
|
||||
candidate_cars: A list of cars to consider.
|
||||
|
||||
Returns:
|
||||
A list containing the subset of candidate_cars that could have sent msg.
|
||||
"""
|
||||
compatible_cars = []
|
||||
|
||||
for car_name in candidate_cars:
|
||||
car_fingerprints = _FINGERPRINTS[car_name]
|
||||
|
||||
for fingerprint in car_fingerprints:
|
||||
# add alien debug address
|
||||
if is_valid_for_fingerprint(msg, fingerprint | _DEBUG_ADDRESS):
|
||||
compatible_cars.append(car_name)
|
||||
break
|
||||
|
||||
return compatible_cars
|
||||
|
||||
|
||||
def all_legacy_fingerprint_cars():
|
||||
"""Returns a list of all known car strings, FPv1 only."""
|
||||
return list(_FINGERPRINTS.keys())
|
||||
|
||||
|
||||
# A dict that maps old platform strings to their latest representations
|
||||
MIGRATION = {
|
||||
"ACURA ILX 2016 ACURAWATCH PLUS": HONDA.ACURA_ILX,
|
||||
"ACURA RDX 2018 ACURAWATCH PLUS": HONDA.ACURA_RDX,
|
||||
"ACURA RDX 2020 TECH": HONDA.ACURA_RDX_3G,
|
||||
"AUDI A3": VW.AUDI_A3_MK3,
|
||||
"HONDA ACCORD 2018 HYBRID TOURING": HONDA.HONDA_ACCORD,
|
||||
"HONDA ACCORD 1.5T 2018": HONDA.HONDA_ACCORD,
|
||||
"HONDA ACCORD 2018 LX 1.5T": HONDA.HONDA_ACCORD,
|
||||
"HONDA ACCORD 2018 SPORT 2T": HONDA.HONDA_ACCORD,
|
||||
"HONDA ACCORD 2T 2018": HONDA.HONDA_ACCORD,
|
||||
"HONDA ACCORD HYBRID 2018": HONDA.HONDA_ACCORD,
|
||||
"HONDA CIVIC 2016 TOURING": HONDA.HONDA_CIVIC,
|
||||
"HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019": HONDA.HONDA_CIVIC_BOSCH,
|
||||
"HONDA CIVIC SEDAN 1.6 DIESEL": HONDA.HONDA_CIVIC_BOSCH_DIESEL,
|
||||
"HONDA CR-V 2016 EXECUTIVE": HONDA.HONDA_CRV_EU,
|
||||
"HONDA CR-V 2016 TOURING": HONDA.HONDA_CRV,
|
||||
"HONDA CR-V 2017 EX": HONDA.HONDA_CRV_5G,
|
||||
"HONDA CR-V 2019 HYBRID": HONDA.HONDA_CRV_HYBRID,
|
||||
"HONDA FIT 2018 EX": HONDA.HONDA_FIT,
|
||||
"HONDA HRV 2019 TOURING": HONDA.HONDA_HRV,
|
||||
"HONDA INSIGHT 2019 TOURING": HONDA.HONDA_INSIGHT,
|
||||
"HONDA ODYSSEY 2018 EX-L": HONDA.HONDA_ODYSSEY,
|
||||
"HONDA PILOT 2017 TOURING": HONDA.HONDA_PILOT,
|
||||
"HONDA PILOT 2019 ELITE": HONDA.HONDA_PILOT,
|
||||
"HONDA PILOT 2019": HONDA.HONDA_PILOT,
|
||||
"HONDA PASSPORT 2021": HONDA.HONDA_PILOT,
|
||||
"HONDA RIDGELINE 2017 BLACK EDITION": HONDA.HONDA_RIDGELINE,
|
||||
"HYUNDAI ELANTRA LIMITED ULTIMATE 2017": HYUNDAI.HYUNDAI_ELANTRA,
|
||||
"HYUNDAI SANTA FE LIMITED 2019": HYUNDAI.HYUNDAI_SANTA_FE,
|
||||
"HYUNDAI TUCSON DIESEL 2019": HYUNDAI.HYUNDAI_TUCSON,
|
||||
"KIA OPTIMA 2016": HYUNDAI.KIA_OPTIMA_G4,
|
||||
"KIA OPTIMA 2019": HYUNDAI.KIA_OPTIMA_G4_FL,
|
||||
"KIA OPTIMA SX 2019 & 2016": HYUNDAI.KIA_OPTIMA_G4_FL,
|
||||
"LEXUS CT 200H 2018": TOYOTA.LEXUS_CTH,
|
||||
"LEXUS ES 300H 2018": TOYOTA.LEXUS_ES,
|
||||
"LEXUS ES 300H 2019": TOYOTA.LEXUS_ES_TSS2,
|
||||
"LEXUS IS300 2018": TOYOTA.LEXUS_IS,
|
||||
"LEXUS NX300 2018": TOYOTA.LEXUS_NX,
|
||||
"LEXUS NX300H 2018": TOYOTA.LEXUS_NX,
|
||||
"LEXUS RX 350 2016": TOYOTA.LEXUS_RX,
|
||||
"LEXUS RX350 2020": TOYOTA.LEXUS_RX_TSS2,
|
||||
"LEXUS RX450 HYBRID 2020": TOYOTA.LEXUS_RX_TSS2,
|
||||
"TOYOTA SIENNA XLE 2018": TOYOTA.TOYOTA_SIENNA,
|
||||
"TOYOTA C-HR HYBRID 2018": TOYOTA.TOYOTA_CHR,
|
||||
"TOYOTA COROLLA HYBRID TSS2 2019": TOYOTA.TOYOTA_COROLLA_TSS2,
|
||||
"TOYOTA RAV4 HYBRID 2019": TOYOTA.TOYOTA_RAV4_TSS2,
|
||||
"LEXUS ES HYBRID 2019": TOYOTA.LEXUS_ES_TSS2,
|
||||
"LEXUS NX HYBRID 2018": TOYOTA.LEXUS_NX,
|
||||
"LEXUS NX HYBRID 2020": TOYOTA.LEXUS_NX_TSS2,
|
||||
"LEXUS RX HYBRID 2020": TOYOTA.LEXUS_RX_TSS2,
|
||||
"TOYOTA ALPHARD HYBRID 2021": TOYOTA.TOYOTA_ALPHARD_TSS2,
|
||||
"TOYOTA AVALON HYBRID 2019": TOYOTA.TOYOTA_AVALON_2019,
|
||||
"TOYOTA AVALON HYBRID 2022": TOYOTA.TOYOTA_AVALON_TSS2,
|
||||
"TOYOTA CAMRY HYBRID 2018": TOYOTA.TOYOTA_CAMRY,
|
||||
"TOYOTA CAMRY HYBRID 2021": TOYOTA.TOYOTA_CAMRY_TSS2,
|
||||
"TOYOTA C-HR HYBRID 2022": TOYOTA.TOYOTA_CHR_TSS2,
|
||||
"TOYOTA HIGHLANDER HYBRID 2020": TOYOTA.TOYOTA_HIGHLANDER_TSS2,
|
||||
"TOYOTA RAV4 HYBRID 2022": TOYOTA.TOYOTA_RAV4_TSS2_2022,
|
||||
"TOYOTA RAV4 HYBRID 2023": TOYOTA.TOYOTA_RAV4_TSS2_2023,
|
||||
"TOYOTA HIGHLANDER HYBRID 2018": TOYOTA.TOYOTA_HIGHLANDER,
|
||||
"LEXUS ES HYBRID 2018": TOYOTA.LEXUS_ES,
|
||||
"LEXUS RX HYBRID 2017": TOYOTA.LEXUS_RX,
|
||||
"HYUNDAI TUCSON HYBRID 4TH GEN": HYUNDAI.HYUNDAI_TUCSON_4TH_GEN,
|
||||
"KIA SPORTAGE HYBRID 5TH GEN": HYUNDAI.KIA_SPORTAGE_5TH_GEN,
|
||||
"KIA SORENTO PLUG-IN HYBRID 4TH GEN": HYUNDAI.KIA_SORENTO_HEV_4TH_GEN,
|
||||
"CADILLAC ESCALADE ESV PLATINUM 2019": GM.CADILLAC_ESCALADE_ESV_2019,
|
||||
|
||||
# Removal of platform_str, see https://github.com/commaai/openpilot/pull/31868/
|
||||
"COMMA BODY": BODY.COMMA_BODY,
|
||||
"CHRYSLER PACIFICA HYBRID 2017": CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID,
|
||||
"CHRYSLER_PACIFICA_2017_HYBRID": CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID,
|
||||
"CHRYSLER PACIFICA HYBRID 2018": CHRYSLER.CHRYSLER_PACIFICA_2018_HYBRID,
|
||||
"CHRYSLER PACIFICA HYBRID 2019": CHRYSLER.CHRYSLER_PACIFICA_2019_HYBRID,
|
||||
"CHRYSLER PACIFICA 2018": CHRYSLER.CHRYSLER_PACIFICA_2018,
|
||||
"CHRYSLER PACIFICA 2020": CHRYSLER.CHRYSLER_PACIFICA_2020,
|
||||
"DODGE DURANGO 2021": CHRYSLER.DODGE_DURANGO,
|
||||
"JEEP GRAND CHEROKEE V6 2018": CHRYSLER.JEEP_GRAND_CHEROKEE,
|
||||
"JEEP GRAND CHEROKEE 2019": CHRYSLER.JEEP_GRAND_CHEROKEE_2019,
|
||||
"RAM 1500 5TH GEN": CHRYSLER.RAM_1500_5TH_GEN,
|
||||
"RAM HD 5TH GEN": CHRYSLER.RAM_HD_5TH_GEN,
|
||||
"FORD BRONCO SPORT 1ST GEN": FORD.FORD_BRONCO_SPORT_MK1,
|
||||
"FORD ESCAPE 4TH GEN": FORD.FORD_ESCAPE_MK4,
|
||||
"FORD EXPLORER 6TH GEN": FORD.FORD_EXPLORER_MK6,
|
||||
"FORD F-150 14TH GEN": FORD.FORD_F_150_MK14,
|
||||
"FORD F-150 LIGHTNING 1ST GEN": FORD.FORD_F_150_LIGHTNING_MK1,
|
||||
"FORD FOCUS 4TH GEN": FORD.FORD_FOCUS_MK4,
|
||||
"FORD MAVERICK 1ST GEN": FORD.FORD_MAVERICK_MK1,
|
||||
"FORD MUSTANG MACH-E 1ST GEN": FORD.FORD_MUSTANG_MACH_E_MK1,
|
||||
"HOLDEN ASTRA RS-V BK 2017": GM.HOLDEN_ASTRA,
|
||||
"CHEVROLET VOLT PREMIER 2017": GM.CHEVROLET_VOLT,
|
||||
"CADILLAC ATS Premium Performance 2018": GM.CADILLAC_ATS,
|
||||
"CHEVROLET MALIBU PREMIER 2017": GM.CHEVROLET_MALIBU,
|
||||
"GMC ACADIA DENALI 2018": GM.GMC_ACADIA,
|
||||
"BUICK LACROSSE 2017": GM.BUICK_LACROSSE,
|
||||
"BUICK REGAL ESSENCE 2018": GM.BUICK_REGAL,
|
||||
"CADILLAC ESCALADE 2017": GM.CADILLAC_ESCALADE,
|
||||
"CADILLAC ESCALADE ESV 2016": GM.CADILLAC_ESCALADE_ESV,
|
||||
"CADILLAC ESCALADE ESV 2019": GM.CADILLAC_ESCALADE_ESV_2019,
|
||||
"CHEVROLET BOLT EUV 2022": GM.CHEVROLET_BOLT_EUV,
|
||||
"CHEVROLET SILVERADO 1500 2020": GM.CHEVROLET_SILVERADO,
|
||||
"CHEVROLET EQUINOX 2019": GM.CHEVROLET_EQUINOX,
|
||||
"CHEVROLET TRAILBLAZER 2021": GM.CHEVROLET_TRAILBLAZER,
|
||||
"HONDA ACCORD 2018": HONDA.HONDA_ACCORD,
|
||||
"HONDA CIVIC (BOSCH) 2019": HONDA.HONDA_CIVIC_BOSCH,
|
||||
"HONDA CIVIC SEDAN 1.6 DIESEL 2019": HONDA.HONDA_CIVIC_BOSCH_DIESEL,
|
||||
"HONDA CIVIC 2022": HONDA.HONDA_CIVIC_2022,
|
||||
"HONDA CR-V 2017": HONDA.HONDA_CRV_5G,
|
||||
"HONDA CR-V HYBRID 2019": HONDA.HONDA_CRV_HYBRID,
|
||||
"HONDA HR-V 2023": HONDA.HONDA_HRV_3G,
|
||||
"ACURA RDX 2020": HONDA.ACURA_RDX_3G,
|
||||
"HONDA INSIGHT 2019": HONDA.HONDA_INSIGHT,
|
||||
"HONDA E 2020": HONDA.HONDA_E,
|
||||
"ACURA ILX 2016": HONDA.ACURA_ILX,
|
||||
"HONDA CR-V 2016": HONDA.HONDA_CRV,
|
||||
"HONDA CR-V EU 2016": HONDA.HONDA_CRV_EU,
|
||||
"HONDA FIT 2018": HONDA.HONDA_FIT,
|
||||
"HONDA FREED 2020": HONDA.HONDA_FREED,
|
||||
"HONDA HRV 2019": HONDA.HONDA_HRV,
|
||||
"HONDA ODYSSEY 2018": HONDA.HONDA_ODYSSEY,
|
||||
"ACURA RDX 2018": HONDA.ACURA_RDX,
|
||||
"HONDA PILOT 2017": HONDA.HONDA_PILOT,
|
||||
"HONDA RIDGELINE 2017": HONDA.HONDA_RIDGELINE,
|
||||
"HONDA CIVIC 2016": HONDA.HONDA_CIVIC,
|
||||
"HYUNDAI AZERA 6TH GEN": HYUNDAI.HYUNDAI_AZERA_6TH_GEN,
|
||||
"HYUNDAI AZERA HYBRID 6TH GEN": HYUNDAI.HYUNDAI_AZERA_HEV_6TH_GEN,
|
||||
"HYUNDAI ELANTRA 2017": HYUNDAI.HYUNDAI_ELANTRA,
|
||||
"HYUNDAI I30 N LINE 2019 & GT 2018 DCT": HYUNDAI.HYUNDAI_ELANTRA_GT_I30,
|
||||
"HYUNDAI ELANTRA 2021": HYUNDAI.HYUNDAI_ELANTRA_2021,
|
||||
"HYUNDAI ELANTRA HYBRID 2021": HYUNDAI.HYUNDAI_ELANTRA_HEV_2021,
|
||||
"HYUNDAI GENESIS 2015-2016": HYUNDAI.HYUNDAI_GENESIS,
|
||||
"HYUNDAI IONIQ HYBRID 2017-2019": HYUNDAI.HYUNDAI_IONIQ,
|
||||
"HYUNDAI IONIQ HYBRID 2020-2022": HYUNDAI.HYUNDAI_IONIQ_HEV_2022,
|
||||
"HYUNDAI IONIQ ELECTRIC LIMITED 2019": HYUNDAI.HYUNDAI_IONIQ_EV_LTD,
|
||||
"HYUNDAI IONIQ ELECTRIC 2020": HYUNDAI.HYUNDAI_IONIQ_EV_2020,
|
||||
"HYUNDAI IONIQ PLUG-IN HYBRID 2019": HYUNDAI.HYUNDAI_IONIQ_PHEV_2019,
|
||||
"HYUNDAI IONIQ PHEV 2020": HYUNDAI.HYUNDAI_IONIQ_PHEV,
|
||||
"HYUNDAI KONA 2020": HYUNDAI.HYUNDAI_KONA,
|
||||
"HYUNDAI KONA ELECTRIC 2019": HYUNDAI.HYUNDAI_KONA_EV,
|
||||
"HYUNDAI KONA ELECTRIC 2022": HYUNDAI.HYUNDAI_KONA_EV_2022,
|
||||
"HYUNDAI KONA ELECTRIC 2ND GEN": HYUNDAI.HYUNDAI_KONA_EV_2ND_GEN,
|
||||
"HYUNDAI KONA HYBRID 2020": HYUNDAI.HYUNDAI_KONA_HEV,
|
||||
"HYUNDAI SANTA FE 2019": HYUNDAI.HYUNDAI_SANTA_FE,
|
||||
"HYUNDAI SANTA FE 2022": HYUNDAI.HYUNDAI_SANTA_FE_2022,
|
||||
"HYUNDAI SANTA FE HYBRID 2022": HYUNDAI.HYUNDAI_SANTA_FE_HEV_2022,
|
||||
"HYUNDAI SANTA FE PlUG-IN HYBRID 2022": HYUNDAI.HYUNDAI_SANTA_FE_PHEV_2022,
|
||||
"HYUNDAI SONATA 2020": HYUNDAI.HYUNDAI_SONATA,
|
||||
"HYUNDAI SONATA 2019": HYUNDAI.HYUNDAI_SONATA_LF,
|
||||
"HYUNDAI STARIA 4TH GEN": HYUNDAI.HYUNDAI_STARIA_4TH_GEN,
|
||||
"HYUNDAI TUCSON 2019": HYUNDAI.HYUNDAI_TUCSON,
|
||||
"HYUNDAI PALISADE 2020": HYUNDAI.HYUNDAI_PALISADE,
|
||||
"HYUNDAI VELOSTER 2019": HYUNDAI.HYUNDAI_VELOSTER,
|
||||
"HYUNDAI SONATA HYBRID 2021": HYUNDAI.HYUNDAI_SONATA_HYBRID,
|
||||
"HYUNDAI IONIQ 5 2022": HYUNDAI.HYUNDAI_IONIQ_5,
|
||||
"HYUNDAI IONIQ 6 2023": HYUNDAI.HYUNDAI_IONIQ_6,
|
||||
"HYUNDAI TUCSON 4TH GEN": HYUNDAI.HYUNDAI_TUCSON_4TH_GEN,
|
||||
"HYUNDAI SANTA CRUZ 1ST GEN": HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN,
|
||||
"HYUNDAI CUSTIN 1ST GEN": HYUNDAI.HYUNDAI_CUSTIN_1ST_GEN,
|
||||
"KIA FORTE E 2018 & GT 2021": HYUNDAI.KIA_FORTE,
|
||||
"KIA K5 2021": HYUNDAI.KIA_K5_2021,
|
||||
"KIA K5 HYBRID 2020": HYUNDAI.KIA_K5_HEV_2020,
|
||||
"KIA K8 HYBRID 1ST GEN": HYUNDAI.KIA_K8_HEV_1ST_GEN,
|
||||
"KIA NIRO EV 2020": HYUNDAI.KIA_NIRO_EV,
|
||||
"KIA NIRO EV 2ND GEN": HYUNDAI.KIA_NIRO_EV_2ND_GEN,
|
||||
"KIA NIRO HYBRID 2019": HYUNDAI.KIA_NIRO_PHEV,
|
||||
"KIA NIRO PLUG-IN HYBRID 2022": HYUNDAI.KIA_NIRO_PHEV_2022,
|
||||
"KIA NIRO HYBRID 2021": HYUNDAI.KIA_NIRO_HEV_2021,
|
||||
"KIA NIRO HYBRID 2ND GEN": HYUNDAI.KIA_NIRO_HEV_2ND_GEN,
|
||||
"KIA OPTIMA 4TH GEN": HYUNDAI.KIA_OPTIMA_G4,
|
||||
"KIA OPTIMA 4TH GEN FACELIFT": HYUNDAI.KIA_OPTIMA_G4_FL,
|
||||
"KIA OPTIMA HYBRID 2017 & SPORTS 2019": HYUNDAI.KIA_OPTIMA_H,
|
||||
"KIA OPTIMA HYBRID 4TH GEN FACELIFT": HYUNDAI.KIA_OPTIMA_H_G4_FL,
|
||||
"KIA SELTOS 2021": HYUNDAI.KIA_SELTOS,
|
||||
"KIA SPORTAGE 5TH GEN": HYUNDAI.KIA_SPORTAGE_5TH_GEN,
|
||||
"KIA SORENTO GT LINE 2018": HYUNDAI.KIA_SORENTO,
|
||||
"KIA SORENTO 4TH GEN": HYUNDAI.KIA_SORENTO_4TH_GEN,
|
||||
"KIA SORENTO HYBRID 4TH GEN": HYUNDAI.KIA_SORENTO_HEV_4TH_GEN,
|
||||
"KIA STINGER GT2 2018": HYUNDAI.KIA_STINGER,
|
||||
"KIA STINGER 2022": HYUNDAI.KIA_STINGER_2022,
|
||||
"KIA CEED INTRO ED 2019": HYUNDAI.KIA_CEED,
|
||||
"KIA EV6 2022": HYUNDAI.KIA_EV6,
|
||||
"KIA CARNIVAL 4TH GEN": HYUNDAI.KIA_CARNIVAL_4TH_GEN,
|
||||
"GENESIS GV60 ELECTRIC 1ST GEN": HYUNDAI.GENESIS_GV60_EV_1ST_GEN,
|
||||
"GENESIS G70 2018": HYUNDAI.GENESIS_G70,
|
||||
"GENESIS G70 2020": HYUNDAI.GENESIS_G70_2020,
|
||||
"GENESIS GV70 1ST GEN": HYUNDAI.GENESIS_GV70_1ST_GEN,
|
||||
"GENESIS G80 2017": HYUNDAI.GENESIS_G80,
|
||||
"GENESIS G90 2017": HYUNDAI.GENESIS_G90,
|
||||
"GENESIS GV80 2023": HYUNDAI.GENESIS_GV80,
|
||||
"MAZDA CX-5": MAZDA.MAZDA_CX5,
|
||||
"MAZDA CX-9": MAZDA.MAZDA_CX9,
|
||||
"MAZDA 3": MAZDA.MAZDA_3,
|
||||
"MAZDA 6": MAZDA.MAZDA_6,
|
||||
"MAZDA CX-9 2021": MAZDA.MAZDA_CX9_2021,
|
||||
"MAZDA CX-5 2022": MAZDA.MAZDA_CX5_2022,
|
||||
"NISSAN X-TRAIL 2017": NISSAN.NISSAN_XTRAIL,
|
||||
"NISSAN LEAF 2018": NISSAN.NISSAN_LEAF,
|
||||
"NISSAN LEAF 2018 Instrument Cluster": NISSAN.NISSAN_LEAF_IC,
|
||||
"NISSAN ROGUE 2019": NISSAN.NISSAN_ROGUE,
|
||||
"NISSAN ALTIMA 2020": NISSAN.NISSAN_ALTIMA,
|
||||
"SUBARU ASCENT LIMITED 2019": SUBARU.SUBARU_ASCENT,
|
||||
"SUBARU OUTBACK 6TH GEN": SUBARU.SUBARU_OUTBACK,
|
||||
"SUBARU LEGACY 7TH GEN": SUBARU.SUBARU_LEGACY,
|
||||
"SUBARU IMPREZA LIMITED 2019": SUBARU.SUBARU_IMPREZA,
|
||||
"SUBARU IMPREZA SPORT 2020": SUBARU.SUBARU_IMPREZA_2020,
|
||||
"SUBARU CROSSTREK HYBRID 2020": SUBARU.SUBARU_CROSSTREK_HYBRID,
|
||||
"SUBARU FORESTER 2019": SUBARU.SUBARU_FORESTER,
|
||||
"SUBARU FORESTER HYBRID 2020": SUBARU.SUBARU_FORESTER_HYBRID,
|
||||
"SUBARU FORESTER 2017 - 2018": SUBARU.SUBARU_FORESTER_PREGLOBAL,
|
||||
"SUBARU LEGACY 2015 - 2018": SUBARU.SUBARU_LEGACY_PREGLOBAL,
|
||||
"SUBARU OUTBACK 2015 - 2017": SUBARU.SUBARU_OUTBACK_PREGLOBAL,
|
||||
"SUBARU OUTBACK 2018 - 2019": SUBARU.SUBARU_OUTBACK_PREGLOBAL_2018,
|
||||
"SUBARU FORESTER 2022": SUBARU.SUBARU_FORESTER_2022,
|
||||
"SUBARU OUTBACK 7TH GEN": SUBARU.SUBARU_OUTBACK_2023,
|
||||
"SUBARU ASCENT 2023": SUBARU.SUBARU_ASCENT_2023,
|
||||
"TOYOTA ALPHARD 2020": TOYOTA.TOYOTA_ALPHARD_TSS2,
|
||||
"TOYOTA AVALON 2016": TOYOTA.TOYOTA_AVALON,
|
||||
"TOYOTA AVALON 2019": TOYOTA.TOYOTA_AVALON_2019,
|
||||
"TOYOTA AVALON 2022": TOYOTA.TOYOTA_AVALON_TSS2,
|
||||
"TOYOTA CAMRY 2018": TOYOTA.TOYOTA_CAMRY,
|
||||
"TOYOTA CAMRY 2021": TOYOTA.TOYOTA_CAMRY_TSS2,
|
||||
"TOYOTA C-HR 2018": TOYOTA.TOYOTA_CHR,
|
||||
"TOYOTA C-HR 2021": TOYOTA.TOYOTA_CHR_TSS2,
|
||||
"TOYOTA COROLLA 2017": TOYOTA.TOYOTA_COROLLA,
|
||||
"TOYOTA COROLLA TSS2 2019": TOYOTA.TOYOTA_COROLLA_TSS2,
|
||||
"TOYOTA HIGHLANDER 2017": TOYOTA.TOYOTA_HIGHLANDER,
|
||||
"TOYOTA HIGHLANDER 2020": TOYOTA.TOYOTA_HIGHLANDER_TSS2,
|
||||
"TOYOTA PRIUS 2017": TOYOTA.TOYOTA_PRIUS,
|
||||
"TOYOTA PRIUS v 2017": TOYOTA.TOYOTA_PRIUS_V,
|
||||
"TOYOTA PRIUS TSS2 2021": TOYOTA.TOYOTA_PRIUS_TSS2,
|
||||
"TOYOTA RAV4 2017": TOYOTA.TOYOTA_RAV4,
|
||||
"TOYOTA RAV4 HYBRID 2017": TOYOTA.TOYOTA_RAV4H,
|
||||
"TOYOTA RAV4 2019": TOYOTA.TOYOTA_RAV4_TSS2,
|
||||
"TOYOTA RAV4 2022": TOYOTA.TOYOTA_RAV4_TSS2_2022,
|
||||
"TOYOTA RAV4 2023": TOYOTA.TOYOTA_RAV4_TSS2_2023,
|
||||
"TOYOTA MIRAI 2021": TOYOTA.TOYOTA_MIRAI,
|
||||
"TOYOTA SIENNA 2018": TOYOTA.TOYOTA_SIENNA,
|
||||
"LEXUS CT HYBRID 2018": TOYOTA.LEXUS_CTH,
|
||||
"LEXUS ES 2018": TOYOTA.LEXUS_ES,
|
||||
"LEXUS ES 2019": TOYOTA.LEXUS_ES_TSS2,
|
||||
"LEXUS IS 2018": TOYOTA.LEXUS_IS,
|
||||
"LEXUS IS 2023": TOYOTA.LEXUS_IS_TSS2,
|
||||
"LEXUS NX 2018": TOYOTA.LEXUS_NX,
|
||||
"LEXUS NX 2020": TOYOTA.LEXUS_NX_TSS2,
|
||||
"LEXUS LC 2024": TOYOTA.LEXUS_LC_TSS2,
|
||||
"LEXUS RC 2020": TOYOTA.LEXUS_RC,
|
||||
"LEXUS RX 2016": TOYOTA.LEXUS_RX,
|
||||
"LEXUS RX 2020": TOYOTA.LEXUS_RX_TSS2,
|
||||
"LEXUS GS F 2016": TOYOTA.LEXUS_GS_F,
|
||||
"VOLKSWAGEN ARTEON 1ST GEN": VW.VOLKSWAGEN_ARTEON_MK1,
|
||||
"VOLKSWAGEN ATLAS 1ST GEN": VW.VOLKSWAGEN_ATLAS_MK1,
|
||||
"VOLKSWAGEN CADDY 3RD GEN": VW.VOLKSWAGEN_CADDY_MK3,
|
||||
"VOLKSWAGEN CRAFTER 2ND GEN": VW.VOLKSWAGEN_CRAFTER_MK2,
|
||||
"VOLKSWAGEN GOLF 7TH GEN": VW.VOLKSWAGEN_GOLF_MK7,
|
||||
"VOLKSWAGEN JETTA 6TH GEN": VW.VOLKSWAGEN_JETTA_MK6,
|
||||
"VOLKSWAGEN JETTA 7TH GEN": VW.VOLKSWAGEN_JETTA_MK7,
|
||||
"VOLKSWAGEN PASSAT 7TH GEN": VW.VOLKSWAGEN_PASSAT_MK7,
|
||||
"VOLKSWAGEN PASSAT 8TH GEN": VW.VOLKSWAGEN_PASSAT_MK8,
|
||||
"VOLKSWAGEN PASSAT NMS": VW.VOLKSWAGEN_PASSAT_NMS,
|
||||
"VOLKSWAGEN POLO 6TH GEN": VW.VOLKSWAGEN_POLO_MK6,
|
||||
"VOLKSWAGEN SHARAN 2ND GEN": VW.VOLKSWAGEN_SHARAN_MK2,
|
||||
"VOLKSWAGEN TAOS 1ST GEN": VW.VOLKSWAGEN_TAOS_MK1,
|
||||
"VOLKSWAGEN T-CROSS 1ST GEN": VW.VOLKSWAGEN_TCROSS_MK1,
|
||||
"VOLKSWAGEN TIGUAN 2ND GEN": VW.VOLKSWAGEN_TIGUAN_MK2,
|
||||
"VOLKSWAGEN TOURAN 2ND GEN": VW.VOLKSWAGEN_TOURAN_MK2,
|
||||
"VOLKSWAGEN TRANSPORTER T6.1": VW.VOLKSWAGEN_TRANSPORTER_T61,
|
||||
"VOLKSWAGEN T-ROC 1ST GEN": VW.VOLKSWAGEN_TROC_MK1,
|
||||
"AUDI A3 3RD GEN": VW.AUDI_A3_MK3,
|
||||
"AUDI Q2 1ST GEN": VW.AUDI_Q2_MK1,
|
||||
"AUDI Q3 2ND GEN": VW.AUDI_Q3_MK2,
|
||||
"AUDI Q5 1st GEN": VW.AUDI_Q5_MK1,
|
||||
"Porsche Macan 1st GEN": VW.PORSCHE_MACAN_MK1,
|
||||
"SEAT ATECA 1ST GEN": VW.SEAT_ATECA_MK1,
|
||||
"SEAT LEON 3RD GEN": VW.SEAT_ATECA_MK1,
|
||||
"SEAT_LEON_MK3": VW.SEAT_ATECA_MK1,
|
||||
"SKODA FABIA 4TH GEN": VW.SKODA_FABIA_MK4,
|
||||
"SKODA KAMIQ 1ST GEN": VW.SKODA_KAMIQ_MK1,
|
||||
"SKODA KAROQ 1ST GEN": VW.SKODA_KAROQ_MK1,
|
||||
"SKODA KODIAQ 1ST GEN": VW.SKODA_KODIAQ_MK1,
|
||||
"SKODA OCTAVIA 3RD GEN": VW.SKODA_OCTAVIA_MK3,
|
||||
"SKODA SCALA 1ST GEN": VW.SKODA_KAMIQ_MK1,
|
||||
"SKODA_SCALA_MK1": VW.SKODA_KAMIQ_MK1,
|
||||
"SKODA SUPERB 3RD GEN": VW.SKODA_SUPERB_MK3,
|
||||
|
||||
"mock": MOCK.MOCK,
|
||||
}
|
||||
203
artifacts/package_sources/iqdbc/iqdbc/car/ford/carcontroller.py
Normal file
203
artifacts/package_sources/iqdbc/iqdbc/car/ford/carcontroller.py
Normal file
@@ -0,0 +1,203 @@
|
||||
import math
|
||||
import numpy as np
|
||||
from iqdbc.can import CANPacker
|
||||
from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, apply_hysteresis, structs
|
||||
from iqdbc.car.lateral import ISO_LATERAL_ACCEL, apply_std_steer_angle_limits
|
||||
from iqdbc.car.ford import fordcan
|
||||
from iqdbc.car.ford.values import CarControllerParams, FordFlags, CAR
|
||||
from iqdbc.car.interfaces import CarControllerBase, V_CRUISE_MAX
|
||||
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||
|
||||
# CAN FD limits:
|
||||
# Limit to average banked road since safety doesn't have the roll
|
||||
AVERAGE_ROAD_ROLL = 0.06 # ~3.4 degrees, 6% superelevation. higher actual roll raises lateral acceleration
|
||||
MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL) # ~2.4 m/s^2
|
||||
|
||||
|
||||
def anti_overshoot(apply_curvature, apply_curvature_last, v_ego):
|
||||
diff = 0.1
|
||||
tau = 5 # 5s smooths over the overshoot
|
||||
dt = DT_CTRL * CarControllerParams.STEER_STEP
|
||||
alpha = 1 - np.exp(-dt / tau)
|
||||
|
||||
lataccel = apply_curvature * (v_ego ** 2)
|
||||
last_lataccel = apply_curvature_last * (v_ego ** 2)
|
||||
last_lataccel = apply_hysteresis(lataccel, last_lataccel, diff)
|
||||
last_lataccel = alpha * lataccel + (1 - alpha) * last_lataccel
|
||||
|
||||
output_curvature = last_lataccel / (max(v_ego, 1) ** 2)
|
||||
|
||||
return float(np.interp(v_ego, [5, 10], [apply_curvature, output_curvature]))
|
||||
|
||||
|
||||
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
|
||||
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:
|
||||
creep_accel = np.interp(v_ego, [1., 3.], [0.6, 0.])
|
||||
creep_accel = np.interp(accel, [0., 0.2], [creep_accel, 0.])
|
||||
accel -= creep_accel
|
||||
return float(accel)
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_names, CP, CP_IQ):
|
||||
super().__init__(dbc_names, CP, CP_IQ)
|
||||
self.packer = CANPacker(dbc_names[Bus.pt])
|
||||
self.CAN = fordcan.CanBus(CP)
|
||||
|
||||
self.apply_curvature_last = 0
|
||||
self.anti_overshoot_curvature_last = 0
|
||||
self.accel = 0.0
|
||||
self.gas = 0.0
|
||||
self.brake_request = False
|
||||
self.main_on_last = False
|
||||
self.lkas_enabled_last = False
|
||||
self.steer_alert_last = False
|
||||
self.lead_distance_bars_last = None
|
||||
self.distance_bar_frame = 0
|
||||
|
||||
def update(self, CC, CC_IQ, CS, now_nanos):
|
||||
can_sends = []
|
||||
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
|
||||
main_on = CS.out.cruiseState.available
|
||||
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw)
|
||||
fcw_alert = hud_control.visualAlert == VisualAlert.fcw
|
||||
|
||||
### acc buttons ###
|
||||
if CC.cruiseControl.cancel:
|
||||
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True))
|
||||
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, cancel=True))
|
||||
elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0:
|
||||
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, resume=True))
|
||||
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, resume=True))
|
||||
# if stock lane centering isn't off, send a button press to toggle it off
|
||||
# the stock system checks for steering pressed, and eventually disengages cruise control
|
||||
elif CS.acc_tja_status_stock_values["Tja_D_Stat"] != 0 and (self.frame % CarControllerParams.ACC_UI_STEP) == 0:
|
||||
can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, tja_toggle=True))
|
||||
|
||||
### lateral control ###
|
||||
# send steer msg at 20Hz
|
||||
if (self.frame % CarControllerParams.STEER_STEP) == 0:
|
||||
# Bronco and some other cars consistently overshoot curv requests
|
||||
# Apply some deadzone + smoothing convergence to avoid oscillations
|
||||
if self.CP.carFingerprint in (CAR.FORD_BRONCO_SPORT_MK1, CAR.FORD_F_150_MK14):
|
||||
self.anti_overshoot_curvature_last = anti_overshoot(actuators.curvature, self.anti_overshoot_curvature_last, CS.out.vEgoRaw)
|
||||
apply_curvature = self.anti_overshoot_curvature_last
|
||||
else:
|
||||
apply_curvature = actuators.curvature
|
||||
|
||||
# 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(apply_curvature, self.apply_curvature_last, current_curvature,
|
||||
CS.out.vEgoRaw, 0., CC.latActive, self.CP)
|
||||
|
||||
if self.CP.flags & FordFlags.CANFD:
|
||||
# TODO: extended mode
|
||||
# Ford uses four individual signals to dictate how to drive to the car. Curvature alone (limited to 0.02m/s^2)
|
||||
# can actuate the steering for a large portion of any lateral movements. However, in order to get further control on
|
||||
# steer actuation, the other three signals are necessary. Ford controls vehicles differently than most other makes.
|
||||
# A detailed explanation on ford control can be found here:
|
||||
# https://www.f150gen14.com/forum/threads/introducing-bluepilot-a-ford-specific-fork-for-comma3x-openpilot.24241/#post-457706
|
||||
mode = 1 if CC.latActive else 0
|
||||
counter = (self.frame // CarControllerParams.STEER_STEP) % 0x10
|
||||
can_sends.append(fordcan.create_lat_ctl2_msg(self.packer, self.CAN, mode, 0., 0., -self.apply_curvature_last, 0., counter))
|
||||
else:
|
||||
can_sends.append(fordcan.create_lat_ctl_msg(self.packer, self.CAN, CC.latActive, 0., 0., -self.apply_curvature_last, 0.))
|
||||
|
||||
# send lka msg at 33Hz
|
||||
if (self.frame % CarControllerParams.LKA_STEP) == 0:
|
||||
can_sends.append(fordcan.create_lka_msg(self.packer, self.CAN))
|
||||
|
||||
### longitudinal control ###
|
||||
# send acc msg at 50Hz
|
||||
if self.CP.openpilotLongitudinalControl and (self.frame % CarControllerParams.ACC_CONTROL_STEP) == 0:
|
||||
accel = actuators.accel
|
||||
gas = accel
|
||||
|
||||
if CC.longActive:
|
||||
# Compensate for engine creep at low speed.
|
||||
# Either the ABS does not account for engine creep, or the correction is very slow
|
||||
# TODO: verify this applies to EV/hybrid
|
||||
accel = apply_creep_compensation(accel, CS.out.vEgo)
|
||||
|
||||
# The stock system has been seen rate limiting the brake accel to 5 m/s^3,
|
||||
# however even 3.5 m/s^3 causes some overshoot with a step response.
|
||||
accel = max(accel, self.accel - (3.5 * CarControllerParams.ACC_CONTROL_STEP * DT_CTRL))
|
||||
|
||||
accel = float(np.clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
gas = float(np.clip(gas, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
|
||||
|
||||
# Both gas and accel are in m/s^2, accel is used solely for braking
|
||||
if not CC.longActive or gas < CarControllerParams.MIN_GAS:
|
||||
gas = CarControllerParams.INACTIVE_GAS
|
||||
|
||||
# PCM applies pitch compensation to gas/accel, but we need to compensate for the brake/pre-charge bits
|
||||
accel_due_to_pitch = 0.0
|
||||
if len(CC.orientationNED) == 3:
|
||||
accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY
|
||||
|
||||
accel_pitch_compensated = accel + accel_due_to_pitch
|
||||
if accel_pitch_compensated > 0.3 or not CC.longActive:
|
||||
self.brake_request = False
|
||||
elif accel_pitch_compensated < 0.0:
|
||||
self.brake_request = True
|
||||
|
||||
stopping = CC.actuators.longControlState == LongCtrlState.stopping
|
||||
# TODO: look into using the actuators packet to send the desired speed
|
||||
can_sends.append(fordcan.create_acc_msg(self.packer, self.CAN, CC.longActive, gas, accel, stopping, self.brake_request, v_ego_kph=V_CRUISE_MAX))
|
||||
|
||||
self.accel = accel
|
||||
self.gas = gas
|
||||
|
||||
### ui ###
|
||||
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert)
|
||||
# send lkas ui msg at 1Hz or if ui state changes
|
||||
if (self.frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui:
|
||||
can_sends.append(fordcan.create_lkas_ui_msg(self.packer, self.CAN, main_on, CC.latActive, steer_alert, hud_control, CS.lkas_status_stock_values))
|
||||
|
||||
# send acc ui msg at 5Hz or if ui state changes
|
||||
if hud_control.leadDistanceBars != self.lead_distance_bars_last:
|
||||
send_ui = True
|
||||
self.distance_bar_frame = self.frame
|
||||
|
||||
if (self.frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui:
|
||||
show_distance_bars = self.frame - self.distance_bar_frame < 400
|
||||
can_sends.append(fordcan.create_acc_ui_msg(self.packer, self.CAN, self.CP, main_on, CC.latActive,
|
||||
fcw_alert, CS.out.cruiseState.standstill, show_distance_bars,
|
||||
hud_control, CS.acc_tja_status_stock_values))
|
||||
|
||||
self.main_on_last = main_on
|
||||
self.lkas_enabled_last = CC.latActive
|
||||
self.steer_alert_last = steer_alert
|
||||
self.lead_distance_bars_last = hud_control.leadDistanceBars
|
||||
|
||||
new_actuators = actuators.as_builder()
|
||||
new_actuators.curvature = self.apply_curvature_last
|
||||
new_actuators.accel = self.accel
|
||||
new_actuators.gas = self.gas
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
123
artifacts/package_sources/iqdbc/iqdbc/car/ford/carstate.py
Normal file
123
artifacts/package_sources/iqdbc/iqdbc/car/ford/carstate.py
Normal file
@@ -0,0 +1,123 @@
|
||||
from iqdbc.can import CANDefine, CANParser
|
||||
from iqdbc.car import Bus, create_button_events, structs
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.ford.fordcan import CanBus
|
||||
from iqdbc.car.ford.values import DBC, CarControllerParams, FordFlags
|
||||
from iqdbc.car.interfaces import CarStateBase
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
GearShifter = structs.CarState.GearShifter
|
||||
TransmissionType = structs.CarParams.TransmissionType
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP, CP_IQ):
|
||||
CarStateBase.__init__(self, CP, CP_IQ)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
|
||||
if CP.transmissionType == TransmissionType.automatic:
|
||||
self.shifter_values = can_define.dv["PowertrainData_10"]["TrnRng_D_Rq"]
|
||||
|
||||
self.distance_button = 0
|
||||
self.lc_button = 0
|
||||
|
||||
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
|
||||
ret = structs.CarState()
|
||||
ret_iq = structs.IQCarState()
|
||||
|
||||
# Occasionally on startup, the ABS module recalibrates the steering pinion offset, so we need to block engagement
|
||||
# The vehicle usually recovers out of this state within a minute of normal driving
|
||||
ret.vehicleSensorsInvalid = cp.vl["SteeringPinion_Data"]["StePinCompAnEst_D_Qf"] != 3
|
||||
|
||||
# car speed
|
||||
ret.vEgoRaw = cp.vl["BrakeSysFeatures"]["Veh_V_ActlBrk"] * CV.KPH_TO_MS
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"]
|
||||
ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1
|
||||
|
||||
# gas pedal
|
||||
ret.gasPressed = cp.vl["EngVehicleSpThrottle"]["ApedPos_Pc_ActlArb"] / 100. > 1e-6
|
||||
|
||||
# brake pedal
|
||||
ret.brake = cp.vl["BrakeSnData_4"]["BrkTot_Tq_Actl"] / 32756. # torque in Nm
|
||||
ret.brakePressed = cp.vl["EngBrakeData"]["BpedDrvAppl_D_Actl"] == 2
|
||||
ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2)
|
||||
|
||||
# steering wheel
|
||||
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"]
|
||||
ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"]
|
||||
ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE, 5)
|
||||
ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1
|
||||
ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3)
|
||||
ret.espDisabled = cp.vl["Cluster_Info1_FD1"]["DrvSlipCtlMde_D_Rq"] != 0 # 0 is default mode
|
||||
|
||||
if self.CP.flags & FordFlags.CANFD:
|
||||
# this signal is always 0 on non-CAN FD cars
|
||||
ret.steerFaultTemporary |= cp.vl["Lane_Assist_Data3_FD1"]["LatCtlSte_D_Stat"] not in (1, 2, 3)
|
||||
|
||||
# cruise state
|
||||
is_metric = cp.vl["INSTRUMENT_PANEL"]["METRIC_UNITS"] == 1 if not self.CP.flags & FordFlags.CANFD else False
|
||||
ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * (CV.KPH_TO_MS if is_metric else CV.MPH_TO_MS)
|
||||
ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5)
|
||||
ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5)
|
||||
ret.cruiseState.nonAdaptive = cp.vl["Cluster_Info1_FD1"]["AccEnbl_B_RqDrv"] == 0
|
||||
ret.cruiseState.standstill = cp.vl["EngBrakeData"]["AccStopMde_D_Rq"] == 3
|
||||
ret.accFaulted = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (1, 2)
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
ret.accFaulted = ret.accFaulted or cp_cam.vl["ACCDATA"]["CmbbDeny_B_Actl"] == 1
|
||||
|
||||
# gear
|
||||
if self.CP.transmissionType == TransmissionType.automatic:
|
||||
gear = self.shifter_values.get(cp.vl["PowertrainData_10"]["TrnRng_D_Rq"])
|
||||
ret.gearShifter = self.parse_gear_shifter(gear)
|
||||
elif self.CP.transmissionType == TransmissionType.manual:
|
||||
if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]):
|
||||
ret.gearShifter = GearShifter.reverse
|
||||
else:
|
||||
ret.gearShifter = GearShifter.drive
|
||||
|
||||
# safety
|
||||
ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"])
|
||||
ret.stockAeb = bool(cp_cam.vl["ACCDATA_2"]["CmbbBrkDecel_B_Rq"])
|
||||
|
||||
# button presses
|
||||
ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1
|
||||
ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2
|
||||
# TODO: block this going to the camera otherwise it will enable stock TJA
|
||||
ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
|
||||
prev_distance_button = self.distance_button
|
||||
prev_lc_button = self.lc_button
|
||||
self.distance_button = cp.vl["Steering_Data_FD1"]["AccButtnGapTogglePress"]
|
||||
self.lc_button = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"])
|
||||
|
||||
# lock info
|
||||
ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"],
|
||||
cp.vl["BodyInfo_3_FD1"]["DrStatRl_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatRr_B_Actl"]])
|
||||
ret.seatbeltUnlatched = cp.vl["RCMStatusMessage2_FD1"]["FirstRowBuckleDriver"] == 2
|
||||
|
||||
# blindspot sensors
|
||||
if self.CP.enableBsm:
|
||||
cp_bsm = cp_cam if self.CP.flags & FordFlags.CANFD else cp
|
||||
ret.leftBlindspot = cp_bsm.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0
|
||||
ret.rightBlindspot = cp_bsm.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0
|
||||
|
||||
# Stock steering buttons so that we can passthru blinkers etc.
|
||||
self.buttons_stock_values = cp.vl["Steering_Data_FD1"]
|
||||
# Stock values from IPMA so that we can retain some stock functionality
|
||||
self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"]
|
||||
self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"]
|
||||
|
||||
ret.buttonEvents = [
|
||||
*create_button_events(self.distance_button, prev_distance_button, {1: ButtonType.gapAdjustCruise}),
|
||||
*create_button_events(self.lc_button, prev_lc_button, {1: ButtonType.lkas}),
|
||||
]
|
||||
return ret, ret_iq
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers(CP, CP_IQ):
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).main),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).camera),
|
||||
}
|
||||
223
artifacts/package_sources/iqdbc/iqdbc/car/ford/fingerprints.py
Normal file
223
artifacts/package_sources/iqdbc/iqdbc/car/ford/fingerprints.py
Normal file
@@ -0,0 +1,223 @@
|
||||
""" AUTO-FORMATTED USING iqdbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.car.ford.values import CAR
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
FW_VERSIONS = {
|
||||
CAR.FORD_BRONCO_SPORT_MK1: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'LX6C-2D053-RD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-2D053-RE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-2D053-RF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'M1PT-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'M1PT-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_ESCAPE_MK4: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'LX6C-14D003-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'LX6C-2D053-NS\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-2D053-NT\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-2D053-NY\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-2D053-SA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LX6C-2D053-SD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'LJ6T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LJ6T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LV4T-14F397-GG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_ESCAPE_MK4_5: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'PZ11-14D003-EA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'PZ1C-2D053-EJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'PJ6T-14H102-ABL\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_EXPLORER_MK6: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'M1MC-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'M1MC-14D003-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'P1MC-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'L1MC-2D053-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-BF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-BJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'L1MC-2D053-KB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'LB5T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LB5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LB5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LC5T-14F397-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LC5T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_EXPEDITION_MK4: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'NL14-14D003-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'RL14-2D053-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'ML3T-14H102-ABT\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_F_150_MK14: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'ML3V-14D003-BC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'ML3V-14D003-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'NL34-2D053-CA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PL34-2D053-CA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PL34-2D053-CC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PL3V-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PL3V-2D053-BB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'ML3T-14D049-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'ML3T-14H102-ABR\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'ML3T-14H102-ABS\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'ML3T-14H102-ABT\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PJ6T-14H102-ABS\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RJ6T-14H102-ACJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RJ6T-14H102-BBC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_F_150_LIGHTNING_MK1: {
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'PL38-2D053-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RL38-2D053-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'ML3T-14H102-ABT\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RJ6T-14H102-ACJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RJ6T-14H102-BBC\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
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',
|
||||
],
|
||||
},
|
||||
CAR.FORD_MUSTANG_MACH_E_MK1: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'LJ9C-14D003-AM\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LJ9C-14D003-CC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LJ9C-14D003-FA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LJ9C-14D003-GA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LJ9C-14D003-HA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'LK9C-2D053-CK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'LK9C-2D053-CN\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'ML3T-14H102-ABS\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RJ6T-14H102-BAE\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_FOCUS_MK4: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'JX61-2D053-CJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'JX7T-14D049-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'JX7T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_MAVERICK_MK1: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'NZ6C-14D003-AK\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'NZ6C-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'NZ6C-2D053-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'NZ6C-2D053-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'NZ6C-2D053-AG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PZ6C-2D053-ED\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PZ6C-2D053-EE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PZ6C-2D053-EF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'NZ6T-14D049-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'NZ6T-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.FORD_RANGER_MK2: {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b'NB3C-14D003-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'NL14-14D003-AE\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RB3C-14D003-AA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b'PB3C-2D053-ZD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PB3C-2D053-ZG\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'PB3C-2D053-ZJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b'ML3T-14D049-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b'PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
b'RJ6T-14H102-BBB\x00\x00\x00\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
}
|
||||
342
artifacts/package_sources/iqdbc/iqdbc/car/ford/fordcan.py
Normal file
342
artifacts/package_sources/iqdbc/iqdbc/car/ford/fordcan.py
Normal file
@@ -0,0 +1,342 @@
|
||||
from iqdbc.car import CanBusBase, structs
|
||||
|
||||
HUDControl = structs.CarControl.HUDControl
|
||||
|
||||
|
||||
class CanBus(CanBusBase):
|
||||
def __init__(self, CP=None, fingerprint=None) -> None:
|
||||
super().__init__(CP, fingerprint)
|
||||
|
||||
@property
|
||||
def main(self) -> int:
|
||||
return self.offset
|
||||
|
||||
@property
|
||||
def radar(self) -> int:
|
||||
return self.offset + 1
|
||||
|
||||
@property
|
||||
def camera(self) -> int:
|
||||
return self.offset + 2
|
||||
|
||||
|
||||
def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray) -> int:
|
||||
curvature = (dat[2] << 3) | ((dat[3]) >> 5)
|
||||
curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5)
|
||||
path_angle = ((dat[3] & 0x1F) << 6) | ((dat[4]) >> 2)
|
||||
path_offset = ((dat[4] & 0x3) << 8) | dat[5]
|
||||
|
||||
checksum = mode + counter
|
||||
for sig_val in (curvature, curvature_rate, path_angle, path_offset):
|
||||
checksum += sig_val + (sig_val >> 8)
|
||||
|
||||
return 0xFF - (checksum & 0xFF)
|
||||
|
||||
|
||||
def create_lka_msg(packer, CAN: CanBus):
|
||||
"""
|
||||
Creates an empty CAN message for the Ford LKA Command.
|
||||
|
||||
This command can apply "Lane Keeping Aid" maneuvers, which are subject to the PSCM lockout.
|
||||
|
||||
Frequency is 33Hz.
|
||||
"""
|
||||
|
||||
return packer.make_can_msg("Lane_Assist_Data1", CAN.main, {})
|
||||
|
||||
|
||||
def create_lat_ctl_msg(packer, CAN: CanBus, lat_active: bool, path_offset: float, path_angle: float, curvature: float,
|
||||
curvature_rate: float):
|
||||
"""
|
||||
Creates a CAN message for the Ford TJA/LCA Command.
|
||||
|
||||
This command can apply "Lane Centering" maneuvers: continuous lane centering for traffic jam assist and highway
|
||||
driving. It is not subject to the PSCM lockout.
|
||||
|
||||
Ford lane centering command uses a third order polynomial to describe the road centerline. The polynomial is defined
|
||||
by the following coefficients:
|
||||
c0: lateral offset between the vehicle and the centerline (positive is right)
|
||||
c1: heading angle between the vehicle and the centerline (positive is right)
|
||||
c2: curvature of the centerline (positive is left)
|
||||
c3: rate of change of curvature of the centerline
|
||||
As the PSCM combines this information with other sensor data, such as the vehicle's yaw rate and speed, the steering
|
||||
angle cannot be easily controlled.
|
||||
|
||||
The PSCM should be configured to accept TJA/LCA commands before these commands will be processed. This can be done
|
||||
using tools such as Forscan.
|
||||
|
||||
Frequency is 20Hz.
|
||||
"""
|
||||
|
||||
values = {
|
||||
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter
|
||||
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1]
|
||||
"LatCtl_D_Rq": 1 if lat_active else 0, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft,
|
||||
# 3=InterventionRight, 4-7=NotUsed [0|7]
|
||||
"LatCtlRampType_D_Rq": 0, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
|
||||
# Makes no difference with curvature control
|
||||
"LatCtlPrecision_D_Rq": 1, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
|
||||
# The stock system always uses comfortable
|
||||
"LatCtlPathOffst_L_Actl": path_offset, # Path offset [-5.12|5.11] meter
|
||||
"LatCtlPath_An_Actl": path_angle, # Path angle [-0.5|0.5235] radians
|
||||
"LatCtlCurv_NoRate_Actl": curvature_rate, # Curvature rate [-0.001024|0.00102375] 1/meter^2
|
||||
"LatCtlCurv_No_Actl": curvature, # Curvature [-0.02|0.02094] 1/meter
|
||||
}
|
||||
return packer.make_can_msg("LateralMotionControl", CAN.main, values)
|
||||
|
||||
|
||||
def create_lat_ctl2_msg(packer, CAN: CanBus, mode: int, path_offset: float, path_angle: float, curvature: float,
|
||||
curvature_rate: float, counter: int):
|
||||
"""
|
||||
Create a CAN message for the new Ford Lane Centering command.
|
||||
|
||||
This message is used on the CAN FD platform and replaces the old LateralMotionControl message. It is similar but has
|
||||
additional signals for a counter and checksum.
|
||||
|
||||
Frequency is 20Hz.
|
||||
"""
|
||||
|
||||
values = {
|
||||
"LatCtl_D2_Rq": mode, # Mode: 0=None, 1=PathFollowingLimitedMode, 2=PathFollowingExtendedMode,
|
||||
# 3=SafeRampOut, 4-7=NotUsed [0|7]
|
||||
"LatCtlRampType_D_Rq": 0, # 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3]
|
||||
"LatCtlPrecision_D_Rq": 1, # 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3]
|
||||
"LatCtlPathOffst_L_Actl": path_offset, # [-5.12|5.11] meter
|
||||
"LatCtlPath_An_Actl": path_angle, # [-0.5|0.5235] radians
|
||||
"LatCtlCurv_No_Actl": curvature, # [-0.02|0.02094] 1/meter
|
||||
"LatCtlCrv_NoRate2_Actl": curvature_rate, # [-0.001024|0.001023] 1/meter^2
|
||||
"HandsOffCnfm_B_Rq": 0, # 0=Inactive, 1=Active [0|1]
|
||||
"LatCtlPath_No_Cnt": counter, # [0|15]
|
||||
"LatCtlPath_No_Cs": 0, # [0|255]
|
||||
}
|
||||
|
||||
# calculate checksum
|
||||
dat = packer.make_can_msg("LateralMotionControl2", 0, values)[1]
|
||||
values["LatCtlPath_No_Cs"] = calculate_lat_ctl2_checksum(mode, counter, dat)
|
||||
|
||||
return packer.make_can_msg("LateralMotionControl2", CAN.main, values)
|
||||
|
||||
|
||||
def create_acc_msg(packer, CAN: CanBus, long_active: bool, gas: float, accel: float, stopping: bool, brake_request, v_ego_kph: float):
|
||||
"""
|
||||
Creates a CAN message for the Ford ACC Command.
|
||||
|
||||
This command can be used to enable ACC, to set the ACC gas/brake/decel values
|
||||
and to disable ACC.
|
||||
|
||||
Frequency is 50Hz.
|
||||
"""
|
||||
values = {
|
||||
"AccBrkTot_A_Rq": accel, # Brake total accel request: [-20|11.9449] m/s^2
|
||||
"Cmbb_B_Enbl": 1 if long_active else 0, # Enabled: 0=No, 1=Yes
|
||||
"AccPrpl_A_Rq": gas, # Acceleration request: [-5|5.23] m/s^2
|
||||
# No observed acceleration seen from this signal alone. During stock system operation, it appears to
|
||||
# be the raw acceleration request (AccPrpl_A_Rq when positive, AccBrkTot_A_Rq when negative)
|
||||
"AccPrpl_A_Pred": -5.0, # Acceleration request: [-5|5.23] m/s^2
|
||||
"AccResumEnbl_B_Rq": 1 if long_active else 0,
|
||||
# No observed acceleration seen from this signal alone
|
||||
"AccVeh_V_Trg": v_ego_kph, # Target speed: [0|255] km/h
|
||||
# TODO: we may be able to improve braking response by utilizing pre-charging better
|
||||
# When setting these two bits without AccBrkTot_A_Rq, an initial jerk is observed and car may be able to brake temporarily with AccPrpl_A_Rq
|
||||
"AccBrkPrchg_B_Rq": 1 if brake_request else 0, # Pre-charge brake request: 0=No, 1=Yes
|
||||
"AccBrkDecel_B_Rq": 1 if brake_request else 0, # Deceleration request: 0=Inactive, 1=Active
|
||||
"AccStopStat_B_Rq": 1 if stopping else 0,
|
||||
}
|
||||
return packer.make_can_msg("ACCDATA", CAN.main, values)
|
||||
|
||||
|
||||
def create_acc_ui_msg(packer, CAN: CanBus, CP, main_on: bool, enabled: bool, fcw_alert: bool, standstill: bool,
|
||||
show_distance_bars: bool, hud_control, stock_values: dict):
|
||||
"""
|
||||
Creates a CAN message for the Ford IPC adaptive cruise, forward collision warning and traffic jam
|
||||
assist status.
|
||||
|
||||
Stock functionality is maintained by passing through unmodified signals.
|
||||
|
||||
Frequency is 5Hz.
|
||||
"""
|
||||
|
||||
# Tja_D_Stat
|
||||
if enabled:
|
||||
if hud_control.leftLaneDepart:
|
||||
status = 3 # ActiveInterventionLeft
|
||||
elif hud_control.rightLaneDepart:
|
||||
status = 4 # ActiveInterventionRight
|
||||
else:
|
||||
status = 2 # Active
|
||||
elif main_on:
|
||||
if hud_control.leftLaneDepart:
|
||||
status = 5 # ActiveWarningLeft
|
||||
elif hud_control.rightLaneDepart:
|
||||
status = 6 # ActiveWarningRight
|
||||
else:
|
||||
status = 1 # Standby
|
||||
else:
|
||||
status = 0 # Off
|
||||
|
||||
values = {s: stock_values[s] for s in [
|
||||
"HaDsply_No_Cs",
|
||||
"HaDsply_No_Cnt",
|
||||
"AccStopStat_D_Dsply", # ACC stopped status message
|
||||
"AccTrgDist2_D_Dsply", # ACC target distance
|
||||
"AccStopRes_B_Dsply",
|
||||
"TjaWarn_D_Rq", # TJA warning
|
||||
"TjaMsgTxt_D_Dsply", # TJA text
|
||||
"IaccLamp_D_Rq", # iACC status icon
|
||||
"AccMsgTxt_D2_Rq", # ACC text
|
||||
"FcwDeny_B_Dsply", # FCW disabled
|
||||
"FcwMemStat_B_Actl", # FCW enabled setting
|
||||
"AccTGap_B_Dsply", # ACC time gap display setting
|
||||
"CadsAlignIncplt_B_Actl",
|
||||
"AccFllwMde_B_Dsply", # ACC follow mode display setting
|
||||
"CadsRadrBlck_B_Actl",
|
||||
"CmbbPostEvnt_B_Dsply", # AEB event status
|
||||
"AccStopMde_B_Dsply", # ACC stop mode display setting
|
||||
"FcwMemSens_D_Actl", # FCW sensitivity setting
|
||||
"FcwMsgTxt_D_Rq", # FCW text
|
||||
"AccWarn_D_Dsply", # ACC warning
|
||||
"FcwVisblWarn_B_Rq", # FCW visible alert
|
||||
"FcwAudioWarn_B_Rq", # FCW audio alert
|
||||
"AccTGap_D_Dsply", # ACC time gap
|
||||
"AccMemEnbl_B_RqDrv", # ACC adaptive/normal setting
|
||||
"FdaMem_B_Stat", # FDA enabled setting
|
||||
]}
|
||||
|
||||
values.update({
|
||||
"Tja_D_Stat": status, # TJA status
|
||||
})
|
||||
|
||||
if CP.openpilotLongitudinalControl:
|
||||
values.update({
|
||||
"AccStopStat_D_Dsply": 2 if standstill else 0, # Stopping status text
|
||||
"AccMsgTxt_D2_Rq": 0, # ACC text
|
||||
"AccTGap_B_Dsply": 1 if show_distance_bars else 0, # Show time gap control UI
|
||||
"AccFllwMde_B_Dsply": 1 if hud_control.leadVisible else 0, # Lead indicator
|
||||
"AccStopMde_B_Dsply": 1 if standstill else 0,
|
||||
"AccWarn_D_Dsply": 0, # ACC warning
|
||||
"AccTGap_D_Dsply": hud_control.leadDistanceBars, # Time gap
|
||||
})
|
||||
|
||||
# Forwards FCW alert from IPMA
|
||||
if fcw_alert:
|
||||
values["FcwVisblWarn_B_Rq"] = 1 # FCW visible alert
|
||||
|
||||
return packer.make_can_msg("ACCDATA_3", CAN.main, values)
|
||||
|
||||
|
||||
def create_lkas_ui_msg(packer, CAN: CanBus, main_on: bool, enabled: bool, steer_alert: bool, hud_control,
|
||||
stock_values: dict):
|
||||
"""
|
||||
Creates a CAN message for the Ford IPC IPMA/LKAS status.
|
||||
|
||||
Show the LKAS status with the "driver assist" lines in the IPC.
|
||||
|
||||
Stock functionality is maintained by passing through unmodified signals.
|
||||
|
||||
Frequency is 1Hz.
|
||||
"""
|
||||
|
||||
# LaActvStats_D_Dsply
|
||||
# R Intvn Warn Supprs Avail No
|
||||
# L
|
||||
# Intvn 24 19 14 9 4
|
||||
# Warn 23 18 13 8 3
|
||||
# Supprs 22 17 12 7 2
|
||||
# Avail 21 16 11 6 1
|
||||
# No 20 15 10 5 0
|
||||
#
|
||||
# TODO: test suppress state
|
||||
if enabled:
|
||||
lines = 0 # NoLeft_NoRight
|
||||
if hud_control.leftLaneDepart:
|
||||
lines += 4
|
||||
elif hud_control.leftLaneVisible:
|
||||
lines += 1
|
||||
if hud_control.rightLaneDepart:
|
||||
lines += 20
|
||||
elif hud_control.rightLaneVisible:
|
||||
lines += 5
|
||||
elif main_on:
|
||||
lines = 0
|
||||
else:
|
||||
if hud_control.leftLaneDepart:
|
||||
lines = 3 # WarnLeft_NoRight
|
||||
elif hud_control.rightLaneDepart:
|
||||
lines = 15 # NoLeft_WarnRight
|
||||
else:
|
||||
lines = 30 # LA_Off
|
||||
|
||||
hands_on_wheel_dsply = 1 if steer_alert else 0
|
||||
|
||||
values = {s: stock_values[s] for s in [
|
||||
"FeatConfigIpmaActl",
|
||||
"FeatNoIpmaActl",
|
||||
"PersIndexIpma_D_Actl",
|
||||
"AhbcRampingV_D_Rq", # AHB ramping
|
||||
"LaDenyStats_B_Dsply", # LKAS error
|
||||
"CamraDefog_B_Req", # Windshield heater?
|
||||
"CamraStats_D_Dsply", # Camera status
|
||||
"DasAlrtLvl_D_Dsply", # DAS alert level
|
||||
"DasStats_D_Dsply", # DAS status
|
||||
"DasWarn_D_Dsply", # DAS warning
|
||||
"AhbHiBeam_D_Rq", # AHB status
|
||||
"Passthru_63",
|
||||
"Passthru_48",
|
||||
]}
|
||||
|
||||
values.update({
|
||||
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31]
|
||||
"LaHandsOff_D_Dsply": hands_on_wheel_dsply, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed
|
||||
})
|
||||
return packer.make_can_msg("IPMA_Data", CAN.main, values)
|
||||
|
||||
|
||||
def create_button_msg(packer, bus: int, stock_values: dict, cancel=False, resume=False, tja_toggle=False):
|
||||
"""
|
||||
Creates a CAN message for the Ford SCCM buttons/switches.
|
||||
|
||||
Includes cruise control buttons, turn lights and more.
|
||||
|
||||
Frequency is 10Hz.
|
||||
"""
|
||||
|
||||
values = {s: stock_values[s] for s in [
|
||||
"HeadLghtHiFlash_D_Stat", # SCCM Passthrough the remaining buttons
|
||||
"TurnLghtSwtch_D_Stat", # SCCM Turn signal switch
|
||||
"WiprFront_D_Stat",
|
||||
"LghtAmb_D_Sns",
|
||||
"AccButtnGapDecPress",
|
||||
"AccButtnGapIncPress",
|
||||
"AslButtnOnOffCnclPress",
|
||||
"AslButtnOnOffPress",
|
||||
"LaSwtchPos_D_Stat",
|
||||
"CcAslButtnCnclResPress",
|
||||
"CcAslButtnDeny_B_Actl",
|
||||
"CcAslButtnIndxDecPress",
|
||||
"CcAslButtnIndxIncPress",
|
||||
"CcAslButtnOffCnclPress",
|
||||
"CcAslButtnOnOffCncl",
|
||||
"CcAslButtnOnPress",
|
||||
"CcAslButtnResDecPress",
|
||||
"CcAslButtnResIncPress",
|
||||
"CcAslButtnSetDecPress",
|
||||
"CcAslButtnSetIncPress",
|
||||
"CcAslButtnSetPress",
|
||||
"CcButtnOffPress",
|
||||
"CcButtnOnOffCnclPress",
|
||||
"CcButtnOnOffPress",
|
||||
"CcButtnOnPress",
|
||||
"HeadLghtHiFlash_D_Actl",
|
||||
"HeadLghtHiOn_B_StatAhb",
|
||||
"AhbStat_B_Dsply",
|
||||
"AccButtnGapTogglePress",
|
||||
"WiprFrontSwtch_D_Stat",
|
||||
"HeadLghtHiCtrl_D_RqAhb",
|
||||
]}
|
||||
|
||||
values.update({
|
||||
"CcAslButtnCnclPress": 1 if cancel else 0, # CC cancel button
|
||||
"CcAsllButtnResPress": 1 if resume else 0, # CC resume button
|
||||
"TjaButtnOnOffPress": 1 if tja_toggle else 0, # LCA/TJA toggle button
|
||||
})
|
||||
return packer.make_can_msg("Steering_Data_FD1", bus, values)
|
||||
100
artifacts/package_sources/iqdbc/iqdbc/car/ford/interface.py
Normal file
100
artifacts/package_sources/iqdbc/iqdbc/car/ford/interface.py
Normal file
@@ -0,0 +1,100 @@
|
||||
import numpy as np
|
||||
from iqdbc.car import Bus, get_safety_config, structs
|
||||
from iqdbc.car.carlog import carlog
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.ford.carcontroller import CarController
|
||||
from iqdbc.car.ford.carstate import CarState
|
||||
from iqdbc.car.ford.fordcan import CanBus
|
||||
from iqdbc.car.ford.radar_interface import RadarInterface
|
||||
from iqdbc.car.ford.values import CarControllerParams, DBC, Ecu, FordFlags, RADAR, FordSafetyFlags
|
||||
from iqdbc.car.interfaces import CarInterfaceBase
|
||||
|
||||
TransmissionType = structs.CarParams.TransmissionType
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
CarState = CarState
|
||||
CarController = CarController
|
||||
RadarInterface = RadarInterface
|
||||
|
||||
DRIVABLE_GEARS = (structs.CarState.GearShifter.low, structs.CarState.GearShifter.manumatic)
|
||||
|
||||
@staticmethod
|
||||
def get_pid_accel_limits(CP, CP_IQ, current_speed, cruise_speed):
|
||||
# PCM doesn't allow acceleration near cruise_speed,
|
||||
# so limit limits of pid to prevent windup
|
||||
ACCEL_MAX_VALS = [CarControllerParams.ACCEL_MAX, 0.2]
|
||||
ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .4]
|
||||
return CarControllerParams.ACCEL_MIN, np.interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS)
|
||||
|
||||
@staticmethod
|
||||
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
|
||||
ret.brand = "ford"
|
||||
|
||||
ret.radarUnavailable = Bus.radar not in DBC[candidate]
|
||||
ret.steerControlType = structs.CarParams.SteerControlType.angle
|
||||
ret.steerActuatorDelay = 0.2
|
||||
ret.steerLimitTimer = 1.0
|
||||
ret.steerAtStandstill = True
|
||||
|
||||
ret.longitudinalTuning.kiBP = [0.]
|
||||
ret.longitudinalTuning.kiV = [0.5]
|
||||
|
||||
if not ret.radarUnavailable and DBC[candidate][Bus.radar] == RADAR.DELPHI_MRR:
|
||||
# average of 33.3 Hz radar timestep / 4 scan modes = 60 ms
|
||||
# MRR_Header_Timestamps->CAN_DET_TIME_SINCE_MEAS reports 61.3 ms
|
||||
ret.radarDelay = 0.06
|
||||
|
||||
CAN = CanBus(fingerprint=fingerprint)
|
||||
cfgs = [get_safety_config(structs.CarParams.SafetyModel.ford)]
|
||||
if CAN.main >= 4:
|
||||
cfgs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput))
|
||||
ret.safetyConfigs = cfgs
|
||||
|
||||
ret.alphaLongitudinalAvailable = ret.radarUnavailable
|
||||
if alpha_long or not ret.radarUnavailable:
|
||||
ret.safetyConfigs[-1].safetyParam |= FordSafetyFlags.LONG_CONTROL.value
|
||||
ret.openpilotLongitudinalControl = True
|
||||
|
||||
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
|
||||
found_ecus = [fw.ecu for fw in car_fw]
|
||||
if Ecu.shiftByWire in found_ecus or 0x5A in fingerprint[CAN.main] or docs:
|
||||
ret.transmissionType = TransmissionType.automatic
|
||||
else:
|
||||
ret.transmissionType = TransmissionType.manual
|
||||
ret.minEnableSpeed = 20.0 * CV.MPH_TO_MS
|
||||
|
||||
# BSM: Side_Detect_L_Stat, Side_Detect_R_Stat
|
||||
# TODO: detect bsm in car_fw?
|
||||
ret.enableBsm = 0x3A6 in fingerprint[CAN.main] and 0x3A7 in fingerprint[CAN.main]
|
||||
|
||||
# LCA can steer down to zero
|
||||
ret.minSteerSpeed = 0.
|
||||
|
||||
ret.autoResumeSng = ret.minEnableSpeed == -1.
|
||||
ret.centerToFront = ret.wheelbase * 0.44
|
||||
return ret
|
||||
@@ -0,0 +1,272 @@
|
||||
import numpy as np
|
||||
from typing import cast
|
||||
from collections import defaultdict
|
||||
from math import cos, sin
|
||||
from dataclasses import dataclass
|
||||
from iqdbc.can import CANParser
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.ford.fordcan import CanBus
|
||||
from iqdbc.car.ford.values import DBC, RADAR
|
||||
from iqdbc.car.interfaces import RadarInterfaceBase
|
||||
|
||||
DELPHI_ESR_RADAR_MSGS = list(range(0x500, 0x540))
|
||||
|
||||
DELPHI_MRR_RADAR_START_ADDR = 0x120
|
||||
DELPHI_MRR_RADAR_HEADER_ADDR = 0x174 # MRR_Header_SensorCoverage
|
||||
DELPHI_MRR_RADAR_MSG_COUNT = 64
|
||||
|
||||
DELPHI_MRR_RADAR_RANGE_COVERAGE = {0: 42, 1: 164, 2: 45, 3: 175} # scan index to detection range (m)
|
||||
DELPHI_MRR_MIN_LONG_RANGE_DIST = 30 # meters
|
||||
DELPHI_MRR_CLUSTER_THRESHOLD = 5 # meters, lateral distance and relative velocity are weighted
|
||||
|
||||
|
||||
@dataclass
|
||||
class Cluster:
|
||||
dRel: float = 0.0
|
||||
yRel: float = 0.0
|
||||
vRel: float = 0.0
|
||||
trackId: int = 0
|
||||
|
||||
|
||||
def cluster_points(pts_l: list[list[float]], pts2_l: list[list[float]], max_dist: float) -> list[int]:
|
||||
"""
|
||||
Clusters a collection of points based on another collection of points. This is useful for correlating clusters through time.
|
||||
Points in pts2 not close enough to any point in pts are assigned -1.
|
||||
Args:
|
||||
pts_l: List of points to base the new clusters on
|
||||
pts2_l: List of points to cluster using pts
|
||||
max_dist: Max distance from cluster center to candidate point
|
||||
|
||||
Returns:
|
||||
List of cluster indices for pts2 that correspond to pts
|
||||
"""
|
||||
|
||||
if not len(pts2_l):
|
||||
return []
|
||||
|
||||
if not len(pts_l):
|
||||
return [-1] * len(pts2_l)
|
||||
|
||||
max_dist_sq = max_dist ** 2
|
||||
pts = np.array(pts_l)
|
||||
pts2 = np.array(pts2_l)
|
||||
|
||||
# Compute squared norms
|
||||
pts_norm_sq = np.sum(pts ** 2, axis=1)
|
||||
pts2_norm_sq = np.sum(pts2 ** 2, axis=1)
|
||||
|
||||
# Compute squared Euclidean distances using the identity
|
||||
# dist_sq[i, j] = ||pts2[i]||^2 + ||pts[j]||^2 - 2 * pts2[i] . pts[j]
|
||||
dist_sq = pts2_norm_sq[:, np.newaxis] + pts_norm_sq[np.newaxis, :] - 2 * np.dot(pts2, pts.T)
|
||||
dist_sq = np.maximum(dist_sq, 0.0)
|
||||
|
||||
# Find the closest cluster for each point and assign its index
|
||||
closest_clusters = np.argmin(dist_sq, axis=1)
|
||||
closest_dist_sq = dist_sq[np.arange(len(pts2)), closest_clusters]
|
||||
cluster_idxs = np.where(closest_dist_sq < max_dist_sq, closest_clusters, -1)
|
||||
|
||||
return cast(list[int], cluster_idxs.tolist())
|
||||
|
||||
|
||||
def _create_delphi_esr_radar_can_parser(CP) -> CANParser:
|
||||
msg_n = len(DELPHI_ESR_RADAR_MSGS)
|
||||
messages = list(zip(DELPHI_ESR_RADAR_MSGS, [20] * msg_n, strict=True))
|
||||
|
||||
return CANParser(RADAR.DELPHI_ESR, messages, CanBus(CP).radar)
|
||||
|
||||
|
||||
def _create_delphi_mrr_radar_can_parser(CP) -> CANParser:
|
||||
messages = [
|
||||
("MRR_Header_InformationDetections", 33),
|
||||
("MRR_Header_SensorCoverage", 33),
|
||||
]
|
||||
|
||||
for i in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
|
||||
msg = f"MRR_Detection_{i:03d}"
|
||||
messages += [(msg, 33)]
|
||||
|
||||
return CANParser(RADAR.DELPHI_MRR, messages, CanBus(CP).radar)
|
||||
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP, CP_IQ):
|
||||
super().__init__(CP, CP_IQ)
|
||||
|
||||
self.points: list[list[float]] = []
|
||||
self.clusters: list[Cluster] = []
|
||||
|
||||
self.updated_messages = set()
|
||||
self.track_id = 0
|
||||
self.radar = DBC[CP.carFingerprint].get(Bus.radar)
|
||||
self.scan_index_invalid_cnt = 0
|
||||
self.radar_unavailable_cnt = 0
|
||||
self.prev_headerScanIndex = 0
|
||||
if CP.radarUnavailable:
|
||||
self.rcp = None
|
||||
elif self.radar == RADAR.DELPHI_ESR:
|
||||
self.rcp = _create_delphi_esr_radar_can_parser(CP)
|
||||
self.trigger_msg = DELPHI_ESR_RADAR_MSGS[-1]
|
||||
self.valid_cnt = {key: 0 for key in DELPHI_ESR_RADAR_MSGS}
|
||||
elif self.radar == RADAR.DELPHI_MRR:
|
||||
self.rcp = _create_delphi_mrr_radar_can_parser(CP)
|
||||
self.trigger_msg = DELPHI_MRR_RADAR_HEADER_ADDR
|
||||
else:
|
||||
raise ValueError(f"Unsupported radar: {self.radar}")
|
||||
|
||||
def update(self, can_strings):
|
||||
if self.rcp is None:
|
||||
return super().update(None)
|
||||
|
||||
vls = self.rcp.update(can_strings)
|
||||
self.updated_messages.update(vls)
|
||||
|
||||
if self.trigger_msg not in self.updated_messages:
|
||||
return None
|
||||
self.updated_messages.clear()
|
||||
|
||||
ret = structs.RadarData()
|
||||
if not self.rcp.can_valid:
|
||||
ret.errors.canError = True
|
||||
|
||||
if self.radar == RADAR.DELPHI_ESR:
|
||||
self._update_delphi_esr()
|
||||
elif self.radar == RADAR.DELPHI_MRR:
|
||||
_update = self._update_delphi_mrr(ret)
|
||||
if not _update:
|
||||
return None
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
return ret
|
||||
|
||||
def _update_delphi_esr(self):
|
||||
for ii in sorted(self.updated_messages):
|
||||
cpt = self.rcp.vl[ii]
|
||||
|
||||
if cpt['X_Rel'] > 0.00001:
|
||||
self.valid_cnt[ii] = 0 # reset counter
|
||||
|
||||
if cpt['X_Rel'] > 0.00001:
|
||||
self.valid_cnt[ii] += 1
|
||||
else:
|
||||
self.valid_cnt[ii] = max(self.valid_cnt[ii] - 1, 0)
|
||||
#print ii, self.valid_cnt[ii], cpt['VALID'], cpt['X_Rel'], cpt['Angle']
|
||||
|
||||
# radar point only valid if there have been enough valid measurements
|
||||
if self.valid_cnt[ii] > 0:
|
||||
if ii not in self.pts:
|
||||
self.pts[ii] = structs.RadarData.RadarPoint()
|
||||
self.pts[ii].trackId = self.track_id
|
||||
self.track_id += 1
|
||||
self.pts[ii].dRel = cpt['X_Rel'] # from front of car
|
||||
self.pts[ii].yRel = cpt['X_Rel'] * cpt['Angle'] * CV.DEG_TO_RAD # in car frame's y axis, left is positive
|
||||
self.pts[ii].vRel = cpt['V_Rel']
|
||||
self.pts[ii].aRel = float('nan')
|
||||
self.pts[ii].yvRel = float('nan')
|
||||
self.pts[ii].measured = True
|
||||
else:
|
||||
if ii in self.pts:
|
||||
del self.pts[ii]
|
||||
|
||||
def _update_delphi_mrr(self, ret: structs.RadarData):
|
||||
headerScanIndex = int(self.rcp.vl["MRR_Header_InformationDetections"]['CAN_SCAN_INDEX']) & 0b11
|
||||
|
||||
# In reverse, the radar continually sends the last messages. Mark this as invalid
|
||||
if (self.prev_headerScanIndex + 1) % 4 != headerScanIndex:
|
||||
self.radar_unavailable_cnt += 1
|
||||
else:
|
||||
self.radar_unavailable_cnt = 0
|
||||
self.prev_headerScanIndex = headerScanIndex
|
||||
|
||||
if self.radar_unavailable_cnt >= 5:
|
||||
self.pts.clear()
|
||||
self.points.clear()
|
||||
self.clusters.clear()
|
||||
ret.errors.radarUnavailableTemporary = True
|
||||
return True
|
||||
|
||||
# Use points with Doppler coverage of +-60 m/s, reduces similar points
|
||||
if headerScanIndex not in (2, 3):
|
||||
return False
|
||||
|
||||
if DELPHI_MRR_RADAR_RANGE_COVERAGE[headerScanIndex] != int(self.rcp.vl["MRR_Header_SensorCoverage"]["CAN_RANGE_COVERAGE"]):
|
||||
self.scan_index_invalid_cnt += 1
|
||||
else:
|
||||
self.scan_index_invalid_cnt = 0
|
||||
|
||||
# Rarely MRR_Header_InformationDetections can fail to send a message. The scan index is skipped in this case
|
||||
if self.scan_index_invalid_cnt >= 5:
|
||||
ret.errors.wrongConfig = True
|
||||
|
||||
for ii in range(1, DELPHI_MRR_RADAR_MSG_COUNT + 1):
|
||||
msg = self.rcp.vl[f"MRR_Detection_{ii:03d}"]
|
||||
|
||||
# SCAN_INDEX rotates through 0..3 on each message for different measurement modes
|
||||
# Indexes 0 and 2 have a max range of ~40m, 1 and 3 are ~170m (MRR_Header_SensorCoverage->CAN_RANGE_COVERAGE)
|
||||
# Indexes 0 and 1 have a Doppler coverage of +-71 m/s, 2 and 3 have +-60 m/s
|
||||
scanIndex = msg[f"CAN_SCAN_INDEX_2LSB_{ii:02d}"]
|
||||
|
||||
# Throw out old measurements. Very unlikely to happen, but is proper behavior
|
||||
if scanIndex != headerScanIndex:
|
||||
continue
|
||||
|
||||
valid = bool(msg[f"CAN_DET_VALID_LEVEL_{ii:02d}"])
|
||||
|
||||
# Long range measurement mode is more sensitive and can detect the road surface
|
||||
dist = msg[f"CAN_DET_RANGE_{ii:02d}"] # m [0|255.984]
|
||||
if scanIndex in (1, 3) and dist < DELPHI_MRR_MIN_LONG_RANGE_DIST:
|
||||
valid = False
|
||||
|
||||
if valid:
|
||||
azimuth = msg[f"CAN_DET_AZIMUTH_{ii:02d}"] # rad [-3.1416|3.13964]
|
||||
distRate = msg[f"CAN_DET_RANGE_RATE_{ii:02d}"] # m/s [-128|127.984]
|
||||
dRel = cos(azimuth) * dist # m from front of car
|
||||
yRel = -sin(azimuth) * dist # in car frame's y axis, left is positive
|
||||
|
||||
self.points.append([dRel, yRel * 2, distRate * 2])
|
||||
|
||||
# Cluster and publish using stored points once we've cycled through all 4 scan modes
|
||||
if headerScanIndex != 3:
|
||||
return False
|
||||
|
||||
# Cluster points from this cycle against the centroids from the previous cycle
|
||||
prev_keys = [[p.dRel, p.yRel * 2, p.vRel * 2] for p in self.clusters]
|
||||
labels = cluster_points(prev_keys, self.points, DELPHI_MRR_CLUSTER_THRESHOLD)
|
||||
|
||||
points_by_track_id = defaultdict(list)
|
||||
for idx, label in enumerate(labels):
|
||||
if label != -1:
|
||||
points_by_track_id[self.clusters[label].trackId].append(self.points[idx])
|
||||
else:
|
||||
points_by_track_id[self.track_id].append(self.points[idx])
|
||||
self.track_id += 1
|
||||
|
||||
self.clusters = []
|
||||
for idx, (track_id, pts) in enumerate(points_by_track_id.items()):
|
||||
dRel = [p[0] for p in pts]
|
||||
min_dRel = min(dRel)
|
||||
dRel = sum(dRel) / len(dRel)
|
||||
|
||||
yRel = [p[1] for p in pts]
|
||||
yRel = sum(yRel) / len(yRel) / 2
|
||||
|
||||
vRel = [p[2] for p in pts]
|
||||
vRel = sum(vRel) / len(vRel) / 2
|
||||
|
||||
# FIXME: creating capnp RadarPoint and accessing attributes are both expensive, so we store a dataclass and reuse the RadarPoint
|
||||
self.clusters.append(Cluster(dRel=dRel, yRel=yRel, vRel=vRel, trackId=track_id))
|
||||
|
||||
if idx not in self.pts:
|
||||
self.pts[idx] = structs.RadarData.RadarPoint(measured=True, aRel=float('nan'), yvRel=float('nan'))
|
||||
|
||||
self.pts[idx].dRel = min_dRel
|
||||
self.pts[idx].yRel = yRel
|
||||
self.pts[idx].vRel = vRel
|
||||
self.pts[idx].trackId = track_id
|
||||
|
||||
for idx in range(len(points_by_track_id), len(self.pts)):
|
||||
del self.pts[idx]
|
||||
|
||||
self.points = []
|
||||
|
||||
return True
|
||||
28
artifacts/package_sources/iqdbc/iqdbc/car/ford/tests/print_platform_codes.py
Executable file
28
artifacts/package_sources/iqdbc/iqdbc/car/ford/tests/print_platform_codes.py
Executable file
@@ -0,0 +1,28 @@
|
||||
#!/usr/bin/env python3
|
||||
from collections import defaultdict
|
||||
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.car.ford.values import get_platform_codes
|
||||
from iqdbc.car.ford.fingerprints import FW_VERSIONS
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
if __name__ == "__main__":
|
||||
cars_for_code: defaultdict = defaultdict(lambda: defaultdict(set))
|
||||
|
||||
for car_model, ecus in FW_VERSIONS.items():
|
||||
print(car_model)
|
||||
for ecu in sorted(ecus):
|
||||
platform_codes = get_platform_codes(ecus[ecu])
|
||||
for code in platform_codes:
|
||||
cars_for_code[ecu][code].add(car_model)
|
||||
|
||||
print(f' (Ecu.{ecu[0]}, {hex(ecu[1])}, {ecu[2]}):')
|
||||
print(f' Codes: {sorted(platform_codes)}')
|
||||
print()
|
||||
|
||||
print('\nCar models vs. platform codes:')
|
||||
for ecu, codes in cars_for_code.items():
|
||||
print(f' (Ecu.{ecu[0]}, {hex(ecu[1])}, {ecu[2]}):')
|
||||
for code, cars in codes.items():
|
||||
print(f' {code!r}: {sorted(map(str, cars))}')
|
||||
@@ -0,0 +1,142 @@
|
||||
import random
|
||||
from collections.abc import Iterable
|
||||
|
||||
from hypothesis import settings, given, strategies as st
|
||||
from parameterized import parameterized
|
||||
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.car.fw_versions import build_fw_dict
|
||||
from iqdbc.car.ford.values import CAR, FW_QUERY_CONFIG, FW_PATTERN, get_platform_codes
|
||||
from iqdbc.car.ford.fingerprints import FW_VERSIONS
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
|
||||
ECU_ADDRESSES = {
|
||||
Ecu.eps: 0x730, # Power Steering Control Module (PSCM)
|
||||
Ecu.abs: 0x760, # Anti-Lock Brake System (ABS)
|
||||
Ecu.fwdRadar: 0x764, # Cruise Control Module (CCM)
|
||||
Ecu.fwdCamera: 0x706, # Image Processing Module A (IPMA)
|
||||
Ecu.engine: 0x7E0, # Powertrain Control Module (PCM)
|
||||
Ecu.shiftByWire: 0x732, # Gear Shift Module (GSM)
|
||||
Ecu.debug: 0x7D0, # Accessory Protocol Interface Module (APIM)
|
||||
}
|
||||
|
||||
|
||||
ECU_PART_NUMBER = {
|
||||
Ecu.eps: [
|
||||
b"14D003",
|
||||
],
|
||||
Ecu.abs: [
|
||||
b"2D053",
|
||||
],
|
||||
Ecu.fwdRadar: [
|
||||
b"14D049",
|
||||
],
|
||||
Ecu.fwdCamera: [
|
||||
b"14F397", # Ford Q3
|
||||
b"14H102", # Ford Q4
|
||||
],
|
||||
}
|
||||
|
||||
|
||||
class TestFordFW:
|
||||
def test_fw_query_config(self):
|
||||
for (ecu, addr, subaddr) in FW_QUERY_CONFIG.extra_ecus:
|
||||
assert ecu in ECU_ADDRESSES, "Unknown ECU"
|
||||
assert addr == ECU_ADDRESSES[ecu], "ECU address mismatch"
|
||||
assert subaddr is None, "Unexpected ECU subaddress"
|
||||
|
||||
@parameterized.expand(FW_VERSIONS.items())
|
||||
def test_fw_versions(self, car_model: str, fw_versions: dict[tuple[int, int, int | None], Iterable[bytes]]):
|
||||
for (ecu, addr, subaddr), fws in fw_versions.items():
|
||||
assert ecu in ECU_PART_NUMBER, "Unexpected ECU"
|
||||
assert addr == ECU_ADDRESSES[ecu], "ECU address mismatch"
|
||||
assert subaddr is None, "Unexpected ECU subaddress"
|
||||
|
||||
for fw in fws:
|
||||
assert len(fw) == 24, "Expected ECU response to be 24 bytes"
|
||||
|
||||
match = FW_PATTERN.match(fw)
|
||||
assert match is not None, f"Unable to parse FW: {fw!r}"
|
||||
if match:
|
||||
part_number = match.group("part_number")
|
||||
assert part_number in ECU_PART_NUMBER[ecu], f"Unexpected part number for {fw!r}"
|
||||
|
||||
codes = get_platform_codes([fw])
|
||||
assert 1 == len(codes), f"Unable to parse FW: {fw!r}"
|
||||
|
||||
@settings(max_examples=100)
|
||||
@given(data=st.data())
|
||||
def test_platform_codes_fuzzy_fw(self, data):
|
||||
"""Ensure function doesn't raise an exception"""
|
||||
fw_strategy = st.lists(st.binary())
|
||||
fws = data.draw(fw_strategy)
|
||||
get_platform_codes(fws)
|
||||
|
||||
def test_platform_codes_spot_check(self):
|
||||
# Asserts basic platform code parsing behavior for a few cases
|
||||
results = get_platform_codes([
|
||||
b"JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
b"NZ6T-14F397-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
b"PJ6T-14H102-ABJ\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
b"LB5A-14C204-EAC\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
])
|
||||
assert results == {(b"X6A", b"J"), (b"Z6T", b"N"), (b"J6T", b"P"), (b"B5A", b"L")}
|
||||
|
||||
def test_fuzzy_match(self):
|
||||
for platform, fw_by_addr in FW_VERSIONS.items():
|
||||
# Ensure there's no overlaps in platform codes
|
||||
for _ in range(20):
|
||||
car_fw = []
|
||||
for ecu, fw_versions in fw_by_addr.items():
|
||||
ecu_name, addr, sub_addr = ecu
|
||||
fw = random.choice(fw_versions)
|
||||
car_fw.append(CarParams.CarFw(ecu=ecu_name, fwVersion=fw, address=addr,
|
||||
subAddress=0 if sub_addr is None else sub_addr))
|
||||
|
||||
CP = CarParams(carFw=car_fw)
|
||||
matches = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(build_fw_dict(CP.carFw), CP.carVin, FW_VERSIONS)
|
||||
assert matches == {platform}
|
||||
|
||||
def test_match_fw_fuzzy(self):
|
||||
offline_fw = {
|
||||
(Ecu.eps, 0x730, None): [
|
||||
b"L1MC-14D003-AJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
b"L1MC-14D003-AL\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
],
|
||||
(Ecu.abs, 0x760, None): [
|
||||
b"L1MC-2D053-BA\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
b"L1MC-2D053-BD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
],
|
||||
(Ecu.fwdRadar, 0x764, None): [
|
||||
b"LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
b"LB5T-14D049-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
],
|
||||
# We consider all model year hints for ECU, even with different platform codes
|
||||
(Ecu.fwdCamera, 0x706, None): [
|
||||
b"LB5T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
b"NC5T-14F397-AF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
|
||||
],
|
||||
}
|
||||
expected_fingerprint = CAR.FORD_EXPLORER_MK6
|
||||
|
||||
# ensure that we fuzzy match on all non-exact FW with changed revisions
|
||||
live_fw = {
|
||||
(0x730, None): {b"L1MC-14D003-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"},
|
||||
(0x760, None): {b"L1MC-2D053-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"},
|
||||
(0x764, None): {b"LB5T-14D049-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"},
|
||||
(0x706, None): {b"LB5T-14F397-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"},
|
||||
}
|
||||
candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw})
|
||||
assert candidates == {expected_fingerprint}
|
||||
|
||||
# model year hint in between the range should match
|
||||
live_fw[(0x706, None)] = {b"MB5T-14F397-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}
|
||||
candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw,})
|
||||
assert candidates == {expected_fingerprint}
|
||||
|
||||
# unseen model year hint should not match
|
||||
live_fw[(0x760, None)] = {b"M1MC-2D053-XX\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00"}
|
||||
candidates = FW_QUERY_CONFIG.match_fw_to_car_fuzzy(live_fw, '', {expected_fingerprint: offline_fw})
|
||||
assert len(candidates) == 0, "Should not match new model year hint"
|
||||
315
artifacts/package_sources/iqdbc/iqdbc/car/ford/values.py
Normal file
315
artifacts/package_sources/iqdbc/iqdbc/car/ford/values.py
Normal file
@@ -0,0 +1,315 @@
|
||||
import copy
|
||||
import re
|
||||
from dataclasses import dataclass, field, replace
|
||||
from enum import Enum, IntFlag
|
||||
|
||||
from iqdbc.car import Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
|
||||
from iqdbc.car.lateral import AngleSteeringLimits
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column
|
||||
from iqdbc.car.fw_query_definitions import FwQueryConfig, LiveFwVersions, OfflineFwVersions, Request, StdQueries, p16
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
STEER_STEP = 5 # LateralMotionControl, 20Hz
|
||||
LKA_STEP = 3 # Lane_Assist_Data1, 33Hz
|
||||
ACC_CONTROL_STEP = 2 # ACCDATA, 50Hz
|
||||
LKAS_UI_STEP = 100 # IPMA_Data, 1Hz
|
||||
ACC_UI_STEP = 20 # ACCDATA_3, 5Hz
|
||||
BUTTONS_STEP = 5 # Steering_Data_FD1, 10Hz, but send twice as fast
|
||||
|
||||
STEER_DRIVER_ALLOWANCE = 1.0 # Driver intervention threshold, Nm
|
||||
|
||||
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
|
||||
ACCEL_MIN = -3.5 # m/s^2 max deceleration
|
||||
MIN_GAS = -0.5
|
||||
INACTIVE_GAS = -5.0
|
||||
|
||||
def __init__(self, CP):
|
||||
pass
|
||||
|
||||
|
||||
class FordSafetyFlags(IntFlag):
|
||||
LONG_CONTROL = 1
|
||||
CANFD = 2
|
||||
|
||||
|
||||
class FordFlags(IntFlag):
|
||||
# Static flags
|
||||
CANFD = 1
|
||||
|
||||
|
||||
class RADAR:
|
||||
DELPHI_ESR = 'ford_fusion_2018_adas'
|
||||
DELPHI_MRR = 'FORD_CADS'
|
||||
|
||||
|
||||
class Footnote(Enum):
|
||||
FOCUS = CarFootnote(
|
||||
"Refers only to the Focus Mk4 (C519) available in Europe/China/Taiwan/Australasia, not the Focus Mk3 (C346) in " +
|
||||
"North and South America/Southeast Asia.",
|
||||
Column.MODEL,
|
||||
)
|
||||
|
||||
|
||||
@dataclass
|
||||
class FordCarDocs(CarDocs):
|
||||
package: str = "Co-Pilot360 Assist+"
|
||||
hybrid: bool = False
|
||||
plug_in_hybrid: bool = False
|
||||
|
||||
def init_make(self, CP: CarParams):
|
||||
harness = CarHarness.ford_q4 if CP.flags & FordFlags.CANFD else CarHarness.ford_q3
|
||||
self.car_parts = CarParts.common([harness])
|
||||
|
||||
if harness == CarHarness.ford_q4:
|
||||
self.setup_video = "https://www.youtube.com/watch?v=uUGkH6C_EQU"
|
||||
|
||||
if CP.carFingerprint in (CAR.FORD_F_150_MK14, CAR.FORD_F_150_LIGHTNING_MK1, CAR.FORD_EXPEDITION_MK4):
|
||||
self.setup_video = "https://www.youtube.com/watch?v=MewJc9LYp9M"
|
||||
|
||||
|
||||
@dataclass
|
||||
class FordPlatformConfig(PlatformConfig):
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: {
|
||||
Bus.pt: 'ford_lincoln_base_pt',
|
||||
Bus.radar: RADAR.DELPHI_MRR,
|
||||
})
|
||||
|
||||
def init(self):
|
||||
for car_docs in list(self.car_docs):
|
||||
if car_docs.hybrid:
|
||||
name = f"{car_docs.make} {car_docs.model} Hybrid {car_docs.years}"
|
||||
self.car_docs.append(replace(copy.deepcopy(car_docs), name=name))
|
||||
if car_docs.plug_in_hybrid:
|
||||
name = f"{car_docs.make} {car_docs.model} Plug-in Hybrid {car_docs.years}"
|
||||
self.car_docs.append(replace(copy.deepcopy(car_docs), name=name))
|
||||
|
||||
|
||||
@dataclass
|
||||
class FordCANFDPlatformConfig(FordPlatformConfig):
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: {
|
||||
Bus.pt: 'ford_lincoln_base_pt',
|
||||
})
|
||||
|
||||
def init(self):
|
||||
super().init()
|
||||
self.flags |= FordFlags.CANFD
|
||||
|
||||
|
||||
@dataclass
|
||||
class FordF150LightningPlatform(FordCANFDPlatformConfig):
|
||||
def init(self):
|
||||
super().init()
|
||||
|
||||
# Don't show in docs until this issue is resolved. See https://github.com/commaai/openpilot/issues/30302
|
||||
self.car_docs = []
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
FORD_BRONCO_SPORT_MK1 = FordPlatformConfig(
|
||||
[FordCarDocs("Ford Bronco Sport 2021-24")],
|
||||
CarSpecs(mass=1625, wheelbase=2.67, steerRatio=17.7),
|
||||
)
|
||||
FORD_ESCAPE_MK4 = FordPlatformConfig(
|
||||
[
|
||||
FordCarDocs("Ford Escape 2020-22", hybrid=True, plug_in_hybrid=True),
|
||||
FordCarDocs("Ford Kuga 2020-23", "Adaptive Cruise Control with Lane Centering", hybrid=True, plug_in_hybrid=True),
|
||||
],
|
||||
CarSpecs(mass=1750, wheelbase=2.71, steerRatio=16.7),
|
||||
)
|
||||
FORD_ESCAPE_MK4_5 = FordCANFDPlatformConfig(
|
||||
[
|
||||
FordCarDocs("Ford Escape 2023-24", hybrid=True, plug_in_hybrid=True, setup_video="https://www.youtube.com/watch?v=M6uXf4b2SHM"),
|
||||
FordCarDocs("Ford Kuga Hybrid 2024", "All"),
|
||||
FordCarDocs("Ford Kuga Plug-in Hybrid 2024", "All"),
|
||||
],
|
||||
CarSpecs(mass=1750, wheelbase=2.71, steerRatio=16.7),
|
||||
)
|
||||
FORD_EXPLORER_MK6 = FordPlatformConfig(
|
||||
[
|
||||
FordCarDocs("Ford Explorer 2020-24", hybrid=True), # Hybrid: Limited and Platinum only
|
||||
FordCarDocs("Lincoln Aviator 2020-24", "Co-Pilot360 Plus", plug_in_hybrid=True), # Hybrid: Grand Touring only
|
||||
],
|
||||
CarSpecs(mass=2050, wheelbase=3.025, steerRatio=16.8),
|
||||
)
|
||||
FORD_EXPEDITION_MK4 = FordCANFDPlatformConfig(
|
||||
[FordCarDocs("Ford Expedition 2022-24", "Co-Pilot360 Assist 2.0", hybrid=False)],
|
||||
CarSpecs(mass=2000, wheelbase=3.69, steerRatio=17.0),
|
||||
)
|
||||
FORD_F_150_MK14 = FordCANFDPlatformConfig(
|
||||
[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")],
|
||||
CarSpecs(mass=2948, wheelbase=3.70, steerRatio=16.9),
|
||||
)
|
||||
FORD_FOCUS_MK4 = FordPlatformConfig(
|
||||
[FordCarDocs("Ford Focus 2018", "Adaptive Cruise Control with Lane Centering", footnotes=[Footnote.FOCUS], hybrid=True)], # mHEV only
|
||||
CarSpecs(mass=1350, wheelbase=2.7, steerRatio=15.0),
|
||||
)
|
||||
FORD_MAVERICK_MK1 = FordPlatformConfig(
|
||||
[
|
||||
FordCarDocs("Ford Maverick 2022", "LARIAT Luxury", hybrid=True),
|
||||
FordCarDocs("Ford Maverick 2023-24", "Co-Pilot360 Assist", hybrid=True),
|
||||
],
|
||||
CarSpecs(mass=1650, wheelbase=3.076, steerRatio=17.0),
|
||||
)
|
||||
FORD_MUSTANG_MACH_E_MK1 = FordCANFDPlatformConfig(
|
||||
[FordCarDocs("Ford Mustang Mach-E 2021-24", "All", setup_video="https://www.youtube.com/watch?v=AR4_eTF3b_A")],
|
||||
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", setup_video="https://www.youtube.com/watch?v=2oJlXCKYOy0")],
|
||||
CarSpecs(mass=2000, wheelbase=3.27, steerRatio=17.0),
|
||||
)
|
||||
|
||||
|
||||
# FW response contains a combined software and part number
|
||||
# A-Z except no I, O or W
|
||||
# e.g. NZ6A-14C204-AAA
|
||||
# 1222-333333-444
|
||||
# 1 = Model year hint (approximates model year/generation)
|
||||
# 2 = Platform hint
|
||||
# 3 = Part number
|
||||
# 4 = Software version
|
||||
FW_ALPHABET = b'A-HJ-NP-VX-Z'
|
||||
FW_PATTERN = re.compile(b'^(?P<model_year_hint>[' + FW_ALPHABET + b'])' +
|
||||
b'(?P<platform_hint>[0-9' + FW_ALPHABET + b']{3})-' +
|
||||
b'(?P<part_number>[0-9' + FW_ALPHABET + b']{5,6})-' +
|
||||
b'(?P<software_revision>[' + FW_ALPHABET + b']{2,})\x00*$')
|
||||
|
||||
|
||||
def get_platform_codes(fw_versions: list[bytes] | set[bytes]) -> set[tuple[bytes, bytes]]:
|
||||
codes = set()
|
||||
for fw in fw_versions:
|
||||
match = FW_PATTERN.match(fw)
|
||||
if match is not None:
|
||||
codes.add((match.group('platform_hint'), match.group('model_year_hint')))
|
||||
|
||||
return codes
|
||||
|
||||
|
||||
def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, vin: str, offline_fw_versions: OfflineFwVersions) -> set[str]:
|
||||
candidates: set[str] = set()
|
||||
|
||||
for candidate, fws in offline_fw_versions.items():
|
||||
# Keep track of ECUs which pass all checks (platform hint, within model year hint range)
|
||||
valid_found_ecus = set()
|
||||
valid_expected_ecus = {ecu[1:] for ecu in fws if ecu[0] in PLATFORM_CODE_ECUS}
|
||||
for ecu, expected_versions in fws.items():
|
||||
addr = ecu[1:]
|
||||
# Only check ECUs expected to have platform codes
|
||||
if ecu[0] not in PLATFORM_CODE_ECUS:
|
||||
continue
|
||||
|
||||
# Expected platform codes & model year hints
|
||||
codes = get_platform_codes(expected_versions)
|
||||
expected_platform_codes = {code for code, _ in codes}
|
||||
expected_model_year_hints = {model_year_hint for _, model_year_hint in codes}
|
||||
|
||||
# Found platform codes & model year hints
|
||||
codes = get_platform_codes(live_fw_versions.get(addr, set()))
|
||||
found_platform_codes = {code for code, _ in codes}
|
||||
found_model_year_hints = {model_year_hint for _, model_year_hint in codes}
|
||||
|
||||
# Check platform code matches for any found versions
|
||||
if not any(found_platform_code in expected_platform_codes for found_platform_code in found_platform_codes):
|
||||
break
|
||||
|
||||
# Check any model year hint within range in the database. Note that some models have more than one
|
||||
# platform code per ECU which we don't consider as separate ranges
|
||||
if not any(min(expected_model_year_hints) <= found_model_year_hint <= max(expected_model_year_hints) for
|
||||
found_model_year_hint in found_model_year_hints):
|
||||
break
|
||||
|
||||
valid_found_ecus.add(addr)
|
||||
|
||||
# If all live ECUs pass all checks for candidate, add it as a match
|
||||
if valid_expected_ecus.issubset(valid_found_ecus):
|
||||
candidates.add(candidate)
|
||||
|
||||
return candidates
|
||||
|
||||
|
||||
# All of these ECUs must be present and are expected to have platform codes we can match
|
||||
PLATFORM_CODE_ECUS = (Ecu.abs, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.eps)
|
||||
|
||||
DATA_IDENTIFIER_FORD_ASBUILT = 0xDE00
|
||||
|
||||
ASBUILT_BLOCKS: list[tuple[int, list]] = [
|
||||
(1, [Ecu.debug, Ecu.fwdCamera, Ecu.eps]),
|
||||
(2, [Ecu.abs, Ecu.debug, Ecu.eps]),
|
||||
(3, [Ecu.abs, Ecu.debug, Ecu.eps]),
|
||||
(4, [Ecu.debug, Ecu.fwdCamera]),
|
||||
(5, [Ecu.debug]),
|
||||
(6, [Ecu.debug]),
|
||||
(7, [Ecu.debug]),
|
||||
(8, [Ecu.debug]),
|
||||
(9, [Ecu.debug]),
|
||||
(16, [Ecu.debug, Ecu.fwdCamera]),
|
||||
(18, [Ecu.fwdCamera]),
|
||||
(20, [Ecu.fwdCamera]),
|
||||
(21, [Ecu.fwdCamera]),
|
||||
]
|
||||
|
||||
|
||||
def ford_asbuilt_block_request(block_id: int):
|
||||
return bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + p16(DATA_IDENTIFIER_FORD_ASBUILT + block_id - 1)
|
||||
|
||||
|
||||
def ford_asbuilt_block_response(block_id: int):
|
||||
return bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + p16(DATA_IDENTIFIER_FORD_ASBUILT + block_id - 1)
|
||||
|
||||
|
||||
FW_QUERY_CONFIG = FwQueryConfig(
|
||||
requests=[
|
||||
# CAN and CAN FD queries are combined.
|
||||
# FIXME: For CAN FD, ECUs respond with frames larger than 8 bytes on the powertrain bus
|
||||
Request(
|
||||
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
|
||||
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire],
|
||||
logging=True,
|
||||
),
|
||||
Request(
|
||||
[StdQueries.TESTER_PRESENT_REQUEST, StdQueries.MANUFACTURER_SOFTWARE_VERSION_REQUEST],
|
||||
[StdQueries.TESTER_PRESENT_RESPONSE, StdQueries.MANUFACTURER_SOFTWARE_VERSION_RESPONSE],
|
||||
whitelist_ecus=[Ecu.abs, Ecu.debug, Ecu.engine, Ecu.eps, Ecu.fwdCamera, Ecu.fwdRadar, Ecu.shiftByWire],
|
||||
bus=0,
|
||||
auxiliary=True,
|
||||
),
|
||||
*[Request(
|
||||
[StdQueries.TESTER_PRESENT_REQUEST, ford_asbuilt_block_request(block_id)],
|
||||
[StdQueries.TESTER_PRESENT_RESPONSE, ford_asbuilt_block_response(block_id)],
|
||||
whitelist_ecus=ecus,
|
||||
bus=0,
|
||||
logging=True,
|
||||
) for block_id, ecus in ASBUILT_BLOCKS],
|
||||
],
|
||||
extra_ecus=[
|
||||
(Ecu.engine, 0x7e0, None), # Powertrain Control Module
|
||||
# Note: We are unlikely to get a response from behind the gateway
|
||||
(Ecu.shiftByWire, 0x732, None), # Gear Shift Module
|
||||
(Ecu.debug, 0x7d0, None), # Accessory Protocol Interface Module
|
||||
],
|
||||
# Custom fuzzy fingerprinting function using platform and model year hints
|
||||
match_fw_to_car_fuzzy=match_fw_to_car_fuzzy,
|
||||
)
|
||||
|
||||
DBC = CAR.create_dbc_map()
|
||||
@@ -0,0 +1,153 @@
|
||||
import copy
|
||||
from dataclasses import dataclass, field
|
||||
import struct
|
||||
from collections.abc import Callable
|
||||
|
||||
from iqdbc.car import uds
|
||||
from iqdbc.car.structs import CarParams
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
AddrType = tuple[int, int | None]
|
||||
EcuAddrBusType = tuple[int, int | None, int]
|
||||
EcuAddrSubAddr = tuple[Ecu, int, int | None]
|
||||
|
||||
LiveFwVersions = dict[AddrType, set[bytes]]
|
||||
OfflineFwVersions = dict[str, dict[EcuAddrSubAddr, list[bytes]]]
|
||||
|
||||
# A global list of addresses we will only ever consider for VIN responses
|
||||
# engine, hybrid controller, Ford abs, Hyundai CAN FD cluster, 29-bit engine, PGM-FI
|
||||
# TODO: move these to each brand's FW query config
|
||||
STANDARD_VIN_ADDRS = [0x7e0, 0x7e2, 0x760, 0x7c6, 0x18da10f1, 0x18da0ef1]
|
||||
|
||||
ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.abs, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa]
|
||||
ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()}
|
||||
|
||||
|
||||
def p16(val):
|
||||
return struct.pack("!H", val)
|
||||
|
||||
|
||||
class StdQueries:
|
||||
# FW queries
|
||||
TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT, 0x0])
|
||||
TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40, 0x0])
|
||||
|
||||
SHORT_TESTER_PRESENT_REQUEST = bytes([uds.SERVICE_TYPE.TESTER_PRESENT])
|
||||
SHORT_TESTER_PRESENT_RESPONSE = bytes([uds.SERVICE_TYPE.TESTER_PRESENT + 0x40])
|
||||
|
||||
DEFAULT_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL,
|
||||
uds.SESSION_TYPE.DEFAULT])
|
||||
DEFAULT_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40,
|
||||
uds.SESSION_TYPE.DEFAULT, 0x0, 0x32, 0x1, 0xf4])
|
||||
|
||||
EXTENDED_DIAGNOSTIC_REQUEST = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL,
|
||||
uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC])
|
||||
EXTENDED_DIAGNOSTIC_RESPONSE = bytes([uds.SERVICE_TYPE.DIAGNOSTIC_SESSION_CONTROL + 0x40,
|
||||
uds.SESSION_TYPE.EXTENDED_DIAGNOSTIC, 0x0, 0x32, 0x1, 0xf4])
|
||||
|
||||
MANUFACTURER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER)
|
||||
MANUFACTURER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_NUMBER)
|
||||
|
||||
SUPPLIER_SOFTWARE_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_VERSION_NUMBER)
|
||||
SUPPLIER_SOFTWARE_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.SYSTEM_SUPPLIER_ECU_SOFTWARE_VERSION_NUMBER)
|
||||
|
||||
MANUFACTURER_ECU_HARDWARE_NUMBER_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_HARDWARE_NUMBER)
|
||||
MANUFACTURER_ECU_HARDWARE_NUMBER_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_HARDWARE_NUMBER)
|
||||
|
||||
UDS_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
|
||||
UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
|
||||
p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
|
||||
|
||||
OBD_VERSION_REQUEST = b'\x09\x04'
|
||||
OBD_VERSION_RESPONSE = b'\x49\x04'
|
||||
|
||||
# VIN queries
|
||||
OBD_VIN_REQUEST = b'\x09\x02'
|
||||
OBD_VIN_RESPONSE = b'\x49\x02\x01'
|
||||
|
||||
UDS_VIN_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + p16(uds.DATA_IDENTIFIER_TYPE.VIN)
|
||||
UDS_VIN_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + p16(uds.DATA_IDENTIFIER_TYPE.VIN)
|
||||
|
||||
GM_VIN_REQUEST = b'\x1a\x90'
|
||||
GM_VIN_RESPONSE = b'\x5a\x90'
|
||||
|
||||
KWP_VIN_REQUEST = b'\x21\x81'
|
||||
KWP_VIN_RESPONSE = b'\x61\x81'
|
||||
|
||||
|
||||
@dataclass
|
||||
class Request:
|
||||
request: list[bytes]
|
||||
response: list[bytes]
|
||||
whitelist_ecus: list[Ecu] = field(default_factory=list)
|
||||
rx_offset: int = 0x8
|
||||
bus: int = 1
|
||||
# Whether this query should be run on the first auxiliary panda (CAN FD cars for example)
|
||||
auxiliary: bool = False
|
||||
# FW responses from these queries will not be used for fingerprinting
|
||||
logging: bool = False
|
||||
# pandad toggles OBD multiplexing on/off as needed
|
||||
obd_multiplexing: bool = True
|
||||
|
||||
|
||||
@dataclass
|
||||
class FwQueryConfig:
|
||||
requests: list[Request]
|
||||
# TODO: make this automatic and remove hardcoded lists, or do fingerprinting with ecus
|
||||
# Overrides and removes from essential ecus for specific models and ecus (exact matching)
|
||||
non_essential_ecus: dict[Ecu, list[str]] = field(default_factory=dict)
|
||||
# Ecus added for data collection, not to be fingerprinted on
|
||||
extra_ecus: list[tuple[Ecu, int, int | None]] = field(default_factory=list)
|
||||
# Function a brand can implement to provide better fuzzy matching. Takes in FW versions and VIN,
|
||||
# returns set of candidates. Only will match if one candidate is returned
|
||||
match_fw_to_car_fuzzy: Callable[[LiveFwVersions, str, OfflineFwVersions], set[str]] | None = None
|
||||
refine_fw_matches: Callable[[set[str], str], set[str]] | None = None
|
||||
|
||||
def __post_init__(self):
|
||||
# Asserts that a request exists if extra ecus are used
|
||||
if len(self.extra_ecus):
|
||||
assert len(self.requests), "Must define a request with extra ecus"
|
||||
|
||||
# All extra ecus should be used in a request
|
||||
for ecu, _, _ in self.extra_ecus:
|
||||
assert (any(ecu in request.whitelist_ecus for request in self.requests) or
|
||||
any(not request.whitelist_ecus for request in self.requests)), f"Ecu.{ECU_NAME[ecu]} not in any request"
|
||||
|
||||
# These ECUs are already not in ESSENTIAL_ECUS which the fingerprint functions give a pass if missing
|
||||
unnecessary_non_essential_ecus = set(self.non_essential_ecus) - set(ESSENTIAL_ECUS)
|
||||
assert unnecessary_non_essential_ecus == set(), ("Declaring non-essential ECUs non-essential is not required: " +
|
||||
f"{', '.join([f'Ecu.{ECU_NAME[ecu]}' for ecu in unnecessary_non_essential_ecus])}")
|
||||
|
||||
# Asserts equal length request and response lists
|
||||
for request_obj in self.requests:
|
||||
assert len(request_obj.request) == len(request_obj.response), ("Request and response lengths do not match: " +
|
||||
f"{request_obj.request} vs. {request_obj.response}")
|
||||
|
||||
# No request on the OBD port (bus 1, multiplexed) should be run on an aux panda
|
||||
assert not (request_obj.auxiliary and request_obj.bus == 1 and request_obj.obd_multiplexing), ("OBD multiplexed request should not " +
|
||||
f"be marked auxiliary: {request_obj}")
|
||||
|
||||
# Add aux requests (second panda) for all requests that are marked as auxiliary
|
||||
for i in range(len(self.requests)):
|
||||
if self.requests[i].auxiliary:
|
||||
new_request = copy.deepcopy(self.requests[i])
|
||||
new_request.bus += 4
|
||||
self.requests.append(new_request)
|
||||
|
||||
def get_all_ecus(self, offline_fw_versions: OfflineFwVersions,
|
||||
include_extra_ecus: bool = True) -> set[EcuAddrSubAddr]:
|
||||
# Add ecus in database + extra ecus
|
||||
brand_ecus = {ecu for ecus in offline_fw_versions.values() for ecu in ecus}
|
||||
|
||||
if include_extra_ecus:
|
||||
brand_ecus |= set(self.extra_ecus)
|
||||
|
||||
return brand_ecus
|
||||
331
artifacts/package_sources/iqdbc/iqdbc/car/fw_versions.py
Normal file
331
artifacts/package_sources/iqdbc/iqdbc/car/fw_versions.py
Normal file
@@ -0,0 +1,331 @@
|
||||
from collections import defaultdict
|
||||
from collections.abc import Callable, Iterator
|
||||
from typing import Protocol, TypeVar
|
||||
|
||||
from tqdm import tqdm
|
||||
|
||||
from iqdbc.car import uds
|
||||
from iqdbc.car.can_definitions import CanRecvCallable, CanSendCallable
|
||||
from iqdbc.car.carlog import carlog
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.car.ecu_addrs import get_ecu_addrs
|
||||
from iqdbc.car.fingerprints import FW_VERSIONS
|
||||
from iqdbc.car.fw_query_definitions import ESSENTIAL_ECUS, AddrType, EcuAddrBusType, FwQueryConfig, LiveFwVersions, OfflineFwVersions
|
||||
from iqdbc.car.interfaces import get_interface_attr
|
||||
from iqdbc.car.isotp_parallel_query import IsoTpParallelQuery
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
FUZZY_EXCLUDE_ECUS = [Ecu.fwdCamera, Ecu.fwdRadar, Ecu.eps, Ecu.debug]
|
||||
|
||||
FW_QUERY_CONFIGS: dict[str, FwQueryConfig] = get_interface_attr('FW_QUERY_CONFIG', ignore_none=True)
|
||||
VERSIONS = get_interface_attr('FW_VERSIONS', ignore_none=True)
|
||||
|
||||
MODEL_TO_BRAND = {c: b for b, e in VERSIONS.items() for c in e}
|
||||
REQUESTS = [(brand, config, r) for brand, config in FW_QUERY_CONFIGS.items() for r in config.requests]
|
||||
|
||||
T = TypeVar('T')
|
||||
ObdCallback = Callable[[bool], None]
|
||||
|
||||
|
||||
def chunks(l: list[T], n: int = 128) -> Iterator[list[T]]:
|
||||
for i in range(0, len(l), n):
|
||||
yield l[i:i + n]
|
||||
|
||||
|
||||
def is_brand(brand: str, filter_brand: str | None) -> bool:
|
||||
"""Returns if brand matches filter_brand or no brand filter is specified"""
|
||||
return filter_brand is None or brand == filter_brand
|
||||
|
||||
|
||||
def build_fw_dict(fw_versions: list[CarParams.CarFw], filter_brand: str | None = None) -> dict[AddrType, set[bytes]]:
|
||||
fw_versions_dict: defaultdict[AddrType, set[bytes]] = defaultdict(set)
|
||||
for fw in fw_versions:
|
||||
if is_brand(fw.brand, filter_brand) and not fw.logging:
|
||||
sub_addr = fw.subAddress if fw.subAddress != 0 else None
|
||||
fw_versions_dict[(fw.address, sub_addr)].add(fw.fwVersion)
|
||||
return dict(fw_versions_dict)
|
||||
|
||||
|
||||
class MatchFwToCar(Protocol):
|
||||
def __call__(self, live_fw_versions: LiveFwVersions, match_brand: str | None = None, log: bool = True) -> set[str]:
|
||||
...
|
||||
|
||||
|
||||
def match_fw_to_car_fuzzy(live_fw_versions: LiveFwVersions, match_brand: str | None = None, log: bool = True, exclude: str | None = None) -> set[str]:
|
||||
"""Do a fuzzy FW match. This function will return a match, and the number of firmware version
|
||||
that were matched uniquely to that specific car. If multiple ECUs uniquely match to different cars
|
||||
the match is rejected."""
|
||||
|
||||
# Build lookup table from (addr, sub_addr, fw) to list of candidate cars
|
||||
all_fw_versions = defaultdict(list)
|
||||
for candidate, fw_by_addr in FW_VERSIONS.items():
|
||||
if not is_brand(MODEL_TO_BRAND[candidate], match_brand):
|
||||
continue
|
||||
|
||||
if candidate == exclude:
|
||||
continue
|
||||
|
||||
for addr, fws in fw_by_addr.items():
|
||||
# These ECUs are known to be shared between models (EPS only between hybrid/ICE version)
|
||||
# Getting this exactly right isn't crucial, but excluding camera and radar makes it almost
|
||||
# impossible to get 3 matching versions, even if two models with shared parts are released at the same
|
||||
# time and only one is in our database.
|
||||
if addr[0] in FUZZY_EXCLUDE_ECUS:
|
||||
continue
|
||||
for f in fws:
|
||||
all_fw_versions[(addr[1], addr[2], f)].append(candidate)
|
||||
|
||||
matched_ecus = set()
|
||||
match: str | None = None
|
||||
for addr, versions in live_fw_versions.items():
|
||||
ecu_key = (addr[0], addr[1])
|
||||
for version in versions:
|
||||
# All cars that have this FW response on the specified address
|
||||
candidates = all_fw_versions[(*ecu_key, version)]
|
||||
|
||||
if len(candidates) == 1:
|
||||
matched_ecus.add(ecu_key)
|
||||
if match is None:
|
||||
match = candidates[0]
|
||||
# We uniquely matched two different cars. No fuzzy match possible
|
||||
elif match != candidates[0]:
|
||||
return set()
|
||||
|
||||
# Note that it is possible to match to a candidate without all its ECUs being present
|
||||
# if there are enough matches. FIXME: parameterize this or require all ECUs to exist like exact matching
|
||||
if match and len(matched_ecus) >= 2:
|
||||
if log:
|
||||
carlog.error(f"Fingerprinted {match} using fuzzy match. {len(matched_ecus)} matching ECUs")
|
||||
return {match}
|
||||
else:
|
||||
return set()
|
||||
|
||||
|
||||
def match_fw_to_car_exact(live_fw_versions: LiveFwVersions, match_brand: str | None = None,
|
||||
log: bool = True, extra_fw_versions: dict | None = None) -> set[str]:
|
||||
"""Do an exact FW match. Returns all cars that match the given
|
||||
FW versions for a list of "essential" ECUs. If an ECU is not considered
|
||||
essential the FW version can be missing to get a fingerprint, but if it's present it
|
||||
needs to match the database."""
|
||||
if extra_fw_versions is None:
|
||||
extra_fw_versions = {}
|
||||
|
||||
invalid = set()
|
||||
candidates = {c: f for c, f in FW_VERSIONS.items() if
|
||||
is_brand(MODEL_TO_BRAND[c], match_brand)}
|
||||
|
||||
for candidate, fws in candidates.items():
|
||||
config = FW_QUERY_CONFIGS[MODEL_TO_BRAND[candidate]]
|
||||
for ecu, expected_versions in fws.items():
|
||||
expected_versions = expected_versions + extra_fw_versions.get(candidate, {}).get(ecu, [])
|
||||
ecu_type = ecu[0]
|
||||
addr = ecu[1:]
|
||||
|
||||
found_versions = live_fw_versions.get(addr, set())
|
||||
if not len(found_versions):
|
||||
# Some models can sometimes miss an ecu, or show on two different addresses
|
||||
# FIXME: this logic can be improved to be more specific, should require one of the two addresses
|
||||
if candidate in config.non_essential_ecus.get(ecu_type, []):
|
||||
continue
|
||||
|
||||
# Ignore non essential ecus
|
||||
if ecu_type not in ESSENTIAL_ECUS:
|
||||
continue
|
||||
|
||||
# Virtual debug ecu doesn't need to match the database
|
||||
if ecu_type == Ecu.debug:
|
||||
continue
|
||||
|
||||
if not any(found_version in expected_versions for found_version in found_versions):
|
||||
invalid.add(candidate)
|
||||
break
|
||||
|
||||
return set(candidates.keys()) - invalid
|
||||
|
||||
|
||||
def match_fw_to_car(fw_versions: list[CarParams.CarFw], vin: str, allow_exact: bool = True,
|
||||
allow_fuzzy: bool = True, log: bool = True) -> tuple[bool, set[str]]:
|
||||
# Try exact matching first
|
||||
exact_matches: list[tuple[bool, MatchFwToCar]] = []
|
||||
if allow_exact:
|
||||
exact_matches = [(True, match_fw_to_car_exact)]
|
||||
if allow_fuzzy:
|
||||
exact_matches.append((False, match_fw_to_car_fuzzy))
|
||||
|
||||
for exact_match, match_func in exact_matches:
|
||||
# For each brand, attempt to fingerprint using all FW returned from its queries
|
||||
matches: set[str] = set()
|
||||
for brand in VERSIONS.keys():
|
||||
fw_versions_dict = build_fw_dict(fw_versions, filter_brand=brand)
|
||||
config = FW_QUERY_CONFIGS[brand]
|
||||
brand_matches = match_func(fw_versions_dict, match_brand=brand, log=log)
|
||||
if exact_match and len(brand_matches) > 1 and config.refine_fw_matches is not None:
|
||||
brand_matches = config.refine_fw_matches(brand_matches, vin)
|
||||
matches |= brand_matches
|
||||
|
||||
# If specified and no matches so far, fall back to brand's fuzzy fingerprinting function
|
||||
if not exact_match and not len(matches) and config.match_fw_to_car_fuzzy is not None:
|
||||
matches |= config.match_fw_to_car_fuzzy(fw_versions_dict, vin, VERSIONS[brand])
|
||||
|
||||
if len(matches):
|
||||
return exact_match, matches
|
||||
|
||||
return True, set()
|
||||
|
||||
|
||||
def get_present_ecus(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, num_pandas: int = 1) -> set[EcuAddrBusType]:
|
||||
# queries are split by OBD multiplexing mode
|
||||
queries: dict[bool, list[list[EcuAddrBusType]]] = {True: [], False: []}
|
||||
parallel_queries: dict[bool, list[EcuAddrBusType]] = {True: [], False: []}
|
||||
responses: set[EcuAddrBusType] = set()
|
||||
|
||||
for brand, config, r in REQUESTS:
|
||||
# Skip query if no panda available
|
||||
if r.bus > num_pandas * 4 - 1:
|
||||
continue
|
||||
|
||||
for ecu_type, addr, sub_addr in config.get_all_ecus(VERSIONS[brand]):
|
||||
# Only query ecus in whitelist if whitelist is not empty
|
||||
if len(r.whitelist_ecus) == 0 or ecu_type in r.whitelist_ecus:
|
||||
a = (addr, sub_addr, r.bus)
|
||||
# Build set of queries
|
||||
if sub_addr is None:
|
||||
if a not in parallel_queries[r.obd_multiplexing]:
|
||||
parallel_queries[r.obd_multiplexing].append(a)
|
||||
else: # subaddresses must be queried one by one
|
||||
if [a] not in queries[r.obd_multiplexing]:
|
||||
queries[r.obd_multiplexing].append([a])
|
||||
|
||||
# Build set of expected responses to filter
|
||||
response_addr = uds.get_rx_addr_for_tx_addr(addr, r.rx_offset)
|
||||
responses.add((response_addr, sub_addr, r.bus))
|
||||
|
||||
for obd_multiplexing in queries:
|
||||
queries[obd_multiplexing].insert(0, parallel_queries[obd_multiplexing])
|
||||
|
||||
ecu_responses = set()
|
||||
for obd_multiplexing in queries:
|
||||
set_obd_multiplexing(obd_multiplexing)
|
||||
for query in queries[obd_multiplexing]:
|
||||
ecu_responses.update(get_ecu_addrs(can_recv, can_send, set(query), responses, timeout=0.1))
|
||||
return ecu_responses
|
||||
|
||||
|
||||
def get_brand_ecu_matches(ecu_rx_addrs: set[EcuAddrBusType]) -> dict[str, list[bool]]:
|
||||
"""Returns dictionary of brands and matches with ECUs in their FW versions"""
|
||||
|
||||
brand_rx_addrs = {brand: set() for brand in FW_QUERY_CONFIGS}
|
||||
brand_matches = {brand: [] for brand, _, _ in REQUESTS}
|
||||
|
||||
# Since we can't know what request an ecu responded to, add matches for all possible rx offsets
|
||||
for brand, config, r in REQUESTS:
|
||||
for ecu in config.get_all_ecus(VERSIONS[brand]):
|
||||
if len(r.whitelist_ecus) == 0 or ecu[0] in r.whitelist_ecus:
|
||||
brand_rx_addrs[brand].add((uds.get_rx_addr_for_tx_addr(ecu[1], r.rx_offset), ecu[2]))
|
||||
|
||||
for brand, addrs in brand_rx_addrs.items():
|
||||
for addr in addrs:
|
||||
# TODO: check bus from request as well
|
||||
brand_matches[brand].append(addr in [addr[:2] for addr in ecu_rx_addrs])
|
||||
|
||||
return brand_matches
|
||||
|
||||
|
||||
def get_fw_versions_ordered(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, vin: str,
|
||||
ecu_rx_addrs: set[EcuAddrBusType], timeout: float = 0.1, num_pandas: int = 1, progress: bool = False) -> list[CarParams.CarFw]:
|
||||
"""Queries for FW versions ordering brands by likelihood, breaks when exact match is found"""
|
||||
|
||||
all_car_fw = []
|
||||
brand_matches = get_brand_ecu_matches(ecu_rx_addrs)
|
||||
|
||||
# Sort brands by number of matching ECUs first, then percentage of matching ECUs in the database
|
||||
# This allows brands with only one ECU to be queried first (e.g. Tesla)
|
||||
for brand in sorted(brand_matches, key=lambda b: (brand_matches[b].count(True), brand_matches[b].count(True) / len(brand_matches[b])), reverse=True):
|
||||
# Skip this brand if there are no matching present ECUs
|
||||
if True not in brand_matches[brand]:
|
||||
continue
|
||||
|
||||
car_fw = get_fw_versions(can_recv, can_send, set_obd_multiplexing, query_brand=brand, timeout=timeout, num_pandas=num_pandas, progress=progress)
|
||||
all_car_fw.extend(car_fw)
|
||||
|
||||
# If there is a match using this brand's FW alone, finish querying early
|
||||
_, matches = match_fw_to_car(car_fw, vin, log=False)
|
||||
if len(matches) == 1:
|
||||
break
|
||||
|
||||
return all_car_fw
|
||||
|
||||
|
||||
def get_fw_versions(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, query_brand: str | None = None,
|
||||
extra: OfflineFwVersions | None = None, timeout: float = 0.1, num_pandas: int = 1, progress: bool = False) -> list[CarParams.CarFw]:
|
||||
versions = VERSIONS.copy()
|
||||
|
||||
if query_brand is not None:
|
||||
versions = {query_brand: versions[query_brand]}
|
||||
|
||||
if extra is not None:
|
||||
versions.update(extra)
|
||||
|
||||
# Extract ECU addresses to query from fingerprints
|
||||
# ECUs using a subaddress need be queried one by one, the rest can be done in parallel
|
||||
addrs = []
|
||||
parallel_addrs = []
|
||||
ecu_types = {}
|
||||
|
||||
for brand, brand_versions in versions.items():
|
||||
config = FW_QUERY_CONFIGS[brand]
|
||||
for ecu_type, addr, sub_addr in config.get_all_ecus(brand_versions):
|
||||
a = (brand, addr, sub_addr)
|
||||
if a not in ecu_types:
|
||||
ecu_types[a] = ecu_type
|
||||
|
||||
if sub_addr is None:
|
||||
if a not in parallel_addrs:
|
||||
parallel_addrs.append(a)
|
||||
else:
|
||||
if [a] not in addrs:
|
||||
addrs.append([a])
|
||||
|
||||
addrs.insert(0, parallel_addrs)
|
||||
|
||||
# Get versions and build capnp list to put into CarParams
|
||||
car_fw = []
|
||||
requests = [(brand, config, r) for brand, config, r in REQUESTS if is_brand(brand, query_brand)]
|
||||
for addr_group in tqdm(addrs, disable=not progress): # split by subaddr, if any
|
||||
for addr_chunk in chunks(addr_group):
|
||||
for brand, config, r in requests:
|
||||
# Skip query if no panda available
|
||||
if r.bus > num_pandas * 4 - 1:
|
||||
continue
|
||||
|
||||
# Toggle OBD multiplexing for each request
|
||||
if r.bus % 4 == 1:
|
||||
set_obd_multiplexing(r.obd_multiplexing)
|
||||
|
||||
try:
|
||||
query_addrs = [(a, s) for (b, a, s) in addr_chunk if b in (brand, 'any') and
|
||||
(len(r.whitelist_ecus) == 0 or ecu_types[(b, a, s)] in r.whitelist_ecus)]
|
||||
|
||||
if query_addrs:
|
||||
query = IsoTpParallelQuery(can_send, can_recv, r.bus, query_addrs, r.request, r.response, r.rx_offset)
|
||||
for (tx_addr, sub_addr), version in query.get_data(timeout).items():
|
||||
f = CarParams.CarFw()
|
||||
|
||||
f.ecu = ecu_types.get((brand, tx_addr, sub_addr), Ecu.unknown)
|
||||
f.fwVersion = version
|
||||
f.address = tx_addr
|
||||
f.responseAddress = uds.get_rx_addr_for_tx_addr(tx_addr, r.rx_offset)
|
||||
f.request = r.request
|
||||
f.brand = brand
|
||||
f.bus = r.bus
|
||||
f.logging = r.logging or (f.ecu, tx_addr, sub_addr) in config.extra_ecus
|
||||
f.obdMultiplexing = r.obd_multiplexing
|
||||
|
||||
if sub_addr is not None:
|
||||
f.subAddress = sub_addr
|
||||
|
||||
car_fw.append(f)
|
||||
except Exception:
|
||||
carlog.exception("FW query exception")
|
||||
|
||||
return car_fw
|
||||
163
artifacts/package_sources/iqdbc/iqdbc/car/gm/carcontroller.py
Normal file
163
artifacts/package_sources/iqdbc/iqdbc/car/gm/carcontroller.py
Normal file
@@ -0,0 +1,163 @@
|
||||
import numpy as np
|
||||
from iqdbc.can import CANPacker
|
||||
from iqdbc.car import Bus, DT_CTRL, structs
|
||||
from iqdbc.car.lateral import apply_driver_steer_torque_limits
|
||||
from iqdbc.car.gm import gmcan
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons
|
||||
from iqdbc.car.interfaces import CarControllerBase
|
||||
|
||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||
NetworkLocation = structs.CarParams.NetworkLocation
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
# Camera cancels up to 0.1s after brake is pressed, ECM allows 0.5s
|
||||
CAMERA_CANCEL_DELAY_FRAMES = 10
|
||||
# Enforce a minimum interval between steering messages to avoid a fault
|
||||
MIN_STEER_MSG_INTERVAL_MS = 15
|
||||
|
||||
|
||||
class CarController(CarControllerBase):
|
||||
def __init__(self, dbc_names, CP, CP_IQ):
|
||||
super().__init__(dbc_names, CP, CP_IQ)
|
||||
self.start_time = 0.
|
||||
self.apply_torque_last = 0
|
||||
self.apply_gas = 0
|
||||
self.apply_brake = 0
|
||||
self.last_steer_frame = 0
|
||||
self.last_button_frame = 0
|
||||
self.cancel_counter = 0
|
||||
|
||||
self.lka_steering_cmd_counter = 0
|
||||
self.lka_icon_status_last = (False, False)
|
||||
|
||||
self.params = CarControllerParams(self.CP)
|
||||
|
||||
self.packer_pt = CANPacker(DBC[self.CP.carFingerprint][Bus.pt])
|
||||
self.packer_obj = CANPacker(DBC[self.CP.carFingerprint][Bus.radar])
|
||||
self.packer_ch = CANPacker(DBC[self.CP.carFingerprint][Bus.chassis])
|
||||
|
||||
def update(self, CC, CC_IQ, CS, now_nanos):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
hud_alert = hud_control.visualAlert
|
||||
hud_v_cruise = hud_control.setSpeed
|
||||
if hud_v_cruise > 70:
|
||||
hud_v_cruise = 0
|
||||
|
||||
# Send CAN commands.
|
||||
can_sends = []
|
||||
|
||||
# Steering (Active: 50Hz, inactive: 10Hz)
|
||||
steer_step = self.params.STEER_STEP if CC.latActive else self.params.INACTIVE_STEER_STEP
|
||||
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
# Also send at 50Hz:
|
||||
# - on startup, first few msgs are blocked
|
||||
# - until we're in sync with camera so counters align when relay closes, preventing a fault.
|
||||
# openpilot can subtly drift, so this is activated throughout a drive to stay synced
|
||||
out_of_sync = self.lka_steering_cmd_counter % 4 != (CS.cam_lka_steering_cmd_counter + 1) % 4
|
||||
if CS.loopback_lka_steering_cmd_ts_nanos == 0 or out_of_sync:
|
||||
steer_step = self.params.STEER_STEP
|
||||
|
||||
self.lka_steering_cmd_counter += 1 if CS.loopback_lka_steering_cmd_updated else 0
|
||||
|
||||
# Avoid GM EPS faults when transmitting messages too close together: skip this transmit if we
|
||||
# received the ASCMLKASteeringCmd loopback confirmation too recently
|
||||
last_lka_steer_msg_ms = (now_nanos - CS.loopback_lka_steering_cmd_ts_nanos) * 1e-6
|
||||
if (self.frame - self.last_steer_frame) >= steer_step and last_lka_steer_msg_ms > MIN_STEER_MSG_INTERVAL_MS:
|
||||
# Initialize ASCMLKASteeringCmd counter using the camera until we get a msg on the bus
|
||||
if CS.loopback_lka_steering_cmd_ts_nanos == 0:
|
||||
self.lka_steering_cmd_counter = CS.pt_lka_steering_cmd_counter + 1
|
||||
|
||||
if CC.latActive:
|
||||
new_torque = int(round(actuators.torque * self.params.STEER_MAX))
|
||||
apply_torque = apply_driver_steer_torque_limits(new_torque, self.apply_torque_last, CS.out.steeringTorque, self.params)
|
||||
else:
|
||||
apply_torque = 0
|
||||
|
||||
self.last_steer_frame = self.frame
|
||||
self.apply_torque_last = apply_torque
|
||||
idx = self.lka_steering_cmd_counter % 4
|
||||
can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_torque, idx, CC.latActive))
|
||||
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
# Gas/regen, brakes, and UI commands - all at 25Hz
|
||||
if self.frame % 4 == 0:
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
if not CC.longActive:
|
||||
# ASCM sends max regen when not enabled
|
||||
self.apply_gas = self.params.INACTIVE_REGEN
|
||||
self.apply_brake = 0
|
||||
else:
|
||||
self.apply_gas = float(np.interp(actuators.accel, self.params.GAS_LOOKUP_BP, self.params.GAS_LOOKUP_V))
|
||||
self.apply_brake = int(round(np.interp(actuators.accel, self.params.BRAKE_LOOKUP_BP, self.params.BRAKE_LOOKUP_V)))
|
||||
# Don't allow any gas above inactive regen while stopping
|
||||
# FIXME: brakes aren't applied immediately when enabling at a stop
|
||||
if stopping:
|
||||
self.apply_gas = self.params.INACTIVE_REGEN
|
||||
|
||||
idx = (self.frame // 4) % 4
|
||||
|
||||
at_full_stop = CC.longActive and CS.out.standstill
|
||||
near_stop = CC.longActive and (abs(CS.out.vEgo) < self.params.NEAR_STOP_BRAKE_PHASE)
|
||||
friction_brake_bus = CanBus.CHASSIS
|
||||
# GM Camera exceptions
|
||||
# TODO: can we always check the longControlState?
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
at_full_stop = at_full_stop and stopping
|
||||
friction_brake_bus = CanBus.POWERTRAIN
|
||||
|
||||
# GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation
|
||||
can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, CC.enabled, at_full_stop))
|
||||
can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, friction_brake_bus, self.apply_brake,
|
||||
idx, CC.enabled, near_stop, at_full_stop, self.CP))
|
||||
|
||||
# Send dashboard UI commands (ACC status)
|
||||
send_fcw = hud_alert == VisualAlert.fcw
|
||||
can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, CC.enabled,
|
||||
hud_v_cruise * CV.MS_TO_KPH, hud_control, send_fcw))
|
||||
|
||||
# Radar needs to know current speed and yaw rate (50hz),
|
||||
# and that ADAS is alive (10hz)
|
||||
if not self.CP.radarUnavailable:
|
||||
tt = self.frame * DT_CTRL
|
||||
time_and_headlights_step = 10
|
||||
if self.frame % time_and_headlights_step == 0:
|
||||
idx = (self.frame // time_and_headlights_step) % 4
|
||||
can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx))
|
||||
can_sends.append(gmcan.create_adas_headlights_status(self.packer_obj, CanBus.OBSTACLE))
|
||||
|
||||
speed_and_accelerometer_step = 2
|
||||
if self.frame % speed_and_accelerometer_step == 0:
|
||||
idx = (self.frame // speed_and_accelerometer_step) % 4
|
||||
can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx))
|
||||
can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, abs(CS.out.vEgo), idx))
|
||||
|
||||
if self.CP.networkLocation == NetworkLocation.gateway and self.frame % self.params.ADAS_KEEPALIVE_STEP == 0:
|
||||
can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN)
|
||||
|
||||
else:
|
||||
# While car is braking, cancel button causes ECM to enter a soft disable state with a fault status.
|
||||
# A delayed cancellation allows camera to cancel and avoids a fault when user depresses brake quickly
|
||||
self.cancel_counter = self.cancel_counter + 1 if CC.cruiseControl.cancel else 0
|
||||
|
||||
# Stock longitudinal, integrated at camera
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
|
||||
if self.cancel_counter > CAMERA_CANCEL_DELAY_FRAMES:
|
||||
self.last_button_frame = self.frame
|
||||
can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.CAMERA, CS.buttons_counter, CruiseButtons.CANCEL))
|
||||
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
# Silence "Take Steering" alert sent by camera, forward PSCMStatus with HandsOffSWlDetectionStatus=1
|
||||
if self.frame % 10 == 0:
|
||||
can_sends.append(gmcan.create_pscm_status(self.packer_pt, CanBus.CAMERA, CS.pscm_status))
|
||||
|
||||
new_actuators = actuators.as_builder()
|
||||
new_actuators.torque = self.apply_torque_last / self.params.STEER_MAX
|
||||
new_actuators.torqueOutputCan = self.apply_torque_last
|
||||
new_actuators.gas = self.apply_gas
|
||||
new_actuators.brake = self.apply_brake
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
186
artifacts/package_sources/iqdbc/iqdbc/car/gm/carstate.py
Normal file
186
artifacts/package_sources/iqdbc/iqdbc/car/gm/carstate.py
Normal file
@@ -0,0 +1,186 @@
|
||||
import copy
|
||||
from iqdbc.can import CANDefine, CANParser
|
||||
from iqdbc.car import Bus, create_button_events, structs
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.interfaces import CarStateBase
|
||||
from iqdbc.car.gm.values import DBC, AccState, CruiseButtons, STEER_THRESHOLD, SDGM_CAR, ALT_ACCS
|
||||
|
||||
from iqdbc.lvbs.car.gm.iq_carstate import IQCarState
|
||||
from iqdbc.lvbs.car.gm.iq_values import GMFlagsIQ
|
||||
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
TransmissionType = structs.CarParams.TransmissionType
|
||||
NetworkLocation = structs.CarParams.NetworkLocation
|
||||
|
||||
STANDSTILL_THRESHOLD = 10 * 0.0311
|
||||
|
||||
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
|
||||
CruiseButtons.MAIN: ButtonType.mainCruise, CruiseButtons.CANCEL: ButtonType.cancel}
|
||||
|
||||
|
||||
class CarState(CarStateBase, IQCarState):
|
||||
def __init__(self, CP, CP_IQ):
|
||||
CarStateBase.__init__(self, CP, CP_IQ)
|
||||
IQCarState.__init__(self, CP, CP_IQ)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
|
||||
self.shifter_values = can_define.dv["ECMPRDNL2"]["PRNDL2"]
|
||||
self.cluster_speed_hyst_gap = CV.KPH_TO_MS / 2.
|
||||
self.cluster_min_speed = CV.KPH_TO_MS / 2.
|
||||
|
||||
self.loopback_lka_steering_cmd_updated = False
|
||||
self.loopback_lka_steering_cmd_ts_nanos = 0
|
||||
self.pt_lka_steering_cmd_counter = 0
|
||||
self.cam_lka_steering_cmd_counter = 0
|
||||
self.buttons_counter = 0
|
||||
|
||||
self.distance_button = 0
|
||||
|
||||
def update_button_enable(self, buttonEvents: list[structs.CarState.ButtonEvent]):
|
||||
if not self.CP.pcmCruise:
|
||||
for b in buttonEvents:
|
||||
# The ECM allows enabling on falling edge of set, but only rising edge of resume
|
||||
if (b.type == ButtonType.accelCruise and b.pressed) or \
|
||||
(b.type == ButtonType.decelCruise and not b.pressed):
|
||||
return True
|
||||
return False
|
||||
|
||||
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
|
||||
pt_cp = can_parsers[Bus.pt]
|
||||
cam_cp = can_parsers[Bus.cam]
|
||||
loopback_cp = can_parsers[Bus.loopback]
|
||||
|
||||
ret = structs.CarState()
|
||||
ret_iq = structs.IQCarState()
|
||||
|
||||
prev_cruise_buttons = self.cruise_buttons
|
||||
prev_distance_button = self.distance_button
|
||||
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
|
||||
self.distance_button = pt_cp.vl["ASCMSteeringButton"]["DistanceButton"]
|
||||
self.buttons_counter = pt_cp.vl["ASCMSteeringButton"]["RollingCounter"]
|
||||
self.pscm_status = copy.copy(pt_cp.vl["PSCMStatus"])
|
||||
|
||||
# Variables used for avoiding LKAS faults
|
||||
self.loopback_lka_steering_cmd_updated = len(loopback_cp.vl_all["ASCMLKASteeringCmd"]["RollingCounter"]) > 0
|
||||
if self.loopback_lka_steering_cmd_updated:
|
||||
self.loopback_lka_steering_cmd_ts_nanos = loopback_cp.ts_nanos["ASCMLKASteeringCmd"]["RollingCounter"]
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
self.pt_lka_steering_cmd_counter = pt_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
|
||||
self.cam_lka_steering_cmd_counter = cam_cp.vl["ASCMLKASteeringCmd"]["RollingCounter"]
|
||||
|
||||
# This is to avoid a fault where you engage while still moving backwards after shifting to D.
|
||||
# An Equinox has been seen with an unsupported status (3), so only check if either wheel is in reverse (2)
|
||||
left_whl_sign = -1 if pt_cp.vl["EBCMWheelSpdRear"]["RLWheelDir"] == 2 else 1
|
||||
right_whl_sign = -1 if pt_cp.vl["EBCMWheelSpdRear"]["RRWheelDir"] == 2 else 1
|
||||
self.parse_wheel_speeds(ret,
|
||||
left_whl_sign * pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"],
|
||||
right_whl_sign * pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"],
|
||||
left_whl_sign * pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"],
|
||||
right_whl_sign * pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"],
|
||||
)
|
||||
# sample rear wheel speeds to match the safety which only uses the rear CAN message
|
||||
# standstill=True if ECM allows engagement with brake
|
||||
ret.standstill = abs(pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"]) <= STANDSTILL_THRESHOLD and \
|
||||
abs(pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"]) <= STANDSTILL_THRESHOLD
|
||||
|
||||
if pt_cp.vl["ECMPRDNL2"]["ManualMode"] == 1:
|
||||
ret.gearShifter = self.parse_gear_shifter("T")
|
||||
else:
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL2"]["PRNDL2"], None))
|
||||
|
||||
ret.brake = pt_cp.vl["ECMAcceleratorPos"]["BrakePedalPos"]
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
ret.brakePressed = pt_cp.vl["ECMEngineStatus"]["BrakePressed"] != 0
|
||||
else:
|
||||
# Some Volt 2016-17 have loose brake pedal push rod retainers which causes the ECM to believe
|
||||
# that the brake is being intermittently pressed without user interaction.
|
||||
# To avoid a cruise fault we need to use a conservative brake position threshold
|
||||
# https://static.nhtsa.gov/odi/tsbs/2017/MC-10137629-9999.pdf
|
||||
ret.brakePressed = ret.brake >= 8
|
||||
|
||||
# Regen braking is braking
|
||||
if self.CP.transmissionType == TransmissionType.direct:
|
||||
ret.regenBraking = pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0
|
||||
|
||||
ret.gasPressed = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. > 1e-5
|
||||
|
||||
ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelAngle"]
|
||||
ret.steeringRateDeg = pt_cp.vl["PSCMSteeringAngle"]["SteeringWheelRate"]
|
||||
ret.steeringTorque = pt_cp.vl["PSCMStatus"]["LKADriverAppldTrq"]
|
||||
ret.steeringTorqueEps = pt_cp.vl["PSCMStatus"]["LKATorqueDelivered"]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
|
||||
|
||||
# 0 inactive, 1 active, 2 temporarily limited, 3 failed
|
||||
self.lkas_status = pt_cp.vl["PSCMStatus"]["LKATorqueDeliveredStatus"]
|
||||
ret.steerFaultTemporary = self.lkas_status == 2
|
||||
ret.steerFaultPermanent = self.lkas_status == 3
|
||||
|
||||
# 1 - open, 0 - closed
|
||||
ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]["FrontLeftDoor"] == 1 or
|
||||
pt_cp.vl["BCMDoorBeltStatus"]["FrontRightDoor"] == 1 or
|
||||
pt_cp.vl["BCMDoorBeltStatus"]["RearLeftDoor"] == 1 or
|
||||
pt_cp.vl["BCMDoorBeltStatus"]["RearRightDoor"] == 1)
|
||||
|
||||
# 1 - latched
|
||||
ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]["LeftSeatBelt"] == 0
|
||||
ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 1
|
||||
ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2
|
||||
|
||||
ret.parkingBrake = pt_cp.vl["BCMGeneralPlatformStatus"]["ParkBrakeSwActive"] == 1
|
||||
ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0
|
||||
ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1
|
||||
ret.accFaulted = (pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.FAULTED or
|
||||
pt_cp.vl["EBCMFrictionBrakeStatus"]["FrictionBrakeUnavailable"] == 1)
|
||||
|
||||
ret.cruiseState.enabled = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] != AccState.OFF
|
||||
ret.cruiseState.standstill = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] == AccState.STANDSTILL
|
||||
if self.CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
if self.CP.carFingerprint not in ALT_ACCS and not self.CP_IQ.flags & GMFlagsIQ.NON_ACC:
|
||||
ret.cruiseState.speed = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCSpeedSetpoint"] * CV.KPH_TO_MS
|
||||
# This FCW signal only works for SDGM cars. CAM cars send FCW on GMLAN but this bit is always 0 for them
|
||||
ret.stockFcw = cam_cp.vl["ASCMActiveCruiseControlStatus"]["FCWAlert"] != 0
|
||||
if self.CP.pcmCruise:
|
||||
# openpilot controls nonAdaptive when not pcmCruise
|
||||
ret.cruiseState.nonAdaptive = cam_cp.vl["ASCMActiveCruiseControlStatus"]["ACCCruiseState"] not in (2, 3)
|
||||
else:
|
||||
ret.cruiseState.speed = pt_cp.vl["ECMCruiseControl"]["CruiseSetSpeed"] * CV.KPH_TO_MS
|
||||
|
||||
if self.CP.carFingerprint not in SDGM_CAR:
|
||||
ret.stockAeb = cam_cp.vl["AEBCmd"]["AEBCmdActive"] != 0
|
||||
|
||||
if self.CP.enableBsm:
|
||||
ret.leftBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["LeftBSM"] == 1
|
||||
ret.rightBlindspot = pt_cp.vl["BCMBlindSpotMonitor"]["RightBSM"] == 1
|
||||
|
||||
# Don't add event if transitioning from INIT, unless it's to an actual button
|
||||
if self.cruise_buttons != CruiseButtons.UNPRESS or prev_cruise_buttons != CruiseButtons.INIT:
|
||||
ret.buttonEvents = [
|
||||
*create_button_events(self.cruise_buttons, prev_cruise_buttons, BUTTONS_DICT,
|
||||
unpressed_btn=CruiseButtons.UNPRESS),
|
||||
*create_button_events(self.distance_button, prev_distance_button,
|
||||
{1: ButtonType.gapAdjustCruise})
|
||||
]
|
||||
|
||||
if ret.vEgo < self.CP.minSteerSpeed:
|
||||
ret.lowSpeedAlert = True
|
||||
|
||||
IQCarState.update(self, ret, can_parsers)
|
||||
|
||||
return ret, ret_iq
|
||||
|
||||
@staticmethod
|
||||
def get_can_parsers(CP, CP_IQ):
|
||||
pt_messages = []
|
||||
if CP.networkLocation == NetworkLocation.fwdCamera:
|
||||
pt_messages += [
|
||||
("ASCMLKASteeringCmd", float('nan')),
|
||||
]
|
||||
|
||||
loopback_messages = [
|
||||
("ASCMLKASteeringCmd", float('nan')),
|
||||
]
|
||||
|
||||
return {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, 0),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], 2),
|
||||
Bus.loopback: CANParser(DBC[CP.carFingerprint][Bus.pt], loopback_messages, 128),
|
||||
}
|
||||
84
artifacts/package_sources/iqdbc/iqdbc/car/gm/fingerprints.py
Normal file
84
artifacts/package_sources/iqdbc/iqdbc/car/gm/fingerprints.py
Normal file
@@ -0,0 +1,84 @@
|
||||
# ruff: noqa: E501
|
||||
""" AUTO-FORMATTED USING iqdbc/car/debug/format_fingerprints.py, EDIT STRUCTURE THERE."""
|
||||
from iqdbc.car.gm.values import CAR
|
||||
from iqdbc.lvbs.car.iq_fingerprints import extend_fingerprints
|
||||
from iqdbc.lvbs.car.gm.iq_fingerprints import FINGERPRINTS_EXT
|
||||
|
||||
# Trailblazer also matches as a SILVERADO, TODO: split with fw versions
|
||||
# FIXME: There are Equinox users with different message lengths, specifically 304 and 320
|
||||
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.HOLDEN_ASTRA: [{
|
||||
190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 401: 8, 413: 8, 417: 8, 419: 8, 422: 1, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 458: 5, 479: 8, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 647: 5, 707: 8, 715: 8, 723: 8, 753: 5, 761: 7, 806: 1, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1009: 8, 1011: 6, 1017: 8, 1019: 3, 1020: 8, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 8, 1280: 4, 1300: 8, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1908: 7, 1912: 7, 1919: 7
|
||||
}],
|
||||
CAR.CHEVROLET_VOLT: [{
|
||||
170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 289: 8, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 647: 3, 707: 8, 711: 6, 715: 8, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 961: 8, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1928: 7, 2016: 8, 2020: 8, 2024: 8, 2028: 8
|
||||
},
|
||||
{
|
||||
170: 8, 171: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 288: 5, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 4, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 577: 8, 578: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 715: 8, 717: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1516: 8, 1601: 8, 1618: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7, 1930: 7, 2016: 8, 2018: 8, 2020: 8, 2024: 8, 2028: 8
|
||||
},
|
||||
{
|
||||
170: 8, 171: 8, 189: 7, 190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 209: 7, 211: 2, 241: 6, 288: 5, 289: 1, 290: 1, 298: 2, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 368: 8, 381: 2, 384: 8, 386: 5, 388: 8, 389: 2, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 458: 8, 479: 3, 481: 7, 485: 8, 489: 5, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 3, 508: 8, 512: 3, 528: 4, 530: 8, 532: 6, 537: 5, 539: 8, 542: 7, 546: 7, 550: 8, 554: 3, 558: 8, 560: 6, 562: 4, 563: 5, 564: 5, 565: 5, 566: 5, 567: 3, 568: 1, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 761: 7, 810: 8, 821: 4, 823: 7, 832: 8, 840: 5, 842: 5, 844: 8, 853: 8, 866: 4, 961: 8, 967: 4, 969: 8, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 5, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1273: 3, 1275: 3, 1280: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1922: 7, 1927: 7
|
||||
}],
|
||||
CAR.BUICK_LACROSSE: [{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 353: 3, 381: 6, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 503: 1, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 5, 707: 8, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 872: 1, 882: 8, 890: 1, 892: 2, 893: 1, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1022: 1, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1243: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1913: 7, 1914: 7, 1916: 7, 1918: 7, 1919: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8, 2026: 8
|
||||
}],
|
||||
CAR.BUICK_REGAL: [{
|
||||
190: 8, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 8, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 8, 419: 8, 422: 4, 426: 8, 431: 8, 442: 8, 451: 8, 452: 8, 453: 8, 455: 7, 456: 8, 463: 3, 479: 8, 481: 7, 485: 8, 487: 8, 489: 8, 495: 8, 497: 8, 499: 3, 500: 8, 501: 8, 508: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 569: 3, 573: 1, 577: 8, 578: 8, 579: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 884: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 8, 969: 8, 977: 8, 979: 8, 985: 8, 1001: 8, 1005: 6, 1009: 8, 1011: 8, 1013: 3, 1017: 8, 1020: 8, 1024: 8, 1025: 8, 1026: 8, 1027: 8, 1028: 8, 1029: 8, 1030: 8, 1031: 8, 1032: 2, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 8, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 8, 1263: 8, 1265: 8, 1267: 8, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1602: 8, 1603: 7, 1611: 8, 1618: 8, 1906: 8, 1907: 7, 1912: 7, 1914: 7, 1916: 7, 1919: 7, 1930: 7, 2016: 8, 2018: 8, 2019: 8, 2024: 8, 2026: 8
|
||||
}],
|
||||
CAR.CADILLAC_ATS: [{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 368: 3, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 462: 4, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 491: 2, 493: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 723: 2, 753: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 882: 8, 890: 1, 892: 2, 893: 2, 894: 1, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1241: 3, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1904: 7, 1906: 7, 1907: 7, 1912: 7, 1916: 7, 1917: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.CHEVROLET_MALIBU: [{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 456: 8, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 528: 5, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1013: 3, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1930: 7, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.GMC_ACADIA: [{
|
||||
190: 6, 192: 5, 193: 8, 197: 8, 199: 4, 201: 6, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 1, 290: 1, 298: 8, 304: 8, 309: 8, 313: 8, 320: 8, 322: 7, 328: 1, 352: 7, 368: 8, 381: 8, 384: 8, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 458: 8, 460: 4, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 5, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 512: 3, 530: 8, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 568: 2, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 801: 8, 803: 8, 804: 3, 805: 8, 832: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1003: 5, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7
|
||||
},
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 313: 8, 320: 3, 322: 7, 328: 1, 338: 6, 340: 6, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 393: 8, 398: 8, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1225: 8, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1417: 8, 1601: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1919: 7, 1920: 7, 1930: 7, 2016: 8, 2024: 8
|
||||
}],
|
||||
CAR.CADILLAC_ESCALADE: [{
|
||||
170: 8, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 322: 7, 328: 1, 352: 5, 381: 6, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 407: 4, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 460: 5, 462: 4, 463: 3, 479: 3, 481: 7, 485: 8, 487: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 534: 2, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 573: 1, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 761: 7, 801: 8, 804: 3, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 6, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1417: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1914: 7, 1917: 7, 1918: 7, 1919: 7, 1920: 7, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 2001: 8, 2017: 8, 2018: 8, 2020: 8, 2026: 8
|
||||
}],
|
||||
CAR.CADILLAC_ESCALADE_ESV: [{
|
||||
309: 1, 848: 8, 849: 8, 850: 8, 851: 8, 852: 8, 853: 8, 854: 3, 1056: 6, 1057: 8, 1058: 8, 1059: 8, 1060: 8, 1061: 8, 1062: 8, 1063: 8, 1064: 8, 1065: 8, 1066: 8, 1067: 8, 1068: 8, 1120: 8, 1121: 8, 1122: 8, 1123: 8, 1124: 8, 1125: 8, 1126: 8, 1127: 8, 1128: 8, 1129: 8, 1130: 8, 1131: 8, 1132: 8, 1133: 8, 1134: 8, 1135: 8, 1136: 8, 1137: 8, 1138: 8, 1139: 8, 1140: 8, 1141: 8, 1142: 8, 1143: 8, 1146: 8, 1147: 8, 1148: 8, 1149: 8, 1150: 8, 1151: 8, 1216: 8, 1217: 8, 1218: 8, 1219: 8, 1220: 8, 1221: 8, 1222: 8, 1223: 8, 1224: 8, 1225: 8, 1226: 8, 1232: 8, 1233: 8, 1234: 8, 1235: 8, 1236: 8, 1237: 8, 1238: 8, 1239: 8, 1240: 8, 1241: 8, 1242: 8, 1787: 8, 1788: 8
|
||||
}],
|
||||
CAR.CADILLAC_ESCALADE_ESV_2019: [{
|
||||
715: 8, 840: 5, 717: 5, 869: 4, 880: 6, 289: 8, 454: 8, 842: 5, 460: 5, 463: 3, 801: 8, 170: 8, 190: 6, 241: 6, 201: 8, 417: 7, 211: 2, 419: 1, 398: 8, 426: 7, 487: 8, 442: 8, 451: 8, 452: 8, 453: 6, 479: 3, 311: 8, 500: 6, 647: 6, 193: 8, 707: 8, 197: 8, 209: 7, 199: 4, 455: 7, 313: 8, 481: 7, 485: 8, 489: 8, 249: 8, 393: 7, 407: 7, 413: 8, 422: 4, 431: 8, 501: 8, 499: 3, 810: 8, 508: 8, 381: 8, 462: 4, 532: 6, 562: 8, 386: 8, 761: 7, 573: 1, 554: 3, 719: 5, 560: 8, 1279: 4, 388: 8, 288: 5, 1005: 6, 497: 8, 844: 8, 961: 8, 967: 4, 977: 8, 979: 8, 985: 5, 1001: 8, 1017: 8, 1019: 2, 1020: 8, 1217: 8, 510: 8, 866: 4, 304: 1, 969: 8, 384: 4, 1033: 7, 1009: 8, 1034: 7, 1296: 4, 1930: 7, 1105: 5, 1013: 5, 1225: 7, 1919: 7, 320: 3, 534: 2, 352: 5, 298: 8, 1223: 2, 1233: 8, 608: 8, 1265: 8, 609: 6, 1267: 1, 1417: 8, 610: 6, 1906: 7, 611: 6, 612: 8, 613: 8, 208: 8, 564: 5, 309: 8, 1221: 5, 1280: 4, 1249: 8, 1907: 7, 1257: 6, 1300: 8, 1920: 7, 563: 5, 1322: 6, 1323: 4, 1328: 4, 1917: 7, 328: 1, 1912: 7, 1914: 7, 804: 3, 1918: 7
|
||||
}],
|
||||
CAR.CHEVROLET_BOLT_EUV: [{
|
||||
189: 7, 190: 7, 193: 8, 197: 8, 201: 8, 209: 7, 211: 3, 241: 6, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 451: 8, 452: 8, 453: 6, 458: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 566: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
|
||||
}],
|
||||
CAR.CHEVROLET_SILVERADO: [{
|
||||
190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 534: 2, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 761: 7, 789: 5, 800: 6, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
|
||||
},
|
||||
{
|
||||
190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 3, 288: 5, 289: 8, 298: 8, 304: 3, 309: 8, 311: 8, 313: 8, 320: 4, 322: 7, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 528: 5, 532: 6, 534: 2, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 761: 7, 789: 5, 800: 6, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
|
||||
}],
|
||||
CAR.CHEVROLET_EQUINOX: [{
|
||||
190: 6, 193: 8, 197: 8, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 510: 8, 528: 5, 532: 6, 560: 8, 562: 8, 563: 5, 565: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 715: 8, 717: 5, 753: 5, 761: 7, 789: 5, 800: 6, 810: 8, 840: 5, 842: 5, 844: 8, 869: 4, 880: 6, 977: 8, 1001: 8, 1011: 6, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1271: 8, 1280: 4, 1296: 4, 1300: 8, 1611: 8, 1930: 7
|
||||
},
|
||||
{
|
||||
190: 6, 201: 8, 211: 2, 717: 5, 241: 6, 451: 8, 298: 8, 452: 8, 453: 6, 479: 3, 485: 8, 249: 8, 500: 6, 587: 8, 1611: 8, 289: 8, 481: 7, 193: 8, 197: 8, 209: 7, 455: 7, 489: 8, 309: 8, 413: 8, 501: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 311: 8, 510: 8, 528: 5, 532: 6, 715: 8, 560: 8, 562: 8, 707: 8, 789: 5, 869: 4, 880: 6, 761: 7, 840: 5, 842: 5, 844: 8, 313: 8, 381: 8, 386: 8, 810: 8, 322: 7, 384: 4, 800: 6, 1033: 7, 1034: 7, 1296: 4, 753: 5, 388: 8, 288: 5, 497: 8, 463: 3, 304: 3, 977: 8, 1001: 8, 1280: 4, 320: 4, 352: 5, 563: 5, 565: 5, 1221: 5, 1011: 6, 1017: 8, 1020: 8, 1249: 8, 1300: 8, 328: 1, 1217: 8, 1233: 8, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1930: 7, 1271: 8
|
||||
}],
|
||||
CAR.CADILLAC_XT4: [{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 353: 3, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 503: 2, 508: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 567: 5, 573: 1, 577: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 719: 5, 761: 7, 806: 1, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 872: 1, 880: 6, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 5, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1037: 5, 1105: 5, 1187: 5, 1195: 3, 1217: 8, 1221: 5, 1223: 2, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1273: 3, 1276: 2, 1277: 7, 1278: 4, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1417: 8, 1512: 8, 1517: 8, 1601: 8, 1609: 8, 1613: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1858: 8, 1860: 8, 1863: 8, 1872: 8, 1875: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1920: 8, 1924: 8, 1930: 7, 1937: 8, 1953: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1984: 8, 1988: 8, 2000: 8, 2001: 8, 2002: 8, 2016: 8, 2017: 8, 2018: 8, 2020: 8, 2021: 8, 2024: 8, 2026: 8
|
||||
}],
|
||||
CAR.CHEVROLET_VOLT_2019: [{
|
||||
170: 8, 189: 7, 190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 209: 7, 211: 2, 241: 6, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 1, 308: 4, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 331: 3, 352: 5, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 390: 7, 417: 7, 419: 1, 426: 7, 451: 8, 452: 8, 453: 6, 454: 8, 456: 8, 479: 3, 481: 7, 485: 8, 489: 8, 493: 8, 495: 4, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 528: 5, 532: 6, 546: 7, 550: 8, 554: 3, 558: 8, 560: 8, 562: 8, 563: 5, 564: 5, 565: 5, 566: 7, 567: 5, 573: 1, 577: 8, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 3, 707: 8, 711: 6, 715: 8, 717: 5, 761: 7, 810: 8, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 967: 4, 969: 8, 975: 2, 977: 8, 979: 7, 988: 6, 989: 8, 995: 7, 1001: 8, 1005: 6, 1009: 8, 1017: 8, 1019: 2, 1020: 8, 1033: 7, 1034: 7, 1105: 5, 1187: 4, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1227: 4, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1265: 8, 1267: 1, 1268: 2, 1273: 3, 1275: 3, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1328: 4, 1345: 8, 1417: 8, 1512: 8, 1513: 8, 1516: 8, 1517: 8, 1601: 8, 1609: 8, 1611: 8, 1618: 8, 1613: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1871: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1905: 7, 1906: 7, 1907: 7, 1910: 7, 1912: 7, 1920: 8, 1922: 7, 1927: 7, 1930: 7, 1937: 8, 1953: 8, 1954: 8, 1955: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1988: 8, 1990: 8, 2000: 8, 2001: 8, 2004: 8, 2017: 8, 2018: 8, 2020: 8, 2021: 8, 2023: 8, 2025: 8, 2028: 8, 2031: 8
|
||||
}],
|
||||
CAR.CHEVROLET_TRAVERSE: [{
|
||||
190: 6, 193: 8, 197: 8, 199: 4, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 257: 8, 288: 5, 289: 8, 292: 2, 298: 8, 304: 3, 309: 8, 313: 8, 320: 4, 322: 7, 328: 1, 331: 3, 352: 5, 368: 3, 381: 8, 384: 4, 386: 8, 388: 8, 393: 7, 398: 8, 401: 8, 407: 7, 413: 8, 417: 7, 419: 1, 422: 4, 426: 7, 431: 8, 442: 8, 451: 8, 452: 8, 453: 6, 454: 8, 455: 7, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 499: 3, 500: 6, 501: 8, 508: 8, 510: 8, 532: 6, 554: 3, 560: 8, 562: 8, 563: 5, 564: 5, 567: 5, 573: 1, 577: 8, 578: 8, 579: 8, 587: 8, 603: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 647: 6, 707: 8, 715: 8, 717: 5, 723: 4, 730: 4, 753: 5, 761: 7, 840: 5, 842: 5, 844: 8, 866: 4, 869: 4, 880: 6, 961: 8, 969: 8, 975: 2, 977: 8, 979: 8, 985: 5, 1001: 8, 1005: 6, 1009: 8, 1011: 6, 1013: 5, 1017: 8, 1020: 8, 1033: 7, 1034: 7, 1105: 5, 1217: 8, 1221: 5, 1223: 3, 1225: 7, 1233: 8, 1236: 8, 1249: 8, 1257: 6, 1259: 8, 1261: 7, 1263: 4, 1265: 8, 1267: 1, 1268: 2, 1271: 8, 1279: 4, 1280: 4, 1296: 4, 1300: 8, 1322: 6, 1323: 4, 1328: 4, 1345: 8, 1346: 8, 1347: 8, 1355: 8, 1362: 8, 1417: 8, 1512: 8, 1514: 8, 1601: 8, 1602: 8, 1603: 7, 1609: 8, 1611: 8, 1613: 8, 1618: 8, 1649: 8, 1792: 8, 1793: 8, 1798: 8, 1799: 8, 1810: 8, 1813: 8, 1824: 8, 1825: 8, 1840: 8, 1842: 8, 1856: 8, 1858: 8, 1859: 8, 1860: 8, 1862: 8, 1863: 8, 1871: 8, 1872: 8, 1875: 8, 1879: 8, 1882: 8, 1888: 8, 1889: 8, 1892: 8, 1906: 7, 1907: 7, 1912: 7, 1919: 7, 1920: 7, 1927: 8, 1930: 7, 1937: 8, 1953: 8, 1954: 8, 1955: 8, 1968: 8, 1969: 8, 1971: 8, 1975: 8, 1988: 8, 1990: 8, 2000: 8, 2001: 8, 2004: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2024: 8, 2026: 8
|
||||
}],
|
||||
CAR.GMC_YUKON: [{
|
||||
190: 6, 193: 8, 197: 8, 201: 8, 208: 8, 209: 7, 211: 2, 241: 6, 249: 8, 288: 5, 289: 8, 298: 8, 304: 1, 309: 8, 311: 8, 313: 8, 320: 3, 328: 1, 352: 5, 381: 8, 384: 4, 386: 8, 388: 8, 413: 8, 451: 8, 452: 8, 453: 6, 455: 7, 460: 5, 463: 3, 479: 3, 481: 7, 485: 8, 489: 8, 497: 8, 500: 6, 501: 8, 510: 8, 528: 5, 532: 6, 534: 2, 562: 8, 563: 5, 587: 8, 608: 8, 609: 6, 610: 6, 611: 6, 612: 8, 613: 8, 707: 8, 761: 7, 800: 6, 801: 8, 810: 8, 840: 5, 842: 5, 844: 8, 848: 4, 977: 8, 1001: 8, 1017: 8, 1020: 8, 1217: 8, 1221: 5, 1233: 8, 1249: 8, 1265: 8, 1267: 1, 1280: 4, 1300: 8, 1355: 8, 1611: 8
|
||||
}],
|
||||
}
|
||||
|
||||
FW_VERSIONS: dict[str, dict[tuple, list[bytes]]] = {
|
||||
}
|
||||
|
||||
|
||||
FINGERPRINTS = extend_fingerprints(FINGERPRINTS, FINGERPRINTS_EXT)
|
||||
171
artifacts/package_sources/iqdbc/iqdbc/car/gm/gmcan.py
Normal file
171
artifacts/package_sources/iqdbc/iqdbc/car/gm/gmcan.py
Normal file
@@ -0,0 +1,171 @@
|
||||
from iqdbc.car.can_definitions import CanData
|
||||
from iqdbc.car.gm.values import CAR
|
||||
|
||||
|
||||
def create_buttons(packer, bus, idx, button):
|
||||
values = {
|
||||
"ACCButtons": button,
|
||||
"RollingCounter": idx,
|
||||
"ACCAlwaysOne": 1,
|
||||
"DistanceButton": 0,
|
||||
}
|
||||
|
||||
checksum = 240 + int(values["ACCAlwaysOne"] * 0xf)
|
||||
checksum += values["RollingCounter"] * (0x4ef if values["ACCAlwaysOne"] != 0 else 0x3f0)
|
||||
checksum -= int(values["ACCButtons"] - 1) << 4 # not correct if value is 0
|
||||
checksum -= 2 * values["DistanceButton"]
|
||||
|
||||
values["SteeringButtonChecksum"] = checksum
|
||||
return packer.make_can_msg("ASCMSteeringButton", bus, values)
|
||||
|
||||
|
||||
def create_pscm_status(packer, bus, pscm_status):
|
||||
values = {s: pscm_status[s] for s in [
|
||||
"HandsOffSWDetectionMode",
|
||||
"HandsOffSWlDetectionStatus",
|
||||
"LKATorqueDeliveredStatus",
|
||||
"LKADriverAppldTrq",
|
||||
"LKATorqueDelivered",
|
||||
"LKATotalTorqueDelivered",
|
||||
"RollingCounter",
|
||||
"PSCMStatusChecksum",
|
||||
]}
|
||||
checksum_mod = int(1 - values["HandsOffSWlDetectionStatus"]) << 5
|
||||
values["HandsOffSWlDetectionStatus"] = 1
|
||||
values["PSCMStatusChecksum"] += checksum_mod
|
||||
return packer.make_can_msg("PSCMStatus", bus, values)
|
||||
|
||||
|
||||
def create_steering_control(packer, bus, apply_torque, idx, lkas_active):
|
||||
values = {
|
||||
"LKASteeringCmdActive": lkas_active,
|
||||
"LKASteeringCmd": apply_torque,
|
||||
"RollingCounter": idx,
|
||||
"LKASteeringCmdChecksum": 0x1000 - (lkas_active << 11) - (apply_torque & 0x7ff) - idx
|
||||
}
|
||||
|
||||
return packer.make_can_msg("ASCMLKASteeringCmd", bus, values)
|
||||
|
||||
|
||||
def create_adas_keepalive(bus):
|
||||
dat = b"\x00\x00\x00\x00\x00\x00\x00"
|
||||
return [CanData(0x409, dat, bus), CanData(0x40a, dat, bus)]
|
||||
|
||||
|
||||
def create_gas_regen_command(packer, bus, throttle, idx, enabled, at_full_stop):
|
||||
values = {
|
||||
"GasRegenCmdActive": enabled,
|
||||
"RollingCounter": idx,
|
||||
"GasRegenCmd": throttle,
|
||||
"GasRegenFullStopActive": at_full_stop,
|
||||
"GasRegenAccType": 1,
|
||||
}
|
||||
|
||||
dat = packer.make_can_msg("ASCMGasRegenCmd", bus, values)[1]
|
||||
values["GasRegenChecksum"] = ((1 - enabled) << 24) | \
|
||||
(((0xff - dat[1]) & 0xff) << 16) | \
|
||||
(((0xff - dat[2]) & 0xff) << 8) | \
|
||||
((0x100 - dat[3] - idx) & 0xff)
|
||||
|
||||
return packer.make_can_msg("ASCMGasRegenCmd", bus, values)
|
||||
|
||||
|
||||
def create_friction_brake_command(packer, bus, apply_brake, idx, enabled, near_stop, at_full_stop, CP):
|
||||
mode = 0x1
|
||||
|
||||
# TODO: Understand this better. Volts and ICE Camera ACC cars are 0x1 when enabled with no brake
|
||||
if enabled and CP.carFingerprint in (CAR.CHEVROLET_BOLT_EUV,):
|
||||
mode = 0x9
|
||||
|
||||
if apply_brake > 0:
|
||||
mode = 0xa
|
||||
if at_full_stop:
|
||||
mode = 0xd
|
||||
|
||||
# TODO: this is to have GM bringing the car to complete stop,
|
||||
# but currently it conflicts with OP controls, so turned off. Not set by all cars
|
||||
#elif near_stop:
|
||||
# mode = 0xb
|
||||
|
||||
brake = (0x1000 - apply_brake) & 0xfff
|
||||
checksum = (0x10000 - (mode << 12) - brake - idx) & 0xffff
|
||||
|
||||
values = {
|
||||
"RollingCounter": idx,
|
||||
"FrictionBrakeMode": mode,
|
||||
"FrictionBrakeChecksum": checksum,
|
||||
"FrictionBrakeCmd": -apply_brake
|
||||
}
|
||||
|
||||
return packer.make_can_msg("EBCMFrictionBrakeCmd", bus, values)
|
||||
|
||||
|
||||
def create_acc_dashboard_command(packer, bus, enabled, target_speed_kph, hud_control, fcw):
|
||||
target_speed = min(target_speed_kph, 255)
|
||||
|
||||
values = {
|
||||
"ACCAlwaysOne": 1,
|
||||
"ACCResumeButton": 0,
|
||||
"ACCSpeedSetpoint": target_speed,
|
||||
"ACCGapLevel": hud_control.leadDistanceBars * enabled, # 3 "far", 0 "inactive"
|
||||
"ACCCmdActive": enabled,
|
||||
"ACCAlwaysOne2": 1,
|
||||
"ACCLeadCar": hud_control.leadVisible,
|
||||
"FCWAlert": 0x3 if fcw else 0
|
||||
}
|
||||
|
||||
return packer.make_can_msg("ASCMActiveCruiseControlStatus", bus, values)
|
||||
|
||||
|
||||
def create_adas_time_status(bus, tt, idx):
|
||||
dat = [(tt >> 20) & 0xff, (tt >> 12) & 0xff, (tt >> 4) & 0xff,
|
||||
((tt & 0xf) << 4) + (idx << 2)]
|
||||
chksum = 0x1000 - dat[0] - dat[1] - dat[2] - dat[3]
|
||||
chksum = chksum & 0xfff
|
||||
dat += [0x40 + (chksum >> 8), chksum & 0xff, 0x12]
|
||||
return CanData(0xa1, bytes(dat), bus)
|
||||
|
||||
|
||||
def create_adas_steering_status(bus, idx):
|
||||
dat = [idx << 6, 0xf0, 0x20, 0, 0, 0]
|
||||
chksum = 0x60 + sum(dat)
|
||||
dat += [chksum >> 8, chksum & 0xff]
|
||||
return CanData(0x306, bytes(dat), bus)
|
||||
|
||||
|
||||
def create_adas_accelerometer_speed_status(bus, speed_ms, idx):
|
||||
spd = int(speed_ms * 16) & 0xfff
|
||||
accel = 0 & 0xfff
|
||||
# 0 if in park/neutral, 0x10 if in reverse, 0x08 for D/L
|
||||
#stick = 0x08
|
||||
near_range_cutoff = 0x27
|
||||
near_range_mode = 1 if spd <= near_range_cutoff else 0
|
||||
far_range_mode = 1 - near_range_mode
|
||||
dat = [0x08, spd >> 4, ((spd & 0xf) << 4) | (accel >> 8), accel & 0xff, 0]
|
||||
chksum = 0x62 + far_range_mode + (idx << 2) + dat[0] + dat[1] + dat[2] + dat[3] + dat[4]
|
||||
dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff]
|
||||
return CanData(0x308, bytes(dat), bus)
|
||||
|
||||
|
||||
def create_adas_headlights_status(packer, bus):
|
||||
values = {
|
||||
"Always42": 0x42,
|
||||
"Always4": 0x4,
|
||||
}
|
||||
return packer.make_can_msg("ASCMHeadlight", bus, values)
|
||||
|
||||
|
||||
def create_lka_icon_command(bus, active, critical, steer):
|
||||
if active and steer == 1:
|
||||
if critical:
|
||||
dat = b"\x50\xc0\x14"
|
||||
else:
|
||||
dat = b"\x50\x40\x18"
|
||||
elif active:
|
||||
if critical:
|
||||
dat = b"\x40\xc0\x14"
|
||||
else:
|
||||
dat = b"\x40\x40\x18"
|
||||
else:
|
||||
dat = b"\x00\x00\x00"
|
||||
return CanData(0x104c006c, dat, bus)
|
||||
265
artifacts/package_sources/iqdbc/iqdbc/car/gm/interface.py
Executable file
265
artifacts/package_sources/iqdbc/iqdbc/car/gm/interface.py
Executable file
@@ -0,0 +1,265 @@
|
||||
#!/usr/bin/env python3
|
||||
from math import fabs, exp
|
||||
import numpy as np
|
||||
|
||||
from iqdbc.car import get_safety_config, structs
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.gm.carcontroller import CarController
|
||||
from iqdbc.car.gm.carstate import CarState
|
||||
from iqdbc.car.gm.radar_interface import RadarInterface, RADAR_HEADER_MSG, CAMERA_DATA_HEADER_MSG
|
||||
from iqdbc.car.gm.values import CAR, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, SDGM_CAR, ALT_ACCS, CanBus, GMSafetyFlags
|
||||
from iqdbc.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, LateralAccelFromTorqueCallbackType
|
||||
|
||||
from iqdbc.lvbs.car.gm.iq_interface import IQCarInterface
|
||||
from iqdbc.lvbs.car.gm.iq_values import GMFlagsIQ, GMSafetyFlagsIQ
|
||||
|
||||
TransmissionType = structs.CarParams.TransmissionType
|
||||
NetworkLocation = structs.CarParams.NetworkLocation
|
||||
|
||||
# Bolt Non-ACC uses the 4th (d) tune parameter; stock tunes zero it out.
|
||||
NON_LINEAR_TORQUE_PARAMS_IQ = {
|
||||
CAR.CHEVROLET_BOLT_NON_ACC: [2.24, 1.1, 0.28, -0.07],
|
||||
CAR.CHEVROLET_BOLT_NON_ACC_1ST_GEN: [1.8, 1.1, 0.3, -0.045],
|
||||
}
|
||||
|
||||
NON_LINEAR_TORQUE_PARAMS = {
|
||||
CAR.CHEVROLET_BOLT_EUV: [2.6531724862969748, 1.0, 0.1919764879840985, 0.009054123646805178],
|
||||
CAR.GMC_ACADIA: [4.78003305, 1.0, 0.3122, 0.05591772],
|
||||
CAR.CHEVROLET_SILVERADO: [3.29974374, 1.0, 0.25571356, 0.0465122],
|
||||
**NON_LINEAR_TORQUE_PARAMS_IQ,
|
||||
}
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase, IQCarInterface):
|
||||
CarState = CarState
|
||||
CarController = CarController
|
||||
RadarInterface = RadarInterface
|
||||
|
||||
DRIVABLE_GEARS = (structs.CarState.GearShifter.sport, structs.CarState.GearShifter.low,
|
||||
structs.CarState.GearShifter.eco, structs.CarState.GearShifter.manumatic)
|
||||
|
||||
def __init__(self, CP, CP_IQ):
|
||||
CarInterfaceBase.__init__(self, CP, CP_IQ)
|
||||
IQCarInterface.__init__(self, CP, CarInterfaceBase)
|
||||
|
||||
@staticmethod
|
||||
def get_pid_accel_limits(CP, CP_IQ, current_speed, cruise_speed):
|
||||
return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
|
||||
|
||||
# Determined by iteratively plotting and minimizing error for f(angle, speed) = steer.
|
||||
@staticmethod
|
||||
def get_steer_feedforward_volt(desired_angle, v_ego):
|
||||
desired_angle *= 0.02904609
|
||||
sigmoid = desired_angle / (1 + fabs(desired_angle))
|
||||
return 0.10006696 * sigmoid * (v_ego + 3.12485927)
|
||||
|
||||
def get_steer_feedforward_function(self):
|
||||
if self.CP.carFingerprint == CAR.CHEVROLET_VOLT:
|
||||
return self.get_steer_feedforward_volt
|
||||
else:
|
||||
return CarInterfaceBase.get_steer_feedforward_default
|
||||
|
||||
def get_lataccel_torque_siglin(self) -> tuple[list[float], np.ndarray]:
|
||||
|
||||
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, d = non_linear_torque_params
|
||||
d = d if NON_LINEAR_TORQUE_PARAMS_IQ.get(self.CP.carFingerprint) else 0.0
|
||||
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) + d
|
||||
return float(steer_torque)
|
||||
|
||||
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 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"
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.gm)]
|
||||
ret.autoResumeSng = False
|
||||
ret.enableBsm = 0x142 in fingerprint[CanBus.POWERTRAIN]
|
||||
|
||||
if candidate in EV_CAR:
|
||||
ret.transmissionType = TransmissionType.direct
|
||||
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.EV.value
|
||||
else:
|
||||
ret.transmissionType = TransmissionType.automatic
|
||||
|
||||
ret.longitudinalTuning.kiBP = [5., 35.]
|
||||
|
||||
if candidate in (CAMERA_ACC_CAR | SDGM_CAR):
|
||||
ret.alphaLongitudinalAvailable = candidate not in SDGM_CAR
|
||||
ret.networkLocation = NetworkLocation.fwdCamera
|
||||
ret.radarUnavailable = True # no radar
|
||||
ret.pcmCruise = True
|
||||
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.HW_CAM.value
|
||||
ret.minEnableSpeed = -1 if candidate in SDGM_CAR else 5 * CV.KPH_TO_MS
|
||||
ret.minSteerSpeed = 10 * CV.KPH_TO_MS
|
||||
|
||||
# Tuning for experimental long
|
||||
ret.longitudinalTuning.kiV = [2.0, 1.5]
|
||||
|
||||
if alpha_long:
|
||||
ret.pcmCruise = False
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.HW_CAM_LONG.value
|
||||
|
||||
if candidate in ALT_ACCS:
|
||||
ret.alphaLongitudinalAvailable = False
|
||||
ret.openpilotLongitudinalControl = False
|
||||
ret.minEnableSpeed = -1. # engage speed is decided by PCM
|
||||
|
||||
else: # ASCM, OBD-II harness
|
||||
ret.openpilotLongitudinalControl = True
|
||||
ret.networkLocation = NetworkLocation.gateway
|
||||
# LRR messages can take up to a few seconds to start sending after ignition, check camera data as well which starts earlier
|
||||
ret.radarUnavailable = RADAR_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and CAMERA_DATA_HEADER_MSG not in fingerprint[CanBus.OBSTACLE] and not docs
|
||||
ret.pcmCruise = False # stock non-adaptive cruise control is kept off
|
||||
# supports stop and go, but initial engage must (conservatively) be above 18mph
|
||||
ret.minEnableSpeed = 18 * CV.MPH_TO_MS
|
||||
ret.minSteerSpeed = 7 * CV.MPH_TO_MS
|
||||
|
||||
# Tuning
|
||||
ret.longitudinalTuning.kiV = [2.4, 1.5]
|
||||
|
||||
# These cars have been put into dashcam only due to both a lack of users and test coverage.
|
||||
# These cars likely still work fine. Once a user confirms each car works and a test route is
|
||||
# added to iqdbc/car/tests/routes.py, we can remove it from this list.
|
||||
ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.CHEVROLET_MALIBU, CAR.BUICK_REGAL} or \
|
||||
(ret.networkLocation == NetworkLocation.gateway and ret.radarUnavailable)
|
||||
|
||||
# Start with a baseline tuning for all GM vehicles. Override tuning as needed in each model section below.
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.00]]
|
||||
ret.lateralTuning.pid.kf = 0.00004 # full torque for 20 deg at 80mph means 0.00007818594
|
||||
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet
|
||||
|
||||
ret.steerLimitTimer = 0.4
|
||||
ret.longitudinalActuatorDelay = 0.5 # large delay to initially start braking
|
||||
|
||||
if candidate == CAR.CHEVROLET_VOLT:
|
||||
ret.lateralTuning.pid.kpBP = [0., 40.]
|
||||
ret.lateralTuning.pid.kpV = [0., 0.17]
|
||||
ret.lateralTuning.pid.kiBP = [0.]
|
||||
ret.lateralTuning.pid.kiV = [0.]
|
||||
ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_volt()
|
||||
ret.steerActuatorDelay = 0.2
|
||||
|
||||
elif candidate == CAR.GMC_ACADIA:
|
||||
ret.minEnableSpeed = -1. # engage speed is decided by pcm
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.BUICK_LACROSSE:
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.CADILLAC_ESCALADE:
|
||||
ret.minEnableSpeed = -1. # engage speed is decided by pcm
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate in (CAR.CADILLAC_ESCALADE_ESV, CAR.CADILLAC_ESCALADE_ESV_2019):
|
||||
ret.minEnableSpeed = -1. # engage speed is decided by pcm
|
||||
|
||||
if candidate == CAR.CADILLAC_ESCALADE_ESV:
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[10., 41.0], [10., 41.0]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.13, 0.24], [0.01, 0.02]]
|
||||
ret.lateralTuning.pid.kf = 0.000045
|
||||
else:
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.CHEVROLET_BOLT_EUV:
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.CHEVROLET_SILVERADO:
|
||||
# On the Bolt, the ECM and camera independently check that you are either above 5 kph or at a stop
|
||||
# with foot on brake to allow engagement, but this platform only has that check in the camera.
|
||||
# TODO: check if this is split by EV/ICE with more platforms in the future
|
||||
if ret.openpilotLongitudinalControl:
|
||||
ret.minEnableSpeed = -1.
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.CHEVROLET_EQUINOX:
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.CHEVROLET_TRAILBLAZER:
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.CADILLAC_XT4:
|
||||
ret.steerActuatorDelay = 0.2
|
||||
ret.minSteerSpeed = 30 * CV.MPH_TO_MS
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.CHEVROLET_VOLT_2019:
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.CHEVROLET_TRAVERSE:
|
||||
ret.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
elif candidate == CAR.GMC_YUKON:
|
||||
ret.steerActuatorDelay = 0.5
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
ret.dashcamOnly = True # Needs steerRatio, tireStiffness, and lat accel factor tuning
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def _get_params_iq(stock_cp: structs.CarParams, ret: structs.IQCarParams, candidate, fingerprint: dict[int, dict[int, int]],
|
||||
car_fw: list[structs.CarParams.CarFw], alpha_long: bool, is_release_iq: bool, docs: bool) -> structs.IQCarParams:
|
||||
if candidate in (CAR.CHEVROLET_MALIBU_NON_ACC_9TH_GEN, CAR.CHEVROLET_BOLT_NON_ACC, CAR.CHEVROLET_BOLT_NON_ACC_1ST_GEN,
|
||||
CAR.CHEVROLET_BOLT_NON_ACC_2ND_GEN, CAR.CHEVROLET_TRAILBLAZER_NON_ACC_2ND_GEN):
|
||||
stock_cp.steerActuatorDelay = 0.2
|
||||
CarInterfaceBase.configure_torque_tune(candidate, stock_cp.lateralTuning)
|
||||
elif candidate in (CAR.CHEVROLET_EQUINOX_NON_ACC_3RD_GEN, ):
|
||||
CarInterfaceBase.configure_torque_tune(candidate, stock_cp.lateralTuning)
|
||||
|
||||
# Non-ACC cars steer/long via the forward camera and pcmCruise, not the ASCM.
|
||||
if ret.flags & GMFlagsIQ.NON_ACC:
|
||||
stock_cp.dashcamOnly = False
|
||||
stock_cp.alphaLongitudinalAvailable = False
|
||||
stock_cp.networkLocation = NetworkLocation.fwdCamera
|
||||
stock_cp.openpilotLongitudinalControl = False
|
||||
stock_cp.pcmCruise = True
|
||||
stock_cp.safetyConfigs[0].safetyParam |= GMSafetyFlags.HW_CAM.value
|
||||
ret.iqSafetyFlags |= GMSafetyFlagsIQ.NON_ACC
|
||||
stock_cp.minEnableSpeed = 24 * CV.MPH_TO_MS
|
||||
stock_cp.minSteerSpeed = 3.0
|
||||
|
||||
# Untested Non-ACC platforms ship dashcam-only pending user validation.
|
||||
if candidate in (CAR.CHEVROLET_BOLT_NON_ACC_2ND_GEN, CAR.CHEVROLET_EQUINOX_NON_ACC_3RD_GEN,
|
||||
CAR.CHEVROLET_SUBURBAN_NON_ACC_11TH_GEN, CAR.CADILLAC_CT6_NON_ACC_1ST_GEN,
|
||||
CAR.CHEVROLET_TRAILBLAZER_NON_ACC_2ND_GEN, CAR.CADILLAC_XT5_NON_ACC_1ST_GEN):
|
||||
stock_cp.dashcamOnly = True
|
||||
|
||||
return ret
|
||||
99
artifacts/package_sources/iqdbc/iqdbc/car/gm/radar_interface.py
Executable file
99
artifacts/package_sources/iqdbc/iqdbc/car/gm/radar_interface.py
Executable file
@@ -0,0 +1,99 @@
|
||||
#!/usr/bin/env python3
|
||||
import math
|
||||
from iqdbc.can import CANParser
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.gm.values import DBC, CanBus
|
||||
from iqdbc.car.interfaces import RadarInterfaceBase
|
||||
|
||||
RADAR_HEADER_MSG = 1120 # F_LRR_Obj_Header
|
||||
CAMERA_DATA_HEADER_MSG = 1056 # F_Vision_Obj_Header
|
||||
SLOT_1_MSG = RADAR_HEADER_MSG + 1
|
||||
NUM_SLOTS = 20
|
||||
|
||||
# Actually it's 0x47f, but can parser only reports
|
||||
# messages that are present in DBC
|
||||
LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS
|
||||
|
||||
|
||||
def create_radar_can_parser(car_fingerprint):
|
||||
# C1A-ARS3-A by Continental
|
||||
radar_targets = list(range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS))
|
||||
signals = list(zip(['FLRRNumValidTargets',
|
||||
'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt',
|
||||
'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt',
|
||||
'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] +
|
||||
['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS +
|
||||
['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS +
|
||||
['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS,
|
||||
[RADAR_HEADER_MSG] * 7 + radar_targets * 6, strict=True))
|
||||
|
||||
messages = list({(s[1], 14) for s in signals})
|
||||
|
||||
return CANParser(DBC[car_fingerprint][Bus.radar], messages, CanBus.OBSTACLE)
|
||||
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP, CP_IQ):
|
||||
super().__init__(CP, CP_IQ)
|
||||
|
||||
self.rcp = None if CP.radarUnavailable else create_radar_can_parser(CP.carFingerprint)
|
||||
|
||||
self.trigger_msg = LAST_RADAR_MSG
|
||||
self.updated_messages = set()
|
||||
|
||||
def update(self, can_strings):
|
||||
if self.rcp is None:
|
||||
return super().update(None)
|
||||
|
||||
vls = self.rcp.update(can_strings)
|
||||
self.updated_messages.update(vls)
|
||||
|
||||
if self.trigger_msg not in self.updated_messages:
|
||||
return None
|
||||
|
||||
ret = structs.RadarData()
|
||||
header = self.rcp.vl[RADAR_HEADER_MSG]
|
||||
fault = header['FLRRSnsrBlckd'] or header['FLRRSnstvFltPrsntInt'] or \
|
||||
header['FLRRYawRtPlsblityFlt'] or header['FLRRHWFltPrsntInt'] or \
|
||||
header['FLRRAntTngFltPrsnt'] or header['FLRRAlgnFltPrsnt']
|
||||
if not self.rcp.can_valid:
|
||||
ret.errors.canError = True
|
||||
if fault:
|
||||
ret.errors.radarFault = True
|
||||
|
||||
currentTargets = set()
|
||||
num_targets = header['FLRRNumValidTargets']
|
||||
|
||||
# Not all radar messages describe targets,
|
||||
# no need to monitor all of the self.rcp.msgs_upd
|
||||
for ii in self.updated_messages:
|
||||
if ii == RADAR_HEADER_MSG:
|
||||
continue
|
||||
|
||||
if num_targets == 0:
|
||||
break
|
||||
|
||||
cpt = self.rcp.vl[ii]
|
||||
# Zero distance means it's an empty target slot
|
||||
if cpt['TrkRange'] > 0.0:
|
||||
targetId = cpt['TrkObjectID']
|
||||
currentTargets.add(targetId)
|
||||
if targetId not in self.pts:
|
||||
self.pts[targetId] = structs.RadarData.RadarPoint()
|
||||
self.pts[targetId].trackId = targetId
|
||||
distance = cpt['TrkRange']
|
||||
self.pts[targetId].dRel = distance # from front of car
|
||||
# From driver's pov, left is positive
|
||||
self.pts[targetId].yRel = math.sin(cpt['TrkAzimuth'] * CV.DEG_TO_RAD) * distance
|
||||
self.pts[targetId].vRel = cpt['TrkRangeRate']
|
||||
self.pts[targetId].aRel = float('nan')
|
||||
self.pts[targetId].yvRel = float('nan')
|
||||
|
||||
for oldTarget in list(self.pts.keys()):
|
||||
if oldTarget not in currentTargets:
|
||||
del self.pts[oldTarget]
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
self.updated_messages.clear()
|
||||
return ret
|
||||
@@ -0,0 +1,20 @@
|
||||
from parameterized import parameterized
|
||||
|
||||
from iqdbc.car.gm.fingerprints import FINGERPRINTS
|
||||
from iqdbc.car.gm.values import CAMERA_ACC_CAR, GM_RX_OFFSET
|
||||
|
||||
CAMERA_DIAGNOSTIC_ADDRESS = 0x24b
|
||||
|
||||
|
||||
class TestGMFingerprint:
|
||||
@parameterized.expand(FINGERPRINTS.items())
|
||||
def test_can_fingerprints(self, car_model, fingerprints):
|
||||
assert len(fingerprints) > 0
|
||||
|
||||
assert all(len(finger) for finger in fingerprints)
|
||||
|
||||
# The camera can sometimes be communicating on startup
|
||||
if car_model in CAMERA_ACC_CAR:
|
||||
for finger in fingerprints:
|
||||
for required_addr in (CAMERA_DIAGNOSTIC_ADDRESS, CAMERA_DIAGNOSTIC_ADDRESS + GM_RX_OFFSET):
|
||||
assert finger.get(required_addr) == 8, required_addr
|
||||
343
artifacts/package_sources/iqdbc/iqdbc/car/gm/values.py
Normal file
343
artifacts/package_sources/iqdbc/iqdbc/car/gm/values.py
Normal file
@@ -0,0 +1,343 @@
|
||||
from dataclasses import dataclass, field
|
||||
from enum import Enum, IntFlag
|
||||
|
||||
from iqdbc.car import Bus, PlatformConfig, DbcDict, Platforms, CarSpecs
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.car.docs_definitions import CarDocs, CarFootnote, CarHarness, CarParts, Column, SupportType
|
||||
from iqdbc.lvbs.car.gm.iq_values import GMFlagsIQ
|
||||
from iqdbc.car.fw_query_definitions import FwQueryConfig, Request, StdQueries
|
||||
|
||||
Ecu = CarParams.Ecu
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
STEER_MAX = 300 # GM limit is 3Nm. Used by carcontroller to generate LKA output
|
||||
STEER_STEP = 3 # Active control frames per command (~33hz)
|
||||
INACTIVE_STEER_STEP = 10 # Inactive control frames per command (10hz)
|
||||
STEER_DELTA_UP = 10 # Delta rates require review due to observed EPS weakness
|
||||
STEER_DELTA_DOWN = 15
|
||||
STEER_DRIVER_ALLOWANCE = 65
|
||||
STEER_DRIVER_MULTIPLIER = 4
|
||||
STEER_DRIVER_FACTOR = 100
|
||||
NEAR_STOP_BRAKE_PHASE = 0.5 # m/s
|
||||
|
||||
# Heartbeat for dash "Service Adaptive Cruise" and "Service Front Camera"
|
||||
ADAS_KEEPALIVE_STEP = 100
|
||||
CAMERA_KEEPALIVE_STEP = 100
|
||||
|
||||
# Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
|
||||
# perform the closed loop control, and might need some
|
||||
# to apply some more braking if we're on a downhill slope.
|
||||
# Our controller should still keep the 2 second average above
|
||||
# -3.5 m/s^2 as per planner limits
|
||||
ACCEL_MAX = 2. # m/s^2
|
||||
ACCEL_MIN = -4. # m/s^2
|
||||
|
||||
def __init__(self, CP):
|
||||
# Gas/brake lookups
|
||||
self.MAX_BRAKE = 400 # ~ -4.0 m/s^2 with regen
|
||||
|
||||
if CP.carFingerprint in (CAMERA_ACC_CAR | SDGM_CAR):
|
||||
self.MAX_GAS = 1346.0
|
||||
self.MAX_ACC_REGEN = -540.0
|
||||
self.INACTIVE_REGEN = -500.0
|
||||
# Camera ACC vehicles have no regen while enabled.
|
||||
# Camera transitions to MAX_ACC_REGEN from zero gas and uses friction brakes instantly
|
||||
max_regen_acceleration = 0.
|
||||
|
||||
else:
|
||||
self.MAX_GAS = 1018.0 # Safety limit, not ACC max. Stock ACC >2042 from standstill.
|
||||
self.MAX_ACC_REGEN = -650.0 # Max ACC regen is slightly less than max paddle regen
|
||||
self.INACTIVE_REGEN = -650.0
|
||||
# ICE has much less engine braking force compared to regen in EVs,
|
||||
# lower threshold removes some braking deadzone
|
||||
max_regen_acceleration = -1. if CP.carFingerprint in EV_CAR else -0.1
|
||||
|
||||
self.GAS_LOOKUP_BP = [max_regen_acceleration, 0., self.ACCEL_MAX]
|
||||
self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, 0., self.MAX_GAS]
|
||||
|
||||
self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, max_regen_acceleration]
|
||||
self.BRAKE_LOOKUP_V = [self.MAX_BRAKE, 0.]
|
||||
|
||||
|
||||
class GMSafetyFlags(IntFlag):
|
||||
HW_CAM = 1
|
||||
HW_CAM_LONG = 2
|
||||
EV = 4
|
||||
|
||||
|
||||
class Footnote(Enum):
|
||||
SETUP = CarFootnote(
|
||||
"See more setup details for <a href=\"https://github.com/commaai/openpilot/wiki/gm\" target=\"_blank\">GM</a>.",
|
||||
Column.MAKE, setup_note=True)
|
||||
|
||||
|
||||
@dataclass
|
||||
class GMCarDocs(CarDocs):
|
||||
package: str = "Adaptive Cruise Control (ACC)"
|
||||
|
||||
def init_make(self, CP: CarParams):
|
||||
if CP.networkLocation == CarParams.NetworkLocation.fwdCamera:
|
||||
if CP.carFingerprint in SDGM_CAR:
|
||||
self.car_parts = CarParts.common([CarHarness.gmsdgm])
|
||||
else:
|
||||
self.car_parts = CarParts.common([CarHarness.gm])
|
||||
else:
|
||||
self.footnotes.insert(0, Footnote.SETUP)
|
||||
self.car_parts = CarParts.common([CarHarness.obd_ii])
|
||||
|
||||
|
||||
@dataclass(frozen=True, kw_only=True)
|
||||
class GMCarSpecs(CarSpecs):
|
||||
tireStiffnessFactor: float = 0.444 # not optimized yet
|
||||
|
||||
|
||||
@dataclass
|
||||
class GMPlatformConfig(PlatformConfig):
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: {
|
||||
Bus.pt: 'gm_global_a_powertrain_generated',
|
||||
Bus.radar: 'gm_global_a_object',
|
||||
Bus.chassis: 'gm_global_a_chassis',
|
||||
})
|
||||
|
||||
|
||||
@dataclass
|
||||
class GMASCMPlatformConfig(GMPlatformConfig):
|
||||
def init(self):
|
||||
# ASCM is supported, but due to a janky install and hardware configuration, we are not showing in the car docs
|
||||
self.car_docs = []
|
||||
|
||||
|
||||
@dataclass
|
||||
class GMSDGMPlatformConfig(GMPlatformConfig):
|
||||
def init(self):
|
||||
# Don't show in docs until the harness is sold. See https://github.com/commaai/openpilot/issues/32471
|
||||
self.car_docs = []
|
||||
|
||||
|
||||
@dataclass
|
||||
class GMNonAccCarDocs(GMCarDocs):
|
||||
package: str = "No Adaptive Cruise Control (Non-ACC)"
|
||||
support_type: SupportType = SupportType.COMMUNITY
|
||||
support_link: str = "community"
|
||||
|
||||
|
||||
@dataclass
|
||||
class GMNonSccPlatformConfig(GMPlatformConfig):
|
||||
def init(self):
|
||||
self.iq_flags |= GMFlagsIQ.NON_ACC
|
||||
|
||||
|
||||
class CAR(Platforms):
|
||||
HOLDEN_ASTRA = GMASCMPlatformConfig(
|
||||
[GMCarDocs("Holden Astra 2017")],
|
||||
GMCarSpecs(mass=1363, wheelbase=2.662, steerRatio=15.7, centerToFrontRatio=0.4),
|
||||
)
|
||||
CHEVROLET_VOLT = GMASCMPlatformConfig(
|
||||
[GMCarDocs("Chevrolet Volt 2017-18", min_enable_speed=0, video="https://youtu.be/QeMCN_4TFfQ")],
|
||||
GMCarSpecs(mass=1607, wheelbase=2.69, steerRatio=17.7, centerToFrontRatio=0.45, tireStiffnessFactor=0.469),
|
||||
)
|
||||
CADILLAC_ATS = GMASCMPlatformConfig(
|
||||
[GMCarDocs("Cadillac ATS Premium Performance 2018")],
|
||||
GMCarSpecs(mass=1601, wheelbase=2.78, steerRatio=15.3),
|
||||
)
|
||||
CHEVROLET_MALIBU = GMASCMPlatformConfig(
|
||||
[GMCarDocs("Chevrolet Malibu Premier 2017")],
|
||||
GMCarSpecs(mass=1496, wheelbase=2.83, steerRatio=15.8, centerToFrontRatio=0.4),
|
||||
)
|
||||
GMC_ACADIA = GMASCMPlatformConfig(
|
||||
[GMCarDocs("GMC Acadia 2018", video="https://www.youtube.com/watch?v=0ZN6DdsBUZo")],
|
||||
GMCarSpecs(mass=1975, wheelbase=2.86, steerRatio=14.4, centerToFrontRatio=0.4),
|
||||
)
|
||||
BUICK_LACROSSE = GMASCMPlatformConfig(
|
||||
[GMCarDocs("Buick LaCrosse 2017-19", "Driver Confidence Package 2")],
|
||||
GMCarSpecs(mass=1712, wheelbase=2.91, steerRatio=15.8, centerToFrontRatio=0.4),
|
||||
)
|
||||
BUICK_REGAL = GMASCMPlatformConfig(
|
||||
[GMCarDocs("Buick Regal Essence 2018")],
|
||||
GMCarSpecs(mass=1714, wheelbase=2.83, steerRatio=14.4, centerToFrontRatio=0.4),
|
||||
)
|
||||
CADILLAC_ESCALADE = GMASCMPlatformConfig(
|
||||
[GMCarDocs("Cadillac Escalade 2017", "Driver Assist Package")],
|
||||
GMCarSpecs(mass=2564, wheelbase=2.95, steerRatio=17.3),
|
||||
)
|
||||
CADILLAC_ESCALADE_ESV = GMASCMPlatformConfig(
|
||||
[GMCarDocs("Cadillac Escalade ESV 2016", "Adaptive Cruise Control (ACC) & LKAS")],
|
||||
GMCarSpecs(mass=2739, wheelbase=3.302, steerRatio=17.3, tireStiffnessFactor=1.0),
|
||||
)
|
||||
CADILLAC_ESCALADE_ESV_2019 = GMASCMPlatformConfig(
|
||||
[GMCarDocs("Cadillac Escalade ESV 2019", "Adaptive Cruise Control (ACC) & LKAS")],
|
||||
CADILLAC_ESCALADE_ESV.specs,
|
||||
)
|
||||
CHEVROLET_BOLT_EUV = GMPlatformConfig(
|
||||
[
|
||||
GMCarDocs("Chevrolet Bolt EUV 2022-23", "Premier or Premier Redline Trim, without Super Cruise Package", video="https://youtu.be/xvwzGMUA210"),
|
||||
GMCarDocs("Chevrolet Bolt EV 2022-23", "2LT Trim with Adaptive Cruise Control Package"),
|
||||
],
|
||||
GMCarSpecs(mass=1669, wheelbase=2.63779, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0),
|
||||
)
|
||||
CHEVROLET_SILVERADO = GMPlatformConfig(
|
||||
[
|
||||
GMCarDocs("Chevrolet Silverado 1500 2020-21", "Safety Package II"),
|
||||
GMCarDocs("GMC Sierra 1500 2020-21", "Driver Alert Package II", video="https://youtu.be/5HbNoBLzRwE"),
|
||||
],
|
||||
GMCarSpecs(mass=2450, wheelbase=3.75, steerRatio=16.3, tireStiffnessFactor=1.0),
|
||||
)
|
||||
CHEVROLET_EQUINOX = GMPlatformConfig(
|
||||
[GMCarDocs("Chevrolet Equinox 2019-22")],
|
||||
GMCarSpecs(mass=1588, wheelbase=2.72, steerRatio=14.4, centerToFrontRatio=0.4),
|
||||
)
|
||||
CHEVROLET_TRAILBLAZER = GMPlatformConfig(
|
||||
[GMCarDocs("Chevrolet Trailblazer 2021-22")],
|
||||
GMCarSpecs(mass=1345, wheelbase=2.64, steerRatio=16.8, centerToFrontRatio=0.4, tireStiffnessFactor=1.0),
|
||||
)
|
||||
CADILLAC_XT4 = GMSDGMPlatformConfig(
|
||||
[GMCarDocs("Cadillac XT4 2023", "Driver Assist Package")],
|
||||
GMCarSpecs(mass=1660, wheelbase=2.78, steerRatio=14.4, centerToFrontRatio=0.4),
|
||||
)
|
||||
CHEVROLET_VOLT_2019 = GMSDGMPlatformConfig(
|
||||
[GMCarDocs("Chevrolet Volt 2019", "Adaptive Cruise Control (ACC) & LKAS")],
|
||||
GMCarSpecs(mass=1607, wheelbase=2.69, steerRatio=15.7, centerToFrontRatio=0.45),
|
||||
)
|
||||
CHEVROLET_TRAVERSE = GMSDGMPlatformConfig(
|
||||
[GMCarDocs("Chevrolet Traverse 2022-23", "RS, Premier, or High Country Trim")],
|
||||
GMCarSpecs(mass=1955, wheelbase=3.07, steerRatio=17.9, centerToFrontRatio=0.4),
|
||||
)
|
||||
GMC_YUKON = GMPlatformConfig(
|
||||
[GMCarDocs("GMC Yukon 2019-20", "Adaptive Cruise Control (ACC) & LKAS")],
|
||||
GMCarSpecs(mass=2490, wheelbase=2.94, steerRatio=17.3, centerToFrontRatio=0.5, tireStiffnessFactor=1.0),
|
||||
)
|
||||
|
||||
# IQ.Pilot Non-ACC camera-harness ports (no factory adaptive cruise).
|
||||
CHEVROLET_BOLT_NON_ACC = GMNonSccPlatformConfig(
|
||||
[GMNonAccCarDocs("Chevrolet Bolt EV Non-ACC 2017")],
|
||||
CHEVROLET_BOLT_EUV.specs,
|
||||
)
|
||||
CHEVROLET_BOLT_NON_ACC_1ST_GEN = GMNonSccPlatformConfig(
|
||||
[GMNonAccCarDocs("Chevrolet Bolt EV Non-ACC 2018-21")],
|
||||
CHEVROLET_BOLT_EUV.specs,
|
||||
)
|
||||
CHEVROLET_BOLT_NON_ACC_2ND_GEN = GMNonSccPlatformConfig(
|
||||
[
|
||||
GMNonAccCarDocs("Chevrolet Bolt EUV LT Non-ACC 2022-23"),
|
||||
GMNonAccCarDocs("Chevrolet Bolt EV LT Non-ACC 2022-23"),
|
||||
],
|
||||
CHEVROLET_BOLT_EUV.specs,
|
||||
)
|
||||
CHEVROLET_EQUINOX_NON_ACC_3RD_GEN = GMNonSccPlatformConfig(
|
||||
[GMNonAccCarDocs("Chevrolet Equinox Non-ACC 2019-22")],
|
||||
CHEVROLET_EQUINOX.specs,
|
||||
)
|
||||
CHEVROLET_SUBURBAN_NON_ACC_11TH_GEN = GMNonSccPlatformConfig(
|
||||
[GMNonAccCarDocs("Chevrolet Suburban Non-ACC 2016-20")],
|
||||
CarSpecs(mass=2731, wheelbase=3.302, steerRatio=17.3, centerToFrontRatio=0.49),
|
||||
)
|
||||
CADILLAC_CT6_NON_ACC_1ST_GEN = GMNonSccPlatformConfig(
|
||||
[GMNonAccCarDocs("Cadillac CT6 Non-ACC 2017-18")],
|
||||
CarSpecs(mass=2358, wheelbase=3.11, steerRatio=17.7, centerToFrontRatio=0.4),
|
||||
)
|
||||
CHEVROLET_TRAILBLAZER_NON_ACC_2ND_GEN = GMNonSccPlatformConfig(
|
||||
[GMNonAccCarDocs("Chevrolet Trailblazer Non-ACC 2021-22")],
|
||||
CHEVROLET_TRAILBLAZER.specs,
|
||||
)
|
||||
CHEVROLET_MALIBU_NON_ACC_9TH_GEN = GMNonSccPlatformConfig(
|
||||
[GMNonAccCarDocs("Chevrolet Malibu Non-ACC 2016-23")],
|
||||
CarSpecs(mass=1450, wheelbase=2.8, steerRatio=15.8, centerToFrontRatio=0.4),
|
||||
)
|
||||
CADILLAC_XT5_NON_ACC_1ST_GEN = GMNonSccPlatformConfig(
|
||||
[GMNonAccCarDocs("Cadillac XT5 Non-ACC 2018")],
|
||||
CarSpecs(mass=1810, wheelbase=2.86, steerRatio=16.34, centerToFrontRatio=0.5),
|
||||
)
|
||||
|
||||
|
||||
class CruiseButtons:
|
||||
INIT = 0
|
||||
UNPRESS = 1
|
||||
RES_ACCEL = 2
|
||||
DECEL_SET = 3
|
||||
MAIN = 5
|
||||
CANCEL = 6
|
||||
|
||||
|
||||
class AccState:
|
||||
OFF = 0
|
||||
ACTIVE = 1
|
||||
FAULTED = 3
|
||||
STANDSTILL = 4
|
||||
|
||||
|
||||
class CanBus:
|
||||
POWERTRAIN = 0
|
||||
OBSTACLE = 1
|
||||
CAMERA = 2
|
||||
CHASSIS = 2
|
||||
LOOPBACK = 128
|
||||
DROPPED = 192
|
||||
|
||||
|
||||
# In a Data Module, an identifier is a string used to recognize an object,
|
||||
# either by itself or together with the identifiers of parent objects.
|
||||
# Each returns a 4 byte hex representation of the decimal part number. `b"\x02\x8c\xf0'"` -> 42790951
|
||||
GM_BOOT_SOFTWARE_PART_NUMER_REQUEST = b'\x1a\xc0' # likely does not contain anything useful
|
||||
GM_SOFTWARE_MODULE_1_REQUEST = b'\x1a\xc1'
|
||||
GM_SOFTWARE_MODULE_2_REQUEST = b'\x1a\xc2'
|
||||
GM_SOFTWARE_MODULE_3_REQUEST = b'\x1a\xc3'
|
||||
|
||||
# Part number of XML data file that is used to configure ECU
|
||||
GM_XML_DATA_FILE_PART_NUMBER = b'\x1a\x9c'
|
||||
GM_XML_CONFIG_COMPAT_ID = b'\x1a\x9b' # used to know if XML file is compatible with the ECU software/hardware
|
||||
|
||||
# This DID is for identifying the part number that reflects the mix of hardware,
|
||||
# software, and calibrations in the ECU when it first arrives at the vehicle assembly plant.
|
||||
# If there's an Alpha Code, it's associated with this part number and stored in the DID $DB.
|
||||
GM_END_MODEL_PART_NUMBER_REQUEST = b'\x1a\xcb'
|
||||
GM_END_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST = b'\x1a\xdb'
|
||||
GM_BASE_MODEL_PART_NUMBER_REQUEST = b'\x1a\xcc'
|
||||
GM_BASE_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST = b'\x1a\xdc'
|
||||
GM_FW_RESPONSE = b'\x5a'
|
||||
|
||||
GM_FW_REQUESTS = [
|
||||
GM_BOOT_SOFTWARE_PART_NUMER_REQUEST,
|
||||
GM_SOFTWARE_MODULE_1_REQUEST,
|
||||
GM_SOFTWARE_MODULE_2_REQUEST,
|
||||
GM_SOFTWARE_MODULE_3_REQUEST,
|
||||
GM_XML_DATA_FILE_PART_NUMBER,
|
||||
GM_XML_CONFIG_COMPAT_ID,
|
||||
GM_END_MODEL_PART_NUMBER_REQUEST,
|
||||
GM_END_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST,
|
||||
GM_BASE_MODEL_PART_NUMBER_REQUEST,
|
||||
GM_BASE_MODEL_PART_NUMBER_ALPHA_CODE_REQUEST,
|
||||
]
|
||||
|
||||
GM_RX_OFFSET = 0x400
|
||||
|
||||
FW_QUERY_CONFIG = FwQueryConfig(
|
||||
requests=[request for req in GM_FW_REQUESTS for request in [
|
||||
Request(
|
||||
[StdQueries.SHORT_TESTER_PRESENT_REQUEST, req],
|
||||
[StdQueries.SHORT_TESTER_PRESENT_RESPONSE, GM_FW_RESPONSE + bytes([req[-1]])],
|
||||
rx_offset=GM_RX_OFFSET,
|
||||
bus=0,
|
||||
logging=True,
|
||||
),
|
||||
]],
|
||||
extra_ecus=[(Ecu.fwdCamera, 0x24b, None)],
|
||||
)
|
||||
|
||||
# TODO: detect most of these sets live
|
||||
EV_CAR = {CAR.CHEVROLET_VOLT, CAR.CHEVROLET_VOLT_2019, CAR.CHEVROLET_BOLT_EUV,
|
||||
# Non-ACC EV ports
|
||||
CAR.CHEVROLET_BOLT_NON_ACC, CAR.CHEVROLET_BOLT_NON_ACC_1ST_GEN, CAR.CHEVROLET_BOLT_NON_ACC_2ND_GEN}
|
||||
|
||||
# We're integrated at the camera with VOACC on these cars (instead of ASCM w/ OBD-II harness)
|
||||
CAMERA_ACC_CAR = {CAR.CHEVROLET_BOLT_EUV, CAR.CHEVROLET_SILVERADO, CAR.CHEVROLET_EQUINOX, CAR.CHEVROLET_TRAILBLAZER, CAR.GMC_YUKON}
|
||||
|
||||
# Alt ASCMActiveCruiseControlStatus
|
||||
ALT_ACCS = {CAR.GMC_YUKON}
|
||||
|
||||
# We're integrated at the Safety Data Gateway Module on these cars
|
||||
SDGM_CAR = {CAR.CADILLAC_XT4, CAR.CHEVROLET_VOLT_2019, CAR.CHEVROLET_TRAVERSE}
|
||||
|
||||
STEER_THRESHOLD = 1.0
|
||||
|
||||
DBC = CAR.create_dbc_map()
|
||||
490
artifacts/package_sources/iqdbc/iqdbc/car/honda/carcontroller.py
Normal file
490
artifacts/package_sources/iqdbc/iqdbc/car/honda/carcontroller.py
Normal file
@@ -0,0 +1,490 @@
|
||||
import numpy as np
|
||||
import math
|
||||
|
||||
from iqdbc.can import CANPacker
|
||||
from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, rate_limit, make_tester_present_msg, structs
|
||||
from iqdbc.car.common.pid import PIDController
|
||||
from iqdbc.car.honda import dash_lane, dash_objects, hondacan
|
||||
from iqdbc.car.honda.values import CAR, CruiseButtons, CruiseSettings, HONDA_BOSCH, HONDA_BOSCH_CANFD, HONDA_BOSCH_RADARLESS, \
|
||||
HONDA_BOSCH_TJA_CONTROL, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
|
||||
from iqdbc.car.interfaces import CarControllerBase
|
||||
|
||||
from iqdbc.lvbs.car.honda.aol import AolCarController
|
||||
from iqdbc.lvbs.car.honda.gas_interceptor import GasInterceptorCarController
|
||||
|
||||
VisualAlert = structs.CarControl.HUDControl.VisualAlert
|
||||
LongCtrlState = structs.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
def compute_gb_honda_bosch(accel, speed):
|
||||
# TODO returns 0s, is unused
|
||||
return 0.0, 0.0
|
||||
|
||||
|
||||
def compute_gb_honda_nidec(accel, speed):
|
||||
creep_brake = 0.0
|
||||
creep_speed = 2.3
|
||||
creep_brake_value = 0.15
|
||||
if speed < creep_speed:
|
||||
creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value
|
||||
gb = float(accel) / 4.8 - creep_brake
|
||||
return np.clip(gb, 0.0, 1.0), np.clip(-gb, 0.0, 1.0)
|
||||
|
||||
|
||||
def compute_gas_brake(accel, speed, fingerprint):
|
||||
if fingerprint in HONDA_BOSCH:
|
||||
return compute_gb_honda_bosch(accel, speed)
|
||||
else:
|
||||
return compute_gb_honda_nidec(accel, speed)
|
||||
|
||||
|
||||
# TODO not clear this does anything useful
|
||||
def actuator_hysteresis(brake, braking, brake_steady, v_ego, car_fingerprint):
|
||||
# hyst params
|
||||
brake_hyst_on = 0.02 # to activate brakes exceed this value
|
||||
brake_hyst_off = 0.005 # to deactivate brakes below this value
|
||||
brake_hyst_gap = 0.01 # don't change brake command for small oscillations within this value
|
||||
|
||||
# *** hysteresis logic to avoid brake blinking. go above 0.1 to trigger
|
||||
if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off:
|
||||
brake = 0.
|
||||
braking = brake > 0.
|
||||
|
||||
# for small brake oscillations within brake_hyst_gap, don't change the brake command
|
||||
if brake == 0.:
|
||||
brake_steady = 0.
|
||||
elif brake > brake_steady + brake_hyst_gap:
|
||||
brake_steady = brake - brake_hyst_gap
|
||||
elif brake < brake_steady - brake_hyst_gap:
|
||||
brake_steady = brake + brake_hyst_gap
|
||||
brake = brake_steady
|
||||
|
||||
return brake, braking, brake_steady
|
||||
|
||||
|
||||
def brake_pump_hysteresis(apply_brake, apply_brake_last, last_pump_ts, ts):
|
||||
pump_on = False
|
||||
|
||||
# reset pump timer if:
|
||||
# - there is an increment in brake request
|
||||
# - we are applying steady state brakes and we haven't been running the pump
|
||||
# for more than 20s (to prevent pressure bleeding)
|
||||
if apply_brake > apply_brake_last or (ts - last_pump_ts > 20. and apply_brake > 0):
|
||||
last_pump_ts = ts
|
||||
|
||||
# once the pump is on, run it for at least 0.2s
|
||||
if ts - last_pump_ts < 0.2 and apply_brake > 0:
|
||||
pump_on = True
|
||||
|
||||
return pump_on, last_pump_ts
|
||||
|
||||
|
||||
def process_hud_alert(hud_alert):
|
||||
alert_fcw = False
|
||||
alert_steer_required = False
|
||||
|
||||
# Make sure FCW is prioritized over steering required
|
||||
# TODO: implement separate available LDW alert
|
||||
if hud_alert == VisualAlert.fcw:
|
||||
alert_fcw = True
|
||||
elif hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw):
|
||||
alert_steer_required = True
|
||||
|
||||
return alert_fcw, alert_steer_required
|
||||
|
||||
|
||||
class CarController(CarControllerBase, AolCarController, GasInterceptorCarController):
|
||||
def __init__(self, dbc_names, CP, CP_IQ):
|
||||
CarControllerBase.__init__(self, dbc_names, CP, CP_IQ)
|
||||
AolCarController.__init__(self)
|
||||
GasInterceptorCarController.__init__(self, CP, CP_IQ)
|
||||
self.packer = CANPacker(dbc_names[Bus.pt])
|
||||
self.params = CarControllerParams(CP)
|
||||
self.CAN = hondacan.CanBus(CP)
|
||||
self.tja_control = CP.carFingerprint in HONDA_BOSCH_TJA_CONTROL
|
||||
|
||||
self.lane_renderer = dash_lane.LanePathRenderer()
|
||||
self.dash_object_author = dash_objects.DashObjectAuthor()
|
||||
self.rendered_lane = dash_lane.RenderedLane()
|
||||
self.lkas_hud_key = None
|
||||
self.lkas_state_change_frames = 0
|
||||
|
||||
self.braking = False
|
||||
self.brake_steady = 0.
|
||||
self.brake_last = 0.
|
||||
self.apply_brake_last = 0
|
||||
self.last_pump_ts = 0.
|
||||
self.stopping_counter = 0
|
||||
|
||||
self.accel = 0.0
|
||||
self.speed = 0.0
|
||||
self.gas = 0.0
|
||||
self.brake = 0.0
|
||||
self.last_torque = 0.0
|
||||
self.bosch_last_gas = 0
|
||||
|
||||
self.lkas_button_send_remaining = 0
|
||||
self.last_lkas_button_frame = 0
|
||||
self.radar_disable_counter = 0
|
||||
self.radar_mux = 0
|
||||
# stock RADAR_HUD_CANFD raises its CMBS bit only for a short burst after ACC engages; 10Hz hud ticks
|
||||
self.radar_hud_pulse = 0
|
||||
self.last_acc_enabled = False
|
||||
|
||||
self.gasfactor = 1.0
|
||||
self.gasfactor_before_maxgas = 1.0
|
||||
self.windfactor = 1.0
|
||||
self.windfactor_before_maxgas = 1.0
|
||||
self.windfactor_before_brake = 0.0
|
||||
self.pitch = 0.0
|
||||
|
||||
self.brake_pid = PIDController(k_p=0.0, k_i=1.0, pos_limit=0.0, neg_limit=-2.0, rate=50)
|
||||
self.brake_pid.reset()
|
||||
|
||||
def update(self, CC, CC_IQ, CS, now_nanos):
|
||||
AolCarController.update(self, self.CP, CC, CC_IQ)
|
||||
gas_pedal_force = 0.0
|
||||
min_gas = self.params.BOSCH_GAS_LOOKUP_BP[0]
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
hud_v_cruise = hud_control.setSpeed / CS.v_cruise_factor if hud_control.speedVisible else 255
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
|
||||
if len(CC.orientationNED) == 3:
|
||||
self.pitch = CC.orientationNED[1]
|
||||
hill_brake = math.sin(self.pitch) * ACCELERATION_DUE_TO_GRAVITY
|
||||
|
||||
if CC.longActive:
|
||||
accel = actuators.accel
|
||||
gas, brake = compute_gas_brake(actuators.accel + hill_brake, CS.out.vEgo, self.CP.carFingerprint)
|
||||
else:
|
||||
accel = 0.0
|
||||
gas, brake = 0.0, 0.0
|
||||
|
||||
# *** rate limit 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,
|
||||
CS.out.vEgo, self.CP.carFingerprint)
|
||||
|
||||
# *** rate limit after the enable check ***
|
||||
self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., 3 * DT_CTRL)
|
||||
|
||||
# vehicle hud display, wait for one update from 10Hz 0x304 msg
|
||||
alert_fcw, alert_steer_required = process_hud_alert(hud_control.visualAlert)
|
||||
|
||||
# **** process the car messages ****
|
||||
|
||||
# steer torque is converted back to CAN reference (positive when steering right)
|
||||
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 = []
|
||||
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and self.CP.openpilotLongitudinalControl:
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_CANFD and CS.stock_acc_alive:
|
||||
# CAN FD: the radar is silenced from here rather than from CarInterface.init(), and only once
|
||||
# the comma relay is confirmed open: init() ran under the ELM327 safety mode, so the
|
||||
# replacement ACC_CONTROL stream was blocked until the safety-mode switch landed, and whenever
|
||||
# that took longer than ~110ms after radar silence the brake module latched CRUISE_FAULT for
|
||||
# the whole drive. With the relay open the replacement stream starts within a few frames of
|
||||
# radar silence (see CS.stock_acc_alive), well inside the fault threshold
|
||||
if CS.canfd_relay_open:
|
||||
if self.radar_disable_counter % 50 == 0:
|
||||
# UDS extended diagnostic session, required before CommunicationControl
|
||||
can_sends.append((0x18DAB0F1, b'\x02\x10\x03\x00\x00\x00\x00\x00', self.CAN.pt))
|
||||
elif self.radar_disable_counter % 50 == 5:
|
||||
# UDS CommunicationControl disableRxAndTx (0x80 suppresses the response), retried every
|
||||
# 0.5s until the radar goes silent
|
||||
can_sends.append((0x18DAB0F1, b'\x03\x28\x83\x03\x00\x00\x00\x00', self.CAN.pt))
|
||||
self.radar_disable_counter += 1
|
||||
elif self.frame % 10 == 0:
|
||||
# tester present - w/ no response (keeps radar disabled)
|
||||
can_sends.append(make_tester_present_msg(0x18DAB0F1, self.CAN.pt, suppress_response=True))
|
||||
|
||||
# simulate the disabled canfd radar to prevent faults. These look-alikes are consumed by both the
|
||||
# camera (behind the relay, on the camera bus) and the powertrain: openpilot's own TX is not
|
||||
# forwarded across the open relay, so each frame is packed exactly once (the packer's
|
||||
# counter/checksum only advance once per cycle) and the identical bytes are mirrored onto both
|
||||
# buses (re-packing would double-increment the counter and desync the buses). While the stock
|
||||
# radar is still transmitting it authors all of these itself
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_CANFD and self.CP.openpilotLongitudinalControl and not CS.stock_acc_alive:
|
||||
if CC.enabled and not self.last_acc_enabled:
|
||||
self.radar_hud_pulse = 30 # ~3s at 10Hz, matching the stock 2-6s engage burst
|
||||
self.last_acc_enabled = CC.enabled
|
||||
radar_msgs = []
|
||||
if CS.hud_tick:
|
||||
radar_msgs.append(hondacan.create_radar_hud_canfd(self.packer, self.CAN.pt, CC.enabled, self.radar_hud_pulse > 0))
|
||||
if self.radar_hud_pulse > 0:
|
||||
self.radar_hud_pulse -= 1
|
||||
if CS.supp_tick:
|
||||
radar_msgs.append(hondacan.create_canfd_supplemental(self.packer, self.CAN.pt))
|
||||
if CS.radar_50hz_tick:
|
||||
# Cycle the radar MUX through the stock banks: 1-10, 17-26, 33-42, 49-58. This counter also
|
||||
# drives the LANE_PATH/HUD_OBJECTS mux below: it advances exactly one step per transmitted
|
||||
# frame, so the sweep stays contiguous even when a tick is missed (a frame-derived mux left
|
||||
# holes in the sweep the stock radar never produces).
|
||||
# These must be elif: a bare `if` at a bank start would fall through to the increment,
|
||||
# skipping the bank-start values (17, 33, 49)
|
||||
if self.radar_mux >= 58:
|
||||
self.radar_mux = 1
|
||||
elif self.radar_mux == 10:
|
||||
self.radar_mux = 17
|
||||
elif self.radar_mux == 26:
|
||||
self.radar_mux = 33
|
||||
elif self.radar_mux == 42:
|
||||
self.radar_mux = 49
|
||||
else:
|
||||
self.radar_mux += 1
|
||||
if CS.radar_5hz_tick:
|
||||
# RADAR_LEAD's LANE_PATH_LENGTH must track the valid-point count of the LANE_PATH sweep being
|
||||
# authored, and LEFT_LANE/RIGHT_LANE the per-side line-detected status, in lockstep with the
|
||||
# stock radar's behavior or the dash won't draw the lane lines
|
||||
radar_msgs.extend(hondacan.create_canfd_5hz_radar_messages(self.packer, self.CAN.pt, CS.radar_ref_counter,
|
||||
dash_lane.canfd_lane_length(self.rendered_lane),
|
||||
dash_lane.LANE_LINE_ON if self.rendered_lane.left_line else 0,
|
||||
dash_lane.LANE_LINE_ON if self.rendered_lane.right_line else 0))
|
||||
|
||||
for addr, dat, _ in radar_msgs:
|
||||
can_sends.append((addr, dat, self.CAN.pt))
|
||||
can_sends.append((addr, dat, self.CAN.camera))
|
||||
|
||||
# Send steering command.
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, self.CAN, apply_torque, CC.latActive, self.tja_control))
|
||||
|
||||
# wind brake from air resistance decel at high speed
|
||||
wind_brake = np.interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15]) * self.windfactor # not in m/s2 units
|
||||
wind_brake_ms2 = np.interp(CS.out.vEgo, [0.0, 13.4, 22.4, 31.3, 40.2], [0.000, 0.049, 0.136, 0.267, 0.441]) # in m/s2 units
|
||||
# all of this is only relevant for HONDA NIDEC
|
||||
max_accel = np.interp(CS.out.vEgo, self.params.NIDEC_MAX_ACCEL_BP, self.params.NIDEC_MAX_ACCEL_V)
|
||||
# TODO this 1.44 is just to maintain previous behavior
|
||||
pcm_speed_BP = [-wind_brake,
|
||||
-wind_brake * (3 / 4),
|
||||
0.0,
|
||||
0.5]
|
||||
# The Honda ODYSSEY seems to have different PCM_ACCEL
|
||||
# msgs, is it other cars too?
|
||||
if self.CP_IQ.enableGasInterceptor or not CC.longActive:
|
||||
pcm_speed = 0.0
|
||||
pcm_accel = int(0.0)
|
||||
elif self.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
|
||||
pcm_speed_V = [0.0,
|
||||
np.clip(CS.out.vEgo - 3.0, 0.0, 100.0),
|
||||
np.clip(CS.out.vEgo + 0.0, 0.0, 100.0),
|
||||
np.clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
|
||||
pcm_speed = float(np.interp(gas - brake, pcm_speed_BP, pcm_speed_V))
|
||||
pcm_accel = int(1.0 * self.params.NIDEC_GAS_MAX)
|
||||
else:
|
||||
pcm_speed_V = [0.0,
|
||||
np.clip(CS.out.vEgo - 2.0, 0.0, 100.0),
|
||||
np.clip(CS.out.vEgo + 2.0, 0.0, 100.0),
|
||||
np.clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
|
||||
pcm_speed = float(np.interp(gas - brake, pcm_speed_BP, pcm_speed_V))
|
||||
pcm_accel = int(np.clip((accel / 1.44) / max_accel, 0.0, 1.0) * self.params.NIDEC_GAS_MAX)
|
||||
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 2 == 0 and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS | HONDA_BOSCH_CANFD:
|
||||
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CAN))
|
||||
# If using stock ACC, spam cancel command to kill gas when OP disengages.
|
||||
if pcm_cancel_cmd:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.CANCEL, 0, CS.scm_ambient_light,
|
||||
self.CP.carFingerprint))
|
||||
elif CC.cruiseControl.resume:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.RES_ACCEL, 0, CS.scm_ambient_light,
|
||||
self.CP.carFingerprint))
|
||||
|
||||
else:
|
||||
# Send gas and brake commands.
|
||||
if self.frame % 2 == 0:
|
||||
ts = self.frame * DT_CTRL
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
# low-speed extra brake: the fixed accel command under-delivers approaching a stop, so an
|
||||
# integral-only term closes the gap, releasing at 1 m/s^3 once out of the window
|
||||
if (accel < min_gas) and (CS.out.vEgo < 3.0) and not (-1e-3 < CS.out.vEgo < 1e-3):
|
||||
brake_addon = self.brake_pid.update(error=accel - CS.out.aEgo, speed=CS.out.vEgo)
|
||||
target_accel = min(accel, accel + brake_addon)
|
||||
else:
|
||||
if (self.brake_pid.i < 0.0) and (accel < min_gas):
|
||||
self.brake_pid.i = min(0.0, self.brake_pid.i + 0.02)
|
||||
else:
|
||||
self.brake_pid.reset()
|
||||
target_accel = min(accel, accel + self.brake_pid.i)
|
||||
|
||||
self.accel = float(np.clip(target_accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX))
|
||||
# not using self.accel since the brake pid resets with the gas pedal
|
||||
gas_pedal_force = accel + wind_brake_ms2 * self.windfactor + hill_brake
|
||||
|
||||
# Live-learn gas pedal adjustments when openpilot is controlling gas.
|
||||
if (actuators.longControlState == LongCtrlState.pid) and (not CS.out.gasPressed):
|
||||
gas_error = accel - CS.out.aEgo
|
||||
if gas_error != 0.0 and gas_pedal_force > min_gas:
|
||||
if self.CP.carFingerprint in (CAR.HONDA_INSIGHT, CAR.HONDA_CIVIC_BOSCH): # gas pedal reacts too slowly
|
||||
learn_speed = 150
|
||||
elif self.CP.carFingerprint == CAR.ACURA_RDX_3G: # prevent overreacting to turbo lag
|
||||
learn_speed = 300
|
||||
else:
|
||||
learn_speed = 50
|
||||
self.gasfactor = np.clip(self.gasfactor + gas_error / learn_speed * (gas_pedal_force - min_gas), 0.01, 3.0)
|
||||
if gas_error != 0.0 and (not CS.out.brakePressed) and (CS.out.vEgo > 0.0):
|
||||
wind_learn_speed = 100 if self.CP.carFingerprint == CAR.ACURA_RDX_3G else 1000
|
||||
wind_adjust = 1 + wind_brake_ms2 / wind_learn_speed
|
||||
self.windfactor = np.clip(self.windfactor * (wind_adjust if (gas_error > 0) else 1.0 / wind_adjust), 0.1, 3.0)
|
||||
if gas_pedal_force <= min_gas:
|
||||
self.windfactor = max(self.windfactor, self.windfactor_before_brake)
|
||||
else:
|
||||
self.windfactor_before_brake = self.windfactor
|
||||
if gas_pedal_force >= self.params.BOSCH_ACCEL_MAX:
|
||||
self.gasfactor = min(self.gasfactor, self.gasfactor_before_maxgas)
|
||||
self.windfactor = min(self.windfactor, self.windfactor_before_maxgas)
|
||||
else:
|
||||
self.gasfactor_before_maxgas = self.gasfactor
|
||||
self.windfactor_before_maxgas = self.windfactor
|
||||
self.gas = float(np.interp((gas_pedal_force - min_gas) * self.gasfactor + min_gas,
|
||||
self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V))
|
||||
|
||||
# limit gas ramp to 60 units per frame, matches stock; higher sometimes makes the powertrain ignore the command
|
||||
max_gas = max(60, self.bosch_last_gas + 60)
|
||||
self.gas = min(self.gas, max_gas)
|
||||
self.bosch_last_gas = self.gas
|
||||
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
self.stopping_counter = self.stopping_counter + 1 if stopping else 0
|
||||
# CAN FD: never overlap the stock radar's own ACC_CONTROL stream; ours starts within a few
|
||||
# frames of the radar going silent (see the deferred radar disable above)
|
||||
if not (self.CP.carFingerprint in HONDA_BOSCH_CANFD and CS.stock_acc_alive):
|
||||
can_sends.extend(hondacan.create_acc_commands(self.packer, self.CAN, CC.enabled, CC.longActive, self.accel, self.gas,
|
||||
self.stopping_counter, self.CP, gas_pedal_force))
|
||||
else:
|
||||
apply_brake = np.clip(self.brake_last - wind_brake, 0.0, 1.0)
|
||||
apply_brake = int(np.clip(apply_brake * self.params.NIDEC_BRAKE_MAX, 0, self.params.NIDEC_BRAKE_MAX - 1))
|
||||
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
|
||||
|
||||
pcm_override = True
|
||||
can_sends.append(hondacan.create_brake_command(self.packer, self.CAN, apply_brake, pump_on,
|
||||
pcm_override, pcm_cancel_cmd, alert_fcw,
|
||||
self.CP.carFingerprint, CS.stock_brake, self.CP_IQ))
|
||||
self.apply_brake_last = apply_brake
|
||||
self.brake = apply_brake / self.params.NIDEC_BRAKE_MAX
|
||||
|
||||
gas_error = actuators.accel - CS.out.aEgo
|
||||
if (not CS.out.gasPressed) and (actuators.longControlState == LongCtrlState.pid) and self.CP_IQ.enableGasInterceptor:
|
||||
if gas_error != 0.0 and gas > 0.0:
|
||||
self.gasfactor = np.clip(self.gasfactor + gas_error / 50 * (gas * 4.8), 0.1, 3.0)
|
||||
if gas_error != 0.0 and (not CS.out.brakePressed) and (CS.out.vEgo > 0.0):
|
||||
wind_adjust = 1 + (wind_brake * 4.8) / 1000
|
||||
self.windfactor = np.clip(self.windfactor * (wind_adjust if (gas_error > 0) else 1.0 / wind_adjust), 0.1, 5.0)
|
||||
if gas <= 0.0:
|
||||
self.windfactor = max(self.windfactor, self.windfactor_before_brake)
|
||||
else:
|
||||
self.windfactor_before_brake = self.windfactor
|
||||
|
||||
can_sends.extend(GasInterceptorCarController.update(self, CC, CS, gas * self.gasfactor, brake, wind_brake, self.packer, self.frame))
|
||||
|
||||
# Send dashboard UI commands. On CAN FD, ACC_HUD is a radar look-alike that openpilot only owns
|
||||
# once it has disabled the radar; it rides the phase-locked 10Hz hud tick instead of frame % 10
|
||||
if (self.CP.carFingerprint in HONDA_BOSCH_CANFD and CS.hud_tick and
|
||||
self.CP.openpilotLongitudinalControl and not CS.stock_acc_alive):
|
||||
can_sends.append(hondacan.create_acc_hud(self.packer, self.CAN.pt, self.CP, CC.enabled, pcm_speed, actuators.accel,
|
||||
hud_control, hud_v_cruise, CS.is_metric, CS.acc_hud))
|
||||
|
||||
if self.frame % 10 == 0:
|
||||
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH_CANFD:
|
||||
# On Nidec, this also controls longitudinal positive acceleration
|
||||
can_sends.append(hondacan.create_acc_hud(self.packer, self.CAN.pt, self.CP, CC.enabled, pcm_speed, pcm_accel,
|
||||
hud_control, hud_v_cruise, CS.is_metric, CS.acc_hud))
|
||||
|
||||
steering_available = CS.out.cruiseState.available and CS.out.vEgo > self.CP.minSteerSpeed
|
||||
reduced_steering = CS.out.steeringPressed
|
||||
|
||||
lkas_state_change = None
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
# The key must contain exactly the signals that change the LKAS_HUD payload, nothing more:
|
||||
# a flickering input (like steer saturation) re-triggers the pulse continuously, which keeps
|
||||
# LKAS_STATE_CHANGE high and suppresses the dash lane lines entirely
|
||||
hud_key = (bool(CC.latActive), bool(self.dashed_lanes), bool(alert_steer_required), bool(CS.out.steerFaultPermanent))
|
||||
if hud_key != self.lkas_hud_key:
|
||||
self.lkas_hud_key = hud_key
|
||||
self.lkas_state_change_frames = 30 # 3s at the 10Hz LKAS_HUD rate, matching the stock pulse length
|
||||
lkas_state_change = self.lkas_state_change_frames > 0
|
||||
self.lkas_state_change_frames = max(0, self.lkas_state_change_frames - 1)
|
||||
|
||||
can_sends.extend(hondacan.create_lkas_hud(self.packer, self.CAN.lkas, self.CP, hud_control, CC.latActive,
|
||||
steering_available, reduced_steering, alert_steer_required, CS.lkas_hud, self.dashed_lanes,
|
||||
steer_fault_permanent=CS.out.steerFaultPermanent, lkas_state_change=lkas_state_change))
|
||||
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
# TODO: combining with create_acc_hud block above will change message order and will need replay logs regenerated
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS - HONDA_BOSCH_CANFD):
|
||||
can_sends.append(hondacan.create_radar_hud(self.packer, self.CAN.pt))
|
||||
if self.CP.carFingerprint == CAR.HONDA_CIVIC_BOSCH:
|
||||
can_sends.append(hondacan.create_legacy_brake_command(self.packer, self.CAN.pt))
|
||||
if self.CP.carFingerprint not in HONDA_BOSCH:
|
||||
self.speed = pcm_speed
|
||||
if not self.CP_IQ.enableGasInterceptor:
|
||||
self.gas = pcm_accel / self.params.NIDEC_GAS_MAX
|
||||
|
||||
# Render OP's lane and lead cars on the dash. On CAN FD these are radar look-alikes that only
|
||||
# exist (and are only allowed by panda safety) when the radar is disabled. Radarless keeps the
|
||||
# camera as the dash authority (known-good), so OP does not author these there
|
||||
if (CS.radar_50hz_tick and self.CP.carFingerprint in HONDA_BOSCH_CANFD and self.CP.openpilotLongitudinalControl
|
||||
and not CS.stock_acc_alive):
|
||||
leads = dash_objects.leads_from_model(self.model, CS.out.vEgo)
|
||||
lead = leads[0]
|
||||
lead_d = lead.dRel if lead.status else 0.0
|
||||
self.rendered_lane = self.lane_renderer.update(self.model, CS.out.vEgo, lead_d)
|
||||
mux = self.radar_mux
|
||||
# no LKAS_HUD_2 on CAN FD: the dash reads the lane length from the in-band terminator, so the
|
||||
# path is reshaped into the terminated-prefix form
|
||||
lane_offsets = dash_lane.canfd_lane_offsets(self.rendered_lane)
|
||||
lane_msg = dash_lane.create_lane_path(self.packer, self.CAN.lkas, lane_offsets, mux)
|
||||
can_sends.append(lane_msg)
|
||||
|
||||
# CAN FD cars have no camera HUD_OBJECTS to poll (the disabled radar owned it): author OP's
|
||||
# lead in slot 0 with the other slots blank (tracks=None)
|
||||
tracks = CS.camera_object_tracker.snapshot() if CS.camera_object_tracker is not None else None
|
||||
hud_msg = self.dash_object_author.create(self.packer, self.CAN.lkas, lead, tracks, mux, now_nanos * 1e-9,
|
||||
extra_leads=leads[1:])
|
||||
can_sends.append(hud_msg)
|
||||
|
||||
# the camera (behind the relay) also consumes these; mirror the identical packed bytes onto the
|
||||
# camera bus (packed once, so the counter/checksum stay in lockstep)
|
||||
for addr, dat, _ in (lane_msg, hud_msg):
|
||||
can_sends.append((addr, dat, self.CAN.camera))
|
||||
|
||||
# CAN FD: when stock LKAS is active, the touch-steering-wheel nag eventually forces an ACC
|
||||
# disengagement (a brake tap from the VSA). Disable LKAS automatically and block the driver's LKAS
|
||||
# button by taking over SCM_BUTTONS on the camera bus while engaged (panda blocks the forwarded
|
||||
# stock SCM_BUTTONS while this stream flows). Radarless keeps the stock camera LKAS untouched
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_CANFD and CC.enabled and self.frame % 4 == 0 and \
|
||||
not pcm_cancel_cmd and not CC.cruiseControl.resume:
|
||||
if self.lkas_button_send_remaining == 0 and CS.lkas_hud["LKAS_READY"] and self.frame >= self.last_lkas_button_frame + 500:
|
||||
self.lkas_button_send_remaining = 3
|
||||
|
||||
if self.lkas_button_send_remaining > 0:
|
||||
self.last_lkas_button_frame = self.frame
|
||||
self.lkas_button_send_remaining -= 1
|
||||
cruise_setting = CruiseSettings.LKAS
|
||||
elif CS.cruise_setting == CruiseSettings.LKAS:
|
||||
cruise_setting = 0 # block the driver's LKAS button press
|
||||
else:
|
||||
cruise_setting = CS.cruise_setting
|
||||
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CS.cruise_buttons, cruise_setting,
|
||||
CS.scm_ambient_light, self.CP.carFingerprint, bus=self.CAN.camera))
|
||||
|
||||
# Finalize actuator state for downstream consumers
|
||||
new_actuators = actuators.as_builder()
|
||||
new_actuators.speed = self.speed
|
||||
new_actuators.accel = self.accel
|
||||
new_actuators.gas = self.gas
|
||||
new_actuators.brake = self.brake
|
||||
new_actuators.torque = self.last_torque
|
||||
new_actuators.torqueOutputCan = apply_torque
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
383
artifacts/package_sources/iqdbc/iqdbc/car/honda/carstate.py
Normal file
383
artifacts/package_sources/iqdbc/iqdbc/car/honda/carstate.py
Normal file
@@ -0,0 +1,383 @@
|
||||
import numpy as np
|
||||
from collections import defaultdict
|
||||
|
||||
from iqdbc.can import CANDefine, CANParser
|
||||
from iqdbc.car import Bus, create_button_events, structs, DT_CTRL
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.honda.hondacan import CanBus
|
||||
from iqdbc.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, HONDA_BOSCH_ALT_RADAR, HONDA_BOSCH_CANFD, \
|
||||
HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS, HONDA_BOSCH_TJA_CONTROL, \
|
||||
HondaFlags, CruiseButtons, CruiseSettings, GearShifter, CarControllerParams
|
||||
from iqdbc.car.honda.dash_objects import CameraObjectTracker
|
||||
from iqdbc.car.interfaces import CarStateBase
|
||||
|
||||
from iqdbc.lvbs.car.honda.iq_carstate import IQCarState
|
||||
|
||||
TransmissionType = structs.CarParams.TransmissionType
|
||||
ButtonType = structs.CarState.ButtonEvent.Type
|
||||
|
||||
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
|
||||
CruiseButtons.MAIN: ButtonType.mainCruise, CruiseButtons.CANCEL: ButtonType.cancel}
|
||||
SETTINGS_BUTTONS_DICT = {CruiseSettings.DISTANCE: ButtonType.gapAdjustCruise, CruiseSettings.LKAS: ButtonType.lkas}
|
||||
|
||||
|
||||
class CarState(CarStateBase, IQCarState):
|
||||
def __init__(self, CP, CP_IQ):
|
||||
CarStateBase.__init__(self, CP, CP_IQ)
|
||||
IQCarState.__init__(self, CP, CP_IQ)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint][Bus.pt])
|
||||
|
||||
if CP.transmissionType != TransmissionType.manual:
|
||||
self.gearbox_msg = "GEARBOX_AUTO"
|
||||
if CP.transmissionType == TransmissionType.cvt:
|
||||
self.gearbox_msg = "GEARBOX_CVT"
|
||||
self.shifter_values = can_define.dv[self.gearbox_msg]["GEAR_SHIFTER"]
|
||||
|
||||
self.car_state_scm_msg = "SCM_FEEDBACK"
|
||||
if CP.carFingerprint in HONDA_NIDEC_ALT_SCM_MESSAGES:
|
||||
self.car_state_scm_msg = "SCM_BUTTONS"
|
||||
|
||||
self.brake_error_msg = "HYBRID_BRAKE_ERROR" if CP.flags & HondaFlags.HYBRID else "STANDSTILL"
|
||||
|
||||
self.steer_status_values = defaultdict(lambda: "UNKNOWN", can_define.dv["STEER_STATUS"]["STEER_STATUS"])
|
||||
|
||||
self.brake_switch_prev = False
|
||||
self.brake_switch_active = False
|
||||
self.low_speed_alert = False
|
||||
|
||||
self.dynamic_v_cruise_units = self.CP.carFingerprint in (HONDA_BOSCH_RADARLESS | HONDA_BOSCH_ALT_RADAR |
|
||||
HONDA_BOSCH_TJA_CONTROL | HONDA_BOSCH_CANFD)
|
||||
self.cruise_setting = 0
|
||||
self.v_cruise_pcm_prev = 0
|
||||
|
||||
# When available we use cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] to populate vEgoCluster
|
||||
# However, on cars without a digital speedometer this is not always present (HRV, FIT, CRV 2016, ILX and RDX)
|
||||
self.dash_speed_seen = False
|
||||
self.is_metric = False
|
||||
self.v_cruise_factor = 1.
|
||||
|
||||
self.initial_accFault_cleared = False
|
||||
self.initial_accFault_cleared_timer = int(10 / DT_CTRL) # 10 seconds after startup for initial faults to clear
|
||||
|
||||
self.scm_ambient_light = 0
|
||||
|
||||
self.radar_ref_counter = 0
|
||||
self.radar_5hz_tick_counter = 0
|
||||
self.radar_5hz_tick = False
|
||||
self.supp_tick_counter = 0
|
||||
self.supp_tick = False
|
||||
self.hud_tick_counter = 0
|
||||
self.hud_tick = False
|
||||
self.radar_50hz_tick_counter = 0
|
||||
self.radar_50hz_tick = False
|
||||
|
||||
# CAN FD deferred radar disable (see carcontroller): the stock radar is assumed alive until it has
|
||||
# been silent for a few frames, and the relay is detected open once the camera's STEERING_CONTROL
|
||||
# stops being physically visible on the PT bus
|
||||
self.stock_acc_counter = 0
|
||||
self.stock_acc_alive = False
|
||||
self.camera_steer_counter = 0
|
||||
self.camera_steer_seen = False
|
||||
self.canfd_frames = 0
|
||||
self.canfd_relay_open = False
|
||||
|
||||
# only radarless cameras emit HUD_OBJECTS to poll for adjacent-car positions; on CAN FD the
|
||||
# (disabled) radar owned it, so there is nothing to track
|
||||
self.camera_object_tracker = CameraObjectTracker() if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS else None
|
||||
|
||||
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
if self.CP.enableBsm:
|
||||
cp_body = can_parsers[Bus.body]
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
cp_radar = can_parsers[Bus.radar]
|
||||
|
||||
ret = structs.CarState()
|
||||
ret_iq = structs.IQCarState()
|
||||
|
||||
# car params
|
||||
v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping
|
||||
v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
|
||||
|
||||
# update prevs, update must run once per loop
|
||||
prev_cruise_buttons = self.cruise_buttons
|
||||
prev_cruise_setting = self.cruise_setting
|
||||
self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"]
|
||||
self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"]
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH_RADARLESS | HONDA_BOSCH_CANFD):
|
||||
# The camera consumes SCM_BUTTONS content beyond the buttons (losing/zeroing this byte raises an
|
||||
# adaptive high beam error), so it must be echoed on frames sent in the SCM's place
|
||||
self.scm_ambient_light = cp.vl["SCM_BUTTONS"]["AMBIENT_LIGHT_MAYBE"]
|
||||
|
||||
# used for car hud message
|
||||
# TODO: find CAR_SPEED for HONDA_ODYSSEY_TWN or use ACC_HUD w/ detection
|
||||
self.is_metric = self.CP.carFingerprint in (CAR.HONDA_ODYSSEY_TWN,) or not cp.vl["CAR_SPEED"]["IMPERIAL_UNIT"]
|
||||
self.v_cruise_factor = CV.MPH_TO_MS if self.dynamic_v_cruise_units and not self.is_metric else CV.KPH_TO_MS
|
||||
|
||||
# ******************* parse out can *******************
|
||||
|
||||
# blend in transmission speed at low speed, since it has more low speed accuracy
|
||||
# STANDSTILL->WHEELS_MOVING bit can be noisy around zero, so use XMISSION_SPEED
|
||||
v_wheel = sum([cp.vl["WHEEL_SPEEDS"][f"WHEEL_SPEED_{s}"] for s in ("FL", "FR", "RL", "RR")]) / 4.0 * CV.KPH_TO_MS
|
||||
v_weight = float(np.interp(v_wheel, v_weight_bp, v_weight_v))
|
||||
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * self.CP.wheelSpeedFactor + v_weight * v_wheel
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 1e-5
|
||||
|
||||
# doorOpen is true if we can find any door open, but signal locations vary, and we may only see the driver's door
|
||||
# TODO: Test the eight Nidec cars without SCM signals for driver's door state, may be able to consolidate further
|
||||
if self.CP.flags & HondaFlags.HAS_ALL_DOOR_STATES:
|
||||
ret.doorOpen = any([cp.vl["DOORS_STATUS"]["DOOR_OPEN_FL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_FR"],
|
||||
cp.vl["DOORS_STATUS"]["DOOR_OPEN_RL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_RR"]])
|
||||
elif "DRIVERS_DOOR_OPEN" in cp.vl["SCM_BUTTONS"]:
|
||||
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
|
||||
else:
|
||||
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"])
|
||||
|
||||
ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LAMP"] or not cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LATCHED"])
|
||||
|
||||
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]["STEER_STATUS"]]
|
||||
ret.steerFaultPermanent = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2", "LOW_SPEED_LOCKOUT", "TMP_FAULT")
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH_ALT_RADAR | HONDA_BOSCH_CANFD):
|
||||
# TODO: See if this logic works for all other Honda
|
||||
min_steer_speed = max(CarControllerParams.STEER_GLOBAL_MIN_SPEED, self.CP.minSteerSpeed)
|
||||
expected_low_speed_lockout = steer_status == "LOW_SPEED_LOCKOUT" and ret.vEgo < min_steer_speed
|
||||
ret.steerFaultTemporary = steer_status != "NORMAL" and not expected_low_speed_lockout
|
||||
else:
|
||||
# LOW_SPEED_LOCKOUT is not worth a warning
|
||||
# NO_TORQUE_ALERT_2 can be caused by bump or steering nudge from driver
|
||||
# FIXME: the stock camera stops steering on NO_TORQUE_ALERT_1
|
||||
ret.steerFaultTemporary = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "TJA_LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2")
|
||||
|
||||
# All Honda EPS cut off slightly above standstill, some much higher
|
||||
# Don't alert in the near-standstill range, but alert for per-vehicle configured minimums above that
|
||||
if CarControllerParams.STEER_GLOBAL_MIN_SPEED < ret.vEgo < (self.CP.minSteerSpeed + 0.5):
|
||||
self.low_speed_alert = True
|
||||
elif ret.vEgo > (self.CP.minSteerSpeed + 1.):
|
||||
# TODO: better handle delayed steering enablement on ALT_RADAR cars
|
||||
self.low_speed_alert = False
|
||||
ret.lowSpeedAlert = self.low_speed_alert
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
ret.accFaulted = bool(cp.vl["CRUISE_FAULT_STATUS"]["CRUISE_FAULT"])
|
||||
else:
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH_CANFD | HONDA_BOSCH_TJA_CONTROL) and (self.CP.flags & HondaFlags.BOSCH_ALT_BRAKE):
|
||||
ret.accFaulted = bool(cp.vl["BRAKE_MODULE"]["CRUISE_FAULT"])
|
||||
else:
|
||||
ret.accFaulted = bool(cp.vl[self.brake_error_msg]["BRAKE_ERROR_1"] or cp.vl[self.brake_error_msg]["BRAKE_ERROR_2"])
|
||||
|
||||
# Log non-critical stock ACC/LKAS faults if Nidec (camera)
|
||||
if self.CP.carFingerprint not in HONDA_BOSCH:
|
||||
ret.carFaultedNonCritical = bool(cp_cam.vl["ACC_HUD"]["ACC_PROBLEM"] or cp_cam.vl["LKAS_HUD"]["LKAS_PROBLEM"])
|
||||
|
||||
ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0
|
||||
|
||||
if self.CP.carFingerprint not in (CAR.HONDA_ODYSSEY_TWN,):
|
||||
self.dash_speed_seen = self.dash_speed_seen or cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] > 1e-3
|
||||
if self.dash_speed_seen:
|
||||
conversion = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
|
||||
ret.vEgoCluster = cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] * conversion
|
||||
|
||||
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"]
|
||||
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE_RATE"]
|
||||
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
|
||||
250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
|
||||
ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1
|
||||
ret.parkingBrake = bool(cp.vl[self.car_state_scm_msg]["PARKING_BRAKE_ON"])
|
||||
|
||||
if self.CP.transmissionType == TransmissionType.manual:
|
||||
ret.gearShifter = GearShifter.reverse if bool(cp.vl["SCM_FEEDBACK"]["REVERSE_LIGHT"]) else GearShifter.drive
|
||||
else:
|
||||
gear_position = self.shifter_values.get(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"], None)
|
||||
ret.gearShifter = self.parse_gear_shifter(gear_position)
|
||||
|
||||
ret.gasPressed = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"] > 1e-5
|
||||
|
||||
ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD.get(self.CP.carFingerprint, 1200)
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
# The PCM always manages its own cruise control state, but doesn't publish it
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
ret.cruiseState.nonAdaptive = cp_cam.vl["ACC_HUD"]["CRUISE_CONTROL_LABEL"] != 0
|
||||
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
# ACC_HUD is on camera bus on radarless cars
|
||||
acc_hud = cp_cam.vl["ACC_HUD"] if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS else cp.vl["ACC_HUD"]
|
||||
ret.cruiseState.nonAdaptive = acc_hud["CRUISE_CONTROL_LABEL"] != 0
|
||||
ret.cruiseState.standstill = acc_hud["CRUISE_SPEED"] == 252.
|
||||
|
||||
# On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
|
||||
ret.cruiseState.speed = self.v_cruise_pcm_prev if acc_hud["CRUISE_SPEED"] > 160.0 else acc_hud["CRUISE_SPEED"] * self.v_cruise_factor
|
||||
self.v_cruise_pcm_prev = ret.cruiseState.speed
|
||||
else:
|
||||
ret.cruiseState.speed = cp.vl["CRUISE"]["CRUISE_SPEED_PCM"] * CV.KPH_TO_MS
|
||||
|
||||
if self.CP.flags & HondaFlags.BOSCH_ALT_BRAKE:
|
||||
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
|
||||
else:
|
||||
# brake switch has shown some single time step noise, so only considered when
|
||||
# switch is on for at least 2 consecutive CAN samples
|
||||
# brake switch rises earlier than brake pressed but is never 1 when in park
|
||||
brake_switch_vals = cp.vl_all["POWERTRAIN_DATA"]["BRAKE_SWITCH"]
|
||||
if len(brake_switch_vals):
|
||||
brake_switch = cp.vl["POWERTRAIN_DATA"]["BRAKE_SWITCH"] != 0
|
||||
if len(brake_switch_vals) > 1:
|
||||
self.brake_switch_prev = brake_switch_vals[-2] != 0
|
||||
self.brake_switch_active = brake_switch and self.brake_switch_prev
|
||||
self.brake_switch_prev = brake_switch
|
||||
ret.brakePressed = (cp.vl["POWERTRAIN_DATA"]["BRAKE_PRESSED"] != 0) or self.brake_switch_active
|
||||
|
||||
ret.brake = cp.vl["VSA_STATUS"]["USER_BRAKE"]
|
||||
ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0
|
||||
ret.cruiseState.available = bool(cp.vl[self.car_state_scm_msg]["MAIN_ON"])
|
||||
|
||||
# Bosch cars can report stale ACC faults during early startup.
|
||||
if ret.accFaulted:
|
||||
if (self.CP.carFingerprint in HONDA_BOSCH) and not self.initial_accFault_cleared:
|
||||
# Gate initial stale faults via availability (accFaulted is sticky until offroad).
|
||||
ret.accFaulted = False
|
||||
ret.cruiseState.available = False
|
||||
elif self.initial_accFault_cleared_timer == 0:
|
||||
self.initial_accFault_cleared = True
|
||||
|
||||
if self.initial_accFault_cleared_timer > 0:
|
||||
self.initial_accFault_cleared_timer -= 1
|
||||
|
||||
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
|
||||
if self.CP.carFingerprint in (CAR.HONDA_PILOT, CAR.HONDA_RIDGELINE):
|
||||
if ret.brake > 0.1:
|
||||
ret.brakePressed = True
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
# TODO: find the radarless AEB_STATUS bit and make sure ACCEL_COMMAND is correct to enable AEB alerts
|
||||
if self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS:
|
||||
ret.stockAeb = (not self.CP.openpilotLongitudinalControl) and bool(cp.vl["ACC_CONTROL"]["AEB_STATUS"] and cp.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
|
||||
else:
|
||||
ret.stockAeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5)
|
||||
|
||||
self.acc_hud = False
|
||||
self.lkas_hud = False
|
||||
if self.CP.carFingerprint not in HONDA_BOSCH:
|
||||
ret.stockFcw = cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0
|
||||
self.acc_hud = cp_cam.vl["ACC_HUD"]
|
||||
self.stock_brake = cp_cam.vl["BRAKE_COMMAND"]
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH_RADARLESS | HONDA_BOSCH_CANFD):
|
||||
self.lkas_hud = cp_cam.vl["LKAS_HUD"]
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
# The radar emits low-rate tick reference messages that keep running even while its data
|
||||
# messages are disabled, so the look-alikes are phased to the stock cadence off of them.
|
||||
#
|
||||
# There is a one-frame (10 ms) delay between reading a tick here in carstate and transmitting the
|
||||
# response in carcontroller. The stock radar sends each data message in the SAME frame as its
|
||||
# tick, so we pulse one frame BEFORE the next tick (counter == period-1): the +1 transmit delay
|
||||
# then lands the message on the next tick frame, matching stock.
|
||||
# period (frames @100Hz): 0x710=100, 0x730=10, 0x750=2, RADAR_REFERENCE=20
|
||||
self.radar_ref_counter = cp.vl["RADAR_REFERENCE"]["COUNTER"]
|
||||
|
||||
# 5 Hz: RADAR_REFERENCE (0x3A1) is on the powertrain bus (cp), not the radar bus (cp_radar).
|
||||
# RADAR_LEAD does NOT ride with the reference; stock sends it ~120 ms (12 frames) after, so fire
|
||||
# at frame 11 (+1 transmit delay -> ~120 ms)
|
||||
ref_tick_vals = cp.vl_all.get("RADAR_REFERENCE", {}).get("COUNTER", [])
|
||||
if len(ref_tick_vals) > 0:
|
||||
self.radar_5hz_tick_counter = 0
|
||||
else:
|
||||
self.radar_5hz_tick_counter += 1
|
||||
self.radar_5hz_tick = (self.radar_5hz_tick_counter == 11)
|
||||
|
||||
supp_tick_vals = cp_radar.vl_all.get("RADAR_SUPP_TICK_REFERENCE", {}).get("IGNORE", [])
|
||||
if len(supp_tick_vals) > 0:
|
||||
self.supp_tick_counter = 0
|
||||
else:
|
||||
self.supp_tick_counter += 1
|
||||
self.supp_tick = (self.supp_tick_counter == 99)
|
||||
|
||||
hud_tick_vals = cp_radar.vl_all.get("RADAR_HUD_TICK_REFERENCE", {}).get("IGNORE", [])
|
||||
if len(hud_tick_vals) > 0:
|
||||
self.hud_tick_counter = 0
|
||||
else:
|
||||
self.hud_tick_counter += 1
|
||||
self.hud_tick = (self.hud_tick_counter == 9)
|
||||
|
||||
tick_50hz_vals = cp_radar.vl_all.get("RADAR_50HZ_TICK_REFERENCE", {}).get("IGNORE", [])
|
||||
if len(tick_50hz_vals) > 0:
|
||||
self.radar_50hz_tick_counter = 0
|
||||
else:
|
||||
self.radar_50hz_tick_counter += 1
|
||||
self.radar_50hz_tick = (self.radar_50hz_tick_counter == 1)
|
||||
|
||||
# Deferred radar disable (see carcontroller). The stock radar transmits ACC_CONTROL every 2
|
||||
# frames, so 4 missed frames means it has been silenced; assume alive until then so the
|
||||
# replacement stream never overlaps it
|
||||
self.canfd_frames += 1
|
||||
if len(cp.vl_all.get("ACC_CONTROL", {}).get("COUNTER", [])) > 0:
|
||||
self.stock_acc_counter = 0
|
||||
else:
|
||||
self.stock_acc_counter += 1
|
||||
self.stock_acc_alive = self.stock_acc_counter < 4
|
||||
|
||||
# While the comma relay is closed the camera's STEERING_CONTROL is physically visible on the PT
|
||||
# bus; when the relay opens it disappears (openpilot's own 0xE4 TX is not parsed as RX). As a
|
||||
# fallback, assume the relay is open after 5 s of controls in case the camera was never seen
|
||||
if len(cp.vl_all.get("STEERING_CONTROL", {}).get("COUNTER", [])) > 0:
|
||||
self.camera_steer_counter = 0
|
||||
self.camera_steer_seen = True
|
||||
else:
|
||||
self.camera_steer_counter += 1
|
||||
self.canfd_relay_open = (self.camera_steer_seen and self.camera_steer_counter >= 5) or self.canfd_frames >= 500
|
||||
else:
|
||||
self.supp_tick = False
|
||||
self.hud_tick = False
|
||||
self.radar_5hz_tick = False
|
||||
self.radar_50hz_tick = False
|
||||
|
||||
if self.CP.enableBsm:
|
||||
# BSM messages are on B-CAN, requires a panda forwarding B-CAN messages to CAN 0
|
||||
# more info here: https://github.com/commaai/openpilot/pull/1867
|
||||
ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1
|
||||
ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1
|
||||
|
||||
ret.buttonEvents = [
|
||||
*create_button_events(self.cruise_buttons, prev_cruise_buttons, BUTTONS_DICT),
|
||||
*create_button_events(self.cruise_setting, prev_cruise_setting, SETTINGS_BUTTONS_DICT),
|
||||
]
|
||||
|
||||
IQCarState.update(self, ret, ret_iq, can_parsers)
|
||||
|
||||
if self.camera_object_tracker is not None:
|
||||
self.camera_object_tracker.update(cp_cam)
|
||||
|
||||
return ret, ret_iq
|
||||
|
||||
def get_can_parsers(self, CP, CP_IQ):
|
||||
pt_messages = []
|
||||
cam_messages = []
|
||||
if CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
# Radar-alive and relay-open detection for the deferred radar disable (see carcontroller).
|
||||
# Both messages intentionally go silent (the radar is disabled, the camera ends up behind the
|
||||
# open relay), so subscribe with NaN frequency to skip the alive/timeout checks
|
||||
pt_messages += [("ACC_CONTROL", float('nan')), ("STEERING_CONTROL", float('nan'))]
|
||||
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
# polled by the CameraObjectTracker, but not every radarless camera emits it
|
||||
cam_messages += [("HUD_OBJECTS", float('nan'))]
|
||||
parsers = {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, CanBus(CP).pt),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CanBus(CP).camera),
|
||||
}
|
||||
if CP.enableBsm:
|
||||
parsers[Bus.body] = CANParser(DBC[CP.carFingerprint][Bus.body], [], CanBus(CP).radar)
|
||||
if CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
# The tick references are only read via vl_all, which (unlike vl) does not auto-subscribe
|
||||
# messages, so they must be listed explicitly or they are never parsed.
|
||||
# 0x710 RADAR_SUPP_TICK_REFERENCE (1 Hz), 0x730 RADAR_HUD_TICK_REFERENCE (10 Hz),
|
||||
# 0x750 RADAR_50HZ_TICK_REFERENCE (50 Hz)
|
||||
parsers[Bus.radar] = CANParser(DBC[CP.carFingerprint][Bus.radar], [
|
||||
("RADAR_SUPP_TICK_REFERENCE", 0),
|
||||
("RADAR_HUD_TICK_REFERENCE", 0),
|
||||
("RADAR_50HZ_TICK_REFERENCE", 0),
|
||||
], CanBus(CP).radar)
|
||||
|
||||
return parsers
|
||||
186
artifacts/package_sources/iqdbc/iqdbc/car/honda/dash_lane.py
Normal file
186
artifacts/package_sources/iqdbc/iqdbc/car/honda/dash_lane.py
Normal file
@@ -0,0 +1,186 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
import numpy as np
|
||||
|
||||
POINT_COUNT = 40
|
||||
POINTS_PER_FRAME = 4
|
||||
SWEEP_INDICES = POINT_COUNT // POINTS_PER_FRAME
|
||||
|
||||
# the camera repeats each sweep index across four redundant banks: mux = index + bank*16,
|
||||
# giving mux values 1-10, 17-26, 33-42 and 49-58 for logical indices 0-9
|
||||
MUX_CYCLE = tuple(index + bank * 16 for bank in range(4) for index in range(1, SWEEP_INDICES + 1))
|
||||
|
||||
OFFSET_UNAVAILABLE = 2047
|
||||
OFFSET_VALID_MAX = 2046
|
||||
|
||||
NEAR_M = 2.0
|
||||
FAR_M = 100.0
|
||||
LOOKAHEAD_M = np.linspace(NEAR_M, FAR_M, POINT_COUNT)
|
||||
|
||||
# full swing center -> max turn is slewed over this long so model jumps can't teleport the dash lane
|
||||
SLEW_RATE_HZ = 50.0
|
||||
SLEW_FULL_SCALE_S = 2.0
|
||||
SLEW_MAX_STEP = OFFSET_VALID_MAX / (SLEW_FULL_SCALE_S * SLEW_RATE_HZ)
|
||||
|
||||
|
||||
def _stock_gain(d):
|
||||
# raw offset units per meter of lateral, regressed from stock radar sweeps vs modelV2 lane centers
|
||||
return 29.3 + 0.243 * d - 0.00228 * d ** 2
|
||||
|
||||
|
||||
def _legacy_gain(d):
|
||||
return 6.27 + 0.0106 * d + 0.000354 * d ** 2
|
||||
|
||||
|
||||
GAIN = _stock_gain(LOOKAHEAD_M)
|
||||
|
||||
|
||||
def gain_correction(d: float) -> float:
|
||||
# the HUD lead marker's lateral scale was tuned against lanes drawn with the legacy (flatter) gain
|
||||
# law, so the lead's lateral must ride this ratio to stay on the corrected lane rendering
|
||||
d = min(max(float(d), NEAR_M), FAR_M)
|
||||
return _stock_gain(d) / _legacy_gain(d)
|
||||
|
||||
|
||||
LANE_LINE_ON = 3
|
||||
LANE_LENGTH_MAX_VALUE = 33
|
||||
LANE_WIDTH_DEFAULT = 32
|
||||
|
||||
LINE_PROB_ON = 0.25
|
||||
LINE_PROB_OFF = 0.10
|
||||
HALF_LANE_M = 1.65
|
||||
FULL_REACH_SPEED = 27.0
|
||||
FULL_REACH_LEAD_DIST = 70.0
|
||||
MIN_REACH = 0.15
|
||||
|
||||
|
||||
def encode_lane_path(x, y):
|
||||
x = np.asarray(x, dtype=float)
|
||||
y = np.asarray(y, dtype=float)
|
||||
if x.size < 2 or x.max() < FAR_M:
|
||||
return [OFFSET_UNAVAILABLE] * POINT_COUNT
|
||||
lat = np.interp(LOOKAHEAD_M, x, y)
|
||||
# stock encodes offsets with the opposite lateral sign to openpilot's +left convention
|
||||
raw = np.clip(np.round(-GAIN * lat), -OFFSET_VALID_MAX, OFFSET_VALID_MAX)
|
||||
return [int(v) for v in raw]
|
||||
|
||||
|
||||
# The CAN FD dash has no LKAS_HUD_2 to carry the drawn length: it reads the path as a contiguous valid
|
||||
# prefix ended by an in-band OFFSET_UNAVAILABLE terminator, idles at 6 valid zero offsets (never
|
||||
# all-unavailable), and cross-checks the prefix length against RADAR_LEAD's LANE_PATH_LENGTH.
|
||||
CANFD_MAX_VALID_PTS = 23
|
||||
CANFD_MIN_VALID_PTS = 6
|
||||
CANFD_IDLE_OFFSETS = [0] * CANFD_MIN_VALID_PTS + [OFFSET_UNAVAILABLE] * (POINT_COUNT - CANFD_MIN_VALID_PTS)
|
||||
|
||||
# stock valid-point count is a function of ego speed alone, fit from factory lanes-on RADAR_LEAD frames
|
||||
CANFD_LEN_INTERCEPT = 6.74
|
||||
CANFD_LEN_SLOPE = 0.862
|
||||
|
||||
|
||||
@dataclass
|
||||
class RenderedLane:
|
||||
offsets: list[int] = field(default_factory=lambda: [OFFSET_UNAVAILABLE] * POINT_COUNT)
|
||||
reach: float = 0.0
|
||||
left_line: bool = False
|
||||
right_line: bool = False
|
||||
lane_cross: int = 0
|
||||
v_ego: float = 0.0
|
||||
|
||||
@property
|
||||
def blank(self) -> bool:
|
||||
return self.reach <= 0.0 or self.offsets[0] == OFFSET_UNAVAILABLE
|
||||
|
||||
|
||||
def canfd_lane_length(lane: RenderedLane) -> int:
|
||||
if lane.blank:
|
||||
return CANFD_MIN_VALID_PTS
|
||||
n = round(CANFD_LEN_INTERCEPT + CANFD_LEN_SLOPE * lane.v_ego)
|
||||
return max(CANFD_MIN_VALID_PTS, min(CANFD_MAX_VALID_PTS, n))
|
||||
|
||||
|
||||
def canfd_lane_offsets(lane: RenderedLane) -> list[int]:
|
||||
if lane.blank:
|
||||
return CANFD_IDLE_OFFSETS
|
||||
n_valid = canfd_lane_length(lane)
|
||||
return list(lane.offsets[:n_valid]) + [OFFSET_UNAVAILABLE] * (POINT_COUNT - n_valid)
|
||||
|
||||
|
||||
def create_lane_path(packer, bus, offsets, mux):
|
||||
base = ((mux - 1) % 16) * POINTS_PER_FRAME
|
||||
values = {"MUX": mux}
|
||||
for i in range(POINTS_PER_FRAME):
|
||||
values[f"PATH_OFFSET_{i + 1}"] = offsets[base + i]
|
||||
return packer.make_can_msg("LANE_PATH", bus, values)
|
||||
|
||||
|
||||
def create_lkas_hud_2(packer, bus, counter_2, reach=1.0, lane_cross=0, left_line=True, right_line=True):
|
||||
lane_length = max(0, min(LANE_LENGTH_MAX_VALUE, round(reach * LANE_LENGTH_MAX_VALUE)))
|
||||
shown = lane_length > 0
|
||||
values = {
|
||||
"COUNTER_2": counter_2,
|
||||
"SET_ME_X01": 1,
|
||||
"LANE_WIDTH": LANE_WIDTH_DEFAULT,
|
||||
"LEFT_LANE": LANE_LINE_ON if (shown and left_line) else 0,
|
||||
"RIGHT_LANE": LANE_LINE_ON if (shown and right_line) else 0,
|
||||
"LEFT_LANE_CROSSED": 1 if (shown and lane_cross < 0) else 0,
|
||||
"RIGHT_LANE_CROSSED": 1 if (shown and lane_cross > 0) else 0,
|
||||
"LANE_LENGTH": lane_length,
|
||||
}
|
||||
return packer.make_can_msg("LKAS_HUD_2", bus, values)
|
||||
|
||||
|
||||
class LanePathRenderer:
|
||||
def __init__(self):
|
||||
self._left_on = False
|
||||
self._right_on = False
|
||||
self._shown = None
|
||||
|
||||
def _lane_center(self, model):
|
||||
lls, probs = model.laneLines, model.laneLineProbs
|
||||
if len(lls) < 3 or len(probs) < 3 or len(lls[1].x) == 0:
|
||||
return None, None, False, False
|
||||
|
||||
left = probs[1] >= (LINE_PROB_OFF if self._left_on else LINE_PROB_ON)
|
||||
right = probs[2] >= (LINE_PROB_OFF if self._right_on else LINE_PROB_ON)
|
||||
x = np.array(lls[1].x)
|
||||
yl, yr = np.array(lls[1].y), np.array(lls[2].y)
|
||||
if left and right:
|
||||
y = (yl + yr) / 2.0
|
||||
elif right:
|
||||
y = yr - HALF_LANE_M
|
||||
elif left:
|
||||
y = yl + HALF_LANE_M
|
||||
else:
|
||||
return None, None, False, False
|
||||
return x, y, left, right
|
||||
|
||||
def _slew(self, offsets):
|
||||
# an all-sentinel fit draws nothing: pass through and reset so the next real fit shows unslewed
|
||||
if offsets[0] == OFFSET_UNAVAILABLE:
|
||||
self._shown = None
|
||||
return offsets
|
||||
target = np.asarray(offsets, dtype=float)
|
||||
if self._shown is None:
|
||||
self._shown = target
|
||||
else:
|
||||
self._shown = self._shown + np.clip(target - self._shown, -SLEW_MAX_STEP, SLEW_MAX_STEP)
|
||||
return [int(v) for v in np.round(self._shown)]
|
||||
|
||||
def update(self, model, v_ego, lead_d) -> RenderedLane:
|
||||
x = y = None
|
||||
left_on = right_on = False
|
||||
if model is not None:
|
||||
x, y, left_on, right_on = self._lane_center(model)
|
||||
if x is None:
|
||||
self._shown = None
|
||||
return RenderedLane()
|
||||
self._left_on, self._right_on = left_on, right_on
|
||||
|
||||
reach = float(np.clip(max(v_ego / FULL_REACH_SPEED, lead_d / FULL_REACH_LEAD_DIST, MIN_REACH), 0.0, 1.0))
|
||||
if round(reach * LANE_LENGTH_MAX_VALUE) <= 0:
|
||||
self._shown = None
|
||||
return RenderedLane()
|
||||
return RenderedLane(self._slew(encode_lane_path(x, y)), reach, left_on, right_on, v_ego=v_ego)
|
||||
314
artifacts/package_sources/iqdbc/iqdbc/car/honda/dash_objects.py
Normal file
314
artifacts/package_sources/iqdbc/iqdbc/car/honda/dash_objects.py
Normal file
@@ -0,0 +1,314 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
|
||||
from iqdbc.can.parser import CANParser
|
||||
from iqdbc.car.honda import dash_lane
|
||||
|
||||
NUM_SLOTS = 10
|
||||
LONG_DIST_CAP_M = 195.0
|
||||
|
||||
# byte-faithful empty-slot payload decoded from stock HUD_OBJECTS; an inconsistent frame risks the dash rejecting it
|
||||
INACTIVE = {
|
||||
"OBJECT_ID": 0,
|
||||
"IS_LEAD_CAR": 0,
|
||||
"CAR_TYPE": -1,
|
||||
"ROTATION": -128,
|
||||
"LONG_DIST": 196.9,
|
||||
"LAT_DIST": 204.7,
|
||||
}
|
||||
|
||||
CAR_TYPE_CAR = 7
|
||||
LONG_DIST_MAX_M = 194.0
|
||||
LAT_DIST_LIM_M = 204.7
|
||||
|
||||
# the dash under-scales LAT_DIST ~0.3x in the ego frame; tuned on-car so the lead marker lands on the lane
|
||||
LAT_SCALE = 0.35
|
||||
|
||||
ROT_BAND_M = 1.5
|
||||
ROT_MAX = 6
|
||||
|
||||
REID_GAP_M = 8.0
|
||||
REID_TAU = 1.5
|
||||
REID_REFRACTORY = 1.5
|
||||
MAX_OBJECT_ID = 31
|
||||
|
||||
DREL_SMOOTH_TAU = 0.6
|
||||
YREL_SMOOTH_TAU = 0.5
|
||||
FF_VREL_MIN = 0.5
|
||||
DREL_RESID_CLAMP = 1.5
|
||||
|
||||
LEAD_PROB_ON = 0.5
|
||||
LEAD_PROB_OFF = 0.35
|
||||
LEAD_HOLD_S = 0.6
|
||||
|
||||
# modelV2.leadsV3 entries are one car at three time horizons, not three cars: only render the extra
|
||||
# horizons when spatially distinct from everything already rendered (a genuinely different vehicle)
|
||||
EXTRA_LEAD_SLOTS = (1, 2)
|
||||
EXTRA_LEAD_MIN_SEP_D = 5.0
|
||||
EXTRA_LEAD_MIN_SEP_Y = 1.5
|
||||
|
||||
|
||||
@dataclass
|
||||
class CameraObject:
|
||||
slot: int
|
||||
object_id: int
|
||||
d_rel: float
|
||||
y_rel: float
|
||||
is_lead_car: bool
|
||||
valid: bool
|
||||
car_type: int = -1
|
||||
rotation: int = -128
|
||||
|
||||
|
||||
class CameraObjectTracker:
|
||||
def __init__(self):
|
||||
self._tracks: list[CameraObject] = [
|
||||
CameraObject(slot=i, object_id=0, d_rel=0.0, y_rel=0.0, is_lead_car=False, valid=False)
|
||||
for i in range(NUM_SLOTS)
|
||||
]
|
||||
|
||||
def update(self, cp_cam: CANParser) -> None:
|
||||
vla = cp_cam.vl_all["HUD_OBJECTS"]
|
||||
for mux, oid, ld, yd, lead, ct, rot in zip(vla["MUX"], vla["OBJECT_ID"], vla["LONG_DIST"], vla["LAT_DIST"],
|
||||
vla["IS_LEAD_CAR"], vla["CAR_TYPE"], vla["ROTATION"], strict=True):
|
||||
slot = (int(mux) - 1) % 16
|
||||
if 0 <= slot < NUM_SLOTS:
|
||||
self._tracks[slot] = CameraObject(
|
||||
slot=slot,
|
||||
object_id=int(oid),
|
||||
d_rel=float(ld),
|
||||
y_rel=float(yd),
|
||||
is_lead_car=bool(lead),
|
||||
valid=oid != 0 and ld < LONG_DIST_CAP_M,
|
||||
car_type=int(ct),
|
||||
rotation=int(rot),
|
||||
)
|
||||
|
||||
def snapshot(self) -> list[CameraObject]:
|
||||
return self._tracks
|
||||
|
||||
|
||||
@dataclass
|
||||
class ModelLead:
|
||||
status: bool
|
||||
dRel: float
|
||||
yRel: float
|
||||
vRel: float
|
||||
prob: float = 0.0
|
||||
|
||||
|
||||
def leads_from_model(model, v_ego, n=3):
|
||||
# modelV2's lateral is +right; the dash convention is +left. v is made relative for the smoother.
|
||||
# Data stays populated below LEAD_PROB_ON (status False, prob carried) so the author's hysteresis
|
||||
# can keep an already-rendered lead alive down to LEAD_PROB_OFF instead of blinking it
|
||||
out = []
|
||||
for i in range(n):
|
||||
if model is None or len(model.leadsV3) <= i or len(model.leadsV3[i].x) == 0:
|
||||
out.append(ModelLead(False, 0.0, 0.0, 0.0))
|
||||
continue
|
||||
lead = model.leadsV3[i]
|
||||
out.append(ModelLead(bool(lead.prob >= LEAD_PROB_ON), float(lead.x[0]), -float(lead.y[0]),
|
||||
float(lead.v[0]) - v_ego, prob=float(lead.prob)))
|
||||
return out
|
||||
|
||||
|
||||
def lead_rotation(lateral_left_m: float) -> int:
|
||||
magnitude = min(round(abs(lateral_left_m) / ROT_BAND_M), ROT_MAX)
|
||||
return -magnitude if lateral_left_m > 0 else magnitude
|
||||
|
||||
|
||||
class LeadIdentity:
|
||||
"""Mints a stable OBJECT_ID for the rendered lead, re-IDing on a fresh lead or a range discontinuity.
|
||||
dRel is noisy, so a leaky predictor (feed-forward vRel, leak toward dRel) accumulates the residual
|
||||
instead of a per-sample range-rate test."""
|
||||
|
||||
def __init__(self):
|
||||
self.object_id = 0
|
||||
self._on = False
|
||||
self._pred = 0.0
|
||||
self._prev_t = 0.0
|
||||
self._reid_t = -1e9
|
||||
|
||||
def update(self, status: bool, d_rel: float, v_rel: float, now: float) -> int:
|
||||
if not status:
|
||||
self.object_id = 0
|
||||
self._on = False
|
||||
return 0
|
||||
|
||||
new_lead = not self._on
|
||||
if self._on:
|
||||
dt = max(now - self._prev_t, 1e-3)
|
||||
self._pred += v_rel * dt
|
||||
self._pred += min(dt / REID_TAU, 1.0) * (d_rel - self._pred)
|
||||
if abs(d_rel - self._pred) > REID_GAP_M and now - self._reid_t > REID_REFRACTORY:
|
||||
new_lead = True
|
||||
self._prev_t = now
|
||||
|
||||
if new_lead:
|
||||
self.object_id = self.object_id % MAX_OBJECT_ID + 1
|
||||
self._reid_t = now
|
||||
self._pred = d_rel
|
||||
self._on = True
|
||||
return self.object_id
|
||||
|
||||
|
||||
class MarkerSmoother:
|
||||
"""Stabilizes a rendered marker without lagging real motion: vRel feed-forward on dRel with a
|
||||
clamped leak toward the measurement, plain low-pass on yRel, snapping on an identity change."""
|
||||
|
||||
def __init__(self):
|
||||
self._id = 0
|
||||
self._d = 0.0
|
||||
self._y = 0.0
|
||||
self._t = 0.0
|
||||
|
||||
def update(self, d_rel: float, y_rel: float, v_rel: float, object_id: int, now: float) -> tuple[float, float]:
|
||||
if object_id != self._id:
|
||||
self._id, self._d, self._y, self._t = object_id, d_rel, y_rel, now
|
||||
return d_rel, y_rel
|
||||
dt = max(now - self._t, 1e-3)
|
||||
self._t = now
|
||||
if abs(v_rel) >= FF_VREL_MIN:
|
||||
self._d += v_rel * dt
|
||||
resid = min(max(d_rel - self._d, -DREL_RESID_CLAMP), DREL_RESID_CLAMP)
|
||||
self._d += (1.0 - math.exp(-dt / DREL_SMOOTH_TAU)) * resid
|
||||
self._y += (1.0 - math.exp(-dt / YREL_SMOOTH_TAU)) * (y_rel - self._y)
|
||||
return self._d, self._y
|
||||
|
||||
|
||||
def create_hud_object(packer, bus, mux, track):
|
||||
values = {"MUX": mux}
|
||||
if track is None:
|
||||
values.update(INACTIVE)
|
||||
else:
|
||||
values.update({
|
||||
"OBJECT_ID": int(track["object_id"]),
|
||||
"IS_LEAD_CAR": int(track["is_lead_car"]),
|
||||
"CAR_TYPE": int(track["car_type"]),
|
||||
"ROTATION": int(track["rotation"]),
|
||||
"LONG_DIST": min(max(track["d_rel"], 0.0), LONG_DIST_MAX_M),
|
||||
"LAT_DIST": min(max(track["y_rel"], -LAT_DIST_LIM_M), LAT_DIST_LIM_M),
|
||||
})
|
||||
return packer.make_can_msg("HUD_OBJECTS", bus, values)
|
||||
|
||||
|
||||
def forward_hud_object(packer, bus, mux, tracks):
|
||||
slot = (mux - 1) % 16
|
||||
st = tracks[slot] if (tracks and slot < len(tracks)) else None
|
||||
track = ({"d_rel": st.d_rel, "y_rel": st.y_rel, "object_id": st.object_id, "is_lead_car": st.is_lead_car,
|
||||
"car_type": st.car_type, "rotation": st.rotation} if (st is not None and st.valid) else None)
|
||||
return create_hud_object(packer, bus, mux, track)
|
||||
|
||||
|
||||
class DashObjectAuthor:
|
||||
"""Authors HUD_OBJECTS: openpilot's lead in slot 0 with a stable identity and smoothed marker, the
|
||||
camera's non-lead cars forwarded in slots 1-9 (or distinct extra model leads where there is no
|
||||
camera to forward), one frame per mux tick."""
|
||||
|
||||
def __init__(self):
|
||||
self._identity = LeadIdentity()
|
||||
self._smoother = MarkerSmoother()
|
||||
self._lead_id = 0
|
||||
self._prev_op_id = 0
|
||||
self._lead_on = False
|
||||
self._lead_hold: ModelLead | None = None
|
||||
self._lead_seen_t = -1e9
|
||||
self._extra_ids = {slot: LeadIdentity() for slot in EXTRA_LEAD_SLOTS}
|
||||
self._extra_smooth = {slot: MarkerSmoother() for slot in EXTRA_LEAD_SLOTS}
|
||||
self._extra_emit = dict.fromkeys(EXTRA_LEAD_SLOTS, 0)
|
||||
|
||||
def _gate_lead(self, lead: ModelLead, now: float) -> ModelLead:
|
||||
# leadsV3[0].prob hovers around 0.5 in traffic; hysteresis plus a short dead-reckoned hold keeps
|
||||
# the marker from blinking at a cadence the stock radar never produces
|
||||
if lead.prob >= (LEAD_PROB_OFF if self._lead_on else LEAD_PROB_ON):
|
||||
self._lead_on = True
|
||||
self._lead_hold = lead
|
||||
self._lead_seen_t = now
|
||||
return lead if lead.status else ModelLead(True, lead.dRel, lead.yRel, lead.vRel, lead.prob)
|
||||
if self._lead_on and self._lead_hold is not None and now - self._lead_seen_t < LEAD_HOLD_S:
|
||||
h = self._lead_hold
|
||||
return ModelLead(True, h.dRel + h.vRel * (now - self._lead_seen_t), h.yRel, h.vRel, h.prob)
|
||||
self._lead_on = False
|
||||
self._lead_hold = None
|
||||
return ModelLead(False, 0.0, 0.0, 0.0)
|
||||
|
||||
def _lead_object_id(self, status: bool, op_id: int, stock_lead_id: int | None, in_use: set[int]) -> int:
|
||||
if not status:
|
||||
self._lead_id = 0
|
||||
elif stock_lead_id is not None:
|
||||
self._lead_id = stock_lead_id
|
||||
elif self._lead_id == 0 or op_id != self._prev_op_id or self._lead_id in in_use:
|
||||
# advance from the current id rather than picking the lowest free one: with no camera ids in
|
||||
# use a handoff would keep the same id and the id-keyed smoother would slide between two cars
|
||||
# instead of snapping
|
||||
nxt = self._lead_id % MAX_OBJECT_ID + 1
|
||||
while nxt in in_use:
|
||||
nxt = nxt % MAX_OBJECT_ID + 1
|
||||
self._lead_id = nxt
|
||||
self._prev_op_id = op_id
|
||||
return self._lead_id
|
||||
|
||||
def _update_extras(self, extra_leads, lead, in_use, now):
|
||||
rendered = [(lead.dRel, lead.yRel)] if lead.status else []
|
||||
out = {}
|
||||
for slot, ex in zip(EXTRA_LEAD_SLOTS, extra_leads or (), strict=False):
|
||||
distinct = ex.status and all(abs(ex.dRel - d) >= EXTRA_LEAD_MIN_SEP_D or
|
||||
abs(ex.yRel - y) >= EXTRA_LEAD_MIN_SEP_Y
|
||||
for d, y in rendered)
|
||||
op_id = self._extra_ids[slot].update(distinct, ex.dRel, ex.vRel, now)
|
||||
if not distinct:
|
||||
self._extra_emit[slot] = 0
|
||||
out[slot] = None
|
||||
continue
|
||||
emit = self._extra_emit[slot]
|
||||
if emit == 0 or emit in in_use:
|
||||
emit = op_id
|
||||
while emit in in_use:
|
||||
emit = emit % MAX_OBJECT_ID + 1
|
||||
self._extra_emit[slot] = emit
|
||||
in_use.add(emit)
|
||||
d_rel, y_rel = self._extra_smooth[slot].update(ex.dRel, LAT_SCALE * ex.yRel, ex.vRel, emit, now)
|
||||
rendered.append((ex.dRel, ex.yRel))
|
||||
out[slot] = {"d_rel": d_rel, "y_rel": y_rel, "object_id": emit, "is_lead_car": 0,
|
||||
"car_type": CAR_TYPE_CAR, "rotation": lead_rotation(y_rel / LAT_SCALE)}
|
||||
return out
|
||||
|
||||
def create(self, packer, bus, lead, tracks, mux: int, now: float, extra_leads=None):
|
||||
lead = self._gate_lead(lead, now)
|
||||
op_id = self._identity.update(lead.status, lead.dRel, lead.vRel, now)
|
||||
stock_lead, in_use = None, set()
|
||||
for t in (tracks or ()):
|
||||
if not t.valid:
|
||||
continue
|
||||
if t.is_lead_car:
|
||||
stock_lead = t
|
||||
elif t.slot != 0:
|
||||
in_use.add(t.object_id)
|
||||
stock_lead_id = stock_lead.object_id if stock_lead is not None else None
|
||||
lead_id = self._lead_object_id(lead.status, op_id, stock_lead_id, in_use)
|
||||
if lead.status:
|
||||
in_use.add(lead_id)
|
||||
|
||||
# ride the lane gain-law correction at the lead's distance so the marker tracks the lane rendering
|
||||
lat_scale = LAT_SCALE * dash_lane.gain_correction(lead.dRel)
|
||||
d_rel, y_rel = self._smoother.update(lead.dRel, lat_scale * lead.yRel, lead.vRel, lead_id, now)
|
||||
|
||||
extras = self._update_extras(extra_leads, lead, in_use, now) if tracks is None else {}
|
||||
|
||||
slot = (mux - 1) % 16
|
||||
if slot == 0 and lead.status:
|
||||
track = {"d_rel": d_rel, "y_rel": y_rel, "object_id": lead_id, "is_lead_car": 1,
|
||||
"car_type": stock_lead.car_type if stock_lead is not None else CAR_TYPE_CAR,
|
||||
"rotation": stock_lead.rotation if stock_lead is not None else lead_rotation(y_rel / lat_scale)}
|
||||
elif slot in extras:
|
||||
track = extras[slot]
|
||||
else:
|
||||
st = tracks[slot] if (tracks and slot < len(tracks)) else None
|
||||
# never forward the camera's lead: if OP has no lead, the HUD must not flag one OP isn't acting on
|
||||
track = ({"d_rel": st.d_rel, "y_rel": st.y_rel, "object_id": st.object_id, "is_lead_car": 0,
|
||||
"car_type": st.car_type, "rotation": st.rotation}
|
||||
if (st is not None and st.valid and not st.is_lead_car) else None)
|
||||
return create_hud_object(packer, bus, mux, track)
|
||||
1102
artifacts/package_sources/iqdbc/iqdbc/car/honda/fingerprints.py
Normal file
1102
artifacts/package_sources/iqdbc/iqdbc/car/honda/fingerprints.py
Normal file
File diff suppressed because it is too large
Load Diff
324
artifacts/package_sources/iqdbc/iqdbc/car/honda/hondacan.py
Normal file
324
artifacts/package_sources/iqdbc/iqdbc/car/honda/hondacan.py
Normal file
@@ -0,0 +1,324 @@
|
||||
from iqdbc.car import CanBusBase
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.honda.values import (HondaFlags, HONDA_BOSCH, HONDA_BOSCH_ALT_RADAR, HONDA_BOSCH_RADARLESS,
|
||||
HONDA_BOSCH_CANFD, CarControllerParams)
|
||||
from iqdbc.lvbs.car.honda.iq_values import HondaFlagsIQ
|
||||
|
||||
# CAN bus layout with relay
|
||||
# 0 = ACC-CAN - radar side
|
||||
# 1 = F-CAN B - powertrain
|
||||
# 2 = ACC-CAN - camera side
|
||||
# 3 = F-CAN A - OBDII port
|
||||
|
||||
|
||||
class CanBus(CanBusBase):
|
||||
def __init__(self, CP=None, fingerprint=None) -> None:
|
||||
# use fingerprint if specified
|
||||
super().__init__(CP if fingerprint is None else None, fingerprint)
|
||||
|
||||
# powertrain bus is split instead of radar on radarless and CAN FD Bosch
|
||||
if CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS - HONDA_BOSCH_CANFD):
|
||||
self._pt, self._radar = self.offset + 1, self.offset
|
||||
# normally steering commands are sent to radar, which forwards them to powertrain bus
|
||||
# when radar is disabled, steering commands are sent directly to powertrain bus
|
||||
self._lkas = self._pt if CP.openpilotLongitudinalControl else self._radar
|
||||
else:
|
||||
self._pt, self._radar, self._lkas = self.offset, self.offset + 1, self.offset
|
||||
|
||||
@property
|
||||
def pt(self) -> int:
|
||||
return self._pt
|
||||
|
||||
@property
|
||||
def radar(self) -> int:
|
||||
return self._radar
|
||||
|
||||
@property
|
||||
def camera(self) -> int:
|
||||
return self.offset + 2
|
||||
|
||||
@property
|
||||
def lkas(self) -> int:
|
||||
return self._lkas
|
||||
|
||||
# B-CAN is forwarded to ACC-CAN radar side (CAN 0 on fake ethernet port)
|
||||
@property
|
||||
def body(self) -> int:
|
||||
return self.offset
|
||||
|
||||
|
||||
def create_brake_command(packer, CAN, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, car_fingerprint, stock_brake, CP_IQ):
|
||||
# TODO: do we loose pressure if we keep pump off for long?
|
||||
brakelights = apply_brake > 0
|
||||
brake_rq = apply_brake > 0
|
||||
pcm_fault_cmd = False
|
||||
|
||||
values = {
|
||||
"CRUISE_OVERRIDE": pcm_override,
|
||||
"CRUISE_FAULT_CMD": pcm_fault_cmd,
|
||||
"CRUISE_CANCEL_CMD": pcm_cancel_cmd,
|
||||
"COMPUTER_BRAKE_REQUEST": brake_rq,
|
||||
"SET_ME_1": 1,
|
||||
"BRAKE_LIGHTS": brakelights,
|
||||
"CHIME": stock_brake["CHIME"] if fcw else 0, # send the chime for stock fcw
|
||||
"FCW": fcw << 1, # TODO: Why are there two bits for fcw?
|
||||
"AEB_REQ_1": 0,
|
||||
"AEB_REQ_2": 0,
|
||||
"AEB_STATUS": 0,
|
||||
}
|
||||
|
||||
if CP_IQ.flags & HondaFlagsIQ.NIDEC_HYBRID:
|
||||
values["COMPUTER_BRAKE_HYBRID"] = apply_brake
|
||||
values["BRAKE_PUMP_REQUEST_HYBRID"] = apply_brake > 0
|
||||
else:
|
||||
values["COMPUTER_BRAKE"] = apply_brake
|
||||
values["BRAKE_PUMP_REQUEST"] = pump_on
|
||||
|
||||
return packer.make_can_msg("BRAKE_COMMAND", CAN.pt, values)
|
||||
|
||||
|
||||
def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_counter, CP, gas_force):
|
||||
commands = []
|
||||
min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0]
|
||||
|
||||
control_on = 5 if enabled else 0
|
||||
gas_command = gas if active and gas_force > min_gas_accel else -30000
|
||||
accel_command = accel if active else 0
|
||||
braking = 1 if active and gas_force < min_gas_accel else 0
|
||||
standstill = 1 if active and stopping_counter > 0 else 0
|
||||
standstill_release = 1 if active and stopping_counter == 0 else 0
|
||||
|
||||
# common ACC_CONTROL values
|
||||
acc_control_values = {
|
||||
'ACCEL_COMMAND': accel_command,
|
||||
'STANDSTILL': standstill,
|
||||
}
|
||||
|
||||
if CP.flags & HondaFlags.BOSCH_RADARLESS:
|
||||
acc_control_values.update({
|
||||
"CONTROL_ON": enabled,
|
||||
# hybrid and alt-brake cars require this bit whenever braking; others use it for idle stop after 4s at 50Hz
|
||||
"COMPUTER_BRAKE_ASSIST": braking if CP.flags & (HondaFlags.HYBRID | HondaFlags.BOSCH_ALT_BRAKE) else stopping_counter > 200,
|
||||
})
|
||||
else:
|
||||
acc_control_values.update({
|
||||
'BRAKE_REQUEST': braking,
|
||||
# setting CONTROL_ON causes car to set POWERTRAIN_DATA->ACC_STATUS = 1
|
||||
"CONTROL_ON": control_on,
|
||||
"GAS_COMMAND": gas_command, # used for gas
|
||||
"BRAKE_LIGHTS": braking,
|
||||
"STANDSTILL_RELEASE": standstill_release,
|
||||
})
|
||||
acc_control_on_values = {
|
||||
"SET_TO_3": 0x03,
|
||||
"CONTROL_ON": enabled,
|
||||
"SET_TO_FF": 0xff,
|
||||
"SET_TO_75": 0x75,
|
||||
"SET_TO_30": 0x30,
|
||||
}
|
||||
commands.append(packer.make_can_msg("ACC_CONTROL_ON", CAN.pt, acc_control_on_values))
|
||||
|
||||
commands.append(packer.make_can_msg("ACC_CONTROL", CAN.pt, acc_control_values))
|
||||
return commands
|
||||
|
||||
|
||||
def create_steering_control(packer, CAN, apply_torque, lkas_active, tja_control):
|
||||
values = {
|
||||
"STEER_TORQUE": apply_torque if lkas_active else 0,
|
||||
"STEER_TORQUE_REQUEST": lkas_active,
|
||||
}
|
||||
|
||||
if tja_control:
|
||||
values["STEER_DOWN_TO_ZERO"] = lkas_active
|
||||
|
||||
return packer.make_can_msg("STEERING_CONTROL", CAN.lkas, values)
|
||||
|
||||
|
||||
def create_bosch_supplemental_1(packer, CAN):
|
||||
# non-active params
|
||||
values = {
|
||||
"SET_ME_X04": 0x04,
|
||||
"SET_ME_X80": 0x80,
|
||||
"SET_ME_X10": 0x10,
|
||||
}
|
||||
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", CAN.lkas, values)
|
||||
|
||||
|
||||
def create_acc_hud(packer, bus, CP, enabled, pcm_speed, pcm_accel, hud_control, hud_v_cruise, is_metric, acc_hud):
|
||||
acc_hud_values = {
|
||||
'CRUISE_SPEED': hud_v_cruise,
|
||||
'ENABLE_MINI_CAR': 1 if enabled else 0,
|
||||
# only moves the lead car without ACC_ON
|
||||
'HUD_DISTANCE': hud_control.leadDistanceBars, # wraps to 0 at 4 bars
|
||||
'IMPERIAL_UNIT': int(not is_metric),
|
||||
'HUD_LEAD': 2 if enabled and hud_control.leadVisible else 1 if enabled else 0,
|
||||
'SET_ME_X01_2': 1,
|
||||
}
|
||||
|
||||
if CP.flags & HondaFlags.BOSCH_CANFD:
|
||||
acc_hud_values['SET_ME_X01'] = int(enabled and (bool(acc_hud_values['HUD_LEAD']) or (pcm_accel < 0.2)))
|
||||
acc_hud_values['SET_ME_X01_2'] = int(enabled and (bool(acc_hud_values['HUD_LEAD']) or (pcm_accel < 0.2)))
|
||||
|
||||
if CP.carFingerprint in HONDA_BOSCH:
|
||||
acc_hud_values['ACC_ON'] = int(enabled)
|
||||
acc_hud_values['FCM_OFF'] = 0
|
||||
acc_hud_values['FCM_OFF_2'] = 0
|
||||
else:
|
||||
# Shows the distance bars, TODO: stock camera shows updates temporarily while disabled
|
||||
acc_hud_values['ACC_ON'] = int(enabled)
|
||||
acc_hud_values['PCM_SPEED'] = pcm_speed * CV.MS_TO_KPH
|
||||
acc_hud_values['PCM_GAS'] = pcm_accel
|
||||
acc_hud_values['SET_ME_X01'] = 1
|
||||
acc_hud_values['FCM_OFF'] = acc_hud['FCM_OFF']
|
||||
acc_hud_values['FCM_OFF_2'] = acc_hud['FCM_OFF_2']
|
||||
acc_hud_values['FCM_PROBLEM'] = acc_hud['FCM_PROBLEM']
|
||||
acc_hud_values['ICONS'] = acc_hud['ICONS']
|
||||
|
||||
return packer.make_can_msg("ACC_HUD", bus, acc_hud_values)
|
||||
|
||||
|
||||
def create_lkas_hud(packer, bus, CP, hud_control, lat_active, steering_available, reduced_steering, alert_steer_required, lkas_hud, dashed_lanes,
|
||||
steer_fault_permanent=False, lkas_state_change=None):
|
||||
commands = []
|
||||
|
||||
lkas_hud_values = {
|
||||
'LKAS_READY': 1,
|
||||
'LKAS_STATE_CHANGE': 1,
|
||||
'STEERING_REQUIRED': alert_steer_required,
|
||||
'SOLID_LANES': lat_active,
|
||||
'DASHED_LANES': dashed_lanes,
|
||||
'BEEP': 0,
|
||||
}
|
||||
|
||||
# the stock camera holds LKAS_STATE_CHANGE low, pulsing it high ~3s around HUD state changes;
|
||||
# holding it high permanently suppresses the dash lane-line rendering
|
||||
if lkas_state_change is not None:
|
||||
lkas_hud_values['LKAS_STATE_CHANGE'] = int(lkas_state_change)
|
||||
|
||||
if CP.carFingerprint in (HONDA_BOSCH_RADARLESS | HONDA_BOSCH_CANFD):
|
||||
lkas_hud_values['LANE_LINES'] = 3
|
||||
lkas_hud_values['DASHED_LANES'] = lat_active
|
||||
|
||||
# car likely needs to see LKAS_PROBLEM fall within a specific time frame, so forward from camera
|
||||
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
lkas_hud_values['LKAS_PROBLEM'] = lkas_hud['LKAS_PROBLEM']
|
||||
|
||||
if CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
lkas_hud_values['LKAS_PROBLEM'] = steer_fault_permanent
|
||||
# CAN FD: dashed lanes are the AOL armed indication (dashed_lanes is aol.enabled and not
|
||||
# latActive, which is not standstill-gated - so parked LKAS button presses produce cluster
|
||||
# feedback). ORed with lat_active so the engaged payload keeps SOLID and DASHED set together,
|
||||
# byte-matching the stock camera's lanes-on state
|
||||
lkas_hud_values['DASHED_LANES'] = dashed_lanes or lat_active
|
||||
# every payload change must coincide with an LKAS_STATE_CHANGE pulse (see carcontroller); keyed
|
||||
# on lat_active, not lanesVisible, so the dash LKAS indication follows AOL's lateral state
|
||||
lkas_hud_values['SOLID_LANES'] = lat_active
|
||||
|
||||
if not (CP.flags & HondaFlags.BOSCH_EXT_HUD):
|
||||
lkas_hud_values['RDM_OFF'] = 1
|
||||
lkas_hud_values['LANE_ASSIST_BEEP_OFF'] = 1
|
||||
|
||||
# New HUD concept for selected Bosch cars, overwrites some of the above
|
||||
# TODO: make global across all Honda if feedback is favorable
|
||||
if CP.carFingerprint in HONDA_BOSCH_ALT_RADAR:
|
||||
lkas_hud_values['DASHED_LANES'] = steering_available and lat_active
|
||||
lkas_hud_values['SOLID_LANES'] = lat_active
|
||||
lkas_hud_values['LKAS_PROBLEM'] = lat_active and reduced_steering
|
||||
|
||||
if CP.flags & HondaFlags.BOSCH_EXT_HUD and not CP.openpilotLongitudinalControl:
|
||||
commands.append(packer.make_can_msg('LKAS_HUD_A', bus, lkas_hud_values))
|
||||
commands.append(packer.make_can_msg('LKAS_HUD_B', bus, lkas_hud_values))
|
||||
else:
|
||||
commands.append(packer.make_can_msg('LKAS_HUD', bus, lkas_hud_values))
|
||||
|
||||
return commands
|
||||
|
||||
|
||||
def create_radar_hud(packer, bus):
|
||||
radar_hud_values = {
|
||||
'CMBS_OFF': 0x01,
|
||||
'SET_TO_1': 0x01,
|
||||
}
|
||||
|
||||
return packer.make_can_msg('RADAR_HUD', bus, radar_hud_values)
|
||||
|
||||
|
||||
def create_legacy_brake_command(packer, bus):
|
||||
return packer.make_can_msg("LEGACY_BRAKE_COMMAND", bus, {})
|
||||
|
||||
|
||||
def spam_buttons_command(packer, CAN, cruise_button, cruise_setting, ambient_light, car_fingerprint, bus=None):
|
||||
values = {
|
||||
'CRUISE_BUTTONS': cruise_button,
|
||||
'CRUISE_SETTING': cruise_setting,
|
||||
# the camera consumes this byte too (adaptive high beam); echo the SCM's live value
|
||||
'AMBIENT_LIGHT_MAYBE': ambient_light,
|
||||
}
|
||||
if bus is None:
|
||||
# send buttons to camera on radarless (camera does ACC) cars
|
||||
bus = CAN.camera if car_fingerprint in HONDA_BOSCH_RADARLESS else CAN.pt
|
||||
return packer.make_can_msg("SCM_BUTTONS", bus, values)
|
||||
|
||||
|
||||
def create_radar_hud_canfd(packer, bus, acc, acc_pulse=False):
|
||||
values = {
|
||||
# the stock radar raises this bit only in short bursts right after ACC engages, never held
|
||||
'CMBS_ENABLED_MAYBE': 1 if (acc and acc_pulse) else 0,
|
||||
'ACC_ON': acc,
|
||||
'SET_ME_X01': 0x01,
|
||||
'SET_ME_X01_2': 0x01,
|
||||
}
|
||||
return packer.make_can_msg("RADAR_HUD_CANFD", bus, values)
|
||||
|
||||
|
||||
def create_canfd_supplemental(packer, bus):
|
||||
values = {
|
||||
'SET_ME_X01': 0x01,
|
||||
'SET_ME_X41': 0x41,
|
||||
}
|
||||
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_CANFD", bus, values)
|
||||
|
||||
|
||||
def create_canfd_5hz_radar_messages(packer, bus, radar_ref_cntr, lane_path_length=6, left_lane=0, right_lane=0):
|
||||
commands = []
|
||||
|
||||
radar_lead_values = {
|
||||
'CNTR_REF': radar_ref_cntr,
|
||||
'SET_ME_X01': 0x01,
|
||||
# stock radar transmits a constant 140 here; 120 causes a camera mismatch
|
||||
'TARGET_SPEED_MAYBE': 140,
|
||||
'LEFT_LANE': left_lane,
|
||||
'RIGHT_LANE': right_lane,
|
||||
# the dash cross-checks this against the LANE_PATH in-band terminator; a mismatch suppresses the lane lines
|
||||
'LANE_PATH_LENGTH': lane_path_length,
|
||||
}
|
||||
commands.append(packer.make_can_msg('RADAR_LEAD', bus, radar_lead_values))
|
||||
|
||||
radar_lead2_values = {
|
||||
'SET_ME_X88': 136,
|
||||
'SET_ME_X78': 120,
|
||||
'LEAD_DISTANCE_MAYBE': 0,
|
||||
}
|
||||
commands.append(packer.make_can_msg('RADAR_LEAD2', bus, radar_lead2_values))
|
||||
|
||||
return commands
|
||||
|
||||
|
||||
def honda_checksum(address: int, sig, d: bytearray) -> int:
|
||||
s = 0
|
||||
extended = address > 0x7FF
|
||||
# extended ids above 0x100000 use a different checksum constant, observed on Bosch CAN FD radar messages
|
||||
high_extended = address > 0x100000
|
||||
addr = address
|
||||
while addr:
|
||||
s += addr & 0xF
|
||||
addr >>= 4
|
||||
for i in range(len(d)):
|
||||
x = d[i]
|
||||
if i == len(d) - 1:
|
||||
x >>= 4
|
||||
s += (x & 0xF) + (x >> 4)
|
||||
s = 8 - s
|
||||
if extended:
|
||||
s += 10 if high_extended else 3
|
||||
return s & 0xF
|
||||
392
artifacts/package_sources/iqdbc/iqdbc/car/honda/interface.py
Executable file
392
artifacts/package_sources/iqdbc/iqdbc/car/honda/interface.py
Executable file
@@ -0,0 +1,392 @@
|
||||
#!/usr/bin/env python3
|
||||
import numpy as np
|
||||
from iqdbc.car import get_safety_config, structs, uds
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.disable_ecu import disable_ecu, clear_all_dtcs, clear_ecu_dtcs
|
||||
from iqdbc.car.honda.hondacan import CanBus
|
||||
from iqdbc.car.honda.values import CarControllerParams, HondaFlags, CAR, HONDA_BOSCH, HONDA_BOSCH_CANFD, \
|
||||
HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS, \
|
||||
HONDA_RADAR_SCAN_VERIFIED, HondaSafetyFlags
|
||||
from iqdbc.car.honda.carcontroller import CarController
|
||||
from iqdbc.car.honda.carstate import CarState
|
||||
from iqdbc.car.honda.radar_interface import RadarInterface
|
||||
from iqdbc.car.interfaces import CarInterfaceBase
|
||||
|
||||
from iqdbc.lvbs.car.honda.iq_values import HondaFlagsIQ, HondaSafetyFlagsIQ
|
||||
|
||||
TransmissionType = structs.CarParams.TransmissionType
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
CarState = CarState
|
||||
CarController = CarController
|
||||
RadarInterface = RadarInterface
|
||||
|
||||
DRIVABLE_GEARS = (structs.CarState.GearShifter.sport,)
|
||||
|
||||
@staticmethod
|
||||
def get_pid_accel_limits(CP, CP_IQ, current_speed, cruise_speed):
|
||||
if CP.carFingerprint in HONDA_BOSCH:
|
||||
return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX
|
||||
elif CP_IQ.enableGasInterceptor:
|
||||
return CarControllerParams.NIDEC_ACCEL_MIN, CarControllerParams.NIDEC_ACCEL_MAX
|
||||
else:
|
||||
# NIDECs don't allow acceleration near cruise_speed,
|
||||
# so limit limits of pid to prevent windup
|
||||
ACCEL_MAX_VALS = [CarControllerParams.NIDEC_ACCEL_MAX, 0.2]
|
||||
ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .2]
|
||||
return CarControllerParams.NIDEC_ACCEL_MIN, np.interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS)
|
||||
|
||||
@staticmethod
|
||||
def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, alpha_long, is_release, docs) -> structs.CarParams:
|
||||
ret.brand = "honda"
|
||||
|
||||
CAN = CanBus(ret, fingerprint)
|
||||
|
||||
if candidate in HONDA_BOSCH:
|
||||
cfgs = [get_safety_config(structs.CarParams.SafetyModel.hondaBosch)]
|
||||
if candidate in HONDA_BOSCH_CANFD and CAN.pt >= 4:
|
||||
cfgs.insert(0, get_safety_config(structs.CarParams.SafetyModel.noOutput))
|
||||
ret.safetyConfigs = cfgs
|
||||
|
||||
# The object scan survives openpilot longitudinal: the radar disable is subnet-scoped to the
|
||||
# powertrain bus, while the scan rides the camera-side ACC-CAN
|
||||
ret.radarUnavailable = docs or candidate not in HONDA_RADAR_SCAN_VERIFIED
|
||||
# Disable the radar and let openpilot control longitudinal
|
||||
# WARNING: THIS DISABLES AEB!
|
||||
# If Bosch radarless, this blocks ACC messages from the camera
|
||||
ret.alphaLongitudinalAvailable = True
|
||||
ret.openpilotLongitudinalControl = alpha_long
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
else:
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.hondaNidec)]
|
||||
ret.openpilotLongitudinalControl = True
|
||||
|
||||
ret.pcmCruise = True
|
||||
|
||||
if candidate == CAR.HONDA_CRV_5G:
|
||||
ret.enableBsm = 0x12f8bfa7 in fingerprint[CAN.radar]
|
||||
|
||||
# Detect Bosch cars with new HUD msgs
|
||||
if any(0x33DA in f for f in fingerprint.values()):
|
||||
ret.flags |= HondaFlags.BOSCH_EXT_HUD.value
|
||||
|
||||
if 0x184 in fingerprint[CAN.pt]:
|
||||
ret.flags |= HondaFlags.HYBRID.value
|
||||
|
||||
if ret.flags & HondaFlags.ALLOW_MANUAL_TRANS and all(msg not in fingerprint[CAN.pt] for msg in (0x191, 0x1A3)):
|
||||
# Manual transmission support for allowlisted cars only, to prevent silent fall-through on auto-detection failures
|
||||
ret.transmissionType = TransmissionType.manual
|
||||
elif 0x191 in fingerprint[CAN.pt] and candidate != CAR.ACURA_RDX:
|
||||
# Traditional CVTs, gearshift position in GEARBOX_CVT
|
||||
ret.transmissionType = TransmissionType.cvt
|
||||
else:
|
||||
# Traditional autos, direct-drive EVs and eCVTs, gearshift position in GEARBOX_AUTO
|
||||
ret.transmissionType = TransmissionType.automatic
|
||||
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0], [0]]
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward
|
||||
ret.steerActuatorDelay = 0.1
|
||||
|
||||
if candidate in HONDA_BOSCH:
|
||||
if candidate in HONDA_BOSCH_RADARLESS:
|
||||
ret.stopAccel = CarControllerParams.BOSCH_ACCEL_MIN # stock uses -4.0 m/s^2 once stopped but limited by safety model
|
||||
ret.longitudinalActuatorDelay = 0.25 # s
|
||||
elif candidate in HONDA_BOSCH_CANFD:
|
||||
ret.longitudinalActuatorDelay = 0.05 # near zero, canfd seems to have stock feedforward correction
|
||||
else:
|
||||
ret.longitudinalActuatorDelay = 0.25 # s, per Bosch A log
|
||||
else:
|
||||
# default longitudinal tuning for all hondas
|
||||
ret.longitudinalTuning.kiBP = [0., 5., 35.]
|
||||
ret.longitudinalTuning.kiV = [1.2, 0.8, 0.5]
|
||||
|
||||
# Disable control if EPS mod detected
|
||||
for fw in car_fw:
|
||||
if fw.ecu == "eps" and b"," in fw.fwVersion:
|
||||
ret.dashcamOnly = True
|
||||
|
||||
if candidate == CAR.HONDA_CIVIC:
|
||||
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
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
||||
|
||||
elif candidate == CAR.HONDA_CIVIC_2022:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 5120], [0, 5120]] # TODO: determine if there is a dead zone at the top end
|
||||
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kpV = [[0, 10], [0.05, 0.5]]
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kiV = [[0, 10], [0.0125, 0.125]]
|
||||
|
||||
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
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
|
||||
if ret.transmissionType == TransmissionType.manual:
|
||||
CarControllerParams.BOSCH_GAS_LOOKUP_BP = [-0.2, 2.0]
|
||||
|
||||
elif candidate == CAR.HONDA_ACCORD_11G:
|
||||
ret.steerActuatorDelay = 0.22
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 12747], [0, 12747]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.18]]
|
||||
|
||||
elif candidate == CAR.ACURA_ILX:
|
||||
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 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:
|
||||
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:
|
||||
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.6], [0.18]]
|
||||
ret.wheelSpeedFactor = 1.025
|
||||
|
||||
elif candidate == CAR.HONDA_CRV_6G:
|
||||
ret.steerActuatorDelay = 0.15
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 5100], [0, 5100]]
|
||||
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]]
|
||||
|
||||
elif candidate == CAR.HONDA_FREED:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
|
||||
|
||||
elif candidate in (CAR.HONDA_HRV, CAR.HONDA_HRV_3G):
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
|
||||
if candidate == CAR.HONDA_HRV:
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]]
|
||||
ret.wheelSpeedFactor = 1.025
|
||||
else:
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] # TODO: can probably use some tuning
|
||||
|
||||
elif candidate == CAR.ACURA_RDX:
|
||||
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]]
|
||||
|
||||
elif candidate == CAR.ACURA_RDX_3G:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4095], [0, 4095]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]]
|
||||
|
||||
elif candidate == CAR.HONDA_ODYSSEY:
|
||||
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 == CAR.HONDA_ODYSSEY_TWN:
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end
|
||||
|
||||
elif candidate in (CAR.HONDA_PILOT, CAR.HONDA_PILOT_4G, CAR.HONDA_PASSPORT_4G, CAR.ACURA_MDX_4G_MMR):
|
||||
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.kpBP, ret.lateralTuning.pid.kpV = [[0, 10], [0.05, 0.5]]
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kiV = [[0, 10], [0.0125, 0.125]]
|
||||
|
||||
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]]
|
||||
|
||||
elif candidate in (CAR.HONDA_INSIGHT, CAR.HONDA_NBOX_2G):
|
||||
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.6], [0.18]]
|
||||
|
||||
elif candidate == CAR.HONDA_E:
|
||||
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.6], [0.18]] # TODO: can probably use some tuning
|
||||
|
||||
elif candidate == CAR.HONDA_ODYSSEY_5G_MMR:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3810], [0, 3810]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]]
|
||||
ret.steerActuatorDelay = 0.15
|
||||
CarControllerParams.BOSCH_GAS_LOOKUP_V = [0, 2000]
|
||||
if not ret.openpilotLongitudinalControl:
|
||||
# When using stock ACC, the radar intercepts and filters steering commands the EPS would otherwise accept
|
||||
ret.minSteerSpeed = 70. * CV.KPH_TO_MS
|
||||
|
||||
elif candidate == CAR.ACURA_TLX_2G_MMR:
|
||||
ret.steerActuatorDelay = 0.15
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
|
||||
ret.lateralTuning.pid.kpBP, ret.lateralTuning.pid.kpV = [[0, 10], [0.05, 0.5]]
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kiV = [[0, 10], [0.0125, 0.125]]
|
||||
|
||||
elif candidate == CAR.HONDA_CLARITY:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
||||
|
||||
else:
|
||||
ret.steerActuatorDelay = 0.15
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
|
||||
CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
|
||||
|
||||
if candidate == CAR.HONDA_PILOT_4G:
|
||||
CarControllerParams.BOSCH_GAS_LOOKUP_V = [0, 2200]
|
||||
elif candidate == CAR.ACURA_RDX_3G:
|
||||
CarControllerParams.BOSCH_GAS_LOOKUP_V = [0, 2200]
|
||||
elif candidate == CAR.HONDA_CRV_6G and ret.flags & HondaFlags.HYBRID:
|
||||
CarControllerParams.BOSCH_GAS_LOOKUP_BP = [-0.3, 2.0]
|
||||
|
||||
# These cars use alternate user brake msg (0x1BE)
|
||||
if 0x1BE in fingerprint[CAN.pt] and candidate in HONDA_BOSCH:
|
||||
ret.flags |= HondaFlags.BOSCH_ALT_BRAKE.value
|
||||
|
||||
if ret.flags & HondaFlags.BOSCH_ALT_BRAKE:
|
||||
ret.safetyConfigs[-1].safetyParam |= HondaSafetyFlags.ALT_BRAKE.value
|
||||
if candidate in HONDA_NIDEC_ALT_SCM_MESSAGES:
|
||||
ret.safetyConfigs[-1].safetyParam |= HondaSafetyFlags.NIDEC_ALT.value
|
||||
if ret.openpilotLongitudinalControl and candidate in HONDA_BOSCH:
|
||||
ret.safetyConfigs[-1].safetyParam |= HondaSafetyFlags.BOSCH_LONG.value
|
||||
if candidate in HONDA_BOSCH_RADARLESS:
|
||||
ret.safetyConfigs[-1].safetyParam |= HondaSafetyFlags.RADARLESS.value
|
||||
if candidate in HONDA_BOSCH_CANFD:
|
||||
ret.safetyConfigs[-1].safetyParam |= HondaSafetyFlags.BOSCH_CANFD.value
|
||||
|
||||
# min speed to enable ACC. if car can do stop and go, then set enabling speed
|
||||
# to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not
|
||||
# conflict with PCM acc
|
||||
if (ret.transmissionType == TransmissionType.manual) and (not ret.openpilotLongitudinalControl):
|
||||
ret.autoResumeSng = False
|
||||
else:
|
||||
ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.HONDA_CIVIC})
|
||||
if ret.autoResumeSng:
|
||||
ret.minEnableSpeed = -1.
|
||||
elif candidate == CAR.HONDA_ODYSSEY_TWN:
|
||||
ret.minEnableSpeed = 19. * CV.MPH_TO_MS
|
||||
else:
|
||||
ret.minEnableSpeed = 25.51 * CV.MPH_TO_MS
|
||||
|
||||
ret.steerLimitTimer = 0.8
|
||||
ret.radarDelay = 0.1
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def _get_params_iq(stock_cp: structs.CarParams, ret: structs.IQCarParams, candidate, fingerprint: dict[int, dict[int, int]],
|
||||
car_fw: list[structs.CarParams.CarFw], alpha_long: bool, is_release_iq: bool, docs: bool) -> structs.IQCarParams:
|
||||
CAN = CanBus(stock_cp, fingerprint)
|
||||
|
||||
for fw in car_fw:
|
||||
if fw.ecu == "eps" and b"," in fw.fwVersion:
|
||||
ret.flags |= HondaFlagsIQ.EPS_MODIFIED.value
|
||||
stock_cp.dashcamOnly = False
|
||||
|
||||
if bool(stock_cp.flags & HondaFlags.NIDEC) and bool(stock_cp.flags & HondaFlags.HYBRID):
|
||||
ret.flags |= HondaFlagsIQ.NIDEC_HYBRID.value
|
||||
ret.iqSafetyFlags |= HondaSafetyFlagsIQ.NIDEC_HYBRID
|
||||
# some hybrids use a different brake hold
|
||||
if 0x223 in fingerprint[CAN.pt]:
|
||||
ret.flags |= HondaFlagsIQ.HYBRID_ALT_BRAKEHOLD.value
|
||||
|
||||
if 0x35E in fingerprint[CAN.pt]:
|
||||
ret.flags |= HondaFlagsIQ.HAS_CAMERA_MESSAGES.value
|
||||
|
||||
if candidate == CAR.HONDA_CIVIC:
|
||||
if ret.flags & HondaFlagsIQ.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
|
||||
stock_cp.lateralParams.torqueBP, stock_cp.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]]
|
||||
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.3], [0.1]]
|
||||
|
||||
elif candidate in (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_BOSCH_DIESEL):
|
||||
if ret.flags & HondaFlagsIQ.EPS_MODIFIED:
|
||||
stock_cp.lateralParams.torqueBP, stock_cp.lateralParams.torqueV = [[0, 2564, 8000], [0, 2564, 3840]]
|
||||
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.3], [0.09]] # 2.5x Modded EPS
|
||||
|
||||
elif candidate == CAR.HONDA_CIVIC_2022:
|
||||
if ret.flags & HondaFlagsIQ.EPS_MODIFIED:
|
||||
stock_cp.lateralParams.torqueBP, stock_cp.lateralParams.torqueV = [[0, 2564, 8000], [0, 2564, 3840]]
|
||||
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.3], [0.09]] # 2.5x Modded EPS
|
||||
|
||||
elif candidate == CAR.HONDA_ACCORD:
|
||||
if ret.flags & HondaFlagsIQ.EPS_MODIFIED:
|
||||
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.3], [0.09]]
|
||||
|
||||
elif candidate == CAR.HONDA_CRV_5G:
|
||||
if ret.flags & HondaFlagsIQ.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
|
||||
stock_cp.lateralParams.torqueBP, stock_cp.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]]
|
||||
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.21], [0.07]]
|
||||
|
||||
elif candidate == CAR.HONDA_CLARITY:
|
||||
stock_cp.autoResumeSng = True
|
||||
stock_cp.minEnableSpeed = -1
|
||||
if ret.flags & HondaFlagsIQ.EPS_MODIFIED:
|
||||
for fw in car_fw:
|
||||
if fw.ecu == "eps" and b"-" not in fw.fwVersion and b"," in fw.fwVersion:
|
||||
stock_cp.lateralTuning.pid.kf = 0.00004
|
||||
stock_cp.lateralParams.torqueBP, stock_cp.lateralParams.torqueV = [[0, 5760, 15360], [0, 2560, 3840]]
|
||||
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.1575], [0.05175]]
|
||||
elif fw.ecu == "eps" and b"-" in fw.fwVersion and b"," in fw.fwVersion:
|
||||
stock_cp.lateralParams.torqueBP, stock_cp.lateralParams.torqueV = [[0, 5760, 10240], [0, 2560, 3840]]
|
||||
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.3], [0.1]]
|
||||
else:
|
||||
stock_cp.lateralParams.torqueBP, stock_cp.lateralParams.torqueV = [[0, 2560], [0, 2560]]
|
||||
stock_cp.lateralTuning.pid.kpV, stock_cp.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
||||
|
||||
if candidate in HONDA_BOSCH:
|
||||
pass
|
||||
else:
|
||||
ret.enableGasInterceptor = 0x201 in fingerprint[CAN.pt]
|
||||
stock_cp.pcmCruise = not ret.enableGasInterceptor
|
||||
|
||||
if ret.enableGasInterceptor and candidate not in HONDA_BOSCH:
|
||||
ret.iqSafetyFlags |= HondaSafetyFlagsIQ.GAS_INTERCEPTOR
|
||||
|
||||
stock_cp.autoResumeSng = stock_cp.autoResumeSng or ret.enableGasInterceptor
|
||||
|
||||
if candidate == CAR.HONDA_CITY_7G:
|
||||
ret.longitudinalStoppingSpeedOverride = 2.0
|
||||
ret.stoppingDecelRateOverride = 0.3
|
||||
else:
|
||||
ret.longitudinalStoppingSpeedOverride = 0.5
|
||||
ret.stoppingDecelRateOverride = 0.1
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def init(CP, CP_IQ, can_recv, can_send, communication_control=None):
|
||||
if CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl:
|
||||
if communication_control is None and CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
# CAN FD: only clear DTCs here; the radar silencing itself is deferred to CarController until
|
||||
# the comma relay is confirmed open. init() runs while the panda is still in the ELM327 safety
|
||||
# mode, and silencing the radar from here raced the safety-mode switch: whenever the switch
|
||||
# took longer than ~110 ms after radar silence, the brake module latched CRUISE_FAULT for the
|
||||
# entire drive.
|
||||
#
|
||||
# The brake module's radar lost-communication DTC matures over trips (Honda two-trip
|
||||
# detection): once confirmed from a previous drive, the very next comm-loss detection faults
|
||||
# ~0.16 s after the radar goes silent. Broadcast-clear stored DTCs on the powertrain and
|
||||
# camera buses every drive to reset the maturation counter, and clear the radar's own stored
|
||||
# DTCs so codes accumulated while it was disabled don't re-fault a later drive. Clearing must
|
||||
# precede the radar silence because a DTC clear can take an ECU several hundred ms.
|
||||
# NOTE: ELM327 safety mode allows the 29-bit functional diagnostic address on every bus, so
|
||||
# the broadcast needs no TX allowlist entry in the car safety mode
|
||||
clear_all_dtcs(can_send, [CanBus(CP).pt, CanBus(CP).camera])
|
||||
clear_ecu_dtcs(can_recv, can_send, bus=CanBus(CP).pt, addr=0x18DAB0F1)
|
||||
else:
|
||||
# 0x80 silences response
|
||||
if communication_control is None:
|
||||
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, 0x80 | uds.CONTROL_TYPE.DISABLE_RX_DISABLE_TX,
|
||||
uds.MESSAGE_TYPE.NORMAL_AND_NETWORK_MANAGEMENT])
|
||||
disable_ecu(can_recv, can_send, bus=CanBus(CP).pt, addr=0x18DAB0F1, com_cont_req=communication_control)
|
||||
|
||||
@staticmethod
|
||||
def deinit(CP, can_recv, can_send):
|
||||
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, 0x80 | uds.CONTROL_TYPE.ENABLE_RX_ENABLE_TX,
|
||||
uds.MESSAGE_TYPE.NORMAL_AND_NETWORK_MANAGEMENT])
|
||||
CarInterface.init(CP, None, can_recv, can_send, communication_control)
|
||||
92
artifacts/package_sources/iqdbc/iqdbc/car/honda/radar_interface.py
Executable file
92
artifacts/package_sources/iqdbc/iqdbc/car/honda/radar_interface.py
Executable file
@@ -0,0 +1,92 @@
|
||||
#!/usr/bin/env python3
|
||||
from iqdbc.can import CANParser
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.car.interfaces import RadarInterfaceBase
|
||||
from iqdbc.car.honda.radar_scan import SCAN_DBC_NAME, HondaRadarScanner
|
||||
from iqdbc.car.honda.values import DBC
|
||||
|
||||
|
||||
def _create_nidec_can_parser(car_fingerprint):
|
||||
radar_messages = [0x400] + list(range(0x430, 0x43A)) + list(range(0x440, 0x446))
|
||||
messages = [(m, 20) for m in radar_messages]
|
||||
return CANParser(DBC[car_fingerprint][Bus.radar], messages, 1)
|
||||
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP, CP_IQ):
|
||||
super().__init__(CP, CP_IQ)
|
||||
self.track_id = 0
|
||||
self.radar_fault = False
|
||||
self.radar_wrong_config = False
|
||||
self.radar_off_can = CP.radarUnavailable
|
||||
self.scanner = None
|
||||
|
||||
if self.radar_off_can:
|
||||
self.rcp = None
|
||||
self.trigger_msg = 0x445
|
||||
elif DBC[CP.carFingerprint].get(Bus.radar) == SCAN_DBC_NAME:
|
||||
self.scanner = HondaRadarScanner(CP)
|
||||
self.rcp = self.scanner.rcp
|
||||
self.pts = self.scanner.pts
|
||||
self.trigger_msg = self.scanner.trigger_msg
|
||||
else:
|
||||
# Nidec
|
||||
self.rcp = _create_nidec_can_parser(CP.carFingerprint)
|
||||
self.trigger_msg = 0x445
|
||||
self.updated_messages = set()
|
||||
|
||||
def update(self, can_strings):
|
||||
# in Bosch radar and we are only steering for now, so sleep 0.05s to keep
|
||||
# radard at 20Hz and return no points
|
||||
if self.radar_off_can:
|
||||
return super().update(None)
|
||||
|
||||
vls = self.rcp.update(can_strings)
|
||||
self.updated_messages.update(vls)
|
||||
|
||||
if self.trigger_msg not in self.updated_messages:
|
||||
if self.scanner is not None and self.scanner.sweep_overdue():
|
||||
return self.scanner.quiet_bus_radardata()
|
||||
return None
|
||||
|
||||
rr = self._update(self.updated_messages)
|
||||
self.updated_messages.clear()
|
||||
return rr
|
||||
|
||||
def _update(self, updated_messages):
|
||||
if self.scanner is not None:
|
||||
return self.scanner.process_sweep(updated_messages)
|
||||
|
||||
ret = structs.RadarData()
|
||||
|
||||
for ii in sorted(updated_messages):
|
||||
cpt = self.rcp.vl[ii]
|
||||
if ii == 0x400:
|
||||
# check for radar faults
|
||||
self.radar_fault = cpt['RADAR_STATE'] != 0x79
|
||||
self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
|
||||
elif cpt['LONG_DIST'] < 255:
|
||||
if ii not in self.pts or cpt['NEW_TRACK']:
|
||||
self.pts[ii] = structs.RadarData.RadarPoint()
|
||||
self.pts[ii].trackId = self.track_id
|
||||
self.track_id += 1
|
||||
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
|
||||
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
|
||||
self.pts[ii].vRel = cpt['REL_SPEED']
|
||||
self.pts[ii].aRel = float('nan')
|
||||
self.pts[ii].yvRel = float('nan')
|
||||
self.pts[ii].measured = True
|
||||
else:
|
||||
if ii in self.pts:
|
||||
del self.pts[ii]
|
||||
|
||||
if not self.rcp.can_valid:
|
||||
ret.errors.canError = True
|
||||
if self.radar_fault:
|
||||
ret.errors.radarFault = True
|
||||
if self.radar_wrong_config:
|
||||
ret.errors.wrongConfig = True
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
|
||||
return ret
|
||||
396
artifacts/package_sources/iqdbc/iqdbc/car/honda/radar_scan.py
Normal file
396
artifacts/package_sources/iqdbc/iqdbc/car/honda/radar_scan.py
Normal file
@@ -0,0 +1,396 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import math
|
||||
from collections import deque
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
from iqdbc.can import CANParser
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.car.honda.hondacan import CanBus
|
||||
from iqdbc.car.honda.values import DBC
|
||||
from iqdbc.dbc.generator.honda.honda_radar_scan import QUARTET_KINDS, SCAN_SLOTS, frame_address
|
||||
|
||||
SCAN_DBC_NAME = 'honda_radar_scan_generated'
|
||||
SWEEP_HZ = 15
|
||||
|
||||
SLOT_ADDRS = [tuple(frame_address(slot, kind) for kind in (*QUARTET_KINDS, "MOTION")) for slot in range(SCAN_SLOTS)]
|
||||
ALL_SCAN_ADDRS = [addr for addrs in SLOT_ADDRS for addr in addrs]
|
||||
# slot 15's IDENT frame closes every observed sweep's quartet family; its MOTION companion follows
|
||||
# but must never gate an otherwise valid sweep, so the quartet frame stays the trigger
|
||||
SWEEP_TRIGGER_ADDR = SLOT_ADDRS[SCAN_SLOTS - 1][3]
|
||||
|
||||
DIST_LSB_M = 0.05712
|
||||
DIST_BIAS_M = -3.0
|
||||
BEARING_LSB_RAD = 1.0 / 2048.0
|
||||
BEARING_ZERO = 1024
|
||||
|
||||
STATE_INVALID = 0xF
|
||||
DIST_RAW_INVALID = 0xFFF
|
||||
BEARING_RAW_INVALID = 0x7FF
|
||||
AGE_RAW_INVALID = 0xFFF
|
||||
HANDLE_MIN = 1
|
||||
HANDLE_MAX = 0x3F
|
||||
|
||||
CLOSING_SPEED_RAW_INVALID = 0x7FE
|
||||
CLOSING_SPEED_RAW_MIN = 0
|
||||
CLOSING_SPEED_RAW_MAX = 1728
|
||||
CLOSING_SPEED_RAW_ZERO = 864
|
||||
CLOSING_SPEED_LSB_MPS = 1.0 / 64.0
|
||||
# replay-derived quality gate: the native speed field degrades gradually with its sigma companion;
|
||||
# above this the field is no longer authoritative and the decoder coasts instead
|
||||
CLOSING_SPEED_SIGMA_TRUST_MAX = 511
|
||||
|
||||
DIST_RATIO_RAW_INVALID = 0x3FF
|
||||
DIST_RATIO_LSB = 0.001
|
||||
DIST_RATIO_BIAS = 0.5
|
||||
|
||||
# replay-derived acceptance gates, not recovered firmware constants; innovation is measured from the
|
||||
# previous ACCEPTED observation so a reset can never become the baseline for following sweeps
|
||||
DIST_SIGMA_DEGRADED_RAW = 4
|
||||
DIST_INNOVATION_SOFT_M = 2.0
|
||||
DIST_INNOVATION_HARD_M = 5.0
|
||||
RAW_RATE_LIMIT_MPS = 50.0
|
||||
|
||||
HISTORY_LEN = 8
|
||||
QUIET_TIMEOUT_S = 0.20
|
||||
|
||||
|
||||
def decode_closing_speed(raw, sigma_raw=None):
|
||||
if raw is None:
|
||||
return None
|
||||
raw = int(raw)
|
||||
if raw == CLOSING_SPEED_RAW_INVALID or not CLOSING_SPEED_RAW_MIN <= raw <= CLOSING_SPEED_RAW_MAX:
|
||||
return None
|
||||
if sigma_raw is not None and int(sigma_raw) > CLOSING_SPEED_SIGMA_TRUST_MAX:
|
||||
return None
|
||||
return (raw - CLOSING_SPEED_RAW_ZERO) * CLOSING_SPEED_LSB_MPS
|
||||
|
||||
|
||||
def decode_dist_ratio(raw):
|
||||
if raw is None:
|
||||
return None
|
||||
raw = int(raw)
|
||||
if raw == DIST_RATIO_RAW_INVALID or not 0 <= raw < DIST_RATIO_RAW_INVALID:
|
||||
return None
|
||||
return DIST_RATIO_BIAS + DIST_RATIO_LSB * raw
|
||||
|
||||
|
||||
def ratio_implied_rate(raw, dist, dt):
|
||||
ratio = decode_dist_ratio(raw)
|
||||
if ratio is None or dt <= 0.0 or not math.isfinite(dist):
|
||||
return None
|
||||
return dist * (1.0 - ratio) / dt
|
||||
|
||||
|
||||
def reading_degraded(dist_sigma_raw, presence_raw, speed_sigma_raw):
|
||||
geometry_bad = dist_sigma_raw >= DIST_SIGMA_DEGRADED_RAW or presence_raw in (0, 0x7F)
|
||||
motion_bad = speed_sigma_raw is not None and speed_sigma_raw > CLOSING_SPEED_SIGMA_TRUST_MAX
|
||||
return geometry_bad or motion_bad
|
||||
|
||||
|
||||
@dataclass
|
||||
class SlotReading:
|
||||
slot: int
|
||||
cycle: int
|
||||
age: int
|
||||
handle: int
|
||||
handle_ok: bool
|
||||
coherent: bool
|
||||
dist_raw: int = 0
|
||||
bearing_raw: int = 0
|
||||
dist_sigma_raw: int = 0
|
||||
presence_raw: int = 0
|
||||
speed_raw: int | None = None
|
||||
speed_sigma_raw: int | None = None
|
||||
ratio_raw: int | None = None
|
||||
|
||||
|
||||
@dataclass
|
||||
class ObjectLedger:
|
||||
handle: int
|
||||
prev_cycle: int | None = None
|
||||
prev_age: int | None = None
|
||||
last_seen_nanos: int | None = None
|
||||
wire_slot: int | None = None
|
||||
history: deque = field(default_factory=lambda: deque(maxlen=HISTORY_LEN))
|
||||
held_speed: float | None = None
|
||||
held_speed_nanos: int | None = None
|
||||
|
||||
def continues_incarnation(self, cycle: int, age: int) -> bool:
|
||||
if self.prev_cycle is None or self.prev_age is None:
|
||||
return False
|
||||
cycle_delta = (cycle - self.prev_cycle) & 0xF
|
||||
age_delta = (age - self.prev_age) & 0xFFF
|
||||
return age_delta == 2 * cycle_delta
|
||||
|
||||
def restart_incarnation(self):
|
||||
self.history.clear()
|
||||
self.held_speed = None
|
||||
self.held_speed_nanos = None
|
||||
|
||||
def held_speed_fresh(self, now: int) -> bool:
|
||||
return (self.held_speed is not None and self.held_speed_nanos is not None and
|
||||
(now - self.held_speed_nanos) * 1e-9 <= QUIET_TIMEOUT_S)
|
||||
|
||||
|
||||
def create_scan_parser(CP) -> CANParser:
|
||||
# the object scan is physically on the camera-side ACC-CAN
|
||||
return CANParser(DBC[CP.carFingerprint][Bus.radar], [(addr, SWEEP_HZ) for addr in ALL_SCAN_ADDRS], CanBus(CP).camera)
|
||||
|
||||
|
||||
class HondaRadarScanner:
|
||||
def __init__(self, CP):
|
||||
self.rcp = create_scan_parser(CP)
|
||||
self.trigger_msg = SWEEP_TRIGGER_ADDR
|
||||
self.pts: dict[int, structs.RadarData.RadarPoint] = {}
|
||||
self._ledgers: dict[int, ObjectLedger] = {}
|
||||
self._slot_handles: list[int | None] = [None] * SCAN_SLOTS
|
||||
self._last_sweep_nanos = -1
|
||||
|
||||
def sweep_overdue(self) -> bool:
|
||||
if self._last_sweep_nanos < 0:
|
||||
return False
|
||||
return (self.rcp._last_update_nanos - self._last_sweep_nanos) * 1e-9 > QUIET_TIMEOUT_S
|
||||
|
||||
def quiet_bus_radardata(self) -> structs.RadarData:
|
||||
# whole-bus silence: drop everything and emit an EMPTY RadarData (not None) so radard sheds any
|
||||
# lead within a cycle instead of freezing a phantom
|
||||
self.pts.clear()
|
||||
self._ledgers.clear()
|
||||
self._slot_handles = [None] * SCAN_SLOTS
|
||||
self._last_sweep_nanos = -1
|
||||
ret = structs.RadarData()
|
||||
if not self.rcp.can_valid:
|
||||
ret.errors.canError = True
|
||||
ret.errors.radarUnavailableTemporary = True
|
||||
return ret
|
||||
|
||||
def _drop_ledger(self, handle: int):
|
||||
self._ledgers.pop(handle, None)
|
||||
self.pts.pop(handle, None)
|
||||
for slot, bound in enumerate(self._slot_handles):
|
||||
if bound == handle:
|
||||
self._slot_handles[slot] = None
|
||||
|
||||
def _drop_expired_ledgers(self, now: int):
|
||||
for handle, ledger in list(self._ledgers.items()):
|
||||
if ledger.last_seen_nanos is not None and (now - ledger.last_seen_nanos) * 1e-9 > QUIET_TIMEOUT_S:
|
||||
self._drop_ledger(handle)
|
||||
|
||||
def _read_slot(self, slot: int, updated_messages) -> SlotReading | None:
|
||||
pos, shape, life, ident, motion = SLOT_ADDRS[slot]
|
||||
if not all(addr in updated_messages for addr in (pos, shape, life, ident)):
|
||||
# a missing CAN frame is not a lifecycle event; ledgers expire on their own staleness only
|
||||
return None
|
||||
|
||||
v_pos, v_shape, v_life, v_ident = (self.rcp.vl[a] for a in (pos, shape, life, ident))
|
||||
cycle = int(v_pos['CYCLE'])
|
||||
if not (cycle == int(v_shape['CYCLE']) == int(v_life['CYCLE']) == int(v_ident['CYCLE'])):
|
||||
# the quartet doesn't share one radar cycle: not a coherent observation this window
|
||||
return None
|
||||
|
||||
state = int(v_pos['SCAN_STATE'])
|
||||
dist_raw = int(v_pos['DIST_RAW'])
|
||||
bearing_raw = int(v_pos['BEARING_RAW'])
|
||||
age = int(v_life['AGE_RAW'])
|
||||
handle = int(v_ident['OBJECT_HANDLE'])
|
||||
reading = SlotReading(
|
||||
slot=slot,
|
||||
cycle=cycle,
|
||||
age=age,
|
||||
handle=handle,
|
||||
handle_ok=HANDLE_MIN <= handle <= HANDLE_MAX,
|
||||
coherent=(state != STATE_INVALID and dist_raw != DIST_RAW_INVALID and
|
||||
bearing_raw != BEARING_RAW_INVALID and age != AGE_RAW_INVALID),
|
||||
dist_raw=dist_raw,
|
||||
bearing_raw=bearing_raw,
|
||||
dist_sigma_raw=int(v_pos['DIST_SIGMA_RAW']),
|
||||
presence_raw=int(v_shape['PRESENCE_RAW']),
|
||||
)
|
||||
|
||||
# the MOTION companion only contributes when it rides the same cycle; its absence never
|
||||
# invalidates the quartet, it only removes independent motion evidence
|
||||
if motion in updated_messages:
|
||||
v_motion = self.rcp.vl[motion]
|
||||
if int(v_motion['CYCLE']) == cycle:
|
||||
reading.speed_raw = int(v_motion['CLOSING_SPEED_RAW'])
|
||||
reading.speed_sigma_raw = int(v_motion['CLOSING_SPEED_SIGMA_RAW'])
|
||||
reading.ratio_raw = int(v_motion['DIST_RATIO_RAW'])
|
||||
return reading
|
||||
|
||||
def _elect_by_handle(self, readings: list[SlotReading]) -> dict[int, SlotReading]:
|
||||
# one CAN identity can never yield two points; ties prefer the wire slot already bound to the
|
||||
# ledger, then the lower slot for deterministic handling of a malformed duplicate
|
||||
elected: dict[int, SlotReading] = {}
|
||||
for reading in readings:
|
||||
if not (reading.coherent and reading.handle_ok):
|
||||
# an invalid observation ends publication for the slot's current occupant without destroying
|
||||
# persistent state; the object may be multiplexed elsewhere or return before its deadline
|
||||
hidden = {self._slot_handles[reading.slot]}
|
||||
if reading.handle_ok:
|
||||
hidden.add(reading.handle)
|
||||
for handle in hidden - {None}:
|
||||
self.pts.pop(handle, None)
|
||||
continue
|
||||
current = elected.get(reading.handle)
|
||||
if current is None:
|
||||
elected[reading.handle] = reading
|
||||
continue
|
||||
bound_slot = self._ledgers[reading.handle].wire_slot if reading.handle in self._ledgers else None
|
||||
current_rank = (0 if bound_slot == current.slot else 1, current.slot)
|
||||
candidate_rank = (0 if bound_slot == reading.slot else 1, reading.slot)
|
||||
if candidate_rank < current_rank:
|
||||
elected[reading.handle] = reading
|
||||
return elected
|
||||
|
||||
def _bind_slot(self, ledger: ObjectLedger, reading: SlotReading, now: int):
|
||||
ledger.prev_cycle = reading.cycle
|
||||
ledger.prev_age = reading.age
|
||||
ledger.last_seen_nanos = now
|
||||
ledger.wire_slot = reading.slot
|
||||
for slot, bound in enumerate(self._slot_handles):
|
||||
if slot != reading.slot and bound == ledger.handle:
|
||||
self._slot_handles[slot] = None
|
||||
self._slot_handles[reading.slot] = ledger.handle
|
||||
|
||||
def _coast_point(self, ledger: ObjectLedger, now: int, dist: float, y_rel: float):
|
||||
# coasting keeps trustworthy geometry visible with the last authoritative motion, unmeasured,
|
||||
# instead of publishing a synthesized rate; without fresh held motion the point drops
|
||||
if ledger.held_speed_fresh(now):
|
||||
point = self.pts.get(ledger.handle)
|
||||
if point is not None:
|
||||
point.dRel = dist
|
||||
point.yRel = y_rel
|
||||
point.vRel = ledger.held_speed
|
||||
point.measured = False
|
||||
return
|
||||
ledger.held_speed = None
|
||||
ledger.held_speed_nanos = None
|
||||
self.pts.pop(ledger.handle, None)
|
||||
|
||||
def process_sweep(self, updated_messages) -> structs.RadarData:
|
||||
ret = structs.RadarData()
|
||||
if not self.rcp.can_valid:
|
||||
ret.errors.canError = True
|
||||
|
||||
now = self.rcp._last_update_nanos
|
||||
self._last_sweep_nanos = now
|
||||
self._drop_expired_ledgers(now)
|
||||
|
||||
readings = [r for r in (self._read_slot(slot, updated_messages) for slot in range(SCAN_SLOTS)) if r is not None]
|
||||
elected = self._elect_by_handle(readings)
|
||||
elected_handles = set(elected)
|
||||
|
||||
for handle, reading in sorted(elected.items(), key=lambda item: item[1].slot):
|
||||
# a wire-slot replacement ends publication for the old occupant, but not its persistent state
|
||||
old_handle = self._slot_handles[reading.slot]
|
||||
if old_handle is not None and old_handle != handle and old_handle not in elected_handles:
|
||||
self.pts.pop(old_handle, None)
|
||||
|
||||
ledger = self._ledgers.get(handle)
|
||||
if ledger is None:
|
||||
ledger = ObjectLedger(handle=handle)
|
||||
self._ledgers[handle] = ledger
|
||||
|
||||
if not ledger.continues_incarnation(reading.cycle, reading.age):
|
||||
# the CAN identity stays the external key, but a lifecycle discontinuity starts a new
|
||||
# incarnation and must not inherit the previous object's range-rate history
|
||||
ledger.restart_incarnation()
|
||||
self.pts.pop(handle, None)
|
||||
|
||||
dist = DIST_LSB_M * reading.dist_raw + DIST_BIAS_M
|
||||
bearing = BEARING_LSB_RAD * (reading.bearing_raw - BEARING_ZERO)
|
||||
# the radar consumes range as a forward-axis quantity; positive bearing is left of center,
|
||||
# matching the RadarPoint.yRel sign contract
|
||||
y_rel = dist * math.tan(bearing)
|
||||
|
||||
now_s = now * 1e-9
|
||||
native_speed = decode_closing_speed(reading.speed_raw, reading.speed_sigma_raw)
|
||||
unqualified_speed = decode_closing_speed(reading.speed_raw)
|
||||
# a live native speed rejected only by its sigma companion: geometry acceptance is still decided
|
||||
# on the same terms as every other sweep (high sigma correlates with bad/discontinuous range),
|
||||
# and only then does the point coast instead of publishing a synthesized rate
|
||||
sigma_veto = (native_speed is None and unqualified_speed is not None and
|
||||
reading.speed_sigma_raw is not None and
|
||||
reading.speed_sigma_raw > CLOSING_SPEED_SIGMA_TRUST_MAX)
|
||||
|
||||
degraded = reading_degraded(reading.dist_sigma_raw, reading.presence_raw, reading.speed_sigma_raw)
|
||||
previous = ledger.history[-1] if ledger.history else None
|
||||
ratio_rate = None
|
||||
dist_rejected = False
|
||||
if previous is not None:
|
||||
prev_time, prev_dist = previous
|
||||
dt = now_s - prev_time
|
||||
if dt <= 0.0:
|
||||
dist_rejected = True
|
||||
else:
|
||||
ratio = decode_dist_ratio(reading.ratio_raw)
|
||||
ratio_rate = ratio_implied_rate(reading.ratio_raw, dist, dt)
|
||||
|
||||
residuals_m = []
|
||||
if native_speed is not None:
|
||||
residuals_m.append(abs(dist - (prev_dist + native_speed * dt)))
|
||||
if ratio is not None:
|
||||
residuals_m.append(abs(prev_dist - dist * ratio))
|
||||
|
||||
if residuals_m:
|
||||
innovation_m = min(residuals_m)
|
||||
dist_rejected = (innovation_m > DIST_INNOVATION_HARD_M or
|
||||
(degraded and innovation_m > DIST_INNOVATION_SOFT_M))
|
||||
else:
|
||||
dist_rejected = abs((dist - prev_dist) / dt) > RAW_RATE_LIMIT_MPS
|
||||
|
||||
if dist_rejected:
|
||||
# keep the last accepted point briefly as an unmeasured coast; rejected geometry is never
|
||||
# published and never becomes the baseline for a later derivative
|
||||
accepted_fresh = previous is not None and now_s - previous[0] <= QUIET_TIMEOUT_S
|
||||
point = self.pts.get(handle)
|
||||
if accepted_fresh and point is not None:
|
||||
point.measured = False
|
||||
else:
|
||||
self.pts.pop(handle, None)
|
||||
self._bind_slot(ledger, reading, now)
|
||||
continue
|
||||
|
||||
if sigma_veto:
|
||||
self._coast_point(ledger, now, dist, y_rel)
|
||||
self._bind_slot(ledger, reading, now)
|
||||
continue
|
||||
|
||||
# with neither a qualified native speed nor a usable ratio, the only remaining source is the
|
||||
# raw one-sweep derivative, which must never become an authoritative measurement: coast instead.
|
||||
# A true birth (no previous accepted sample) cannot mature this cycle regardless, so only
|
||||
# intercept once a derivative would have something to poison
|
||||
if native_speed is None and (ratio_rate is None or degraded) and previous is not None:
|
||||
self._coast_point(ledger, now, dist, y_rel)
|
||||
self._bind_slot(ledger, reading, now)
|
||||
continue
|
||||
|
||||
ledger.history.append((now_s, dist))
|
||||
|
||||
speed = native_speed if native_speed is not None else ratio_rate
|
||||
ledger.held_speed = speed
|
||||
ledger.held_speed_nanos = now
|
||||
|
||||
# a birth observation has no range rate yet: keep it as history, publish only once a second
|
||||
# coherent observation of the same identity supplies a finite rate
|
||||
matured = len(ledger.history) >= 2 and math.isfinite(speed)
|
||||
if matured:
|
||||
if handle not in self.pts:
|
||||
point = structs.RadarData.RadarPoint()
|
||||
point.trackId = handle
|
||||
point.aRel = float('nan')
|
||||
point.yvRel = float('nan')
|
||||
self.pts[handle] = point
|
||||
self.pts[handle].dRel = dist
|
||||
self.pts[handle].yRel = y_rel
|
||||
self.pts[handle].vRel = speed
|
||||
self.pts[handle].measured = True
|
||||
else:
|
||||
self.pts.pop(handle, None)
|
||||
|
||||
self._bind_slot(ledger, reading, now)
|
||||
|
||||
ret.points = [self.pts[handle] for handle in sorted(self.pts)]
|
||||
return ret
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user