diff --git a/.github/workflows/ci_weekly_report.yaml b/.github/workflows/ci_weekly_report.yaml index 22b8745872..9821283cb5 100644 --- a/.github/workflows/ci_weekly_report.yaml +++ b/.github/workflows/ci_weekly_report.yaml @@ -58,14 +58,14 @@ jobs: const jobName = job.name.split(" / ")[2]; const runRegex = /\((.*?)\)/; const run = job.name.match(runRegex)[1]; - report[jobName] = report[jobName] || { successes: [], failures: [], cancelled: [] }; + report[jobName] = report[jobName] || { successes: [], failures: [], canceled: [] }; switch (job.conclusion) { case "success": report[jobName].successes.push({ "run_number": run, "link": job.html_url}); break; case "failure": report[jobName].failures.push({ "run_number": run, "link": job.html_url }); break; - case "cancelled": - report[jobName].cancelled.push({ "run_number": run, "link": job.html_url }); break; + case "canceled": + report[jobName].canceled.push({ "run_number": run, "link": job.html_url }); break; } }); return JSON.stringify({"jobs": report}); diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 8c46e15c2f..9d1a546c5f 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -61,13 +61,13 @@ jobs: cd $STRIPPED_DIR ${{ env.RUN }} "release/check-dirty.sh && \ MAX_EXAMPLES=5 $PYTEST -m 'not slow' selfdrive/car" - - name: static analysis + - name: Static analysis timeout-minutes: 1 run: | cd $GITHUB_WORKSPACE cp pyproject.toml $STRIPPED_DIR cd $STRIPPED_DIR - ${{ env.RUN }} "scripts/lint.sh" + ${{ env.RUN }} "scripts/lint.sh --skip check_added_large_files" build: strategy: @@ -139,17 +139,16 @@ jobs: name: static analysis runs-on: ${{ ((github.repository == 'commaai/openpilot') && ((github.event_name != 'pull_request') || - (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-profile-amd64-8x16' || 'ubuntu-latest' }} + (github.event.pull_request.head.repo.full_name == 'commaai/openpilot'))) && 'namespace-profile-amd64-8x16' || 'ubuntu-24.04' }} + env: + PYTHONWARNINGS: default steps: - uses: actions/checkout@v4 - with: - submodules: true - - uses: ./.github/workflows/setup-with-retry - - name: Build openpilot - run: ${{ env.RUN }} "scons -j$(nproc)" - - name: static analysis + - name: Setup + run: tools/op.sh setup + - name: Static analysis timeout-minutes: 1 - run: ${{ env.RUN }} "scripts/lint.sh" + run: tools/op.sh lint unit_tests: name: unit tests diff --git a/.importlinter b/.importlinter index 721e45f1f8..ad1052643f 100644 --- a/.importlinter +++ b/.importlinter @@ -13,9 +13,13 @@ forbidden_modules = cereal capnp openpilot.common - openpilot.common.params + openpilot.selfdrive.controls + openpilot.selfdrive.debug + openpilot.selfdrive.pandad + openpilot.selfdrive.test openpilot.system openpilot.body + openpilot.tools openpilot.docs openpilot.msgq openpilot.panda @@ -26,6 +30,19 @@ forbidden_modules = ignore_imports = # remove these openpilot.selfdrive.car.interfaces -> openpilot.selfdrive.controls.lib.events + openpilot.selfdrive.car.body.carcontroller -> openpilot.selfdrive.controls.lib.pid + openpilot.selfdrive.car.tests.test_docs -> openpilot.common.basedir + openpilot.selfdrive.car.docs -> openpilot.common.basedir + + # car interface will not filter the speed + openpilot.selfdrive.car.interfaces -> openpilot.common.simple_kalman + + openpilot.selfdrive.car.gm.interface -> openpilot.common.basedir + openpilot.selfdrive.car.interfaces -> openpilot.common.basedir + + # params will need to move to new openpilot files that just call car files + openpilot.selfdrive.car.fw_versions -> openpilot.common.params + openpilot.selfdrive.car.ecu_addrs -> openpilot.common.params # these are okay openpilot.selfdrive.car.card -> openpilot.common.swaglog @@ -42,9 +59,13 @@ ignore_imports = openpilot.selfdrive.car.tests.test_car_interfaces -> openpilot.selfdrive.controls.lib.latcontrol_pid openpilot.selfdrive.car.card -> openpilot.common.params openpilot.selfdrive.car.tests.test_models -> openpilot.common.params - # these two will still live in openpilot, but require some modification - openpilot.selfdrive.car.fw_versions -> openpilot.common.params - openpilot.selfdrive.car.ecu_addrs -> openpilot.common.params + openpilot.selfdrive.car.tests.test_models -> openpilot.common.basedir + openpilot.selfdrive.car.card -> openpilot.selfdrive.pandad + openpilot.selfdrive.car.tests.test_docs -> openpilot.selfdrive.debug.dump_car_docs + openpilot.selfdrive.car.tests.test_docs -> openpilot.selfdrive.debug.print_docs_diff + openpilot.selfdrive.car.tests.test_car_interfaces -> openpilot.selfdrive.pandad + openpilot.selfdrive.car.tests.test_models -> openpilot.selfdrive.pandad + openpilot.selfdrive.car.tests.test_car_interfaces -> openpilot.selfdrive.test.fuzzy_generation openpilot.selfdrive.car.tests.test_models -> capnp openpilot.selfdrive.car.tests.test_car_interfaces -> cereal openpilot.selfdrive.car.tests.test_car_interfaces -> cereal.messaging diff --git a/common/utils.py b/common/utils.py deleted file mode 100644 index b9de020ee6..0000000000 --- a/common/utils.py +++ /dev/null @@ -1,11 +0,0 @@ -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) diff --git a/docs/contributing/roadmap.md b/docs/contributing/roadmap.md index 22622a7a96..ce50ad5577 100644 --- a/docs/contributing/roadmap.md +++ b/docs/contributing/roadmap.md @@ -5,6 +5,7 @@ This is the roadmap for the next major openpilot releases. Also check out * [Milestones](https://github.com/commaai/openpilot/milestones) for minor releases * [Projects](https://github.com/commaai/openpilot/projects?query=is%3Aopen) for shorter-term projects not tied to releases * [Bounties](https://comma.ai/bounties) for paid individual issues +* [#current-projects](https://discord.com/channels/469524606043160576/1249579909739708446) in Discord for discussion on work-in-progress projects ## openpilot 0.10 diff --git a/opendbc_repo b/opendbc_repo index 1e9f853615..f4d077b832 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit 1e9f8536151818f08af2ddd478f577f8461af31d +Subproject commit f4d077b832d46ac149c2b07dc37d777dc21237ea diff --git a/pyproject.toml b/pyproject.toml index 24ec80e787..a0131773e8 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -88,6 +88,7 @@ testing = [ "pytest-repeat", "ruff", "codespell", + "pre-commit-hooks", ] dev = [ @@ -167,9 +168,9 @@ testpaths = [ [tool.codespell] quiet-level = 3 # if you've got a short variable name that's getting flagged, add it here -ignore-words-list = "bu,ro,te,ue,alo,hda,ois,nam,nams,ned,som,parm,setts,inout,warmup,bumb,nd,sie,preints,whit,indexIn,ws,uint,grey,deque,stdio,amin,BA,LITE,atEnd,UIs,errorString,arange,FocusIn,od,tim,relA,hist,copyable" +ignore-words-list = "bu,ro,te,ue,alo,hda,ois,nam,nams,ned,som,parm,setts,inout,warmup,bumb,nd,sie,preints,whit,indexIn,ws,uint,grey,deque,stdio,amin,BA,LITE,atEnd,UIs,errorString,arange,FocusIn,od,tim,relA,hist,copyable,jupyter,thead" builtin = "clear,rare,informal,code,names,en-GB_to_en-US" -skip = "./third_party/*, ./tinygrad/*, ./tinygrad_repo/*, ./msgq/*, ./panda/*, ./opendbc/*, ./opendbc_repo/*, ./rednose/*, ./rednose_repo/*, ./teleoprtc/*, ./teleoprtc_repo/*, ./selfdrive/ui/translations/*.ts, uv.lock, *.onnx, ./cereal/gen/*, */c_generated_code/*" +skip = "./third_party/*, ./tinygrad/*, ./tinygrad_repo/*, ./msgq/*, ./panda/*, ./opendbc/*, ./opendbc_repo/*, ./rednose/*, ./rednose_repo/*, ./teleoprtc/*, ./teleoprtc_repo/*, *.ts, uv.lock, *.onnx, ./cereal/gen/*, */c_generated_code/*" [tool.mypy] python_version = "3.11" @@ -177,9 +178,11 @@ plugins = [ "numpy.typing.mypy_plugin", ] exclude = [ - "body/", "cereal/", + "msgq/", + "msgq_repo/", "opendbc/", + "opendbc_repo/", "panda/", "rednose/", "rednose_repo/", @@ -204,6 +207,10 @@ warn_return_any=true # allow implicit optionals for default args implicit_optional = true +local_partial_types=true +explicit_package_bases=true +disable_error_code = "annotation-unchecked" + # https://beta.ruff.rs/docs/configuration/#using-pyprojecttoml [tool.ruff] indent-width = 2 diff --git a/scripts/lint.sh b/scripts/lint.sh index 3da79f62f4..2b6fd07185 100755 --- a/scripts/lint.sh +++ b/scripts/lint.sh @@ -1,11 +1,116 @@ #!/bin/bash set -e +RED='\033[0;31m' +GREEN='\033[0;32m' +UNDERLINE='\033[4m' +BOLD='\033[1m' +NC='\033[0m' + DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )" cd $DIR/../ -# TODO: bring back rest of pre-commit checks: -# https://github.com/commaai/openpilot/blob/4b11c9e914707df9def598616995be2a5d355a6a/.pre-commit-config.yaml#L2 +FAILED=0 + +IGNORED_FILES="uv\.lock|docs\/CARS.md" +IGNORED_DIRS="^third_party.*|^msgq.*|^msgq_repo.*|^opendbc.*|^opendbc_repo.*|^cereal.*|^panda.*|^rednose.*|^rednose_repo.*|^tinygrad.*|^tinygrad_repo.*|^teleoprtc.*|^teleoprtc_repo.*" + +function run() { + shopt -s extglob + case $1 in + $SKIP | $RUN ) return 0 ;; + esac + + echo -en "$1" + + for ((i=0; i<$((50 - ${#1})); i++)); do + echo -n "." + done + + shift 1; + CMD="$@" + + set +e + log="$((eval "$CMD" ) 2>&1)" + + if [[ $? -eq 0 ]]; then + echo -e "[${GREEN}✔${NC}]" + else + echo -e "[${RED}✗${NC}]" + echo "$log" + FAILED=1 + fi + set -e +} + +function run_tests() { + ALL_FILES=$1 + PYTHON_FILES=$2 + + run "ruff" ruff check $PYTHON_FILES --quiet + run "lint-imports" lint-imports + run "check_added_large_files" python3 -m pre_commit_hooks.check_added_large_files --enforce-all $ALL_FILES --maxkb=120 + run "check_shebang_scripts_are_executable" python3 -m pre_commit_hooks.check_shebang_scripts_are_executable $ALL_FILES + + if [[ -z "$FAST" ]]; then + run "mypy" mypy $PYTHON_FILES + run "codespell" codespell $ALL_FILES + fi + + return $FAILED +} + +function help() { + echo "A fast linter" + echo "" + echo -e "${BOLD}${UNDERLINE}Usage:${NC} op lint [TESTS] [OPTIONS]" + echo "" + echo -e "${BOLD}${UNDERLINE}Tests:${NC}" + echo -e " ${BOLD}ruff${NC}" + echo -e " ${BOLD}mypy${NC}" + echo -e " ${BOLD}lint-imports${NC}" + echo -e " ${BOLD}codespell${NC}" + echo -e " ${BOLD}check_added_large_files${NC}" + echo -e " ${BOLD}check_shebang_scripts_are_executable${NC}" + echo "" + echo -e "${BOLD}${UNDERLINE}Options:${NC}" + echo -e " ${BOLD}-f, --fast${NC}" + echo " Skip slow tests" + echo -e " ${BOLD}-s, --skip${NC}" + echo " Specify tests to skip separated by spaces" + echo "" + echo -e "${BOLD}${UNDERLINE}Examples:${NC}" + echo " op lint mypy ruff" + echo " Only run the mypy and ruff tests" + echo "" + echo " op lint --skip mypy ruff" + echo " Skip the mypy and ruff tests" + echo "" + echo " op lint" + echo " Run all the tests" +} + +SKIP="" +RUN="" +while [[ $# -gt 0 ]]; do + case $1 in + -f | --fast ) shift 1; FAST="1" ;; + -s | --skip ) shift 1; SKIP=" " ;; + -h | --help | -help | --h ) help; exit 0 ;; + * ) if [[ -n $SKIP ]]; then SKIP+="$1 "; else RUN+="$1 "; fi; shift 1 ;; + esac +done + +RUN=$([ -z "$RUN" ] && echo "" || echo "!($(echo $RUN | sed 's/ /|/g'))") +SKIP="@($(echo $SKIP | sed 's/ /|/g'))" + +GIT_FILES="$(git ls-files | sed -E "s/$IGNORED_FILES|$IGNORED_DIRS//g")" +ALL_FILES="" +for f in $GIT_FILES; do + if [[ -f $f ]]; then + ALL_FILES+="$f"$'\n' + fi +done +PYTHON_FILES=$(echo "$ALL_FILES" | grep --color=never '.py$' || true) -ruff check . -lint-imports +run_tests "$ALL_FILES" "$PYTHON_FILES" diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index 0cdc6e9d8d..2c6a45d557 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -9,9 +9,9 @@ import capnp from cereal import car from panda.python.uds import SERVICE_TYPE -from openpilot.common.numpy_fast import clip, interp -from openpilot.common.utils import Freezable +from openpilot.selfdrive.car.can_definitions import CanData from openpilot.selfdrive.car.docs_definitions import CarDocs +from openpilot.selfdrive.car.helpers import clip, interp # set up logging carlog = logging.getLogger('carlog') @@ -194,10 +194,6 @@ def get_friction(lateral_accel_error: float, lateral_accel_deadzone: float, fric return friction -def make_can_msg(addr, dat, bus): - return [addr, dat, bus] - - def make_tester_present_msg(addr, bus, subaddr=None, suppress_response=False): dat = [0x02, SERVICE_TYPE.TESTER_PRESENT] if subaddr is not None: @@ -205,7 +201,7 @@ def make_tester_present_msg(addr, bus, subaddr=None, suppress_response=False): dat.append(0x80 if suppress_response else 0x0) # sub-function dat.extend([0x0] * (8 - len(dat))) - return make_can_msg(addr, bytes(dat), bus) + return CanData(addr, bytes(dat), bus) def get_safety_config(safety_model, safety_param = None): @@ -263,6 +259,19 @@ class CarSpecs: 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 PlatformConfig(Freezable): car_docs: list[CarDocs] diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index 0771d7a471..eebf6798a2 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -16,8 +16,8 @@ MAX_TURN_INTEGRATOR = 0.1 # meters class CarController(CarControllerBase): - def __init__(self, dbc_name, CP, VM): - super().__init__(dbc_name, CP, VM) + def __init__(self, dbc_name, CP): + super().__init__(dbc_name, CP) self.packer = CANPacker(dbc_name) # PIDs diff --git a/selfdrive/car/can_definitions.py b/selfdrive/car/can_definitions.py new file mode 100644 index 0000000000..2dba7f26a8 --- /dev/null +++ b/selfdrive/car/can_definitions.py @@ -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]]: ... diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 03351fdb98..d2345f1a05 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -1,27 +1,19 @@ import os import time -from collections.abc import Callable from cereal import car from openpilot.selfdrive.car import carlog +from openpilot.selfdrive.car.can_definitions import CanRecvCallable, CanSendCallable from openpilot.selfdrive.car.interfaces import get_interface_attr from openpilot.selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars from openpilot.selfdrive.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN -from openpilot.selfdrive.car.fw_versions import get_fw_versions_ordered, get_present_ecus, match_fw_to_car +from openpilot.selfdrive.car.fw_versions import ObdCallback, get_fw_versions_ordered, get_present_ecus, match_fw_to_car from openpilot.selfdrive.car.mock.values import CAR as MOCK -import cereal.messaging as messaging from openpilot.selfdrive.car import gen_empty_fingerprint FRAME_FINGERPRINT = 100 # 1s -def get_one_can(logcan): - while True: - can = messaging.recv_one_retry(logcan) - if len(can.can) > 0: - return can - - def load_interfaces(brand_names): ret = {} for brand_name in brand_names: @@ -48,7 +40,7 @@ interface_names = _get_interface_names() interfaces = load_interfaces(interface_names) -def can_fingerprint(next_can: Callable) -> tuple[str | None, dict[int, dict]]: +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 @@ -56,40 +48,41 @@ def can_fingerprint(next_can: Callable) -> tuple[str | None, dict[int, dict]]: done = False while not done: - a = next_can() - - for can in a.can: - # 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) - + # 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: - # 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] + 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 + # 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 + frame += 1 return car_fingerprint, finger # **** for use live only **** -def fingerprint(logcan, sendcan, set_obd_multiplexing, num_pandas, cached_params_raw): +def fingerprint(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, num_pandas: int, cached_params_raw: bytes | None): fixed_fingerprint = os.environ.get('FINGERPRINT', "") skip_fw_query = os.environ.get('SKIP_FW_QUERY', False) disable_fw_cache = os.environ.get('DISABLE_FW_CACHE', False) @@ -115,9 +108,9 @@ def fingerprint(logcan, sendcan, set_obd_multiplexing, num_pandas, cached_params # 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(logcan, sendcan, (0, 1)) - ecu_rx_addrs = get_present_ecus(logcan, sendcan, set_obd_multiplexing, num_pandas=num_pandas) - car_fw = get_fw_versions_ordered(logcan, sendcan, set_obd_multiplexing, vin, ecu_rx_addrs, num_pandas=num_pandas) + 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) @@ -138,8 +131,8 @@ def fingerprint(logcan, sendcan, set_obd_multiplexing, num_pandas, cached_params # CAN fingerprint # drain CAN socket so we get the latest messages - messaging.drain_sock_raw(logcan) - car_fingerprint, finger = can_fingerprint(lambda: get_one_can(logcan)) + can_recv() + car_fingerprint, finger = can_fingerprint(can_recv) exact_match = True source = car.CarParams.FingerprintSource.can @@ -166,8 +159,9 @@ def get_car_interface(CP): return CarInterface(CP, CarController, CarState) -def get_car(logcan, sendcan, set_obd_multiplexing, experimental_long_allowed, num_pandas=1, cached_params=None): - candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(logcan, sendcan, set_obd_multiplexing, num_pandas, cached_params) +def get_car(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, experimental_long_allowed: bool, + num_pandas: int = 1, cached_params: bytes | None = None): + candidate, fingerprints, vin, car_fw, source, exact_match = fingerprint(can_recv, can_send, set_obd_multiplexing, num_pandas, cached_params) if candidate is None: carlog.error({"event": "car doesn't match any fingerprints", "fingerprints": repr(fingerprints)}) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index e878a14c31..4919be30c9 100755 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -12,9 +12,11 @@ from openpilot.common.params import Params from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper from openpilot.common.swaglog import cloudlog, ForwardingHandler -from openpilot.selfdrive.pandad import can_list_to_can_capnp +from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.selfdrive.car import DT_CTRL, carlog -from openpilot.selfdrive.car.car_helpers import get_car, get_one_can +from openpilot.selfdrive.car.can_definitions import CanData, CanRecvCallable, CanSendCallable +from openpilot.selfdrive.car.fw_versions import ObdCallback +from openpilot.selfdrive.car.car_helpers import get_car from openpilot.selfdrive.car.interfaces import CarInterfaceBase from openpilot.selfdrive.controls.lib.events import Events @@ -26,7 +28,7 @@ EventName = car.CarEvent.EventName carlog.addHandler(ForwardingHandler(cloudlog)) -def obd_callback(params: Params): +def obd_callback(params: Params) -> ObdCallback: def set_obd_multiplexing(obd_multiplexing: bool): if params.get_bool("ObdMultiplexingEnabled") != obd_multiplexing: cloudlog.warning(f"Setting OBD multiplexing to {obd_multiplexing}") @@ -37,10 +39,28 @@ def obd_callback(params: Params): return set_obd_multiplexing +def can_comm_callbacks(logcan: messaging.SubSocket, sendcan: messaging.PubSocket) -> tuple[CanRecvCallable, CanSendCallable]: + def can_recv(wait_for_one: bool = False) -> list[list[CanData]]: + """ + wait_for_one: wait the normal logcan socket timeout for a CAN packet, may return empty list if nothing comes + + Returns: CAN packets comprised of CanData objects for easy access + """ + ret = [] + for can in messaging.drain_sock(logcan, wait_for_one=wait_for_one): + ret.append([CanData(msg.address, msg.dat, msg.src) for msg in can.can]) + return ret + + def can_send(msgs: list[CanData]) -> None: + sendcan.send(can_list_to_can_capnp(msgs, msgtype='sendcan')) + + return can_recv, can_send + + class Car: CI: CarInterfaceBase - def __init__(self, CI=None): + def __init__(self, CI=None) -> None: self.can_sock = messaging.sub_sock('can', timeout=20) self.sm = messaging.SubMaster(['pandaStates', 'carControl', 'onroadEvents']) self.pm = messaging.PubMaster(['sendcan', 'carState', 'carParams', 'carOutput']) @@ -55,15 +75,20 @@ class Car: self.params = Params() + self.can_callbacks = can_comm_callbacks(self.can_sock, self.pm.sock['sendcan']) + if CI is None: # wait for one pandaState and one CAN packet print("Waiting for CAN messages...") - get_one_can(self.can_sock) + while True: + can = messaging.recv_one_retry(self.can_sock) + if len(can.can) > 0: + break experimental_long_allowed = self.params.get_bool("ExperimentalLongitudinalEnabled") num_pandas = len(messaging.recv_one_retry(self.sm.sock['pandaStates']).pandaStates) cached_params = self.params.get("CarParamsCache") - self.CI = get_car(self.can_sock, self.pm.sock['sendcan'], obd_callback(self.params), experimental_long_allowed, num_pandas, cached_params) + self.CI = get_car(*self.can_callbacks, obd_callback(self.params), experimental_long_allowed, num_pandas, cached_params) self.CP = self.CI.CP # continue onto next fingerprinting step in pandad @@ -108,7 +133,7 @@ class Car: # Update carState from CAN can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True) - CS = self.CI.update(self.CC_prev, can_strs) + CS = self.CI.update(self.CC_prev, can_capnp_to_list(can_strs)) self.sm.update(0) @@ -166,7 +191,7 @@ class Car: if not self.initialized_prev: # Initialize CarInterface, once controls are ready # TODO: this can make us miss at least a few cycles when doing an ECU knockout - self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) + self.CI.init(self.CP, *self.can_callbacks) # signal pandad to switch to car safety mode self.params.put_bool_nonblocking("ControlsReady", True) diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index c0c03e0e05..c194488de9 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -6,8 +6,8 @@ from openpilot.selfdrive.car.interfaces import CarControllerBase class CarController(CarControllerBase): - def __init__(self, dbc_name, CP, VM): - super().__init__(dbc_name, CP, VM) + def __init__(self, dbc_name, CP): + super().__init__(dbc_name, CP) self.apply_steer_last = 0 self.hud_count = 0 diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 91b922c596..c8b084aa62 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -1,9 +1,9 @@ from cereal import car -from openpilot.common.conversions import Conversions as CV from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine -from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD, RAM_CARS +from openpilot.selfdrive.car.conversions import Conversions as CV +from openpilot.selfdrive.car.interfaces import CarStateBase class CarState(CarStateBase): diff --git a/selfdrive/car/conversions.py b/selfdrive/car/conversions.py new file mode 100644 index 0000000000..b02b33c625 --- /dev/null +++ b/selfdrive/car/conversions.py @@ -0,0 +1,19 @@ +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 diff --git a/selfdrive/car/disable_ecu.py b/selfdrive/car/disable_ecu.py index 5229b519a8..bbf5b4c9ff 100755 --- a/selfdrive/car/disable_ecu.py +++ b/selfdrive/car/disable_ecu.py @@ -8,7 +8,7 @@ EXT_DIAG_RESPONSE = b'\x50\x03' COM_CONT_RESPONSE = b'' -def disable_ecu(logcan, sendcan, bus=0, addr=0x7d0, sub_addr=None, com_cont_req=b'\x28\x83\x01', timeout=0.1, retry=10, debug=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, debug=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. @@ -18,12 +18,12 @@ def disable_ecu(logcan, sendcan, bus=0, addr=0x7d0, sub_addr=None, com_cont_req= for i in range(retry): try: - query = IsoTpParallelQuery(sendcan, logcan, bus, [(addr, sub_addr)], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug) + query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug) for _, _ in query.get_data(timeout).items(): carlog.warning("communication control disable tx/rx ...") - query = IsoTpParallelQuery(sendcan, logcan, bus, [(addr, sub_addr)], [com_cont_req], [COM_CONT_RESPONSE], debug=debug) + query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [com_cont_req], [COM_CONT_RESPONSE], debug=debug) query.get_data(0) carlog.warning("ecu disabled") diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index a04653c140..f039fea9ac 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -5,7 +5,7 @@ from dataclasses import dataclass, field from enum import Enum from cereal import car -from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.car.conversions import Conversions as CV GOOD_TORQUE_THRESHOLD = 1.0 # m/s^2 MODEL_YEARS_RE = r"(?<= )((\d{4}-\d{2})|(\d{4}))(,|$)" diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py index e36c1dc981..336d0d5542 100755 --- a/selfdrive/car/ecu_addrs.py +++ b/selfdrive/car/ecu_addrs.py @@ -2,11 +2,10 @@ import capnp import time -import cereal.messaging as messaging from panda.python.uds import SERVICE_TYPE from openpilot.selfdrive.car import make_tester_present_msg, carlog +from openpilot.selfdrive.car.can_definitions import CanRecvCallable, CanSendCallable from openpilot.selfdrive.car.fw_query_definitions import EcuAddrBusType -from openpilot.selfdrive.pandad import can_list_to_can_capnp def _is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subaddr: int = None) -> bool: @@ -23,26 +22,26 @@ def _is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subad return False -def _get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> set[EcuAddrBusType]: +def _get_all_ecu_addrs(can_recv: CanRecvCallable, can_send: CanSendCallable, bus: int, timeout: float = 1, debug: bool = True) -> 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(logcan, sendcan, queries, responses, timeout=timeout, debug=debug) + return get_ecu_addrs(can_recv, can_send, queries, responses, timeout=timeout, debug=debug) -def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: set[EcuAddrBusType], +def get_ecu_addrs(can_recv: CanRecvCallable, can_send: CanSendCallable, queries: set[EcuAddrBusType], responses: set[EcuAddrBusType], timeout: float = 1, debug: bool = False) -> 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] - messaging.drain_sock_raw(logcan) - sendcan.send(can_list_to_can_capnp(msgs, msgtype='sendcan')) + can_recv() + can_send(msgs) start_time = time.monotonic() while time.monotonic() - start_time < timeout: - can_packets = messaging.drain_sock(logcan, wait_for_one=True) + can_packets = can_recv(wait_for_one=True) for packet in can_packets: - for msg in packet.can: + for msg in packet: if not len(msg.dat): carlog.warning("ECU addr scan: skipping empty remote frame") continue @@ -61,6 +60,7 @@ def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, que if __name__ == "__main__": import argparse + import cereal.messaging as messaging from openpilot.common.params import Params from openpilot.selfdrive.car.card import obd_callback diff --git a/selfdrive/car/filter_simple.py b/selfdrive/car/filter_simple.py new file mode 100644 index 0000000000..0ec7a51562 --- /dev/null +++ b/selfdrive/car/filter_simple.py @@ -0,0 +1,18 @@ +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_alpha(self, rc): + self.alpha = self.dt / (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 diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index b63b741ee7..961d7086c4 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,9 +1,9 @@ from cereal import car from opendbc.can.packer import CANPacker -from openpilot.common.numpy_fast import clip from openpilot.selfdrive.car import apply_std_steer_angle_limits from openpilot.selfdrive.car.ford import fordcan from openpilot.selfdrive.car.ford.values import CarControllerParams, FordFlags +from openpilot.selfdrive.car.helpers import clip from openpilot.selfdrive.car.interfaces import CarControllerBase, V_CRUISE_MAX LongCtrlState = car.CarControl.Actuators.LongControlState @@ -23,9 +23,8 @@ def apply_ford_curvature_limits(apply_curvature, apply_curvature_last, current_c class CarController(CarControllerBase): - def __init__(self, dbc_name, CP, VM): - super().__init__(dbc_name, CP, VM) - self.VM = VM + def __init__(self, dbc_name, CP): + super().__init__(dbc_name, CP) self.packer = CANPacker(dbc_name) self.CAN = fordcan.CanBus(CP) diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index d7130681d4..ed707a5f78 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -1,7 +1,7 @@ from cereal import car from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser -from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.ford.fordcan import CanBus from openpilot.selfdrive.car.ford.values import DBC, CarControllerParams, FordFlags from openpilot.selfdrive.car.interfaces import CarStateBase diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 54cf865109..b872be212b 100644 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -1,7 +1,7 @@ from cereal import car from panda import Panda -from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car import create_button_events, get_safety_config +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.ford.fordcan import CanBus from openpilot.selfdrive.car.ford.values import Ecu, FordFlags from openpilot.selfdrive.car.interfaces import CarInterfaceBase diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index 209bbebae3..f405373661 100644 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -1,7 +1,7 @@ from math import cos, sin from cereal import car from opendbc.can.parser import CANParser -from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.ford.fordcan import CanBus from openpilot.selfdrive.car.ford.values import DBC, RADAR from openpilot.selfdrive.car.interfaces import RadarInterfaceBase diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 87f5f3defd..7cee14bfbd 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -11,6 +11,7 @@ from cereal import car from openpilot.selfdrive.car import carlog from openpilot.selfdrive.car.ecu_addrs import get_ecu_addrs from openpilot.selfdrive.car.fingerprints import FW_VERSIONS +from openpilot.selfdrive.car.can_definitions import CanRecvCallable, CanSendCallable from openpilot.selfdrive.car.fw_query_definitions import AddrType, EcuAddrBusType, FwQueryConfig, LiveFwVersions, OfflineFwVersions from openpilot.selfdrive.car.interfaces import get_interface_attr from openpilot.selfdrive.car.isotp_parallel_query import IsoTpParallelQuery @@ -171,7 +172,7 @@ def match_fw_to_car(fw_versions: list[capnp.lib.capnp._DynamicStructBuilder], vi return True, set() -def get_present_ecus(logcan, sendcan, set_obd_multiplexing: ObdCallback, num_pandas: int = 1) -> set[EcuAddrBusType]: +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: []} @@ -205,7 +206,7 @@ def get_present_ecus(logcan, sendcan, set_obd_multiplexing: ObdCallback, num_pan for obd_multiplexing in queries: set_obd_multiplexing(obd_multiplexing) for query in queries[obd_multiplexing]: - ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1)) + ecu_responses.update(get_ecu_addrs(can_recv, can_send, set(query), responses, timeout=0.1)) return ecu_responses @@ -227,8 +228,9 @@ def get_brand_ecu_matches(ecu_rx_addrs: set[EcuAddrBusType]) -> dict[str, set[Ad return brand_matches -def get_fw_versions_ordered(logcan, sendcan, set_obd_multiplexing: ObdCallback, vin: str, ecu_rx_addrs: set[EcuAddrBusType], timeout: float = 0.1, - num_pandas: int = 1, debug: bool = False, progress: bool = False) -> list[capnp.lib.capnp._DynamicStructBuilder]: +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, debug: bool = False, + progress: bool = False) -> list[capnp.lib.capnp._DynamicStructBuilder]: """Queries for FW versions ordering brands by likelihood, breaks when exact match is found""" all_car_fw = [] @@ -239,7 +241,8 @@ def get_fw_versions_ordered(logcan, sendcan, set_obd_multiplexing: ObdCallback, if not len(brand_matches[brand]): continue - car_fw = get_fw_versions(logcan, sendcan, set_obd_multiplexing, query_brand=brand, timeout=timeout, num_pandas=num_pandas, debug=debug, progress=progress) + car_fw = get_fw_versions(can_recv, can_send, set_obd_multiplexing, query_brand=brand, timeout=timeout, num_pandas=num_pandas, debug=debug, + progress=progress) all_car_fw.extend(car_fw) # If there is a match using this brand's FW alone, finish querying early @@ -250,8 +253,9 @@ def get_fw_versions_ordered(logcan, sendcan, set_obd_multiplexing: ObdCallback, return all_car_fw -def get_fw_versions(logcan, sendcan, set_obd_multiplexing: ObdCallback, query_brand: str = None, extra: OfflineFwVersions = None, timeout: float = 0.1, - num_pandas: int = 1, debug: bool = False, progress: bool = False) -> list[capnp.lib.capnp._DynamicStructBuilder]: +def get_fw_versions(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multiplexing: ObdCallback, query_brand: str = None, + extra: OfflineFwVersions = None, timeout: float = 0.1, num_pandas: int = 1, debug: bool = False, + progress: bool = False) -> list[capnp.lib.capnp._DynamicStructBuilder]: versions = VERSIONS.copy() if query_brand is not None: @@ -301,7 +305,7 @@ def get_fw_versions(logcan, sendcan, set_obd_multiplexing: ObdCallback, query_br (len(r.whitelist_ecus) == 0 or ecu_types[(b, a, s)] in r.whitelist_ecus)] if query_addrs: - query = IsoTpParallelQuery(sendcan, logcan, r.bus, query_addrs, r.request, r.response, r.rx_offset, debug=debug) + query = IsoTpParallelQuery(can_send, can_recv, r.bus, query_addrs, r.request, r.response, r.rx_offset, debug=debug) for (tx_addr, sub_addr), version in query.get_data(timeout).items(): f = car.CarParams.CarFw.new_message() diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index cac4f13ed5..dff280ed9a 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -1,10 +1,10 @@ from cereal import car -from openpilot.common.conversions import Conversions as CV -from openpilot.common.numpy_fast import interp from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.gm import gmcan from openpilot.selfdrive.car.gm.values import DBC, CanBus, CarControllerParams, CruiseButtons +from openpilot.selfdrive.car.helpers import interp from openpilot.selfdrive.car.interfaces import CarControllerBase VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -18,8 +18,8 @@ MIN_STEER_MSG_INTERVAL_MS = 15 class CarController(CarControllerBase): - def __init__(self, dbc_name, CP, VM): - super().__init__(dbc_name, CP, VM) + def __init__(self, dbc_name, CP): + super().__init__(dbc_name, CP) self.start_time = 0. self.apply_steer_last = 0 self.apply_gas = 0 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index a1129c59c8..97dbd389f4 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -1,9 +1,9 @@ import copy from cereal import car -from openpilot.common.conversions import Conversions as CV -from openpilot.common.numpy_fast import mean from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser +from openpilot.selfdrive.car.conversions import Conversions as CV +from openpilot.selfdrive.car.helpers import mean from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.car.gm.values import DBC, AccState, CanBus, STEER_THRESHOLD diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index a70bcccd06..0b7ada3151 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -1,4 +1,4 @@ -from openpilot.selfdrive.car import make_can_msg +from openpilot.selfdrive.car.can_definitions import CanData from openpilot.selfdrive.car.gm.values import CAR @@ -49,7 +49,7 @@ def create_steering_control(packer, bus, apply_steer, idx, lkas_active): def create_adas_keepalive(bus): dat = b"\x00\x00\x00\x00\x00\x00\x00" - return [make_can_msg(0x409, dat, bus), make_can_msg(0x40a, dat, bus)] + return [CanData(0x409, dat, bus), CanData(0x40a, dat, bus)] def create_gas_regen_command(packer, bus, throttle, idx, enabled, at_full_stop): @@ -125,14 +125,14 @@ def create_adas_time_status(bus, tt, idx): chksum = 0x1000 - dat[0] - dat[1] - dat[2] - dat[3] chksum = chksum & 0xfff dat += [0x40 + (chksum >> 8), chksum & 0xff, 0x12] - return make_can_msg(0xa1, bytes(dat), bus) + 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 make_can_msg(0x306, bytes(dat), bus) + return CanData(0x306, bytes(dat), bus) def create_adas_accelerometer_speed_status(bus, speed_ms, idx): @@ -146,7 +146,7 @@ def create_adas_accelerometer_speed_status(bus, speed_ms, idx): 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 make_can_msg(0x308, bytes(dat), bus) + return CanData(0x308, bytes(dat), bus) def create_adas_headlights_status(packer, bus): @@ -170,4 +170,4 @@ def create_lka_icon_command(bus, active, critical, steer): dat = b"\x40\x40\x18" else: dat = b"\x00\x00\x00" - return make_can_msg(0x104c006c, dat, bus) + return CanData(0x104c006c, dat, bus) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 1d4a6066cc..31b874502b 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -5,8 +5,8 @@ from math import fabs, exp from panda import Panda from openpilot.common.basedir import BASEDIR -from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car import create_button_events, get_safety_config, get_friction +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.gm.radar_interface import RADAR_HEADER_MSG from openpilot.selfdrive.car.gm.values import CAR, CruiseButtons, CarControllerParams, EV_CAR, CAMERA_ACC_CAR, CanBus from openpilot.selfdrive.car.interfaces import CarInterfaceBase, TorqueFromLateralAccelCallbackType, FRICTION_THRESHOLD, LatControlInputs, NanoFFModel diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index b893babd31..b305d418b8 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 import math from cereal import car -from openpilot.common.conversions import Conversions as CV from opendbc.can.parser import CANParser +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.gm.values import DBC, CanBus from openpilot.selfdrive.car.interfaces import RadarInterfaceBase diff --git a/selfdrive/car/helpers.py b/selfdrive/car/helpers.py new file mode 100644 index 0000000000..81f6d6a6bc --- /dev/null +++ b/selfdrive/car/helpers.py @@ -0,0 +1,22 @@ +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) + diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 5748812648..0f08ee21a8 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -1,9 +1,9 @@ from collections import namedtuple from cereal import car -from openpilot.common.numpy_fast import clip, interp from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import DT_CTRL, rate_limit, make_tester_present_msg +from openpilot.selfdrive.car.helpers import clip, interp from openpilot.selfdrive.car.honda import hondacan from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams from openpilot.selfdrive.car.interfaces import CarControllerBase @@ -104,8 +104,8 @@ def rate_limit_steer(new_steer, last_steer): class CarController(CarControllerBase): - def __init__(self, dbc_name, CP, VM): - super().__init__(dbc_name, CP, VM) + def __init__(self, dbc_name, CP): + super().__init__(dbc_name, CP) self.packer = CANPacker(dbc_name) self.params = CarControllerParams(CP) self.CAN = hondacan.CanBus(CP) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index c98d1a72d3..956e6a3bd3 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -1,10 +1,10 @@ from collections import defaultdict from cereal import car -from openpilot.common.conversions import Conversions as CV -from openpilot.common.numpy_fast import interp from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser +from openpilot.selfdrive.car.conversions import Conversions as CV +from openpilot.selfdrive.car.helpers import interp from openpilot.selfdrive.car.honda.hondacan import CanBus, get_cruise_speed_conversion from openpilot.selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, \ HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS, \ diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 1be496d951..b350455014 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -1,5 +1,5 @@ -from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car import CanBusBase +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.honda.values import HondaFlags, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, CAR, CarControllerParams # CAN bus layout with relay diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index fdcba8bb81..3eefd8f862 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 from cereal import car from panda import Panda -from openpilot.common.conversions import Conversions as CV -from openpilot.common.numpy_fast import interp +from openpilot.selfdrive.car.conversions import Conversions as CV +from openpilot.selfdrive.car.helpers import interp from openpilot.selfdrive.car.honda.hondacan import CanBus from openpilot.selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CruiseSettings, HondaFlags, CAR, HONDA_BOSCH, \ HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS @@ -217,9 +217,9 @@ class CarInterface(CarInterfaceBase): return ret @staticmethod - def init(CP, logcan, sendcan): + def init(CP, can_recv, can_send): if CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl: - disable_ecu(logcan, sendcan, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03') + disable_ecu(can_recv, can_send, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03') # returns a car.CarState def _update(self, c): diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 5a78995c0a..53e5435f29 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -2,9 +2,9 @@ from dataclasses import dataclass from enum import Enum, IntFlag from cereal import car -from openpilot.common.conversions import Conversions as CV from panda.python import uds from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16 diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 84b565f0cd..0e3a3f0afd 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -1,8 +1,8 @@ from cereal import car -from openpilot.common.conversions import Conversions as CV -from openpilot.common.numpy_fast import clip from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg +from openpilot.selfdrive.car.conversions import Conversions as CV +from openpilot.selfdrive.car.helpers import clip from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, Buttons, CarControllerParams, CANFD_CAR, CAR @@ -43,8 +43,8 @@ def process_hud_alert(enabled, fingerprint, hud_control): class CarController(CarControllerBase): - def __init__(self, dbc_name, CP, VM): - super().__init__(dbc_name, CP, VM) + def __init__(self, dbc_name, CP): + super().__init__(dbc_name, CP) self.CAN = CanBus(CP) self.params = CarControllerParams(CP) self.packer = CANPacker(dbc_name) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 41cb9ee457..1b56b3304e 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -3,9 +3,9 @@ import copy import math from cereal import car -from openpilot.common.conversions import Conversions as CV from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_GEARS, CAMERA_SCC_CAR, \ CANFD_CAR, Buttons, CarControllerParams diff --git a/selfdrive/car/hyundai/hyundaicanfd.py b/selfdrive/car/hyundai/hyundaicanfd.py index 54804f94fd..bd46f18612 100644 --- a/selfdrive/car/hyundai/hyundaicanfd.py +++ b/selfdrive/car/hyundai/hyundaicanfd.py @@ -1,5 +1,5 @@ -from openpilot.common.numpy_fast import clip from openpilot.selfdrive.car import CanBusBase +from openpilot.selfdrive.car.helpers import clip from openpilot.selfdrive.car.hyundai.values import HyundaiFlags diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index ecedf3fd7c..9bb8e7ecf5 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -139,16 +139,16 @@ class CarInterface(CarInterfaceBase): return ret @staticmethod - def init(CP, logcan, sendcan): + def init(CP, can_recv, can_send): if CP.openpilotLongitudinalControl and not (CP.flags & HyundaiFlags.CANFD_CAMERA_SCC.value): addr, bus = 0x7d0, 0 if CP.flags & HyundaiFlags.CANFD_HDA2.value: addr, bus = 0x730, CanBus(CP).ECAN - disable_ecu(logcan, sendcan, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01') + disable_ecu(can_recv, can_send, bus=bus, addr=addr, com_cont_req=b'\x28\x83\x01') # for blinkers if CP.flags & HyundaiFlags.ENABLE_BLINKERS: - disable_ecu(logcan, sendcan, bus=CanBus(CP).ECAN, addr=0x7B1, com_cont_req=b'\x28\x83\x01') + disable_ecu(can_recv, can_send, bus=CanBus(CP).ECAN, addr=0x7B1, com_cont_req=b'\x28\x83\x01') def _update(self, c): ret = self.CS.update(self.cp, self.cp_cam) diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index 8ed0edd319..0db82a7577 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -4,8 +4,8 @@ from enum import Enum, IntFlag from cereal import car from panda.python import uds -from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, p16 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index f8d71ad341..4e434ec386 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -10,14 +10,13 @@ from functools import cache from cereal import car from openpilot.common.basedir import BASEDIR -from openpilot.common.conversions import Conversions as CV from openpilot.common.simple_kalman import KF1D, get_kalman_gain -from openpilot.common.numpy_fast import clip from openpilot.selfdrive.car import DT_CTRL, apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, get_friction, STD_CARGO_KG +from openpilot.selfdrive.car.can_definitions import CanData, CanRecvCallable, CanSendCallable +from openpilot.selfdrive.car.conversions import Conversions as CV +from openpilot.selfdrive.car.helpers import clip from openpilot.selfdrive.car.values import PLATFORMS from openpilot.selfdrive.controls.lib.events import Events -from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel -from openpilot.selfdrive.pandad import can_capnp_to_list ButtonType = car.CarState.ButtonEvent.Type GearShifter = car.CarState.GearShifter @@ -53,7 +52,6 @@ class LatControlInputs(NamedTuple): aego: float -SendCan = tuple[int, bytes, int] TorqueFromLateralAccelCallbackType = Callable[[LatControlInputs, car.CarParams.LateralTorqueTuning, float, float, bool, bool], float] @@ -91,7 +89,6 @@ def get_torque_params(): class CarInterfaceBase(ABC): def __init__(self, CP, CarController, CarState): self.CP = CP - self.VM = VehicleModel(CP) self.frame = 0 self.steering_unpressed = 0 @@ -109,9 +106,9 @@ class CarInterfaceBase(ABC): self.can_parsers = [self.cp, self.cp_cam, self.cp_adas, self.cp_body, self.cp_loopback] dbc_name = "" if self.cp is None else self.cp.dbc_name - self.CC: CarControllerBase = CarController(dbc_name, CP, self.VM) + self.CC: CarControllerBase = CarController(dbc_name, CP) - def apply(self, c: car.CarControl, now_nanos: int) -> tuple[car.CarControl.Actuators, list[SendCan]]: + def apply(self, c: car.CarControl, now_nanos: int) -> tuple[car.CarControl.Actuators, list[CanData]]: return self.CC.update(c, self.CS, now_nanos) @staticmethod @@ -158,7 +155,7 @@ class CarInterfaceBase(ABC): raise NotImplementedError @staticmethod - def init(CP, logcan, sendcan): + def init(CP: car.CarParams, can_recv: CanRecvCallable, can_send: CanSendCallable): pass @staticmethod @@ -231,12 +228,11 @@ class CarInterfaceBase(ABC): def _update(self, c: car.CarControl) -> car.CarState: pass - def update(self, c: car.CarControl, can_strings: list[bytes]) -> car.CarState: + def update(self, c: car.CarControl, can_packets: list[tuple[int, list[CanData]]]) -> car.CarState: # parse can - can_list = can_capnp_to_list(can_strings) for cp in self.can_parsers: if cp is not None: - cp.update_strings(can_list) + cp.update_strings(can_packets) # get CarState ret = self._update(c) @@ -466,12 +462,12 @@ class CarStateBase(ABC): class CarControllerBase(ABC): - def __init__(self, dbc_name: str, CP, VM): + def __init__(self, dbc_name: str, CP): self.CP = CP self.frame = 0 @abstractmethod - def update(self, CC: car.CarControl.Actuators, CS: car.CarState, now_nanos: int) -> tuple[car.CarControl.Actuators, list[SendCan]]: + def update(self, CC: car.CarControl.Actuators, CS: car.CarState, now_nanos: int) -> tuple[car.CarControl.Actuators, list[CanData]]: pass diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index c5f74a8156..8ec0a340e3 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -2,19 +2,18 @@ import time from collections import defaultdict from functools import partial -import cereal.messaging as messaging from openpilot.selfdrive.car import carlog +from openpilot.selfdrive.car.can_definitions import CanData, CanRecvCallable, CanSendCallable from openpilot.selfdrive.car.fw_query_definitions import AddrType -from openpilot.selfdrive.pandad import can_list_to_can_capnp from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_addr_for_tx_addr class IsoTpParallelQuery: - def __init__(self, sendcan: messaging.PubSocket, logcan: messaging.SubSocket, bus: int, addrs: list[int] | list[AddrType], + def __init__(self, can_send: CanSendCallable, can_recv: CanRecvCallable, bus: int, addrs: list[int] | list[AddrType], request: list[bytes], response: list[bytes], response_offset: int = 0x8, functional_addrs: list[int] = None, debug: bool = False, response_pending_timeout: float = 10) -> None: - self.sendcan = sendcan - self.logcan = logcan + self.can_send = can_send + self.can_recv = can_recv self.bus = bus self.request = request self.response = response @@ -27,21 +26,21 @@ class IsoTpParallelQuery: assert tx_addr not in FUNCTIONAL_ADDRS, f"Functional address should be defined in functional_addrs: {hex(tx_addr)}" self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0], rx_offset=response_offset) for tx_addr in real_addrs} - self.msg_buffer: dict[int, list[tuple[int, bytes, int]]] = defaultdict(list) + self.msg_buffer: dict[int, list[CanData]] = defaultdict(list) - def rx(self): + def rx(self) -> None: """Drain can socket and sort messages into buffers based on address""" - can_packets = messaging.drain_sock(self.logcan, wait_for_one=True) + can_packets = self.can_recv(wait_for_one=True) for packet in can_packets: - for msg in packet.can: + for msg in packet: if msg.src == self.bus and msg.address in self.msg_addrs.values(): - self.msg_buffer[msg.address].append((msg.address, msg.dat, msg.src)) + self.msg_buffer[msg.address].append(CanData(msg.address, msg.dat, msg.src)) - def _can_tx(self, tx_addr, dat, bus): + def _can_tx(self, tx_addr: int, dat: bytes, bus: int): """Helper function to send single message""" - msg = [tx_addr, dat, bus] - self.sendcan.send(can_list_to_can_capnp([msg], msgtype='sendcan')) + msg = CanData(tx_addr, dat, bus) + self.can_send([msg]) def _can_rx(self, addr, sub_addr=None): """Helper function to retrieve message with specified address and subadress from buffer""" @@ -62,8 +61,8 @@ class IsoTpParallelQuery: self.msg_buffer[addr] = keep_msgs return msgs - def _drain_rx(self): - messaging.drain_sock_raw(self.logcan) + def _drain_rx(self) -> None: + self.can_recv() self.msg_buffer = defaultdict(list) def _create_isotp_msg(self, tx_addr: int, sub_addr: int | None, rx_addr: int): diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index 23441a3a78..910e2303d8 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -9,8 +9,8 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert class CarController(CarControllerBase): - def __init__(self, dbc_name, CP, VM): - super().__init__(dbc_name, CP, VM) + def __init__(self, dbc_name, CP): + super().__init__(dbc_name, CP) self.apply_steer_last = 0 self.packer = CANPacker(dbc_name) self.brake_counter = 0 diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 83b238fb68..83aa09b66b 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -1,7 +1,7 @@ from cereal import car -from openpilot.common.conversions import Conversions as CV from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.car.mazda.values import DBC, LKAS_LIMITS, MazdaFlags diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index 6992d49ffe..fcec6b18b0 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 from cereal import car -from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS from openpilot.selfdrive.car import create_button_events, get_safety_config +from openpilot.selfdrive.car.conversions import Conversions as CV +from openpilot.selfdrive.car.mazda.values import CAR, LKAS_LIMITS from openpilot.selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index a8c808d582..cfd7067db6 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -2,8 +2,8 @@ from dataclasses import dataclass, field from enum import IntFlag from cereal import car -from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car import CarSpecs, DbcDict, PlatformConfig, Platforms, dbc_dict +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.docs_definitions import CarHarness, CarDocs, CarParts from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 473282edfc..35a70ff106 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -9,8 +9,8 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert class CarController(CarControllerBase): - def __init__(self, dbc_name, CP, VM): - super().__init__(dbc_name, CP, VM) + def __init__(self, dbc_name, CP): + super().__init__(dbc_name, CP) self.car_fingerprint = CP.carFingerprint self.lkas_max_torque = 0 diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 57146b49c4..c38173c3e3 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -2,9 +2,9 @@ import copy from collections import deque from cereal import car from opendbc.can.can_define import CANDefine -from openpilot.selfdrive.car.interfaces import CarStateBase -from openpilot.common.conversions import Conversions as CV from opendbc.can.parser import CANParser +from openpilot.selfdrive.car.conversions import Conversions as CV +from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.car.nissan.values import CAR, DBC, CarControllerParams TORQUE_SAMPLES = 12 diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 6ce006df8b..c8c2e7417b 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -1,6 +1,6 @@ -from openpilot.common.numpy_fast import clip, interp from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg +from openpilot.selfdrive.car.helpers import clip, interp from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.subaru import subarucan from openpilot.selfdrive.car.subaru.values import DBC, GLOBAL_ES_ADDR, CanBus, CarControllerParams, SubaruFlags @@ -12,8 +12,8 @@ MAX_STEER_RATE_FRAMES = 7 # tx control frames needed before torque can be cut class CarController(CarControllerBase): - def __init__(self, dbc_name, CP, VM): - super().__init__(dbc_name, CP, VM) + def __init__(self, dbc_name, CP): + super().__init__(dbc_name, CP) self.apply_steer_last = 0 self.cruise_button_prev = 0 diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 821ff2c151..0501494fdd 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -1,9 +1,9 @@ import copy from cereal import car from opendbc.can.can_define import CANDefine -from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser +from openpilot.selfdrive.car.conversions import Conversions as CV +from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.car.subaru.values import DBC, CanBus, SubaruFlags from openpilot.selfdrive.car import CanSignalRateCalculator diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index cb00934376..0ea71e812f 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -106,6 +106,6 @@ class CarInterface(CarInterfaceBase): return ret @staticmethod - def init(CP, logcan, sendcan): + def init(CP, can_recv, can_send): if CP.flags & SubaruFlags.DISABLE_EYESIGHT: - disable_ecu(logcan, sendcan, bus=2, addr=GLOBAL_ES_ADDR, com_cont_req=b'\x28\x03\x01') + disable_ecu(can_recv, can_send, bus=2, addr=GLOBAL_ES_ADDR, com_cont_req=b'\x28\x03\x01') diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 8ee2a2165e..ed81fd0c50 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -1,14 +1,14 @@ -from openpilot.common.numpy_fast import clip from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import apply_std_steer_angle_limits +from openpilot.selfdrive.car.helpers import clip from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.tesla.teslacan import TeslaCAN from openpilot.selfdrive.car.tesla.values import DBC, CANBUS, CarControllerParams class CarController(CarControllerBase): - def __init__(self, dbc_name, CP, VM): - super().__init__(dbc_name, CP, VM) + def __init__(self, dbc_name, CP): + super().__init__(dbc_name, CP) self.apply_angle_last = 0 self.packer = CANPacker(dbc_name) self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 645ea46014..5aa21cbc7c 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -1,11 +1,11 @@ import copy from collections import deque from cereal import car -from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS -from openpilot.selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine +from openpilot.selfdrive.car.conversions import Conversions as CV +from openpilot.selfdrive.car.tesla.values import CAR, DBC, CANBUS, GEAR_MAP, DOORS, BUTTONS +from openpilot.selfdrive.car.interfaces import CarStateBase class CarState(CarStateBase): def __init__(self, CP): diff --git a/selfdrive/car/tesla/teslacan.py b/selfdrive/car/tesla/teslacan.py index f8cd138e77..22e0ac6c4e 100644 --- a/selfdrive/car/tesla/teslacan.py +++ b/selfdrive/car/tesla/teslacan.py @@ -1,6 +1,6 @@ import crcmod -from openpilot.common.conversions import Conversions as CV +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.tesla.values import CANBUS, CarControllerParams diff --git a/selfdrive/car/tests/test_can_fingerprint.py b/selfdrive/car/tests/test_can_fingerprint.py index f236986d8e..e28b2fe3b4 100644 --- a/selfdrive/car/tests/test_can_fingerprint.py +++ b/selfdrive/car/tests/test_can_fingerprint.py @@ -1,6 +1,6 @@ from parameterized import parameterized -from cereal import log, messaging +from openpilot.selfdrive.car.can_definitions import CanData from openpilot.selfdrive.car.car_helpers import FRAME_FINGERPRINT, can_fingerprint from openpilot.selfdrive.car.fingerprints import _FINGERPRINTS as FINGERPRINTS @@ -11,13 +11,11 @@ class TestCanFingerprint: """Tests online fingerprinting function on offline fingerprints""" for fingerprint in fingerprints: # can have multiple fingerprints for each platform - can = messaging.new_message('can', 1) - can.can = [log.CanData(address=address, dat=b'\x00' * length, src=src) - for address, length in fingerprint.items() for src in (0, 1)] + can = [CanData(address=address, dat=b'\x00' * length, src=src) + for address, length in fingerprint.items() for src in (0, 1)] fingerprint_iter = iter([can]) - empty_can = messaging.new_message('can', 0) - car_fingerprint, finger = can_fingerprint(lambda: next(fingerprint_iter, empty_can)) # noqa: B023 + car_fingerprint, finger = can_fingerprint(lambda **kwargs: [next(fingerprint_iter, [])]) # noqa: B023 assert car_fingerprint == car_model assert finger[0] == fingerprint @@ -32,30 +30,27 @@ class TestCanFingerprint: cases = [] # case 1 - one match, make sure we keep going for 100 frames - can = messaging.new_message('can', 1) - can.can = [log.CanData(address=address, dat=b'\x00' * length, src=src) - for address, length in fingerprint.items() for src in (0, 1)] + can = [CanData(address=address, dat=b'\x00' * length, src=src) + for address, length in fingerprint.items() for src in (0, 1)] cases.append((FRAME_FINGERPRINT, car_model, can)) # case 2 - no matches, make sure we keep going for 100 frames - can = messaging.new_message('can', 1) - can.can = [log.CanData(address=1, dat=b'\x00' * 1, src=src) for src in (0, 1)] # uncommon address + can = [CanData(address=1, dat=b'\x00' * 1, src=src) for src in (0, 1)] # uncommon address cases.append((FRAME_FINGERPRINT, None, can)) # case 3 - multiple matches, make sure we keep going for 200 frames to try to eliminate some - can = messaging.new_message('can', 1) - can.can = [log.CanData(address=2016, dat=b'\x00' * 8, src=src) for src in (0, 1)] # common address + can = [CanData(address=2016, dat=b'\x00' * 8, src=src) for src in (0, 1)] # common address cases.append((FRAME_FINGERPRINT * 2, None, can)) for expected_frames, car_model, can in cases: with subtests.test(expected_frames=expected_frames, car_model=car_model): frames = 0 - def test(): + def can_recv(**kwargs): nonlocal frames frames += 1 - return can # noqa: B023 + return [can] # noqa: B023 - car_fingerprint, _ = can_fingerprint(test) + car_fingerprint, _ = can_fingerprint(can_recv) assert car_fingerprint == car_model - assert frames == expected_frames + 2# TODO: fix extra frames + assert frames == expected_frames + 2 # TODO: fix extra frames diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py index f5283acb1f..46a432f8a2 100644 --- a/selfdrive/car/tests/test_fw_fingerprint.py +++ b/selfdrive/car/tests/test_fw_fingerprint.py @@ -5,13 +5,12 @@ from collections import defaultdict from parameterized import parameterized from cereal import car -from openpilot.selfdrive.car import make_can_msg +from openpilot.selfdrive.car.can_definitions import CanData from openpilot.selfdrive.car.car_helpers import interfaces from openpilot.selfdrive.car.fingerprints import FW_VERSIONS from openpilot.selfdrive.car.fw_versions import ESSENTIAL_ECUS, FW_QUERY_CONFIGS, FUZZY_EXCLUDE_ECUS, VERSIONS, build_fw_dict, \ match_fw_to_car, get_brand_ecu_matches, get_fw_versions, get_fw_versions_ordered, get_present_ecus from openpilot.selfdrive.car.vin import get_vin -from openpilot.selfdrive.pandad import can_list_to_can_capnp CarFw = car.CarParams.CarFw Ecu = car.CarParams.Ecu @@ -19,15 +18,6 @@ Ecu = car.CarParams.Ecu ECU_NAME = {v: k for k, v in Ecu.schema.enumerants.items()} -class FakeSocket: - def receive(self, non_blocking=False): - return (can_list_to_can_capnp([make_can_msg(random.randint(0x600, 0x800), b'\x00' * 8, 0)]) - if random.uniform(0, 1) > 0.5 else None) - - def send(self, msg): - pass - - class TestFwFingerprint: def assertFingerprints(self, candidates, expected): candidates = list(candidates) @@ -214,6 +204,15 @@ class TestFwFingerprintTiming: current_obd_multiplexing: bool total_time: float + @staticmethod + def fake_can_send(msgs): + pass + + @staticmethod + def fake_can_recv(wait_for_one: bool = False) -> list[list[CanData]]: + return ([[CanData(random.randint(0x600, 0x800), b'\x00' * 8, 0)]] + if random.uniform(0, 1) > 0.5 else []) + def fake_set_obd_multiplexing(self, obd_multiplexing): """The 10Hz blocking params loop adds on average 50ms to the query time for each OBD multiplexing change""" if obd_multiplexing != self.current_obd_multiplexing: @@ -225,7 +224,6 @@ class TestFwFingerprintTiming: return {} def _benchmark_brand(self, brand, num_pandas, mocker): - fake_socket = FakeSocket() self.total_time = 0 mocker.patch("openpilot.selfdrive.car.isotp_parallel_query.IsoTpParallelQuery.get_data", self.fake_get_data) for _ in range(self.N): @@ -233,7 +231,7 @@ class TestFwFingerprintTiming: self.current_obd_multiplexing = True t = time.perf_counter() - get_fw_versions(fake_socket, fake_socket, self.fake_set_obd_multiplexing, brand, num_pandas=num_pandas) + get_fw_versions(self.fake_can_recv, self.fake_can_send, self.fake_set_obd_multiplexing, brand, num_pandas=num_pandas) self.total_time += time.perf_counter() - t return self.total_time / self.N @@ -251,12 +249,11 @@ class TestFwFingerprintTiming: self.total_time += timeout return set() - fake_socket = FakeSocket() self.total_time = 0.0 mocker.patch("openpilot.selfdrive.car.fw_versions.get_ecu_addrs", fake_get_ecu_addrs) for _ in range(self.N): self.current_obd_multiplexing = True - get_present_ecus(fake_socket, fake_socket, self.fake_set_obd_multiplexing, num_pandas=2) + get_present_ecus(self.fake_can_recv, self.fake_can_send, self.fake_set_obd_multiplexing, num_pandas=2) self._assert_timing(self.total_time / self.N, present_ecu_ref_time) print(f'get_present_ecus, query time={self.total_time / self.N} seconds') @@ -265,7 +262,7 @@ class TestFwFingerprintTiming: self.total_time = 0.0 mocker.patch("openpilot.selfdrive.car.isotp_parallel_query.IsoTpParallelQuery.get_data", self.fake_get_data) for _ in range(self.N): - get_vin(fake_socket, fake_socket, (0, 1), **args) + get_vin(self.fake_can_recv, self.fake_can_send, (0, 1), **args) self._assert_timing(self.total_time / self.N, vin_ref_times[name]) print(f'get_vin {name} case, query time={self.total_time / self.N} seconds') @@ -324,8 +321,7 @@ class TestFwFingerprintTiming: raise mocker.patch("openpilot.selfdrive.car.carlog.exception", fake_carlog_exception) - fake_socket = FakeSocket() - get_fw_versions_ordered(fake_socket, fake_socket, lambda obd: None, '0' * 17, set()) + get_fw_versions_ordered(self.fake_can_recv, self.fake_can_send, lambda obd: None, '0' * 17, set()) for brand in FW_QUERY_CONFIGS.keys(): with subtests.test(brand=brand): - get_fw_versions(fake_socket, fake_socket, lambda obd: None, brand) + get_fw_versions(self.fake_can_recv, self.fake_can_send, lambda obd: None, brand) diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index 22a80f359a..f57def0362 100644 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -217,7 +217,7 @@ class TestCarModelBase(unittest.TestCase): CC = car.CarControl.new_message().as_reader() for i, msg in enumerate(self.can_msgs): - CS = self.CI.update(CC, (msg.as_builder().to_bytes(),)) + CS = self.CI.update(CC, can_capnp_to_list((msg.as_builder().to_bytes(),))) self.CI.apply(CC, msg.logMonoTime) if CS.canValid: @@ -358,7 +358,7 @@ class TestCarModelBase(unittest.TestCase): can = messaging.new_message('can', 1) can.can = [log.CanData(address=address, dat=dat, src=bus)] - CS = self.CI.update(CC, (can.to_bytes(),)) + CS = self.CI.update(CC, can_capnp_to_list((can.to_bytes(),))) if self.safety.get_gas_pressed_prev() != prev_panda_gas: self.assertEqual(CS.gasPressed, self.safety.get_gas_pressed_prev()) @@ -397,7 +397,7 @@ class TestCarModelBase(unittest.TestCase): # warm up pass, as initial states may be different for can in self.can_msgs[:300]: - self.CI.update(CC, (can.as_builder().to_bytes(), )) + self.CI.update(CC, can_capnp_to_list((can.as_builder().to_bytes(), ))) for msg in filter(lambda m: m.src in range(64), can.can): to_send = libpanda_py.make_CANPacket(msg.address, msg.src % 4, msg.dat) self.safety.safety_rx_hook(to_send) @@ -407,7 +407,7 @@ class TestCarModelBase(unittest.TestCase): checks = defaultdict(int) card = Car(CI=self.CI) for idx, can in enumerate(self.can_msgs): - CS = self.CI.update(CC, (can.as_builder().to_bytes(), )) + CS = self.CI.update(CC, can_capnp_to_list((can.as_builder().to_bytes(), ))) for msg in filter(lambda m: m.src in range(64), can.can): to_send = libpanda_py.make_CANPacket(msg.address, msg.src % 4, msg.dat) ret = self.safety.safety_rx_hook(to_send) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index f844b7dc44..8f7a7ed9dd 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,6 +1,7 @@ from cereal import car -from openpilot.common.numpy_fast import clip -from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_can_msg, make_tester_present_msg +from openpilot.selfdrive.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_tester_present_msg +from openpilot.selfdrive.car.can_definitions import CanData +from openpilot.selfdrive.car.helpers import clip from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.toyota import toyotacan from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \ @@ -26,8 +27,8 @@ MAX_LTA_DRIVER_TORQUE_ALLOWANCE = 150 # slightly above steering pressed allows class CarController(CarControllerBase): - def __init__(self, dbc_name, CP, VM): - super().__init__(dbc_name, CP, VM) + def __init__(self, dbc_name, CP): + super().__init__(dbc_name, CP) self.params = CarControllerParams(self.CP) self.last_steer = 0 self.last_angle = 0 @@ -161,7 +162,7 @@ class CarController(CarControllerBase): # *** static msgs *** for addr, cars, bus, fr_step, vl in STATIC_DSU_MSGS: if self.frame % fr_step == 0 and self.CP.enableDsu and self.CP.carFingerprint in cars: - can_sends.append(make_can_msg(addr, vl, bus)) + can_sends.append(CanData(addr, vl, bus)) # keep radar disabled if self.frame % 20 == 0 and self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index e02293da4e..6037585c90 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,12 +1,12 @@ import copy from cereal import car -from openpilot.common.conversions import Conversions as CV -from openpilot.common.numpy_fast import mean -from openpilot.common.filter_simple import FirstOrderFilter from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from openpilot.selfdrive.car import DT_CTRL +from openpilot.selfdrive.car.conversions import Conversions as CV +from openpilot.selfdrive.car.filter_simple import FirstOrderFilter +from openpilot.selfdrive.car.helpers import mean from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \ TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 6487257bbc..5b2a075ce8 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -138,11 +138,11 @@ class CarInterface(CarInterfaceBase): return ret @staticmethod - def init(CP, logcan, sendcan): + def init(CP, can_recv, can_send): # disable radar if alpha longitudinal toggled on radar-ACC car if CP.flags & ToyotaFlags.DISABLE_RADAR.value: communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, uds.CONTROL_TYPE.ENABLE_RX_DISABLE_TX, uds.MESSAGE_TYPE.NORMAL]) - disable_ecu(logcan, sendcan, bus=0, addr=0x750, sub_addr=0xf, com_cont_req=communication_control) + disable_ecu(can_recv, can_send, bus=0, addr=0x750, sub_addr=0xf, com_cont_req=communication_control) # returns a car.CarState def _update(self, c): diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 8022e8b3ab..ca422c73ef 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -4,9 +4,8 @@ from dataclasses import dataclass, field from enum import Enum, IntFlag from cereal import car -from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms -from openpilot.selfdrive.car import AngleRateLimit, dbc_dict +from openpilot.selfdrive.car import CarSpecs, PlatformConfig, Platforms, AngleRateLimit, dbc_dict +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarDocs, Column, CarParts, CarHarness from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py index 1d2033e791..9a667f3ced 100755 --- a/selfdrive/car/vin.py +++ b/selfdrive/car/vin.py @@ -14,7 +14,7 @@ def is_valid_vin(vin: str): return re.fullmatch(VIN_RE, vin) is not None -def get_vin(logcan, sendcan, buses, timeout=0.1, retry=2, debug=False): +def get_vin(can_recv, can_send, buses, timeout=0.1, retry=2, debug=False): for i in range(retry): for bus in buses: for request, response, valid_buses, vin_addrs, functional_addrs, rx_offset in ( @@ -34,7 +34,7 @@ def get_vin(logcan, sendcan, buses, timeout=0.1, retry=2, debug=False): tx_addrs = [a for a in range(0x700, 0x800) if a != 0x7DF] + list(range(0x18DA00F1, 0x18DB00F1, 0x100)) try: - query = IsoTpParallelQuery(sendcan, logcan, bus, tx_addrs, [request, ], [response, ], response_offset=rx_offset, + query = IsoTpParallelQuery(can_send, can_recv, bus, tx_addrs, [request, ], [response, ], response_offset=rx_offset, functional_addrs=functional_addrs, debug=debug) results = query.get_data(timeout) diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 228900b6e9..4f8552b16a 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -1,8 +1,8 @@ from cereal import car from opendbc.can.packer import CANPacker -from openpilot.common.numpy_fast import clip -from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits +from openpilot.selfdrive.car.conversions import Conversions as CV +from openpilot.selfdrive.car.helpers import clip from openpilot.selfdrive.car.interfaces import CarControllerBase from openpilot.selfdrive.car.volkswagen import mqbcan, pqcan from openpilot.selfdrive.car.volkswagen.values import CANBUS, CarControllerParams, VolkswagenFlags @@ -12,8 +12,8 @@ LongCtrlState = car.CarControl.Actuators.LongControlState class CarController(CarControllerBase): - def __init__(self, dbc_name, CP, VM): - super().__init__(dbc_name, CP, VM) + def __init__(self, dbc_name, CP): + super().__init__(dbc_name, CP) self.CCP = CarControllerParams(CP) self.CCS = pqcan if CP.flags & VolkswagenFlags.PQ else mqbcan self.packer_pt = CANPacker(dbc_name) diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index ec6403496f..c53639a7dd 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -1,8 +1,8 @@ import numpy as np from cereal import car -from openpilot.common.conversions import Conversions as CV -from openpilot.selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser +from openpilot.selfdrive.car.interfaces import CarStateBase +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.volkswagen.values import DBC, CANBUS, NetworkLocation, TransmissionType, GearShifter, \ CarControllerParams, VolkswagenFlags diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 69dd63fefd..27816712f6 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -5,8 +5,8 @@ from enum import Enum, IntFlag, StrEnum from cereal import car from panda.python import uds from opendbc.can.can_define import CANDefine -from openpilot.common.conversions import Conversions as CV from openpilot.selfdrive.car import dbc_dict, CarSpecs, DbcDict, PlatformConfig, Platforms +from openpilot.selfdrive.car.conversions import Conversions as CV from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarDocs, CarParts, Column, \ Device from openpilot.selfdrive.car.fw_query_definitions import EcuAddrSubAddr, FwQueryConfig, Request, p16 diff --git a/selfdrive/pandad/SConscript b/selfdrive/pandad/SConscript index dcc1f9811e..58777cafe9 100644 --- a/selfdrive/pandad/SConscript +++ b/selfdrive/pandad/SConscript @@ -3,7 +3,7 @@ Import('env', 'envCython', 'common', 'messaging') libs = ['usb-1.0', common, messaging, 'pthread'] panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc']) -env.Program('pandad', ['main.cc', 'pandad.cc'], LIBS=[panda] + libs) +env.Program('pandad', ['main.cc', 'pandad.cc', 'panda_safety.cc'], LIBS=[panda] + libs) env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) pandad_python = envCython.Program('pandad_api_impl.so', 'pandad_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"]) diff --git a/selfdrive/pandad/panda_safety.cc b/selfdrive/pandad/panda_safety.cc new file mode 100644 index 0000000000..1d0f72f223 --- /dev/null +++ b/selfdrive/pandad/panda_safety.cc @@ -0,0 +1,79 @@ +#include "selfdrive/pandad/pandad.h" +#include "cereal/messaging/messaging.h" +#include "common/swaglog.h" + +void PandaSafety::configureSafetyMode() { + bool is_onroad = params_.getBool("IsOnroad"); + + if (is_onroad && !safety_configured_) { + updateMultiplexingMode(); + + auto car_params = fetchCarParams(); + if (!car_params.empty()) { + LOGW("got %lu bytes CarParams", car_params.size()); + setSafetyMode(car_params); + safety_configured_ = true; + } + } else if (!is_onroad) { + initialized_ = false; + safety_configured_ = false; + } +} + +void PandaSafety::updateMultiplexingMode() { + // Initialize to ELM327 without OBD multiplexing for initial fingerprinting + if (!initialized_) { + prev_obd_multiplexing_ = false; + for (int i = 0; i < pandas_.size(); ++i) { + pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); + } + initialized_ = true; + } + + // Switch between multiplexing modes based on the OBD multiplexing request + bool obd_multiplexing_requested = params_.getBool("ObdMultiplexingEnabled"); + if (obd_multiplexing_requested != prev_obd_multiplexing_) { + for (int i = 0; i < pandas_.size(); ++i) { + const uint16_t safety_param = (i > 0 || !obd_multiplexing_requested) ? 1U : 0U; + pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param); + } + prev_obd_multiplexing_ = obd_multiplexing_requested; + params_.putBool("ObdMultiplexingChanged", true); + } +} + +std::string PandaSafety::fetchCarParams() { + if (!params_.getBool("FirmwareQueryDone")) { + return {}; + } + LOGW("Finished FW query"); + + LOGW("Waiting for params to set safety model"); + if (!params_.getBool("ControlsReady")) { + return {}; + } + return params_.get("CarParams"); +} + +void PandaSafety::setSafetyMode(const std::string ¶ms_string) { + AlignedBuffer aligned_buf; + capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params_string.data(), params_string.size())); + cereal::CarParams::Reader car_params = cmsg.getRoot(); + + auto safety_configs = car_params.getSafetyConfigs(); + uint16_t alternative_experience = car_params.getAlternativeExperience(); + + for (int i = 0; i < pandas_.size(); ++i) { + // Default to SILENT safety model if not specified + cereal::CarParams::SafetyModel safety_model = cereal::CarParams::SafetyModel::SILENT; + uint16_t safety_param = 0U; + if (i < safety_configs.size()) { + safety_model = safety_configs[i].getSafetyModel(); + safety_param = safety_configs[i].getSafetyParam(); + } + + LOGW("Panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience); + pandas_[i]->set_alternative_experience(alternative_experience); + pandas_[i]->set_safety_model(safety_model, safety_param); + } +} diff --git a/selfdrive/pandad/pandad.cc b/selfdrive/pandad/pandad.cc index 3fc89569c1..71bbd20dad 100644 --- a/selfdrive/pandad/pandad.cc +++ b/selfdrive/pandad/pandad.cc @@ -2,18 +2,15 @@ #include #include -#include #include #include #include -#include -#include #include #include +#include #include "cereal/gen/cpp/car.capnp.h" #include "cereal/messaging/messaging.h" -#include "common/params.h" #include "common/ratekeeper.h" #include "common/swaglog.h" #include "common/timing.h" @@ -43,9 +40,6 @@ #define MIN_IR_POWER 0.0f #define CUTOFF_IL 400 #define SATURATE_IL 1000 -using namespace std::chrono_literals; - -std::atomic ignition(false); ExitHandler do_exit; @@ -59,88 +53,6 @@ bool check_all_connected(const std::vector &pandas) { return true; } -bool safety_setter_thread(std::vector pandas) { - LOGD("Starting safety setter thread"); - - Params p; - - // there should be at least one panda connected - if (pandas.size() == 0) { - return false; - } - - // initialize to ELM327 without OBD multiplexing for fingerprinting - bool obd_multiplexing_enabled = false; - for (int i = 0; i < pandas.size(); i++) { - pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U); - } - - // openpilot can switch between multiplexing modes for different FW queries - while (true) { - if (do_exit || !check_all_connected(pandas) || !ignition) { - return false; - } - - bool obd_multiplexing_requested = p.getBool("ObdMultiplexingEnabled"); - if (obd_multiplexing_requested != obd_multiplexing_enabled) { - for (int i = 0; i < pandas.size(); i++) { - const uint16_t safety_param = (i > 0 || !obd_multiplexing_requested) ? 1U : 0U; - pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param); - } - obd_multiplexing_enabled = obd_multiplexing_requested; - p.putBool("ObdMultiplexingChanged", true); - } - - if (p.getBool("FirmwareQueryDone")) { - LOGW("finished FW query"); - break; - } - util::sleep_for(20); - } - - std::string params; - LOGW("waiting for params to set safety model"); - while (true) { - if (do_exit || !check_all_connected(pandas) || !ignition) { - return false; - } - - if (p.getBool("ControlsReady")) { - params = p.get("CarParams"); - if (params.size() > 0) break; - } - util::sleep_for(100); - } - LOGW("got %lu bytes CarParams", params.size()); - - AlignedBuffer aligned_buf; - capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params.data(), params.size())); - cereal::CarParams::Reader car_params = cmsg.getRoot(); - cereal::CarParams::SafetyModel safety_model; - uint16_t safety_param; - - auto safety_configs = car_params.getSafetyConfigs(); - uint16_t alternative_experience = car_params.getAlternativeExperience(); - for (uint32_t i = 0; i < pandas.size(); i++) { - auto panda = pandas[i]; - - if (safety_configs.size() > i) { - safety_model = safety_configs[i].getSafetyModel(); - safety_param = safety_configs[i].getSafetyParam(); - } else { - // If no safety mode is specified, default to silent - safety_model = cereal::CarParams::SafetyModel::SILENT; - safety_param = 0U; - } - - LOGW("panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience); - panda->set_alternative_experience(alternative_experience); - panda->set_safety_model(safety_model, safety_param); - } - - return true; -} - Panda *connect(std::string serial="", uint32_t index=0) { std::unique_ptr panda; try { @@ -197,16 +109,9 @@ void can_send_thread(std::vector pandas, bool fake_send) { } } -void can_recv_thread(std::vector pandas) { - util::set_thread_name("pandad_can_recv"); - - PubMaster pm({"can"}); - - // run at 100Hz - RateKeeper rk("pandad_can_recv", 100); - std::vector raw_can_data; - - while (!do_exit && check_all_connected(pandas)) { +void can_recv(std::vector &pandas, PubMaster *pm) { + static std::vector raw_can_data; + { bool comms_healthy = true; raw_can_data.clear(); for (const auto& panda : pandas) { @@ -217,17 +122,71 @@ void can_recv_thread(std::vector pandas) { auto evt = msg.initEvent(); evt.setValid(comms_healthy); auto canData = evt.initCan(raw_can_data.size()); - for (uint i = 0; isend("can", msg); } } +void fill_panda_state(cereal::PandaState::Builder &ps, cereal::PandaState::PandaType hw_type, const health_t &health) { + ps.setVoltage(health.voltage_pkt); + ps.setCurrent(health.current_pkt); + ps.setUptime(health.uptime_pkt); + ps.setSafetyTxBlocked(health.safety_tx_blocked_pkt); + ps.setSafetyRxInvalid(health.safety_rx_invalid_pkt); + ps.setIgnitionLine(health.ignition_line_pkt); + ps.setIgnitionCan(health.ignition_can_pkt); + ps.setControlsAllowed(health.controls_allowed_pkt); + ps.setTxBufferOverflow(health.tx_buffer_overflow_pkt); + ps.setRxBufferOverflow(health.rx_buffer_overflow_pkt); + ps.setPandaType(hw_type); + ps.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_mode_pkt)); + ps.setSafetyParam(health.safety_param_pkt); + ps.setFaultStatus(cereal::PandaState::FaultStatus(health.fault_status_pkt)); + ps.setPowerSaveEnabled((bool)(health.power_save_enabled_pkt)); + ps.setHeartbeatLost((bool)(health.heartbeat_lost_pkt)); + ps.setAlternativeExperience(health.alternative_experience_pkt); + ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt)); + ps.setInterruptLoad(health.interrupt_load_pkt); + ps.setFanPower(health.fan_power); + ps.setFanStallCount(health.fan_stall_count); + ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid_pkt)); + ps.setSpiChecksumErrorCount(health.spi_checksum_error_count_pkt); + ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f); + ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f); +} + +void fill_panda_can_state(cereal::PandaState::PandaCanState::Builder &cs, const can_health_t &can_health) { + cs.setBusOff((bool)can_health.bus_off); + cs.setBusOffCnt(can_health.bus_off_cnt); + cs.setErrorWarning((bool)can_health.error_warning); + cs.setErrorPassive((bool)can_health.error_passive); + cs.setLastError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_error)); + cs.setLastStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_stored_error)); + cs.setLastDataError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_error)); + cs.setLastDataStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_stored_error)); + cs.setReceiveErrorCnt(can_health.receive_error_cnt); + cs.setTransmitErrorCnt(can_health.transmit_error_cnt); + cs.setTotalErrorCnt(can_health.total_error_cnt); + cs.setTotalTxLostCnt(can_health.total_tx_lost_cnt); + cs.setTotalRxLostCnt(can_health.total_rx_lost_cnt); + cs.setTotalTxCnt(can_health.total_tx_cnt); + cs.setTotalRxCnt(can_health.total_rx_cnt); + cs.setTotalFwdCnt(can_health.total_fwd_cnt); + cs.setCanSpeed(can_health.can_speed); + cs.setCanDataSpeed(can_health.can_data_speed); + cs.setCanfdEnabled(can_health.canfd_enabled); + cs.setBrsEnabled(can_health.brs_enabled); + cs.setCanfdNonIso(can_health.canfd_non_iso); + cs.setIrq0CallRate(can_health.irq0_call_rate); + cs.setIrq1CallRate(can_health.irq1_call_rate); + cs.setIrq2CallRate(can_health.irq2_call_rate); + cs.setCanCoreResetCnt(can_health.can_core_reset_cnt); +} + std::optional send_panda_states(PubMaster *pm, const std::vector &pandas, bool spoofing_started) { bool ignition_local = false; const uint32_t pandas_cnt = pandas.size(); @@ -305,61 +264,11 @@ std::optional send_panda_states(PubMaster *pm, const std::vector } auto ps = pss[i]; - ps.setVoltage(health.voltage_pkt); - ps.setCurrent(health.current_pkt); - ps.setUptime(health.uptime_pkt); - ps.setSafetyTxBlocked(health.safety_tx_blocked_pkt); - ps.setSafetyRxInvalid(health.safety_rx_invalid_pkt); - ps.setIgnitionLine(health.ignition_line_pkt); - ps.setIgnitionCan(health.ignition_can_pkt); - ps.setControlsAllowed(health.controls_allowed_pkt); - ps.setTxBufferOverflow(health.tx_buffer_overflow_pkt); - ps.setRxBufferOverflow(health.rx_buffer_overflow_pkt); - ps.setPandaType(panda->hw_type); - ps.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_mode_pkt)); - ps.setSafetyParam(health.safety_param_pkt); - ps.setFaultStatus(cereal::PandaState::FaultStatus(health.fault_status_pkt)); - ps.setPowerSaveEnabled((bool)(health.power_save_enabled_pkt)); - ps.setHeartbeatLost((bool)(health.heartbeat_lost_pkt)); - ps.setAlternativeExperience(health.alternative_experience_pkt); - ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt)); - ps.setInterruptLoad(health.interrupt_load_pkt); - ps.setFanPower(health.fan_power); - ps.setFanStallCount(health.fan_stall_count); - ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid_pkt)); - ps.setSpiChecksumErrorCount(health.spi_checksum_error_count_pkt); - ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f); - ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f); - - std::array cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()}; + fill_panda_state(ps, panda->hw_type, health); + auto cs = std::array{ps.initCanState0(), ps.initCanState1(), ps.initCanState2()}; for (uint32_t j = 0; j < PANDA_CAN_CNT; j++) { - const auto &can_health = pandaCanStates[i][j]; - cs[j].setBusOff((bool)can_health.bus_off); - cs[j].setBusOffCnt(can_health.bus_off_cnt); - cs[j].setErrorWarning((bool)can_health.error_warning); - cs[j].setErrorPassive((bool)can_health.error_passive); - cs[j].setLastError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_error)); - cs[j].setLastStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_stored_error)); - cs[j].setLastDataError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_error)); - cs[j].setLastDataStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_stored_error)); - cs[j].setReceiveErrorCnt(can_health.receive_error_cnt); - cs[j].setTransmitErrorCnt(can_health.transmit_error_cnt); - cs[j].setTotalErrorCnt(can_health.total_error_cnt); - cs[j].setTotalTxLostCnt(can_health.total_tx_lost_cnt); - cs[j].setTotalRxLostCnt(can_health.total_rx_lost_cnt); - cs[j].setTotalTxCnt(can_health.total_tx_cnt); - cs[j].setTotalRxCnt(can_health.total_rx_cnt); - cs[j].setTotalFwdCnt(can_health.total_fwd_cnt); - cs[j].setCanSpeed(can_health.can_speed); - cs[j].setCanDataSpeed(can_health.can_data_speed); - cs[j].setCanfdEnabled(can_health.canfd_enabled); - cs[j].setBrsEnabled(can_health.brs_enabled); - cs[j].setCanfdNonIso(can_health.canfd_non_iso); - cs[j].setIrq0CallRate(can_health.irq0_call_rate); - cs[j].setIrq1CallRate(can_health.irq1_call_rate); - cs[j].setIrq2CallRate(can_health.irq2_call_rate); - cs[j].setCanCoreResetCnt(can_health.can_core_reset_cnt); + fill_panda_can_state(cs[j], pandaCanStates[i][j]); } // Convert faults bitset to capnp list @@ -380,7 +289,7 @@ std::optional send_panda_states(PubMaster *pm, const std::vector return ignition_local; } -void send_peripheral_state(PubMaster *pm, Panda *panda) { +void send_peripheral_state(Panda *panda, PubMaster *pm) { // build msg MessageBuilder msg; auto evt = msg.initEvent(); @@ -403,46 +312,23 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) { pm->send("peripheralState", msg); } -void panda_state_thread(std::vector pandas, bool spoofing_started) { - util::set_thread_name("pandad_panda_state"); - - Params params; - SubMaster sm({"controlsState"}); - PubMaster pm({"pandaStates", "peripheralState"}); - - Panda *peripheral_panda = pandas[0]; - bool is_onroad = false; - bool is_onroad_last = false; - std::future safety_future; +void process_panda_state(std::vector &pandas, PubMaster *pm, bool spoofing_started) { + static SubMaster sm({"controlsState"}); std::vector connected_serials; for (Panda *p : pandas) { connected_serials.push_back(p->hw_serial()); } - LOGD("start panda state thread"); - - // run at 10hz - RateKeeper rk("panda_state_thread", 10); - - while (!do_exit && check_all_connected(pandas)) { - // send out peripheralState at 2Hz - if (sm.frame % 5 == 0) { - send_peripheral_state(&pm, peripheral_panda); - } - - auto ignition_opt = send_panda_states(&pm, pandas, spoofing_started); - + { + auto ignition_opt = send_panda_states(pm, pandas, spoofing_started); if (!ignition_opt) { LOGE("Failed to get ignition_opt"); - rk.keepTime(); - continue; + return; } - ignition = *ignition_opt; - // check if we should have pandad reconnect - if (!ignition) { + if (!ignition_opt.value()) { bool comms_healthy = true; for (const auto &panda : pandas) { comms_healthy &= panda->comms_healthy(); @@ -462,52 +348,28 @@ void panda_state_thread(std::vector pandas, bool spoofing_started) { } } } - - if (do_exit) { - break; - } } - is_onroad = params.getBool("IsOnroad"); - - // set new safety on onroad transition, after params are cleared - if (is_onroad && !is_onroad_last) { - if (!safety_future.valid() || safety_future.wait_for(0ms) == std::future_status::ready) { - safety_future = std::async(std::launch::async, safety_setter_thread, pandas); - } else { - LOGW("Safety setter thread already running"); - } - } - - is_onroad_last = is_onroad; - sm.update(0); const bool engaged = sm.allAliveAndValid({"controlsState"}) && sm["controlsState"].getControlsState().getEnabled(); - for (const auto &panda : pandas) { panda->send_heartbeat(engaged); } - - rk.keepTime(); } } +void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control) { + static SubMaster sm({"deviceState", "driverCameraState"}); -void peripheral_control_thread(Panda *panda, bool no_fan_control) { - util::set_thread_name("pandad_peripheral_control"); - - SubMaster sm({"deviceState", "driverCameraState"}); - - uint64_t last_driver_camera_t = 0; - uint16_t prev_fan_speed = 999; - uint16_t ir_pwr = 0; - uint16_t prev_ir_pwr = 999; - - FirstOrderFilter integ_lines_filter(0, 30.0, 0.05); + static uint64_t last_driver_camera_t = 0; + static uint16_t prev_fan_speed = 999; + static uint16_t ir_pwr = 0; + static uint16_t prev_ir_pwr = 999; - while (!do_exit && panda->connected()) { - sm.update(1000); + static FirstOrderFilter integ_lines_filter(0, 30.0, 0.05); + { + sm.update(0); if (sm.updated("deviceState") && !no_fan_control) { // Fan speed uint16_t fan_speed = sm["deviceState"].getDeviceState().getFanSpeedPercentDesired(); @@ -545,9 +407,46 @@ void peripheral_control_thread(Panda *panda, bool no_fan_control) { } } -void pandad_main_thread(std::vector serials) { - LOGW("launching pandad"); +void pandad_run(std::vector &pandas) { + const bool no_fan_control = getenv("NO_FAN_CONTROL") != nullptr; + const bool spoofing_started = getenv("STARTED") != nullptr; + const bool fake_send = getenv("FAKESEND") != nullptr; + + // Start the CAN send thread + std::thread send_thread(can_send_thread, pandas, fake_send); + + RateKeeper rk("pandad", 100); + PubMaster pm({"can", "pandaStates", "peripheralState"}); + PandaSafety panda_safety(pandas); + Panda *peripheral_panda = pandas[0]; + + // Main loop: receive CAN data and process states + while (!do_exit && check_all_connected(pandas)) { + can_recv(pandas, &pm); + + // Process peripheral state at 20 Hz + if (rk.frame() % 5 == 0) { + process_peripheral_state(peripheral_panda, &pm, no_fan_control); + } + // Process panda state at 10 Hz + if (rk.frame() % 10 == 0) { + process_panda_state(pandas, &pm, spoofing_started); + panda_safety.configureSafetyMode(); + } + + // Send out peripheralState at 2Hz + if (rk.frame() % 50 == 0) { + send_peripheral_state(peripheral_panda, &pm); + } + + rk.keepTime(); + } + + send_thread.join(); +} + +void pandad_main_thread(std::vector serials) { if (serials.size() == 0) { serials = Panda::list(); @@ -579,16 +478,7 @@ void pandad_main_thread(std::vector serials) { if (!do_exit) { LOGW("connected to all pandas"); - - std::vector threads; - - threads.emplace_back(panda_state_thread, pandas, getenv("STARTED") != nullptr); - threads.emplace_back(peripheral_control_thread, pandas[0], getenv("NO_FAN_CONTROL") != nullptr); - - threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr); - threads.emplace_back(can_recv_thread, pandas); - - for (auto &t : threads) t.join(); + pandad_run(pandas); } for (Panda *panda : pandas) { diff --git a/selfdrive/pandad/pandad.h b/selfdrive/pandad/pandad.h index 9d35949a8f..14d97551f4 100644 --- a/selfdrive/pandad/pandad.h +++ b/selfdrive/pandad/pandad.h @@ -3,7 +3,24 @@ #include #include +#include "common/params.h" #include "selfdrive/pandad/panda.h" -bool safety_setter_thread(std::vector pandas); void pandad_main_thread(std::vector serials); + +class PandaSafety { +public: + PandaSafety(const std::vector &pandas) : pandas_(pandas) {} + void configureSafetyMode(); + +private: + void updateMultiplexingMode(); + std::string fetchCarParams(); + void setSafetyMode(const std::string ¶ms_string); + + bool initialized_ = false; + bool safety_configured_ = false; + bool prev_obd_multiplexing_ = false; + std::vector pandas_; + Params params_; +}; diff --git a/selfdrive/pandad/pandad_api_impl.pyx b/selfdrive/pandad/pandad_api_impl.pyx index 8a12e1c433..787968f53e 100644 --- a/selfdrive/pandad/pandad_api_impl.pyx +++ b/selfdrive/pandad/pandad_api_impl.pyx @@ -50,7 +50,7 @@ def can_capnp_to_list(strings, msgtype='can'): cdef vector[CanData].iterator it = data.begin() while it != data.end(): d = &deref(it) - frames = [[f.address, (&f.dat[0])[:f.dat.size()], f.src] for f in d.frames] - result.append([d.nanos, frames]) + frames = [(f.address, (&f.dat[0])[:f.dat.size()], f.src) for f in d.frames] + result.append((d.nanos, frames)) preinc(it) return result diff --git a/selfdrive/pandad/tests/test_pandad_loopback.py b/selfdrive/pandad/tests/test_pandad_loopback.py index d2b6d047d5..3f0926662c 100644 --- a/selfdrive/pandad/tests/test_pandad_loopback.py +++ b/selfdrive/pandad/tests/test_pandad_loopback.py @@ -12,7 +12,7 @@ from openpilot.common.retry import retry from openpilot.common.params import Params from openpilot.common.timeout import Timeout from openpilot.selfdrive.pandad import can_list_to_can_capnp -from openpilot.selfdrive.car import make_can_msg +from openpilot.selfdrive.car.can_definitions import CanData from openpilot.system.hardware import TICI from openpilot.selfdrive.test.helpers import phone_only, with_processes @@ -60,7 +60,7 @@ def send_random_can_messages(sendcan, count, num_pandas=1): if (addr, dat) in sent_msgs[bus]: continue sent_msgs[bus].add((addr, dat)) - to_send.append(make_can_msg(addr, dat, bus)) + to_send.append(CanData(addr, dat, bus)) sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan')) return sent_msgs diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 609da3967a..055293f16f 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -22,6 +22,7 @@ from openpilot.common.prefix import OpenpilotPrefix from openpilot.common.timeout import Timeout from openpilot.common.realtime import DT_CTRL from panda.python import ALTERNATIVE_EXPERIENCE +from openpilot.selfdrive.car.card import can_comm_callbacks from openpilot.selfdrive.car.car_helpers import get_car, interfaces from openpilot.system.manager.process_config import managed_processes from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state, available_streams @@ -335,7 +336,7 @@ def card_fingerprint_callback(rc, pm, msgs, fingerprint): m = canmsgs.pop(0) rc.send_sync(pm, "can", m.as_builder().to_bytes()) - rc.wait_for_next_recv(False) + rc.wait_for_next_recv(True) def get_car_params_callback(rc, pm, msgs, fingerprint): @@ -356,7 +357,8 @@ def get_car_params_callback(rc, pm, msgs, fingerprint): for m in canmsgs[:300]: can.send(m.as_builder().to_bytes()) - CP = get_car(can, sendcan, lambda obd: None, Params().get_bool("ExperimentalLongitudinalEnabled"), cached_params=cached_params).CP + can_callbacks = can_comm_callbacks(can, sendcan) + CP = get_car(*can_callbacks, lambda obd: None, Params().get_bool("ExperimentalLongitudinalEnabled"), cached_params=cached_params).CP if not params.get_bool("DisengageOnAccelerator"): CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.DISABLE_DISENGAGE_ON_GAS diff --git a/system/sensord/sensors/bmx055_magn.cc b/system/sensord/sensors/bmx055_magn.cc index 3d0d3d2fc6..223f0337e8 100644 --- a/system/sensord/sensors/bmx055_magn.cc +++ b/system/sensord/sensors/bmx055_magn.cc @@ -78,7 +78,7 @@ int BMX055_Magn::init() { // suspend -> sleep int ret = set_register(BMX055_MAGN_I2C_REG_PWR_0, 0x01); if (ret < 0) { - LOGE("Enabling power failed: %d", ret); + LOGW("Enabling power failed: %d", ret); goto fail; } util::sleep_for(5); // wait until the chip is powered on diff --git a/system/sensord/sensors/i2c_sensor.h b/system/sensord/sensors/i2c_sensor.h index ba100c3b01..6082510083 100644 --- a/system/sensord/sensors/i2c_sensor.h +++ b/system/sensord/sensors/i2c_sensor.h @@ -39,7 +39,7 @@ public: uint8_t chip_id = 0; int ret = read_register(address, &chip_id, 1); if (ret < 0) { - LOGE("Reading chip ID failed: %d", ret); + LOGW("Reading chip ID failed: %d", ret); return -1; } for (int i = 0; i < expected_ids.size(); ++i) { diff --git a/uv.lock b/uv.lock index bca0895ebf..b5a734f300 100644 --- a/uv.lock +++ b/uv.lock @@ -1454,6 +1454,7 @@ testing = [ { name = "hypothesis", marker = "python_version == '3.11' or python_version >= '3.12' or (python_version < '3.12' and (python_version < '3.11' or python_version > '3.11'))" }, { name = "import-linter", marker = "python_version == '3.11' or python_version >= '3.12' or (python_version < '3.12' and (python_version < '3.11' or python_version > '3.11'))" }, { name = "mypy", marker = "python_version == '3.11' or python_version >= '3.12' or (python_version < '3.12' and (python_version < '3.11' or python_version > '3.11'))" }, + { name = "pre-commit-hooks", marker = "python_version == '3.11' or python_version >= '3.12' or (python_version < '3.12' and (python_version < '3.11' or python_version > '3.11'))" }, { name = "pytest", marker = "python_version == '3.11' or python_version >= '3.12' or (python_version < '3.12' and (python_version < '3.11' or python_version > '3.11'))" }, { name = "pytest-asyncio", marker = "python_version == '3.11' or python_version >= '3.12' or (python_version < '3.12' and (python_version < '3.11' or python_version > '3.11'))" }, { name = "pytest-cov", marker = "python_version == '3.11' or python_version >= '3.12' or (python_version < '3.12' and (python_version < '3.11' or python_version > '3.11'))" }, @@ -1637,6 +1638,18 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/9b/fb/a70a4214956182e0d7a9099ab17d50bfcba1056188e9b14f35b9e2b62a0d/portalocker-2.10.1-py3-none-any.whl", hash = "sha256:53a5984ebc86a025552264b459b46a2086e269b21823cb572f8f28ee759e45bf", size = 18423 }, ] +[[distribution]] +name = "pre-commit-hooks" +version = "4.6.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "ruamel-yaml", marker = "python_version == '3.11' or python_version >= '3.12' or (python_version < '3.12' and (python_version < '3.11' or python_version > '3.11'))" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/83/0d/b4e97cd99b26c0cd8265c9f19ee7e55248142f0c9955e4d119de96fa4a13/pre_commit_hooks-4.6.0.tar.gz", hash = "sha256:eb1f43ee67869cd41b4c59017fad4a0f9d4d61201d163f2135535aaf65035a2b", size = 29579 } +wheels = [ + { url = "https://files.pythonhosted.org/packages/95/d8/d09b270248bc2441f966dd094a3de8ae813112d2687036f621202bdef80b/pre_commit_hooks-4.6.0-py2.py3-none-any.whl", hash = "sha256:a69199e6a2d45ec59c1020a81ca1549abddc2afb798276d9a0d951752d6abbfe", size = 41193 }, +] + [[distribution]] name = "progressbar" version = "2.5" @@ -4882,6 +4895,42 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/8c/28/92423fe9673b738c180fb5b6b8ea4203fe4b02c1d20b06b7fae79d11cc24/rerun_sdk-0.17.0-cp38-abi3-win_amd64.whl", hash = "sha256:34e5595a326cbdddfebdf00b08e877358c564fce74cc8c6d617fc89ef3a6aa70", size = 29490986 }, ] +[[distribution]] +name = "ruamel-yaml" +version = "0.18.6" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "ruamel-yaml-clib", marker = "python_version < '3.13' and platform_python_implementation == 'CPython'" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/29/81/4dfc17eb6ebb1aac314a3eb863c1325b907863a1b8b1382cdffcb6ac0ed9/ruamel.yaml-0.18.6.tar.gz", hash = "sha256:8b27e6a217e786c6fbe5634d8f3f11bc63e0f80f6a5890f28863d9c45aac311b", size = 143362 } +wheels = [ + { url = "https://files.pythonhosted.org/packages/73/67/8ece580cc363331d9a53055130f86b096bf16e38156e33b1d3014fffda6b/ruamel.yaml-0.18.6-py3-none-any.whl", hash = "sha256:57b53ba33def16c4f3d807c0ccbc00f8a6081827e81ba2491691b76882d0c636", size = 117761 }, +] + +[[distribution]] +name = "ruamel-yaml-clib" +version = "0.2.8" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/46/ab/bab9eb1566cd16f060b54055dd39cf6a34bfa0240c53a7218c43e974295b/ruamel.yaml.clib-0.2.8.tar.gz", hash = "sha256:beb2e0404003de9a4cab9753a8805a8fe9320ee6673136ed7f04255fe60bb512", size = 213824 } +wheels = [ + { url = "https://files.pythonhosted.org/packages/b1/15/971b385c098e8d0d170893f5ba558452bb7b776a0c90658b8f4dd0e3382b/ruamel.yaml.clib-0.2.8-cp311-cp311-macosx_10_9_universal2.whl", hash = "sha256:bef08cd86169d9eafb3ccb0a39edb11d8e25f3dae2b28f5c52fd997521133069", size = 148870 }, + { url = "https://files.pythonhosted.org/packages/01/b0/4ddef56e9f703d7909febc3a421d709a3482cda25826816ec595b73e3847/ruamel.yaml.clib-0.2.8-cp311-cp311-macosx_13_0_arm64.whl", hash = "sha256:b16420e621d26fdfa949a8b4b47ade8810c56002f5389970db4ddda51dbff248", size = 134475 }, + { url = "https://files.pythonhosted.org/packages/a4/f7/22d6b620ed895a05d40802d8281eff924dc6190f682d933d4efff60db3b5/ruamel.yaml.clib-0.2.8-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.manylinux_2_24_x86_64.whl", hash = "sha256:25c515e350e5b739842fc3228d662413ef28f295791af5e5110b543cf0b57d9b", size = 544020 }, + { url = "https://files.pythonhosted.org/packages/7c/e4/0d19d65e340f93df1c47f323d95fa4b256bb28320290f5fddef90837853a/ruamel.yaml.clib-0.2.8-cp311-cp311-manylinux_2_24_aarch64.whl", hash = "sha256:1707814f0d9791df063f8c19bb51b0d1278b8e9a2353abbb676c2f685dee6afe", size = 642643 }, + { url = "https://files.pythonhosted.org/packages/c9/ff/f781eb5e2ae011e586d5426e2086a011cf1e0f59704a6cad1387975c5a62/ruamel.yaml.clib-0.2.8-cp311-cp311-musllinux_1_1_i686.whl", hash = "sha256:46d378daaac94f454b3a0e3d8d78cafd78a026b1d71443f4966c696b48a6d899", size = 695832 }, + { url = "https://files.pythonhosted.org/packages/e3/41/f62e67ac651358b8f0d60cfb12ab2daf99b1b69eeaa188d0cec809d943a6/ruamel.yaml.clib-0.2.8-cp311-cp311-musllinux_1_1_x86_64.whl", hash = "sha256:09b055c05697b38ecacb7ac50bdab2240bfca1a0c4872b0fd309bb07dc9aa3a9", size = 730923 }, + { url = "https://files.pythonhosted.org/packages/9f/f0/19ab8acbf983cd1b37f47d27ceb8b10a738d60d36316a54bad57e0d73fbb/ruamel.yaml.clib-0.2.8-cp311-cp311-win32.whl", hash = "sha256:53a300ed9cea38cf5a2a9b069058137c2ca1ce658a874b79baceb8f892f915a7", size = 99999 }, + { url = "https://files.pythonhosted.org/packages/ec/54/d8a795997921d87224c65d44499ca595a833093fb215b133f920c1062956/ruamel.yaml.clib-0.2.8-cp311-cp311-win_amd64.whl", hash = "sha256:c2a72e9109ea74e511e29032f3b670835f8a59bbdc9ce692c5b4ed91ccf1eedb", size = 118008 }, + { url = "https://files.pythonhosted.org/packages/7a/a2/eb5e9d088cb9d15c24d956944c09dca0a89108ad6e2e913c099ef36e3f0d/ruamel.yaml.clib-0.2.8-cp312-cp312-macosx_10_9_universal2.whl", hash = "sha256:ebc06178e8821efc9692ea7544aa5644217358490145629914d8020042c24aa1", size = 144636 }, + { url = "https://files.pythonhosted.org/packages/66/98/8de4f22bbfd9135deb3422e96d450c4bc0a57d38c25976119307d2efe0aa/ruamel.yaml.clib-0.2.8-cp312-cp312-macosx_13_0_arm64.whl", hash = "sha256:edaef1c1200c4b4cb914583150dcaa3bc30e592e907c01117c08b13a07255ec2", size = 135684 }, + { url = "https://files.pythonhosted.org/packages/30/d3/5fe978cd01a61c12efd24d65fa68c6f28f28c8073a06cf11db3a854390ca/ruamel.yaml.clib-0.2.8-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.manylinux_2_28_x86_64.whl", hash = "sha256:d176b57452ab5b7028ac47e7b3cf644bcfdc8cacfecf7e71759f7f51a59e5c92", size = 734571 }, + { url = "https://files.pythonhosted.org/packages/55/b3/e2531a050758b717c969cbf76c103b75d8a01e11af931b94ba656117fbe9/ruamel.yaml.clib-0.2.8-cp312-cp312-manylinux_2_24_aarch64.whl", hash = "sha256:1dc67314e7e1086c9fdf2680b7b6c2be1c0d8e3a8279f2e993ca2a7545fecf62", size = 643946 }, + { url = "https://files.pythonhosted.org/packages/0d/aa/06db7ca0995b513538402e11280282c615b5ae5f09eb820460d35fb69715/ruamel.yaml.clib-0.2.8-cp312-cp312-musllinux_1_1_i686.whl", hash = "sha256:3213ece08ea033eb159ac52ae052a4899b56ecc124bb80020d9bbceeb50258e9", size = 692169 }, + { url = "https://files.pythonhosted.org/packages/27/38/4cf4d482b84ecdf51efae6635cc5483a83cf5ca9d9c13e205a750e251696/ruamel.yaml.clib-0.2.8-cp312-cp312-musllinux_1_1_x86_64.whl", hash = "sha256:aab7fd643f71d7946f2ee58cc88c9b7bfc97debd71dcc93e03e2d174628e7e2d", size = 740325 }, + { url = "https://files.pythonhosted.org/packages/6f/67/c62c6eea53a4feb042727a3d6c18f50dc99683c2b199c06bd2a9e3db8e22/ruamel.yaml.clib-0.2.8-cp312-cp312-win32.whl", hash = "sha256:5c365d91c88390c8d0a8545df0b5857172824b1c604e867161e6b3d59a827eaa", size = 98639 }, + { url = "https://files.pythonhosted.org/packages/10/d2/52a3d810d0b5b3720725c0504a27b3fced7b6f310fe928f7019d79387bc1/ruamel.yaml.clib-0.2.8-cp312-cp312-win_amd64.whl", hash = "sha256:1758ce7d8e1a29d23de54a16ae867abd370f01b5a69e1a3ba75223eaa3ca1a1b", size = 115305 }, +] + [[distribution]] name = "rubicon-objc" version = "0.4.9"