diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index 46ab32c361..9870637003 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -69,62 +69,62 @@ jobs: rm -rf /tmp/scons_cache/* && \ scons -j$(nproc) --cache-populate" - build_mac: - name: build macos - runs-on: macos-latest - timeout-minutes: 60 - steps: - - uses: actions/checkout@v2 - with: - submodules: true - - name: Determine pre-existing Homebrew packages - if: steps.dependency-cache.outputs.cache-hit != 'true' - run: | - echo 'EXISTING_CELLAR<> $GITHUB_ENV - ls -1 /usr/local/Cellar >> $GITHUB_ENV - echo 'EOF' >> $GITHUB_ENV - - name: Cache dependencies - id: dependency-cache - uses: actions/cache@v2 - with: - path: | - ~/.pyenv - ~/.local/share/virtualenvs/ - /usr/local/Cellar - ~/github_brew_cache_entries.txt - /tmp/scons_cache - key: macos-${{ hashFiles('tools/mac_setup.sh', 'update_requirements.sh', 'Pipfile*') }} - restore-keys: macos- - - name: Brew link restored dependencies - run: | - if [ -f ~/github_brew_cache_entries.txt ]; then - while read pkg; do - brew link --force "$pkg" # `--force` for keg-only packages - done < ~/github_brew_cache_entries.txt - else - echo "Cache entries not found" - fi - - name: Install dependencies - run: ./tools/mac_setup.sh - - name: Build openpilot - run: | - source tools/openpilot_env.sh - pipenv run selfdrive/manager/build.py - - # cleanup scons cache - rm -rf /tmp/scons_cache/ - pipenv run scons -j$(nproc) --cache-populate - - name: Remove pre-existing Homebrew packages for caching - if: steps.dependency-cache.outputs.cache-hit != 'true' - run: | - cd /usr/local/Cellar - new_cellar=$(ls -1) - comm -12 <(echo "$EXISTING_CELLAR") <(echo "$new_cellar") | while read pkg; do - if [[ $pkg != "zstd" ]]; then # caching step needs zstd - rm -rf "$pkg" - fi - done - comm -13 <(echo "$EXISTING_CELLAR") <(echo "$new_cellar") | tee ~/github_brew_cache_entries.txt + #build_mac: + # name: build macos + # runs-on: macos-latest + # timeout-minutes: 60 + # steps: + # - uses: actions/checkout@v2 + # with: + # submodules: true + # - name: Determine pre-existing Homebrew packages + # if: steps.dependency-cache.outputs.cache-hit != 'true' + # run: | + # echo 'EXISTING_CELLAR<> $GITHUB_ENV + # ls -1 /usr/local/Cellar >> $GITHUB_ENV + # echo 'EOF' >> $GITHUB_ENV + # - name: Cache dependencies + # id: dependency-cache + # uses: actions/cache@v2 + # with: + # path: | + # ~/.pyenv + # ~/.local/share/virtualenvs/ + # /usr/local/Cellar + # ~/github_brew_cache_entries.txt + # /tmp/scons_cache + # key: macos-${{ hashFiles('tools/mac_setup.sh', 'update_requirements.sh', 'Pipfile*') }} + # restore-keys: macos- + # - name: Brew link restored dependencies + # run: | + # if [ -f ~/github_brew_cache_entries.txt ]; then + # while read pkg; do + # brew link --force "$pkg" # `--force` for keg-only packages + # done < ~/github_brew_cache_entries.txt + # else + # echo "Cache entries not found" + # fi + # - name: Install dependencies + # run: ./tools/mac_setup.sh + # - name: Build openpilot + # run: | + # source tools/openpilot_env.sh + # pipenv run selfdrive/manager/build.py + # + # # cleanup scons cache + # rm -rf /tmp/scons_cache/ + # pipenv run scons -j$(nproc) --cache-populate + # - name: Remove pre-existing Homebrew packages for caching + # if: steps.dependency-cache.outputs.cache-hit != 'true' + # run: | + # cd /usr/local/Cellar + # new_cellar=$(ls -1) + # comm -12 <(echo "$EXISTING_CELLAR") <(echo "$new_cellar") | while read pkg; do + # if [[ $pkg != "zstd" ]]; then # caching step needs zstd + # rm -rf "$pkg" + # fi + # done + # comm -13 <(echo "$EXISTING_CELLAR") <(echo "$new_cellar") | tee ~/github_brew_cache_entries.txt build_webcam: name: build webcam @@ -264,9 +264,10 @@ jobs: ./selfdrive/loggerd/tests/test_logger &&\ ./selfdrive/proclogd/tests/test_proclog && \ ./selfdrive/ui/replay/tests/test_replay && \ - ./selfdrive/camerad/test/ae_gray_test" - - name: Upload coverage to Codecov - run: bash <(curl -s https://codecov.io/bash) -v -F unit_tests + ./selfdrive/camerad/test/ae_gray_test && \ + coverage xml" + - name: "Upload coverage to Codecov" + uses: codecov/codecov-action@v2 process_replay: name: process replay @@ -299,9 +300,10 @@ jobs: - name: Run replay run: | ${{ env.RUN }} "scons -j$(nproc) && \ - FILEREADER_CACHE=1 CI=1 coverage run selfdrive/test/process_replay/test_processes.py" - - name: Upload coverage to Codecov - run: bash <(curl -s https://codecov.io/bash) -v -F process_replay + FILEREADER_CACHE=1 CI=1 coverage run selfdrive/test/process_replay/test_processes.py && \ + coverage xml" + - name: "Upload coverage to Codecov" + uses: codecov/codecov-action@v2 - name: Print diff if: always() run: cat selfdrive/test/process_replay/diff.txt @@ -354,7 +356,10 @@ jobs: ${{ env.RUN }} "mkdir -p selfdrive/test/out && \ scons -j$(nproc) && \ cd selfdrive/test/longitudinal_maneuvers && \ - ./test_longitudinal.py" + coverage run ./test_longitudinal.py && \ + coverage xml" + - name: "Upload coverage to Codecov" + uses: codecov/codecov-action@v2 - uses: actions/upload-artifact@v2 if: always() continue-on-error: true @@ -397,13 +402,14 @@ jobs: - name: Test car models run: | ${{ env.RUN }} "scons -j$(nproc) --test && \ - FILEREADER_CACHE=1 pytest selfdrive/test/test_models.py && \ + FILEREADER_CACHE=1 coverage run -m pytest selfdrive/test/test_models.py && \ + coverage xml && \ chmod -R 777 /tmp/comma_download_cache" env: NUM_JOBS: 4 JOB_ID: ${{ matrix.job }} - - name: Upload coverage to Codecov - run: bash <(curl -s https://codecov.io/bash) -v -F test_car_models + - name: "Upload coverage to Codecov" + uses: codecov/codecov-action@v2 docs: name: build docs diff --git a/Jenkinsfile b/Jenkinsfile index d12c8e4946..a44f53faf2 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -157,6 +157,7 @@ pipeline { } */ + /* stage('C3: build') { environment { R3_PUSH = "${env.BRANCH_NAME == 'master' ? '1' : ' '}" @@ -171,6 +172,7 @@ pipeline { ]) } } + */ stage('C3: HW + Unit Tests') { steps { diff --git a/RELEASES.md b/RELEASES.md index 8a1680d40c..47e9c8e1bf 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,12 +1,24 @@ -Version 0.8.13 (2022-XX-XX) +Version 0.8.13 (2022-02-16) ======================== * Improved driver monitoring - * Roll compensation + * Retuned driver pose learner for relaxed driving positions + * Added reliance on driving model to be more scene adaptive + * Matched strictness between comma two and comma three + * Improved performance in turns by compensating for the road bank angle * Improved camera focus on the comma two + * AGNOS 4 + * ADB support + * improved cell auto configuration + * NEOS 19 + * package updates + * stability improvements * Subaru ECU firmware fingerprinting thanks to martinl! * Hyundai Santa Fe Plug-in Hybrid 2022 support thanks to sunnyhaibin! + * Mazda CX-5 2022 support thanks to Jafaral! * Subaru Impreza 2020 support thanks to martinl! * Toyota Avalon 2022 support thanks to sshane! + * Toyota Prius v 2017 support thanks to CT921! + * Volkswagen Caravelle 2020 support thanks to jyoung8607! Version 0.8.12 (2021-12-15) ======================== diff --git a/cereal b/cereal index b8ca92c1cd..03860ae0b2 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit b8ca92c1cd9b4448df09671f60c515460010bc78 +Subproject commit 03860ae0b2b8128cae7768e4301d889e627c9275 diff --git a/common/numpy_fast.py b/common/numpy_fast.py index a8361214d1..878c0005c8 100644 --- a/common/numpy_fast.py +++ b/common/numpy_fast.py @@ -1,6 +1,3 @@ -def int_rnd(x): - return int(round(x)) - def clip(x, lo, hi): return max(lo, min(hi, x)) diff --git a/docs/CARS.md b/docs/CARS.md index a6767208d9..5f14a9278d 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -43,7 +43,7 @@ | Lexus | RX Hybrid 2016-19 | All | Stock3| 0mph | 0mph | | Lexus | RX Hybrid 2020-21 | All | openpilot | 0mph | 0mph | | Lexus | UX Hybrid 2019-21 | All | openpilot | 0mph | 0mph | -| Toyota | Alphard 2020 | All | openpilot | 0mph | 0mph | +| Toyota | Alphard 2019-20 | All | openpilot | 0mph | 0mph | | Toyota | Avalon 2016-21 | TSS-P | Stock3| 20mph1 | 0mph | | Toyota | Avalon 2022 | All | openpilot | 0mph | 0mph | | Toyota | Avalon Hybrid 2019-21 | TSS-P | Stock3| 20mph1 | 0mph | @@ -64,6 +64,7 @@ | Toyota | Mirai 2021 | All | openpilot | 0mph | 0mph | | Toyota | Prius 2016-20 | TSS-P | Stock3| 0mph | 0mph | | Toyota | Prius 2021-22 | All | openpilot | 0mph | 0mph | +| Toyota | Prius v 2017 | TSS-P | Stock3| 20mph1 | 0mph | | Toyota | Prius Prime 2017-20 | All | Stock3| 0mph | 0mph | | Toyota | Prius Prime 2021-22 | All | openpilot | 0mph | 0mph | | Toyota | Rav4 2016-18 | TSS-P | Stock3| 20mph1 | 0mph | @@ -86,10 +87,7 @@ | Audi | Q2 2018 | ACC + Lane Assist | Stock | 0mph | 0mph | | Audi | Q3 2020-21 | ACC + Lane Assist | Stock | 0mph | 0mph | | Audi | S3 2015 | ACC + Lane Assist | Stock | 0mph | 0mph | -| Buick | Regal 20181 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Cadillac | ATS 20181 | Adaptive Cruise | openpilot | 0mph | 7mph | | Cadillac | Escalade ESV 20161 | ACC + LKAS | openpilot | 0mph | 7mph | -| Chevrolet | Malibu 20171 | Adaptive Cruise | openpilot | 0mph | 7mph | | Chevrolet | Volt 2017-181 | Adaptive Cruise | openpilot | 0mph | 7mph | | Chrysler | Pacifica 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | | Chrysler | Pacifica 2020 | Adaptive Cruise | Stock | 0mph | 39mph | @@ -100,7 +98,6 @@ | Genesis | G80 2018 | All | Stock | 0mph | 0mph | | Genesis | G90 2018 | All | Stock | 0mph | 0mph | | GMC | Acadia 20181 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Holden | Astra 20171 | Adaptive Cruise | openpilot | 0mph | 7mph | | Hyundai | Elantra 2017-19 | SCC + LKAS | Stock | 19mph | 34mph | | Hyundai | Elantra 2021 | SCC + LKAS | Stock | 0mph | 0mph | | Hyundai | Elantra Hybrid 2021 | SCC + LKAS | Stock | 0mph | 0mph | @@ -134,6 +131,7 @@ | Kia | Sorento 2018-19 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Stinger 2018 | SCC + LKAS | Stock | 0mph | 0mph | | Kia | Telluride 2020 | SCC + LKAS | Stock | 0mph | 0mph | +| Mazda | CX-5 2022 | All | Stock | 0mph | 0mph | | Mazda | CX-9 2021 | All | Stock | 0mph | 28mph | | Nissan | Altima 2019-20 | ProPILOT | Stock | 0mph | 0mph | | Nissan | Leaf 2018-22 | ProPILOT | Stock | 0mph | 0mph | @@ -141,6 +139,10 @@ | Nissan | X-Trail 2017 | ProPILOT | Stock | 0mph | 0mph | | SEAT | Ateca 2018 | Driver Assistance | Stock | 0mph | 0mph | | SEAT | Leon 2014-2020 | Driver Assistance | Stock | 0mph | 0mph | +| Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph | +| Subaru | Crosstrek 2018-20 | EyeSight | Stock | 0mph | 0mph | +| Subaru | Forester 2019-21 | EyeSight | Stock | 0mph | 0mph | +| Subaru | Impreza 2017-19 | EyeSight | Stock | 0mph | 0mph | | Škoda | Kamiq 20212 | Driver Assistance | Stock | 0mph | 0mph | | Škoda | Karoq 2019 | Driver Assistance | Stock | 0mph | 0mph | | Škoda | Kodiaq 2018-19 | Driver Assistance | Stock | 0mph | 0mph | @@ -148,12 +150,9 @@ | Škoda | Octavia RS 2016 | Driver Assistance | Stock | 0mph | 0mph | | Škoda | Scala 2020 | Driver Assistance | Stock | 0mph | 0mph | | Škoda | Superb 2015-18 | Driver Assistance | Stock | 0mph | 0mph | -| Subaru | Ascent 2019 | EyeSight | Stock | 0mph | 0mph | -| Subaru | Crosstrek 2018-20 | EyeSight | Stock | 0mph | 0mph | -| Subaru | Forester 2019-21 | EyeSight | Stock | 0mph | 0mph | -| Subaru | Impreza 2017-19 | EyeSight | Stock | 0mph | 0mph | | Volkswagen| Arteon 2018, 20214 | Driver Assistance | Stock | 0mph | 0mph | | Volkswagen| Atlas 2018-19, 20224 | Driver Assistance | Stock | 0mph | 0mph | +| Volkswagen| Caravelle 20204 | Driver Assistance | Stock | 0mph | 32mph | | Volkswagen| California 20214 | Driver Assistance | Stock | 0mph | 32mph | | Volkswagen| e-Golf 2014, 2019-20 | Driver Assistance | Stock | 0mph | 0mph | | Volkswagen| Golf 2015-20 | Driver Assistance | Stock | 0mph | 0mph | diff --git a/launch_env.sh b/launch_env.sh index cb0a0572d0..cd0c27f643 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -7,11 +7,11 @@ export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 if [ -z "$REQUIRED_NEOS_VERSION" ]; then - export REQUIRED_NEOS_VERSION="18" + export REQUIRED_NEOS_VERSION="19.1" fi if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="3" + export AGNOS_VERSION="4" fi if [ -z "$PASSIVE" ]; then diff --git a/opendbc b/opendbc index 2ffa8dfa4e..c3d3c71aa7 160000 --- a/opendbc +++ b/opendbc @@ -1 +1 @@ -Subproject commit 2ffa8dfa4e2ccea4d81bdf5f0c62d496f64b53b1 +Subproject commit c3d3c71aa7a37364814f029778070fcb550c7cd3 diff --git a/panda b/panda index a96615d253..f56ebf5b77 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit a96615d25309fc29bad4eff782ef1c5aabd9124b +Subproject commit f56ebf5b776b677bf12ec772b0223274dd798999 diff --git a/pyextra/acados_template/acados_ocp_solver_pyx.pyx b/pyextra/acados_template/acados_ocp_solver_pyx.pyx index b950d880b0..9bcf5bb46a 100644 --- a/pyextra/acados_template/acados_ocp_solver_pyx.pyx +++ b/pyextra/acados_template/acados_ocp_solver_pyx.pyx @@ -417,8 +417,9 @@ cdef class AcadosOcpSolverFast: string_value = value_.encode('utf-8') acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, &string_value[0]) - raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\ - '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields))) + else: + raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\ + '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields))) def __del__(self): diff --git a/release/files_common b/release/files_common index 8d682d383a..faf6a41db7 100644 --- a/release/files_common +++ b/release/files_common @@ -88,6 +88,8 @@ selfdrive/boardd/.gitignore selfdrive/boardd/SConscript selfdrive/boardd/__init__.py selfdrive/boardd/boardd.cc +selfdrive/boardd/boardd.h +selfdrive/boardd/main.cc selfdrive/boardd/boardd.py selfdrive/boardd/boardd_api_impl.pyx selfdrive/boardd/can_list_to_can_capnp.cc @@ -198,6 +200,8 @@ selfdrive/common/version.h selfdrive/common/swaglog.h selfdrive/common/swaglog.cc +selfdrive/common/statlog.h +selfdrive/common/statlog.cc selfdrive/common/util.cc selfdrive/common/util.h selfdrive/common/queue.h @@ -228,17 +232,19 @@ selfdrive/controls/radard.py selfdrive/controls/lib/__init__.py selfdrive/controls/lib/alertmanager.py selfdrive/controls/lib/alerts_offroad.json -selfdrive/controls/lib/events.py +selfdrive/controls/lib/desire_helper.py selfdrive/controls/lib/drive_helpers.py -selfdrive/controls/lib/latcontrol_pid.py +selfdrive/controls/lib/events.py +selfdrive/controls/lib/lane_planner.py +selfdrive/controls/lib/latcontrol_angle.py selfdrive/controls/lib/latcontrol_indi.py selfdrive/controls/lib/latcontrol_lqr.py -selfdrive/controls/lib/latcontrol_angle.py -selfdrive/controls/lib/longcontrol.py +selfdrive/controls/lib/latcontrol_pid.py +selfdrive/controls/lib/latcontrol.py selfdrive/controls/lib/lateral_planner.py -selfdrive/controls/lib/lane_planner.py -selfdrive/controls/lib/pid.py +selfdrive/controls/lib/longcontrol.py selfdrive/controls/lib/longitudinal_planner.py +selfdrive/controls/lib/pid.py selfdrive/controls/lib/radar_helpers.py selfdrive/controls/lib/vehicle_model.py @@ -550,7 +556,7 @@ opendbc/can/dbc_out/.gitignore opendbc/chrysler_pacifica_2017_hybrid.dbc opendbc/chrysler_pacifica_2017_hybrid_private_fusion.dbc -opendbc/gm_global_a_powertrain.dbc +opendbc/gm_global_a_powertrain_generated.dbc opendbc/gm_global_a_object.dbc opendbc/gm_global_a_chassis.dbc @@ -572,8 +578,6 @@ opendbc/honda_crv_hybrid_2019_can_generated.dbc opendbc/honda_fit_ex_2018_can_generated.dbc opendbc/honda_odyssey_exl_2018_generated.dbc opendbc/honda_odyssey_extreme_edition_2018_china_can_generated.dbc -opendbc/honda_pilot_touring_2017_can_generated.dbc -opendbc/honda_ridgeline_black_edition_2017_can_generated.dbc opendbc/honda_insight_ex_2019_can_generated.dbc opendbc/acura_ilx_2016_nidec.dbc @@ -590,22 +594,9 @@ opendbc/subaru_outback_2015_generated.dbc opendbc/subaru_outback_2019_generated.dbc opendbc/subaru_forester_2017_generated.dbc -opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc -opendbc/toyota_rav4_2017_pt_generated.dbc -opendbc/toyota_prius_2017_pt_generated.dbc -opendbc/toyota_corolla_2017_pt_generated.dbc -opendbc/lexus_rx_350_2016_pt_generated.dbc -opendbc/lexus_rx_hybrid_2017_pt_generated.dbc +opendbc/toyota_tnga_k_pt_generated.dbc +opendbc/toyota_new_mc_pt_generated.dbc opendbc/toyota_nodsu_pt_generated.dbc -opendbc/toyota_nodsu_hybrid_pt_generated.dbc -opendbc/toyota_highlander_2017_pt_generated.dbc -opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc -opendbc/toyota_avalon_2017_pt_generated.dbc -opendbc/toyota_sienna_xle_2018_pt_generated.dbc -opendbc/lexus_is_2018_pt_generated.dbc -opendbc/lexus_ct200h_2018_pt_generated.dbc -opendbc/lexus_nx300h_2018_pt_generated.dbc -opendbc/lexus_nx300_2018_pt_generated.dbc opendbc/toyota_adas.dbc opendbc/toyota_tss2_adas.dbc diff --git a/scripts/disable-powersave.py b/scripts/disable-powersave.py new file mode 100755 index 0000000000..f651bc87f1 --- /dev/null +++ b/scripts/disable-powersave.py @@ -0,0 +1,5 @@ +#!/usr/bin/env python3 +from selfdrive.hardware import HARDWARE + +if __name__ == "__main__": + HARDWARE.set_power_save(False) diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index a0c6a0c844..cc7cdd87ad 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -4,35 +4,39 @@ import hashlib import io import json import os -import sys import queue import random import select import socket +import subprocess +import sys +import tempfile import threading import time -import tempfile from collections import namedtuple +from datetime import datetime from functools import partial -from typing import Any +from typing import Any, Dict import requests from jsonrpc import JSONRPCResponseManager, dispatcher -from websocket import ABNF, WebSocketTimeoutException, WebSocketException, create_connection +from websocket import (ABNF, WebSocketException, WebSocketTimeoutException, + create_connection) import cereal.messaging as messaging +from cereal import log from cereal.services import service_list from common.api import Api -from common.file_helpers import CallbackReader from common.basedir import PERSIST +from common.file_helpers import CallbackReader from common.params import Params from common.realtime import sec_since_boot -from selfdrive.hardware import HARDWARE, PC +from selfdrive.hardware import HARDWARE, PC, TICI from selfdrive.loggerd.config import ROOT from selfdrive.loggerd.xattr_cache import getxattr, setxattr -from selfdrive.swaglog import cloudlog, SWAGLOG_DIR -from selfdrive.version import get_version, get_origin, get_short_branch, get_commit from selfdrive.statsd import STATS_DIR +from selfdrive.swaglog import SWAGLOG_DIR, cloudlog +from selfdrive.version import get_commit, get_origin, get_short_branch, get_version ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai') HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4")) @@ -44,8 +48,11 @@ RECONNECT_TIMEOUT_S = 70 RETRY_DELAY = 10 # seconds MAX_RETRY_COUNT = 30 # Try for at most 5 minutes if upload fails immediately +MAX_AGE = 31 * 24 * 3600 # seconds WS_FRAME_SIZE = 4096 +NetworkType = log.DeviceState.NetworkType + dispatcher["echo"] = lambda s: s recv_queue: Any = queue.Queue() send_queue: Any = queue.Queue() @@ -53,9 +60,12 @@ upload_queue: Any = queue.Queue() low_priority_send_queue: Any = queue.Queue() log_recv_queue: Any = queue.Queue() cancelled_uploads: Any = set() -UploadItem = namedtuple('UploadItem', ['path', 'url', 'headers', 'created_at', 'id', 'retry_count', 'current', 'progress'], defaults=(0, False, 0)) +UploadItem = namedtuple('UploadItem', ['path', 'url', 'headers', 'created_at', 'id', 'retry_count', 'current', 'progress', 'allow_cellular'], defaults=(0, False, 0, False)) + +cur_upload_items: Dict[int, Any] = {} -cur_upload_items = {} +class AbortTransferException(Exception): + pass class UploadQueueCache(): @@ -128,7 +138,29 @@ def jsonrpc_handler(end_event): send_queue.put_nowait(json.dumps({"error": str(e)})) -def upload_handler(end_event): +def retry_upload(tid: int, end_event: threading.Event, increase_count: bool = True) -> None: + if cur_upload_items[tid].retry_count < MAX_RETRY_COUNT: + item = cur_upload_items[tid] + new_retry_count = item.retry_count + 1 if increase_count else item.retry_count + + item = item._replace( + retry_count=new_retry_count, + progress=0, + current=False + ) + upload_queue.put_nowait(item) + UploadQueueCache.cache(upload_queue) + + cur_upload_items[tid] = None + + for _ in range(RETRY_DELAY): + time.sleep(1) + if end_event.is_set(): + break + + +def upload_handler(end_event: threading.Event) -> None: + sm = messaging.SubMaster(['deviceState']) tid = threading.get_ident() while not end_event.is_set(): @@ -141,31 +173,53 @@ def upload_handler(end_event): cancelled_uploads.remove(cur_upload_items[tid].id) continue + # Remove item if too old + age = datetime.now() - datetime.fromtimestamp(cur_upload_items[tid].created_at / 1000) + if age.total_seconds() > MAX_AGE: + cloudlog.event("athena.upload_handler.expired", item=cur_upload_items[tid], error=True) + continue + + # Check if uploading over cell is allowed + sm.update(0) + cell = sm['deviceState'].networkType not in [NetworkType.wifi, NetworkType.ethernet] + if cell and (not cur_upload_items[tid].allow_cellular): + retry_upload(tid, end_event, False) + continue + try: def cb(sz, cur): + # Abort transfer if connection changed to cell after starting upload + sm.update(0) + cell = sm['deviceState'].networkType not in [NetworkType.wifi, NetworkType.ethernet] + if cell and (not cur_upload_items[tid].allow_cellular): + raise AbortTransferException + cur_upload_items[tid] = cur_upload_items[tid]._replace(progress=cur / sz if sz else 1) - _do_upload(cur_upload_items[tid], cb) + + network_type = sm['deviceState'].networkType.raw + fn = cur_upload_items[tid].path + try: + sz = os.path.getsize(fn) + except OSError: + sz = -1 + + cloudlog.event("athena.upload_handler.upload_start", fn=fn, sz=sz, network_type=network_type) + response = _do_upload(cur_upload_items[tid], cb) + + if response.status_code not in (200, 201, 403, 412): + cloudlog.event("athena.upload_handler.retry", status_code=response.status_code, fn=fn, sz=sz, network_type=network_type) + retry_upload(tid, end_event) + else: + cloudlog.event("athena.upload_handler.success", fn=fn, sz=sz, network_type=network_type) + UploadQueueCache.cache(upload_queue) - except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError) as e: - cloudlog.warning(f"athena.upload_handler.retry {e} {cur_upload_items[tid]}") - - if cur_upload_items[tid].retry_count < MAX_RETRY_COUNT: - item = cur_upload_items[tid] - item = item._replace( - retry_count=item.retry_count + 1, - progress=0, - current=False - ) - upload_queue.put_nowait(item) - UploadQueueCache.cache(upload_queue) - - cur_upload_items[tid] = None - - for _ in range(RETRY_DELAY): - time.sleep(1) - if end_event.is_set(): - break + except (requests.exceptions.Timeout, requests.exceptions.ConnectionError, requests.exceptions.SSLError): + cloudlog.event("athena.upload_handler.timeout", fn=fn, sz=sz, network_type=network_type) + retry_upload(tid, end_event) + except AbortTransferException: + cloudlog.event("athena.upload_handler.abort", fn=fn, sz=sz, network_type=network_type) + retry_upload(tid, end_event, False) except queue.Empty: pass @@ -266,15 +320,20 @@ def reboot(): @dispatcher.add_method def uploadFileToUrl(fn, url, headers): - return uploadFilesToUrls([[fn, url, headers]]) + return uploadFilesToUrls([{ + "fn": fn, + "url": url, + "headers": headers, + }]) @dispatcher.add_method def uploadFilesToUrls(files_data): items = [] failed = [] - for fn, url, headers in files_data: - if len(fn) == 0 or fn[0] == '/' or '..' in fn: + for file in files_data: + fn = file.get('fn', '') + if len(fn) == 0 or fn[0] == '/' or '..' in fn or 'url' not in file: failed.append(fn) continue path = os.path.join(ROOT, fn) @@ -282,7 +341,14 @@ def uploadFilesToUrls(files_data): failed.append(fn) continue - item = UploadItem(path=path, url=url, headers=headers, created_at=int(time.time() * 1000), id=None) + item = UploadItem( + path=path, + url=file['url'], + headers=file.get('headers', {}), + created_at=int(time.time() * 1000), + id=None, + allow_cellular=file.get('allow_cellular', False), + ) upload_id = hashlib.sha1(str(item).encode()).hexdigest() item = item._replace(id=upload_id) upload_queue.put_nowait(item) @@ -322,6 +388,18 @@ def primeActivated(activated): return {"success": 1} +@dispatcher.add_method +def setBandwithLimit(upload_speed_kbps, download_speed_kbps): + if not TICI: + return {"success": 0, "error": "only supported on comma three"} + + try: + HARDWARE.set_bandwidth_limit(upload_speed_kbps, download_speed_kbps) + return {"success": 1} + except subprocess.CalledProcessError as e: + return {"success": 0, "error": "failed to set limit", "stdout": e.stdout, "stderr": e.stderr} + + def startLocalProxy(global_end_event, remote_ws_uri, local_port): try: if local_port not in LOCAL_PORT_WHITELIST: @@ -386,7 +464,7 @@ def getNetworks(): @dispatcher.add_method def takeSnapshot(): - from selfdrive.camerad.snapshot.snapshot import snapshot, jpeg_write + from selfdrive.camerad.snapshot.snapshot import jpeg_write, snapshot ret = snapshot() if ret is not None: def b64jpeg(x): diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py index f19540eb87..10f5d46f77 100755 --- a/selfdrive/athena/registration.py +++ b/selfdrive/athena/registration.py @@ -10,13 +10,18 @@ from common.params import Params from common.spinner import Spinner from common.basedir import PERSIST from selfdrive.controls.lib.alertmanager import set_offroad_alert -from selfdrive.hardware import HARDWARE +from selfdrive.hardware import HARDWARE, PC from selfdrive.swaglog import cloudlog UNREGISTERED_DONGLE_ID = "UnregisteredDevice" +def is_registered_device() -> bool: + dongle = Params().get("DongleId", encoding='utf-8') + return dongle not in (None, UNREGISTERED_DONGLE_ID) + + def register(show_spinner=False) -> str: params = Params() params.put("SubscriberInfo", HARDWARE.get_subscriber_info()) @@ -86,7 +91,7 @@ def register(show_spinner=False) -> str: if dongle_id: params.put("DongleId", dongle_id) - set_offroad_alert("Offroad_UnofficialHardware", dongle_id == UNREGISTERED_DONGLE_ID) + set_offroad_alert("Offroad_UnofficialHardware", (dongle_id == UNREGISTERED_DONGLE_ID) and not PC) return dongle_id diff --git a/selfdrive/athena/tests/test_athenad.py b/selfdrive/athena/tests/test_athenad.py index 06d762c180..1742ed4347 100755 --- a/selfdrive/athena/tests/test_athenad.py +++ b/selfdrive/athena/tests/test_athenad.py @@ -8,6 +8,7 @@ import time import threading import queue import unittest +from datetime import datetime, timedelta from multiprocessing import Process from pathlib import Path @@ -150,7 +151,7 @@ class TestAthenadMethods(unittest.TestCase): def test_upload_handler(self, host): fn = os.path.join(athenad.ROOT, 'qlog.bz2') Path(fn).touch() - item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='') + item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) end_event = threading.Event() thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) @@ -166,11 +167,36 @@ class TestAthenadMethods(unittest.TestCase): finally: end_event.set() + @with_http_server + @mock.patch('requests.put') + def test_upload_handler_retry(self, host, mock_put): + for status, retry in ((500, True), (412, False)): + mock_put.return_value.status_code = status + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) + + end_event = threading.Event() + thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) + thread.start() + + athenad.upload_queue.put_nowait(item) + try: + self.wait_for_upload() + time.sleep(0.1) + + self.assertEqual(athenad.upload_queue.qsize(), 1 if retry else 0) + finally: + end_event.set() + + if retry: + self.assertEqual(athenad.upload_queue.get().retry_count, 1) + def test_upload_handler_timeout(self): """When an upload times out or fails to connect it should be placed back in the queue""" fn = os.path.join(athenad.ROOT, 'qlog.bz2') Path(fn).touch() - item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='') + item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) item_no_retry = item._replace(retry_count=MAX_RETRY_COUNT) end_event = threading.Event() @@ -197,7 +223,7 @@ class TestAthenadMethods(unittest.TestCase): end_event.set() def test_cancelUpload(self): - item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='id') + item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='id', allow_cellular=True) athenad.upload_queue.put_nowait(item) dispatcher["cancelUpload"](item.id) @@ -215,6 +241,28 @@ class TestAthenadMethods(unittest.TestCase): finally: end_event.set() + def test_cancelExpiry(self): + t_future = datetime.now() - timedelta(days=40) + ts = int(t_future.strftime("%s")) * 1000 + + # Item that would time out if actually uploaded + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=ts, id='', allow_cellular=True) + + + end_event = threading.Event() + thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) + thread.start() + try: + athenad.upload_queue.put_nowait(item) + self.wait_for_upload() + time.sleep(0.1) + + self.assertEqual(athenad.upload_queue.qsize(), 0) + finally: + end_event.set() + def test_listUploadQueueEmpty(self): items = dispatcher["listUploadQueue"]() self.assertEqual(len(items), 0) @@ -223,7 +271,7 @@ class TestAthenadMethods(unittest.TestCase): def test_listUploadQueueCurrent(self, host): fn = os.path.join(athenad.ROOT, 'qlog.bz2') Path(fn).touch() - item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='') + item = athenad.UploadItem(path=fn, url=f"{host}/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='', allow_cellular=True) end_event = threading.Event() thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) @@ -241,7 +289,7 @@ class TestAthenadMethods(unittest.TestCase): end_event.set() def test_listUploadQueue(self): - item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='id') + item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='id', allow_cellular=True) athenad.upload_queue.put_nowait(item) items = dispatcher["listUploadQueue"]() diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript index 07ded56e1b..922107509a 100644 --- a/selfdrive/boardd/SConscript +++ b/selfdrive/boardd/SConscript @@ -1,7 +1,7 @@ Import('env', 'envCython', 'common', 'cereal', 'messaging') libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'] -env.Program('boardd', ['boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=libs) +env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=libs) env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"]) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 6ef964270f..c5310f879e 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -1,3 +1,5 @@ +#include "selfdrive/boardd/boardd.h" + #include #include #include @@ -27,7 +29,6 @@ #include "selfdrive/common/util.h" #include "selfdrive/hardware/hw.h" -#include "selfdrive/boardd/panda.h" #include "selfdrive/boardd/pigeon.h" // -- Multi-panda conventions -- @@ -160,7 +161,8 @@ bool safety_setter_thread(std::vector pandas) { int safety_param; auto safety_configs = car_params.getSafetyConfigs(); - for (uint32_t i=0; i i) { @@ -292,7 +294,7 @@ void send_empty_panda_state(PubMaster *pm) { pm->send("pandaStates", msg); } -bool send_panda_states(PubMaster *pm, const std::vector &pandas, bool spoofing_started) { +std::optional send_panda_states(PubMaster *pm, const std::vector &pandas, bool spoofing_started) { bool ignition_local = false; // build msg @@ -302,34 +304,39 @@ bool send_panda_states(PubMaster *pm, const std::vector &pandas, bool s std::vector pandaStates; for (const auto& panda : pandas){ - health_t pandaState = panda->get_state(); + auto health_opt = panda->get_state(); + if (!health_opt) { + return std::nullopt; + } + + health_t health = *health_opt; if (spoofing_started) { - pandaState.ignition_line = 1; + health.ignition_line_pkt = 1; } - ignition_local |= ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0)); + ignition_local |= ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0)); - pandaStates.push_back(pandaState); + pandaStates.push_back(health); } - for (uint32_t i=0; iset_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); } #ifndef __x86_64__ bool power_save_desired = !ignition_local && !pigeon_active; - if (pandaState.power_save_enabled != power_save_desired) { + if (health.power_save_enabled_pkt != power_save_desired) { panda->set_power_saving(power_save_desired); } // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect - if (!ignition_local && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { + if (!ignition_local && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT); } #endif @@ -339,25 +346,27 @@ bool send_panda_states(PubMaster *pm, const std::vector &pandas, bool s } auto ps = pss[i]; - ps.setUptime(pandaState.uptime); - ps.setIgnitionLine(pandaState.ignition_line); - ps.setIgnitionCan(pandaState.ignition_can); - ps.setControlsAllowed(pandaState.controls_allowed); - ps.setGasInterceptorDetected(pandaState.gas_interceptor_detected); - ps.setCanRxErrs(pandaState.can_rx_errs); - ps.setCanSendErrs(pandaState.can_send_errs); - ps.setCanFwdErrs(pandaState.can_fwd_errs); - ps.setGmlanSendErrs(pandaState.gmlan_send_errs); + ps.setUptime(health.uptime_pkt); + ps.setBlockedCnt(health.blocked_msg_cnt_pkt); + ps.setIgnitionLine(health.ignition_line_pkt); + ps.setIgnitionCan(health.ignition_can_pkt); + ps.setControlsAllowed(health.controls_allowed_pkt); + ps.setGasInterceptorDetected(health.gas_interceptor_detected_pkt); + ps.setCanRxErrs(health.can_rx_errs_pkt); + ps.setCanSendErrs(health.can_send_errs_pkt); + ps.setCanFwdErrs(health.can_fwd_errs_pkt); + ps.setGmlanSendErrs(health.gmlan_send_errs_pkt); ps.setPandaType(panda->hw_type); - ps.setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model)); - ps.setSafetyParam(pandaState.safety_param); - ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status)); - ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled)); - ps.setHeartbeatLost((bool)(pandaState.heartbeat_lost)); - ps.setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status)); + 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.setUnsafeMode(health.unsafe_mode_pkt); + ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt)); // Convert faults bitset to capnp list - std::bitset fault_bits(pandaState.faults); + std::bitset fault_bits(health.faults_pkt); auto faults = ps.initFaults(fault_bits.count()); size_t j = 0; @@ -375,7 +384,12 @@ bool send_panda_states(PubMaster *pm, const std::vector &pandas, bool s } void send_peripheral_state(PubMaster *pm, Panda *panda) { - health_t pandaState = panda->get_state(); + auto pandaState_opt = panda->get_state(); + if (!pandaState_opt) { + return; + } + + health_t pandaState = *pandaState_opt; // build msg MessageBuilder msg; @@ -394,12 +408,12 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) { LOGW("reading hwmon took %lfms", read_time); } } else { - ps.setVoltage(pandaState.voltage); - ps.setCurrent(pandaState.current); + ps.setVoltage(pandaState.voltage_pkt); + ps.setCurrent(pandaState.current_pkt); } uint16_t fan_speed_rpm = panda->get_fan_speed(); - ps.setUsbPowerMode(cereal::PeripheralState::UsbPowerMode(pandaState.usb_power_mode)); + ps.setUsbPowerMode(cereal::PeripheralState::UsbPowerMode(pandaState.usb_power_mode_pkt)); ps.setFanSpeedRpm(fan_speed_rpm); pm->send("peripheralState", msg); @@ -423,7 +437,13 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin // send out peripheralState send_peripheral_state(pm, peripheral_panda); - ignition = send_panda_states(pm, pandas, spoofing_started); + auto ignition_opt = send_panda_states(pm, pandas, spoofing_started); + + if (!ignition_opt) { + continue; + } + + ignition = *ignition_opt; // TODO: make this check fast, currently takes 16ms // check if we have new pandas and are offroad @@ -589,22 +609,11 @@ void pigeon_thread(Panda *panda) { } } -int main(int argc, char *argv[]) { - LOGW("starting boardd"); - - if (!Hardware::PC()) { - int err; - err = util::set_realtime_priority(54); - assert(err == 0); - err = util::set_core_affinity({Hardware::TICI() ? 4 : 3}); - assert(err == 0); - } +void boardd_main_thread(std::vector serials) { + if (serials.size() == 0) serials.push_back(""); - LOGW("attempting to connect"); PubMaster pm({"pandaStates", "peripheralState"}); - - std::vector serials(argv + 1, argv + argc); - if (serials.size() == 0) serials.push_back(""); + LOGW("attempting to connect"); // connect to all provided serials std::vector pandas; diff --git a/selfdrive/boardd/boardd.h b/selfdrive/boardd/boardd.h new file mode 100644 index 0000000000..d3c9e1f94a --- /dev/null +++ b/selfdrive/boardd/boardd.h @@ -0,0 +1,6 @@ +#pragma once + +#include "selfdrive/boardd/panda.h" + +bool safety_setter_thread(std::vector pandas); +void boardd_main_thread(std::vector serials); diff --git a/selfdrive/boardd/main.cc b/selfdrive/boardd/main.cc new file mode 100644 index 0000000000..d802e42f86 --- /dev/null +++ b/selfdrive/boardd/main.cc @@ -0,0 +1,22 @@ +#include + +#include "selfdrive/boardd/boardd.h" +#include "selfdrive/common/swaglog.h" +#include "selfdrive/common/util.h" +#include "selfdrive/hardware/hw.h" + +int main(int argc, char *argv[]) { + LOGW("starting boardd"); + + if (!Hardware::PC()) { + int err; + err = util::set_realtime_priority(54); + assert(err == 0); + err = util::set_core_affinity({Hardware::TICI() ? 4 : 3}); + assert(err == 0); + } + + std::vector serials(argv + 1, argv + argc); + boardd_main_thread(serials); + return 0; +} diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 6850ad41b2..5e621b12cd 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -45,11 +45,11 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { libusb_device_descriptor desc; libusb_get_device_descriptor(dev_list[i], &desc); if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) { - libusb_open(dev_list[i], &dev_handle); - if (dev_handle == NULL) { goto fail; } + int ret = libusb_open(dev_list[i], &dev_handle); + if (dev_handle == NULL || ret < 0) { goto fail; } unsigned char desc_serial[26] = { 0 }; - int ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); + ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); if (ret < 0) { goto fail; } usb_serial = std::string((char *)desc_serial, ret).c_str(); @@ -130,12 +130,14 @@ std::vector Panda::list() { libusb_get_device_descriptor(device, &desc); if (desc.idVendor == 0xbbaa && desc.idProduct == 0xddcc) { libusb_device_handle *handle = NULL; - libusb_open(device, &handle); + int ret = libusb_open(device, &handle); + if (ret < 0) { goto finish; } + unsigned char desc_serial[26] = { 0 }; - int ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); + ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial)); libusb_close(handle); - if (ret < 0) { goto finish; } + serials.push_back(std::string((char *)desc_serial, ret).c_str()); } } @@ -309,10 +311,10 @@ void Panda::set_ir_pwr(uint16_t ir_pwr) { usb_write(0xb0, ir_pwr, 0); } -health_t Panda::get_state() { +std::optional Panda::get_state() { health_t health {0}; - usb_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health)); - return health; + int err = usb_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health)); + return err >= 0 ? std::make_optional(health) : std::nullopt; } void Panda::set_loopback(bool loopback) { diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index 1a18a7f15a..dbd866adf4 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -13,6 +13,7 @@ #include "cereal/gen/cpp/car.capnp.h" #include "cereal/gen/cpp/log.capnp.h" +#include "panda/board/health.h" #define TIMEOUT 0 #define PANDA_BUS_CNT 4 @@ -24,29 +25,6 @@ #define CANPACKET_REJECTED (0xC0U) #define CANPACKET_RETURNED (0x80U) -// copied from panda/board/main.c -struct __attribute__((packed)) health_t { - uint32_t uptime; - uint32_t voltage; - uint32_t current; - uint32_t can_rx_errs; - uint32_t can_send_errs; - uint32_t can_fwd_errs; - uint32_t gmlan_send_errs; - uint32_t faults; - uint8_t ignition_line; - uint8_t ignition_can; - uint8_t controls_allowed; - uint8_t gas_interceptor_detected; - uint8_t car_harness_status; - uint8_t usb_power_mode; - uint8_t safety_model; - int16_t safety_param; - uint8_t fault_status; - uint8_t power_save_enabled; - uint8_t heartbeat_lost; -}; - struct __attribute__((packed)) can_header { uint8_t reserved : 1; uint8_t bus : 3; @@ -102,7 +80,7 @@ class Panda { void set_fan_speed(uint16_t fan_speed); uint16_t get_fan_speed(); void set_ir_pwr(uint16_t ir_pwr); - health_t get_state(); + std::optional get_state(); void set_loopback(bool loopback); std::optional> get_firmware_version(); std::optional get_serial(); diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 424ad78ab3..b313f74e7e 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -19,6 +19,7 @@ #include "selfdrive/hardware/hw.h" #ifdef QCOM +#include "CL/cl_ext_qcom.h" #include "selfdrive/camerad/cameras/camera_qcom.h" #elif QCOM2 #include "selfdrive/camerad/cameras/camera_qcom2.h" @@ -28,6 +29,8 @@ #include "selfdrive/camerad/cameras/camera_replay.h" #endif +ExitHandler do_exit; + class Debayer { public: Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s) { @@ -339,8 +342,6 @@ float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip return lum_med / 256.0; } -extern ExitHandler do_exit; - void *processing_thread(MultiCameraState *cameras, CameraState *cs, process_thread_cb callback) { const char *thread_name = nullptr; if (cs == &cameras->road_cam) { @@ -422,3 +423,27 @@ void common_process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) } s->pm->send("driverCameraState", msg); } + + +void camerad_thread() { + cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); + // TODO: do this for QCOM2 too +#if defined(QCOM) + const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0}; + cl_context context = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err)); +#else + cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); +#endif + + MultiCameraState cameras = {}; + VisionIpcServer vipc_server("camerad", device_id, context); + + cameras_init(&vipc_server, &cameras, device_id, context); + cameras_open(&cameras); + + vipc_server.start_listener(); + + cameras_run(&cameras); + + CL_CHECK(clReleaseContext(context)); +} diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h index 75bd79bfdf..f53f06dceb 100644 --- a/selfdrive/camerad/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -41,6 +41,11 @@ const bool env_send_driver = getenv("SEND_DRIVER") != NULL; const bool env_send_road = getenv("SEND_ROAD") != NULL; const bool env_send_wide_road = getenv("SEND_WIDE_ROAD") != NULL; +// for debugging +// note: ONLY_ROAD doesn't work, likely due to a mixup with wideRoad cam in the kernel +const bool env_only_driver = getenv("ONLY_DRIVER") != NULL; +const bool env_debug_frames = getenv("DEBUG_FRAMES") != NULL; + typedef void (*release_cb)(void *cookie, int buf_idx); typedef struct CameraInfo { @@ -129,3 +134,4 @@ void cameras_open(MultiCameraState *s); void cameras_run(MultiCameraState *s); void cameras_close(MultiCameraState *s); void camera_autoexposure(CameraState *s, float grey_frac); +void camerad_thread(); diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index 373e129a83..e86de9ffd6 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -60,7 +60,7 @@ int cam_control(int fd, int op_code, void *handle, int size) { struct cam_control camcontrol = {0}; camcontrol.op_code = op_code; camcontrol.handle = (uint64_t)handle; - if (size == 0) { + if (size == 0) { camcontrol.size = 8; camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE; } else { @@ -353,6 +353,9 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle); pkt->num_cmd_buf = 2; pkt->kmd_cmd_buf_index = 0; + // YUV has kmd_cmd_buf_offset = 1780 + // I guess this is the ISP command + // YUV also has patch_offset = 0x1030 and num_patches = 10 if (io_mem_handle != 0) { pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*2; @@ -377,28 +380,65 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request buf_desc[0].mem_handle = buf0_mem_handle; buf_desc[0].offset = buf0_offset; - // cam_isp_packet_generic_blob_handler - uint32_t tmp[] = { - // size is 0x20, type is 0(CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG) - 0x2000, - 0x1, 0x0, CAM_ISP_IFE_OUT_RES_RDI_0, 0x1, 0x0, 0x1, 0x0, 0x0, // 1 port, CAM_ISP_IFE_OUT_RES_RDI_0 - // size is 0x38, type is 1(CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG), clocks - 0x3801, - 0x1, 0x4, // Dual mode, 4 RDI wires - 0x18148d00, 0x0, 0x18148d00, 0x0, 0x18148d00, 0x0, // rdi clock - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, // junk? - // offset 0x60 - // size is 0xe0, type is 2(CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG), bandwidth - 0xe002, - 0x1, 0x4, // 4 RDI - 0x0, 0x0, 0x1ad27480, 0x0, 0x1ad27480, 0x0, // left_pix_vote - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, // right_pix_vote - 0x0, 0x0, 0x6ee11c0, 0x2, 0x6ee11c0, 0x2, // rdi_vote - 0x0, 0x0, 0x0, 0x0, - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, - 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0}; + // parsed by cam_isp_packet_generic_blob_handler + struct isp_packet { + uint32_t type_0; + cam_isp_resource_hfr_config resource_hfr; + + uint32_t type_1; + cam_isp_clock_config clock; + uint64_t extra_rdi_hz[3]; + + uint32_t type_2; + cam_isp_bw_config bw; + struct cam_isp_bw_vote extra_rdi_vote[6]; + } __attribute__((packed)) tmp; + memset(&tmp, 0, sizeof(tmp)); + + tmp.type_0 = CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG; + tmp.type_0 |= sizeof(cam_isp_resource_hfr_config) << 8; + static_assert(sizeof(cam_isp_resource_hfr_config) == 0x20); + tmp.resource_hfr = { + .num_ports = 1, // 10 for YUV (but I don't think we need them) + .port_hfr_config[0] = { + .resource_type = CAM_ISP_IFE_OUT_RES_RDI_0, // CAM_ISP_IFE_OUT_RES_FULL for YUV + .subsample_pattern = 1, + .subsample_period = 0, + .framedrop_pattern = 1, + .framedrop_period = 0, + }}; + + tmp.type_1 = CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG; + tmp.type_1 |= (sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) << 8; + static_assert((sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) == 0x38); + tmp.clock = { + .usage_type = 1, // dual mode + .num_rdi = 4, + .left_pix_hz = 404000000, + .right_pix_hz = 404000000, + .rdi_hz[0] = 404000000, + }; + + + tmp.type_2 = CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG; + tmp.type_2 |= (sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) << 8; + static_assert((sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) == 0xe0); + tmp.bw = { + .usage_type = 1, // dual mode + .num_rdi = 4, + .left_pix_vote = { + .resource_id = 0, + .cam_bw_bps = 450000000, + .ext_bw_bps = 450000000, + }, + .rdi_vote[0] = { + .resource_id = 0, + .cam_bw_bps = 8706200000, + .ext_bw_bps = 8706200000, + }, + }; + + static_assert(offsetof(struct isp_packet, type_2) == 0x60); buf_desc[1].size = sizeof(tmp); buf_desc[1].offset = io_mem_handle != 0 ? 0x60 : 0; @@ -406,7 +446,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request buf_desc[1].type = CAM_CMD_BUF_GENERIC; buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON; uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20); - memcpy(buf2, tmp, sizeof(tmp)); + memcpy(buf2, &tmp, sizeof(tmp)); if (io_mem_handle != 0) { io_cfg[0].mem_handle[0] = io_mem_handle; @@ -415,19 +455,20 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request .height = FRAME_HEIGHT, .plane_stride = FRAME_STRIDE, .slice_height = FRAME_HEIGHT, - .meta_stride = 0x0, + .meta_stride = 0x0, // YUV has meta(stride=0x400, size=0x5000) .meta_size = 0x0, .meta_offset = 0x0, - .packer_config = 0x0, - .mode_config = 0x0, + .packer_config = 0x0, // 0xb for YUV + .mode_config = 0x0, // 0x9ef for YUV .tile_config = 0x0, .h_init = 0x0, .v_init = 0x0, }; - io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10; - io_cfg[0].color_pattern = 0x5; - io_cfg[0].bpp = 0xc; - io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0; + io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10; // CAM_FORMAT_UBWC_TP10 for YUV + io_cfg[0].color_space = CAM_COLOR_SPACE_BASE; // CAM_COLOR_SPACE_BT601_FULL for YUV + io_cfg[0].color_pattern = 0x5; // 0x0 for YUV + io_cfg[0].bpp = 0xa; + io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0; // CAM_ISP_IFE_OUT_RES_FULL for YUV io_cfg[0].fence = fence; io_cfg[0].direction = CAM_BUF_OUTPUT; io_cfg[0].subsample_pattern = 0x1; @@ -553,18 +594,14 @@ int open_v4l_by_name_and_index(const char name[], int index, int flags = O_RDWR static void camera_open(CameraState *s) { s->sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", s->camera_num); assert(s->sensor_fd >= 0); - LOGD("opened sensor"); - - s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num); - assert(s->csiphy_fd >= 0); - LOGD("opened csiphy"); + LOGD("opened sensor for %d", s->camera_num); // probe the sensor LOGD("-- Probing sensor %d", s->camera_num); sensors_init(s->multi_cam_state->video0_fd, s->sensor_fd, s->camera_num); // create session - struct cam_req_mgr_session_info session_info = {}; + struct cam_req_mgr_session_info session_info = {}; int ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info)); LOGD("get session: %d 0x%X", ret, session_info.session_hdl); s->session_handle = session_info.session_hdl; @@ -605,7 +642,7 @@ static void camera_open(CameraState *s) { .pixel_clk = 0x0, .batch_size = 0x0, - .dsp_mode = 0x0, + .dsp_mode = CAM_ISP_DSP_MODE_NONE, .hbi_cnt = 0x0, .custom_csid = 0x0, @@ -627,9 +664,13 @@ static void camera_open(CameraState *s) { auto isp_dev_handle = device_acquire(s->multi_cam_state->isp_fd, s->session_handle, &isp_resource); assert(isp_dev_handle); - s->isp_dev_handle = *isp_dev_handle; + s->isp_dev_handle = *isp_dev_handle; LOGD("acquire isp dev"); + s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num); + assert(s->csiphy_fd >= 0); + LOGD("opened csiphy for %d", s->camera_num); + struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {.combo_mode = 0}; auto csiphy_dev_handle = device_acquire(s->csiphy_fd, s->session_handle, &csiphy_acquire_dev_info); assert(csiphy_dev_handle); @@ -645,6 +686,7 @@ static void camera_open(CameraState *s) { //sensors_i2c(s, start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON); //sensors_i2c(s, stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF); + // config csiphy LOG("-- Config CSI PHY"); { @@ -686,8 +728,8 @@ static void camera_open(CameraState *s) { req_mgr_link_info.dev_hdls[0] = s->isp_dev_handle; req_mgr_link_info.dev_hdls[1] = s->sensor_dev_handle; ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info)); - LOGD("link: %d", ret); s->link_handle = req_mgr_link_info.link_hdl; + LOGD("link: %d hdl: 0x%X", ret, s->link_handle); struct cam_req_mgr_link_control req_mgr_link_control = {0}; req_mgr_link_control.ops = CAM_REQ_MGR_LINK_ACTIVATE; @@ -708,15 +750,17 @@ static void camera_open(CameraState *s) { } void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) { - camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx, - VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right - printf("road camera initted \n"); - camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx, - VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD); - printf("wide road camera initted \n"); camera_init(s, v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx, VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER); printf("driver camera initted \n"); + if (!env_only_driver) { + camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx, + VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right + printf("road camera initted \n"); + camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx, + VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD); + printf("wide road camera initted \n"); + } s->sm = new SubMaster({"driverState"}); s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"}); @@ -763,12 +807,14 @@ void cameras_open(MultiCameraState *s) { ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub)); printf("req mgr subscribe: %d\n", ret); - camera_open(&s->road_cam); - printf("road camera opened \n"); - camera_open(&s->wide_road_cam); - printf("wide road camera opened \n"); camera_open(&s->driver_cam); printf("driver camera opened \n"); + if (!env_only_driver) { + camera_open(&s->road_cam); + printf("road camera opened \n"); + camera_open(&s->wide_road_cam); + printf("wide road camera opened \n"); + } } static void camera_close(CameraState *s) { @@ -816,9 +862,11 @@ static void camera_close(CameraState *s) { } void cameras_close(MultiCameraState *s) { - camera_close(&s->road_cam); - camera_close(&s->wide_road_cam); camera_close(&s->driver_cam); + if (!env_only_driver) { + camera_close(&s->road_cam); + camera_close(&s->wide_road_cam); + } delete s->sm; delete s->pm; @@ -1010,16 +1058,20 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) { void cameras_run(MultiCameraState *s) { LOG("-- Starting threads"); std::vector threads; - threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera)); threads.push_back(start_process_thread(s, &s->driver_cam, common_process_driver_camera)); - threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera)); + if (!env_only_driver) { + threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera)); + threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera)); + } // start devices LOG("-- Starting devices"); int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload); - sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); - sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); sensors_i2c(&s->driver_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + if (!env_only_driver) { + sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG); + } // poll events LOG("-- Dequeueing Video events"); @@ -1043,8 +1095,10 @@ void cameras_run(MultiCameraState *s) { if (ret == 0) { if (ev.type == V4L_EVENT_CAM_REQ_MGR_EVENT) { struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)ev.u.data; - // LOGD("v4l2 event: sess_hdl %d, link_hdl %d, frame_id %d, req_id %lld, timestamp 0x%llx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status); - // printf("sess_hdl %d, link_hdl %d, frame_id %lu, req_id %lu, timestamp 0x%lx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status); + // LOGD("v4l2 event: sess_hdl 0x%X, link_hdl 0x%X, frame_id %d, req_id %lld, timestamp 0x%llx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status); + if (env_debug_frames) { + printf("sess_hdl 0x%X, link_hdl 0x%X, frame_id %lu, req_id %lu, timestamp 0x%lx, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl, event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp, event_data->u.frame_msg.sof_status); + } if (event_data->session_hdl == s->road_cam.session_handle) { handle_camera_event(&s->road_cam, event_data); diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index f06bd341df..668410d6f7 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -1,48 +1,11 @@ -#include -#include -#include +#include "selfdrive/camerad/cameras/camera_common.h" #include -#include -#include -#include "libyuv.h" - -#include "cereal/visionipc/visionipc_server.h" -#include "selfdrive/common/clutil.h" #include "selfdrive/common/params.h" -#include "selfdrive/common/swaglog.h" #include "selfdrive/common/util.h" #include "selfdrive/hardware/hw.h" -#ifdef QCOM -#include "selfdrive/camerad/cameras/camera_qcom.h" -#elif QCOM2 -#include "selfdrive/camerad/cameras/camera_qcom2.h" -#elif WEBCAM -#include "selfdrive/camerad/cameras/camera_webcam.h" -#else -#include "selfdrive/camerad/cameras/camera_replay.h" -#endif - -ExitHandler do_exit; - -void party(cl_device_id device_id, cl_context context) { - MultiCameraState cameras = {}; - VisionIpcServer vipc_server("camerad", device_id, context); - - cameras_init(&vipc_server, &cameras, device_id, context); - cameras_open(&cameras); - - vipc_server.start_listener(); - - cameras_run(&cameras); -} - -#ifdef QCOM -#include "CL/cl_ext_qcom.h" -#endif - int main(int argc, char *argv[]) { if (!Hardware::PC()) { int ret; @@ -52,17 +15,6 @@ int main(int argc, char *argv[]) { assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores } - cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); - - // TODO: do this for QCOM2 too -#if defined(QCOM) - const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0}; - cl_context context = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err)); -#else - cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); -#endif - - party(device_id, context); - - CL_CHECK(clReleaseContext(context)); + camerad_thread(); + return 0; } diff --git a/selfdrive/camerad/test/ae_gray_test.cc b/selfdrive/camerad/test/ae_gray_test.cc index 0f14a23794..358d93d759 100644 --- a/selfdrive/camerad/test/ae_gray_test.cc +++ b/selfdrive/camerad/test/ae_gray_test.cc @@ -11,9 +11,10 @@ #include "selfdrive/camerad/cameras/camera_common.h" // needed by camera_common.cc -ExitHandler do_exit; - void camera_autoexposure(CameraState *s, float grey_frac) {} +void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {} +void cameras_open(MultiCameraState *s) {} +void cameras_run(MultiCameraState *s) {} int main() { // set up fake camerabuf diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 66cd178f88..30a0bf6a3a 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -75,33 +75,33 @@ class CarState(CarStateBase): @staticmethod def get_can_parser(CP): signals = [ - # sig_name, sig_address, default - ("PRNDL", "GEAR", 0), - ("DOOR_OPEN_FL", "DOORS", 0), - ("DOOR_OPEN_FR", "DOORS", 0), - ("DOOR_OPEN_RL", "DOORS", 0), - ("DOOR_OPEN_RR", "DOORS", 0), - ("BRAKE_PRESSED_2", "BRAKE_2", 0), - ("ACCEL_134", "ACCEL_GAS_134", 0), - ("SPEED_LEFT", "SPEED_1", 0), - ("SPEED_RIGHT", "SPEED_1", 0), - ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0), - ("STEER_ANGLE", "STEERING", 0), - ("STEERING_RATE", "STEERING", 0), - ("TURN_SIGNALS", "STEERING_LEVERS", 0), - ("ACC_STATUS_2", "ACC_2", 0), - ("HIGH_BEAM_FLASH", "STEERING_LEVERS", 0), - ("ACC_SPEED_CONFIG_KPH", "DASHBOARD", 0), - ("CRUISE_STATE", "DASHBOARD", 0), - ("TORQUE_DRIVER", "EPS_STATUS", 0), - ("TORQUE_MOTOR", "EPS_STATUS", 0), - ("LKAS_STATE", "EPS_STATUS", 1), - ("COUNTER", "EPS_STATUS", -1), - ("TRACTION_OFF", "TRACTION_BUTTON", 0), - ("SEATBELT_DRIVER_UNLATCHED", "SEATBELT_STATUS", 0), + # sig_name, sig_address + ("PRNDL", "GEAR"), + ("DOOR_OPEN_FL", "DOORS"), + ("DOOR_OPEN_FR", "DOORS"), + ("DOOR_OPEN_RL", "DOORS"), + ("DOOR_OPEN_RR", "DOORS"), + ("BRAKE_PRESSED_2", "BRAKE_2"), + ("ACCEL_134", "ACCEL_GAS_134"), + ("SPEED_LEFT", "SPEED_1"), + ("SPEED_RIGHT", "SPEED_1"), + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS"), + ("STEER_ANGLE", "STEERING"), + ("STEERING_RATE", "STEERING"), + ("TURN_SIGNALS", "STEERING_LEVERS"), + ("ACC_STATUS_2", "ACC_2"), + ("HIGH_BEAM_FLASH", "STEERING_LEVERS"), + ("ACC_SPEED_CONFIG_KPH", "DASHBOARD"), + ("CRUISE_STATE", "DASHBOARD"), + ("TORQUE_DRIVER", "EPS_STATUS"), + ("TORQUE_MOTOR", "EPS_STATUS"), + ("LKAS_STATE", "EPS_STATUS"), + ("COUNTER", "EPS_STATUS",), + ("TRACTION_OFF", "TRACTION_BUTTON"), + ("SEATBELT_DRIVER_UNLATCHED", "SEATBELT_STATUS"), ] checks = [ @@ -123,20 +123,20 @@ class CarState(CarStateBase): if CP.enableBsm: signals += [ - ("BLIND_SPOT_RIGHT", "BLIND_SPOT_WARNINGS", 0), - ("BLIND_SPOT_LEFT", "BLIND_SPOT_WARNINGS", 0), + ("BLIND_SPOT_RIGHT", "BLIND_SPOT_WARNINGS"), + ("BLIND_SPOT_LEFT", "BLIND_SPOT_WARNINGS"), ] - checks += [("BLIND_SPOT_WARNINGS", 2)] + checks.append(("BLIND_SPOT_WARNINGS", 2)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @staticmethod def get_cam_can_parser(CP): signals = [ - # sig_name, sig_address, default - ("COUNTER", "LKAS_COMMAND", -1), - ("CAR_MODEL", "LKAS_HUD", -1), - ("LKAS_STATUS_OK", "LKAS_HEARTBIT", -1) + # sig_name, sig_address + ("COUNTER", "LKAS_COMMAND"), + ("CAR_MODEL", "LKAS_HUD"), + ("LKAS_STATUS_OK", "LKAS_HEARTBIT") ] checks = [ ("LKAS_COMMAND", 100), diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index 3139efad34..8882dc2d91 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -11,30 +11,23 @@ NUMBER_MSGS = len(RADAR_MSGS_C) + len(RADAR_MSGS_D) def _create_radar_can_parser(car_fingerprint): msg_n = len(RADAR_MSGS_C) - # list of [(signal name, message name or number, initial values), (...)] - # [('RADAR_STATE', 1024, 0), - # ('LONG_DIST', 1072, 255), - # ('LONG_DIST', 1073, 255), - # ('LONG_DIST', 1074, 255), - # ('LONG_DIST', 1075, 255), + # list of [(signal name, message name or number), (...)] + # [('RADAR_STATE', 1024), + # ('LONG_DIST', 1072), + # ('LONG_DIST', 1073), + # ('LONG_DIST', 1074), + # ('LONG_DIST', 1075), - # The factor and offset are applied by the dbc parsing library, so the - # default values should be after the factor/offset are applied. signals = list(zip(['LONG_DIST'] * msg_n + - ['LAT_DIST'] * msg_n + - ['REL_SPEED'] * msg_n, - RADAR_MSGS_C * 2 + # LONG_DIST, LAT_DIST - RADAR_MSGS_D, # REL_SPEED - [0] * msg_n + # LONG_DIST - [-1000] * msg_n + # LAT_DIST - [-146.278] * msg_n)) # REL_SPEED set to 0, factor/offset to this - # TODO what are the checks actually used for? - # honda only checks the last message, - # toyota checks all the messages. Which do we want? + ['LAT_DIST'] * msg_n + + ['REL_SPEED'] * msg_n, + RADAR_MSGS_C * 2 + # LONG_DIST, LAT_DIST + RADAR_MSGS_D)) # REL_SPEED + checks = list(zip(RADAR_MSGS_C + - RADAR_MSGS_D, - [20]*msg_n + # 20Hz (0.05s) - [20]*msg_n)) # 20Hz (0.05s) + RADAR_MSGS_D, + [20] * msg_n + # 20Hz (0.05s) + [20] * msg_n)) # 20Hz (0.05s) return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1) diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index d71e65352f..eba623f5ce 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -39,20 +39,20 @@ class CarState(CarStateBase): @staticmethod def get_can_parser(CP): signals = [ - # sig_name, sig_address, default - ("WhlRr_W_Meas", "WheelSpeed_CG1", 0.), - ("WhlRl_W_Meas", "WheelSpeed_CG1", 0.), - ("WhlFr_W_Meas", "WheelSpeed_CG1", 0.), - ("WhlFl_W_Meas", "WheelSpeed_CG1", 0.), - ("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1", 0.), - ("Cruise_State", "Cruise_Status", 0.), - ("Set_Speed", "Cruise_Status", 0.), - ("LaActAvail_D_Actl", "Lane_Keep_Assist_Status", 0), - ("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status", 0), - ("LaActDeny_B_Actl", "Lane_Keep_Assist_Status", 0), - ("ApedPosScal_Pc_Actl", "EngineData_14", 0.), - ("Dist_Incr", "Steering_Buttons", 0.), - ("Brake_Drv_Appl", "Cruise_Status", 0.), + # sig_name, sig_address + ("WhlRr_W_Meas", "WheelSpeed_CG1"), + ("WhlRl_W_Meas", "WheelSpeed_CG1"), + ("WhlFr_W_Meas", "WheelSpeed_CG1"), + ("WhlFl_W_Meas", "WheelSpeed_CG1"), + ("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1"), + ("Cruise_State", "Cruise_Status"), + ("Set_Speed", "Cruise_Status"), + ("LaActAvail_D_Actl", "Lane_Keep_Assist_Status"), + ("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status"), + ("LaActDeny_B_Actl", "Lane_Keep_Assist_Status"), + ("ApedPosScal_Pc_Actl", "EngineData_14"), + ("Dist_Incr", "Steering_Buttons"), + ("Brake_Drv_Appl", "Cruise_Status"), ] checks = [] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0, enforce_checks=False) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 12cf6367e3..0faaa3f669 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -21,7 +21,7 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]] # TODO: tune this ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF ret.steerActuatorDelay = 0.1 # Default delay, not measured yet - ret.steerLimitTimer = 0.8 + ret.steerLimitTimer = 1.0 ret.steerRateCost = 1.0 ret.centerToFront = ret.wheelbase * 0.44 tire_stiffness_factor = 0.5328 diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index 20a435b082..94eb8bb0cc 100755 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -7,12 +7,12 @@ from selfdrive.car.interfaces import RadarInterfaceBase RADAR_MSGS = list(range(0x500, 0x540)) + def _create_radar_can_parser(car_fingerprint): msg_n = len(RADAR_MSGS) signals = list(zip(['X_Rel'] * msg_n + ['Angle'] * msg_n + ['V_Rel'] * msg_n, - RADAR_MSGS * 3, - [0] * msg_n + [0] * msg_n + [0] * msg_n)) - checks = list(zip(RADAR_MSGS, [20]*msg_n)) + RADAR_MSGS * 3)) + checks = list(zip(RADAR_MSGS, [20] * msg_n)) return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1) diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 56c7c2a3ca..7ad1e7cf88 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -27,7 +27,7 @@ class CarController(): self.packer_obj = CANPacker(DBC[CP.carFingerprint]['radar']) self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis']) - def update(self, enabled, CS, frame, actuators, + def update(self, c, enabled, CS, frame, actuators, hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): P = self.params @@ -41,7 +41,7 @@ class CarController(): if CS.lka_steering_cmd_counter != self.lka_steering_cmd_counter_last: self.lka_steering_cmd_counter_last = CS.lka_steering_cmd_counter elif (frame % P.STEER_STEP) == 0: - lkas_enabled = enabled and not (CS.out.steerWarning or CS.out.steerError) and CS.out.vEgo > P.MIN_STEER_SPEED + lkas_enabled = c.active and not (CS.out.steerWarning or CS.out.steerError) and CS.out.vEgo > P.MIN_STEER_SPEED if lkas_enabled: new_steer = int(round(actuators.steer * P.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P) @@ -58,7 +58,7 @@ class CarController(): # Gas/regen and brakes - all at 25Hz if (frame % 4) == 0: - if not enabled: + if not c.active: # Stock ECU sends max regen when not enabled. self.apply_gas = P.MAX_ACC_REGEN self.apply_brake = 0 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 5180d2e511..4a6b75fa3f 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -3,8 +3,7 @@ from common.numpy_fast import mean from opendbc.can.can_define import CANDefine from opendbc.can.parser import CANParser from selfdrive.car.interfaces import CarStateBase -from selfdrive.car.gm.values import DBC, CAR, AccState, CanBus, \ - CruiseButtons, STEER_THRESHOLD +from selfdrive.car.gm.values import DBC, CAR, AccState, CanBus, STEER_THRESHOLD class CarState(CarStateBase): @@ -31,10 +30,8 @@ class CarState(CarStateBase): ret.standstill = ret.vEgoRaw < 0.01 ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]["PRNDL"], None)) + ret.brakePressed = pt_cp.vl["ECMEngineStatus"]["Brake_Pressed"] != 0 ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]["BrakePedalPosition"] / 0xd0 - # Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed. - if ret.brake < 10/0xd0: - ret.brake = 0. ret.gas = pt_cp.vl["AcceleratorPedal2"]["AcceleratorPedal2"] / 254. ret.gasPressed = ret.gas > 1e-5 @@ -63,14 +60,13 @@ class CarState(CarStateBase): ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]["TurnSignals"] == 2 self.park_brake = pt_cp.vl["EPBStatus"]["EPBClosed"] - ret.cruiseState.available = bool(pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"]) + ret.cruiseState.available = pt_cp.vl["ECMEngineStatus"]["CruiseMainOn"] != 0 ret.espDisabled = pt_cp.vl["ESPStatus"]["TractionControlOn"] != 1 self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]["CruiseState"] - ret.brakePressed = ret.brake > 1e-5 # Regen braking is braking if self.car_fingerprint == CAR.VOLT: - ret.brakePressed = ret.brakePressed or bool(pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"]) + ret.brakePressed = ret.brakePressed or pt_cp.vl["EBCMRegenPaddle"]["RegenPaddle"] != 0 ret.cruiseState.enabled = self.pcm_acc_status != AccState.OFF ret.cruiseState.standstill = self.pcm_acc_status == AccState.STANDSTILL @@ -79,33 +75,33 @@ class CarState(CarStateBase): @staticmethod def get_can_parser(CP): - # this function generates lists for signal, messages and initial values signals = [ - # sig_name, sig_address, default - ("BrakePedalPosition", "EBCMBrakePedalPosition", 0), - ("FrontLeftDoor", "BCMDoorBeltStatus", 0), - ("FrontRightDoor", "BCMDoorBeltStatus", 0), - ("RearLeftDoor", "BCMDoorBeltStatus", 0), - ("RearRightDoor", "BCMDoorBeltStatus", 0), - ("LeftSeatBelt", "BCMDoorBeltStatus", 0), - ("RightSeatBelt", "BCMDoorBeltStatus", 0), - ("TurnSignals", "BCMTurnSignals", 0), - ("AcceleratorPedal2", "AcceleratorPedal2", 0), - ("CruiseState", "AcceleratorPedal2", 0), - ("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS), - ("SteeringWheelAngle", "PSCMSteeringAngle", 0), - ("SteeringWheelRate", "PSCMSteeringAngle", 0), - ("FLWheelSpd", "EBCMWheelSpdFront", 0), - ("FRWheelSpd", "EBCMWheelSpdFront", 0), - ("RLWheelSpd", "EBCMWheelSpdRear", 0), - ("RRWheelSpd", "EBCMWheelSpdRear", 0), - ("PRNDL", "ECMPRDNL", 0), - ("LKADriverAppldTrq", "PSCMStatus", 0), - ("LKATorqueDelivered", "PSCMStatus", 0), - ("LKATorqueDeliveredStatus", "PSCMStatus", 0), - ("TractionControlOn", "ESPStatus", 0), - ("EPBClosed", "EPBStatus", 0), - ("CruiseMainOn", "ECMEngineStatus", 0), + # sig_name, sig_address + ("BrakePedalPosition", "EBCMBrakePedalPosition"), + ("FrontLeftDoor", "BCMDoorBeltStatus"), + ("FrontRightDoor", "BCMDoorBeltStatus"), + ("RearLeftDoor", "BCMDoorBeltStatus"), + ("RearRightDoor", "BCMDoorBeltStatus"), + ("LeftSeatBelt", "BCMDoorBeltStatus"), + ("RightSeatBelt", "BCMDoorBeltStatus"), + ("TurnSignals", "BCMTurnSignals"), + ("AcceleratorPedal2", "AcceleratorPedal2"), + ("CruiseState", "AcceleratorPedal2"), + ("ACCButtons", "ASCMSteeringButton"), + ("SteeringWheelAngle", "PSCMSteeringAngle"), + ("SteeringWheelRate", "PSCMSteeringAngle"), + ("FLWheelSpd", "EBCMWheelSpdFront"), + ("FRWheelSpd", "EBCMWheelSpdFront"), + ("RLWheelSpd", "EBCMWheelSpdRear"), + ("RRWheelSpd", "EBCMWheelSpdRear"), + ("PRNDL", "ECMPRDNL"), + ("LKADriverAppldTrq", "PSCMStatus"), + ("LKATorqueDelivered", "PSCMStatus"), + ("LKATorqueDeliveredStatus", "PSCMStatus"), + ("TractionControlOn", "ESPStatus"), + ("EPBClosed", "EPBStatus"), + ("CruiseMainOn", "ECMEngineStatus"), + ("Brake_Pressed", "ECMEngineStatus"), ] checks = [ @@ -125,19 +121,15 @@ class CarState(CarStateBase): ] if CP.carFingerprint == CAR.VOLT: - signals += [ - ("RegenPaddle", "EBCMRegenPaddle", 0), - ] - checks += [ - ("EBCMRegenPaddle", 50), - ] + signals.append(("RegenPaddle", "EBCMRegenPaddle")) + checks.append(("EBCMRegenPaddle", 50)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CanBus.POWERTRAIN) @staticmethod def get_loopback_can_parser(CP): signals = [ - ("RollingCounter", "ASCMLKASteeringCmd", 0), + ("RollingCounter", "ASCMLKASteeringCmd"), ] checks = [ diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 45fb55098e..d6a2d3cfee 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -47,7 +47,7 @@ class CarInterface(CarInterfaceBase): # These cars have been put into dashcam only due to both a lack of users and test coverage. # These cars likely still work fine. Once a user confirms each car works and a test route is # added to selfdrive/test/test_routes, we can remove it from this list. - ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU} + ret.dashcamOnly = candidate in {CAR.CADILLAC_ATS, CAR.HOLDEN_ASTRA, CAR.MALIBU, CAR.BUICK_REGAL} # Presence of a camera on the object bus is ok. # Have to go to read_only if ASCM is online (ACC-enabled cars), @@ -226,7 +226,7 @@ class CarInterface(CarInterfaceBase): # In GM, PCM faults out if ACC command overlaps user gas. enabled = c.enabled and not self.CS.out.gasPressed - ret = self.CC.update(enabled, self.CS, self.frame, + ret = self.CC.update(c, enabled, self.CS, self.frame, c.actuators, hud_v_cruise, hud_control.lanesVisible, hud_control.leadVisible, hud_control.visualAlert) diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index d1ad1c1635..66fac54748 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -14,6 +14,7 @@ NUM_SLOTS = 20 # messages that are present in DBC LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS + def create_radar_can_parser(car_fingerprint): if car_fingerprint not in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS, CAR.ESCALADE_ESV): return None @@ -21,17 +22,13 @@ def create_radar_can_parser(car_fingerprint): # C1A-ARS3-A by Continental radar_targets = list(range(SLOT_1_MSG, SLOT_1_MSG + NUM_SLOTS)) signals = list(zip(['FLRRNumValidTargets', - 'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt', - 'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt', - 'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] + - ['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS + - ['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS + - ['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS, - [RADAR_HEADER_MSG] * 7 + radar_targets * 6, - [0] * 7 + - [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + - [0.0] * NUM_SLOTS + [0.0] * NUM_SLOTS + - [0.0] * NUM_SLOTS + [0] * NUM_SLOTS)) + 'FLRRSnsrBlckd', 'FLRRYawRtPlsblityFlt', + 'FLRRHWFltPrsntInt', 'FLRRAntTngFltPrsnt', + 'FLRRAlgnFltPrsnt', 'FLRRSnstvFltPrsntInt'] + + ['TrkRange'] * NUM_SLOTS + ['TrkRangeRate'] * NUM_SLOTS + + ['TrkRangeAccel'] * NUM_SLOTS + ['TrkAzimuth'] * NUM_SLOTS + + ['TrkWidth'] * NUM_SLOTS + ['TrkObjectID'] * NUM_SLOTS, + [RADAR_HEADER_MSG] * 7 + radar_targets * 6)) checks = list({(s[1], 14) for s in signals}) diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index dd00193a1a..8e09076713 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -113,11 +113,11 @@ FINGERPRINTS = { } DBC = { - CAR.HOLDEN_ASTRA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), - CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), - CAR.MALIBU: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), - CAR.ACADIA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), - CAR.CADILLAC_ATS: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), - CAR.BUICK_REGAL: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), - CAR.ESCALADE_ESV: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.HOLDEN_ASTRA: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.VOLT: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.MALIBU: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.ACADIA: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.CADILLAC_ATS: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.BUICK_REGAL: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), + CAR.ESCALADE_ESV: dbc_dict('gm_global_a_powertrain_generated', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'), } diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 33d01f61b6..49581799a7 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -117,7 +117,7 @@ class CarController(): P = self.params - if enabled: + if active: accel = actuators.accel gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint) else: @@ -152,7 +152,7 @@ class CarController(): # steer torque is converted back to CAN reference (positive when steering right) apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V)) - lkas_active = enabled and not CS.steer_not_allowed + lkas_active = active and not CS.steer_not_allowed # Send CAN commands. can_sends = [] diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 029413ad72..0a56a02b94 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -11,34 +11,33 @@ TransmissionType = car.CarParams.TransmissionType def get_can_signals(CP, gearbox_msg, main_on_sig_msg): - # this function generates lists for signal, messages and initial values signals = [ - ("XMISSION_SPEED", "ENGINE_DATA", 0), - ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), - ("STEER_ANGLE", "STEERING_SENSORS", 0), - ("STEER_ANGLE_RATE", "STEERING_SENSORS", 0), - ("MOTOR_TORQUE", "STEER_MOTOR_TORQUE", 0), - ("STEER_TORQUE_SENSOR", "STEER_STATUS", 0), - ("LEFT_BLINKER", "SCM_FEEDBACK", 0), - ("RIGHT_BLINKER", "SCM_FEEDBACK", 0), - ("GEAR", gearbox_msg, 0), - ("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS", 1), - ("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS", 0), - ("BRAKE_PRESSED", "POWERTRAIN_DATA", 0), - ("BRAKE_SWITCH", "POWERTRAIN_DATA", 0), - ("CRUISE_BUTTONS", "SCM_BUTTONS", 0), - ("ESP_DISABLED", "VSA_STATUS", 1), - ("USER_BRAKE", "VSA_STATUS", 0), - ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0), - ("STEER_STATUS", "STEER_STATUS", 5), - ("GEAR_SHIFTER", gearbox_msg, 0), - ("PEDAL_GAS", "POWERTRAIN_DATA", 0), - ("CRUISE_SETTING", "SCM_BUTTONS", 0), - ("ACC_STATUS", "POWERTRAIN_DATA", 0), - ("MAIN_ON", main_on_sig_msg, 0), + ("XMISSION_SPEED", "ENGINE_DATA"), + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS"), + ("STEER_ANGLE", "STEERING_SENSORS"), + ("STEER_ANGLE_RATE", "STEERING_SENSORS"), + ("MOTOR_TORQUE", "STEER_MOTOR_TORQUE"), + ("STEER_TORQUE_SENSOR", "STEER_STATUS"), + ("LEFT_BLINKER", "SCM_FEEDBACK"), + ("RIGHT_BLINKER", "SCM_FEEDBACK"), + ("GEAR", gearbox_msg), + ("SEATBELT_DRIVER_LAMP", "SEATBELT_STATUS"), + ("SEATBELT_DRIVER_LATCHED", "SEATBELT_STATUS"), + ("BRAKE_PRESSED", "POWERTRAIN_DATA"), + ("BRAKE_SWITCH", "POWERTRAIN_DATA"), + ("CRUISE_BUTTONS", "SCM_BUTTONS"), + ("ESP_DISABLED", "VSA_STATUS"), + ("USER_BRAKE", "VSA_STATUS"), + ("BRAKE_HOLD_ACTIVE", "VSA_STATUS"), + ("STEER_STATUS", "STEER_STATUS"), + ("GEAR_SHIFTER", gearbox_msg), + ("PEDAL_GAS", "POWERTRAIN_DATA"), + ("CRUISE_SETTING", "SCM_BUTTONS"), + ("ACC_STATUS", "POWERTRAIN_DATA"), + ("MAIN_ON", main_on_sig_msg), ] checks = [ @@ -65,22 +64,18 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): ] if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL, CAR.ACURA_RDX_3G, CAR.HONDA_E): - checks += [ - (gearbox_msg, 50), - ] + checks.append((gearbox_msg, 50)) else: - checks += [ - (gearbox_msg, 100), - ] + checks.append((gearbox_msg, 100)) if CP.carFingerprint in HONDA_BOSCH_ALT_BRAKE_SIGNAL: - signals += [("BRAKE_PRESSED", "BRAKE_MODULE", 0)] - checks += [("BRAKE_MODULE", 50)] + signals.append(("BRAKE_PRESSED", "BRAKE_MODULE")) + checks.append(("BRAKE_MODULE", 50)) if CP.carFingerprint in HONDA_BOSCH: signals += [ - ("EPB_STATE", "EPB_STATUS", 0), - ("IMPERIAL_UNIT", "CAR_SPEED", 1), + ("EPB_STATE", "EPB_STATUS"), + ("IMPERIAL_UNIT", "CAR_SPEED"), ] checks += [ ("EPB_STATUS", 50), @@ -89,65 +84,65 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg): if not CP.openpilotLongitudinalControl: signals += [ - ("CRUISE_CONTROL_LABEL", "ACC_HUD", 0), - ("CRUISE_SPEED", "ACC_HUD", 0), - ("ACCEL_COMMAND", "ACC_CONTROL", 0), - ("AEB_STATUS", "ACC_CONTROL", 0), + ("CRUISE_CONTROL_LABEL", "ACC_HUD"), + ("CRUISE_SPEED", "ACC_HUD"), + ("ACCEL_COMMAND", "ACC_CONTROL"), + ("AEB_STATUS", "ACC_CONTROL"), ] checks += [ ("ACC_HUD", 10), ("ACC_CONTROL", 50), ] else: # Nidec signals - signals += [("CRUISE_SPEED_PCM", "CRUISE", 0), - ("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS", 0)] + signals += [("CRUISE_SPEED_PCM", "CRUISE"), + ("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS")] if CP.carFingerprint == CAR.ODYSSEY_CHN: - checks += [("CRUISE_PARAMS", 10)] + checks.append(("CRUISE_PARAMS", 10)) else: - checks += [("CRUISE_PARAMS", 50)] + checks.append(("CRUISE_PARAMS", 50)) if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E): - signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)] + signals.append(("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK")) elif CP.carFingerprint == CAR.ODYSSEY_CHN: - signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1)] + signals.append(("DRIVERS_DOOR_OPEN", "SCM_BUTTONS")) elif CP.carFingerprint in (CAR.FREED, CAR.HRV): - signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1), - ("WHEELS_MOVING", "STANDSTILL", 1)] + signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS"), + ("WHEELS_MOVING", "STANDSTILL")] else: - signals += [("DOOR_OPEN_FL", "DOORS_STATUS", 1), - ("DOOR_OPEN_FR", "DOORS_STATUS", 1), - ("DOOR_OPEN_RL", "DOORS_STATUS", 1), - ("DOOR_OPEN_RR", "DOORS_STATUS", 1), - ("WHEELS_MOVING", "STANDSTILL", 1)] + signals += [("DOOR_OPEN_FL", "DOORS_STATUS"), + ("DOOR_OPEN_FR", "DOORS_STATUS"), + ("DOOR_OPEN_RL", "DOORS_STATUS"), + ("DOOR_OPEN_RR", "DOORS_STATUS"), + ("WHEELS_MOVING", "STANDSTILL")] checks += [ ("DOORS_STATUS", 3), ("STANDSTILL", 50), ] if CP.carFingerprint == CAR.CIVIC: - signals += [("IMPERIAL_UNIT", "HUD_SETTING", 0), - ("EPB_STATE", "EPB_STATUS", 0)] + signals += [("IMPERIAL_UNIT", "HUD_SETTING"), + ("EPB_STATE", "EPB_STATUS")] checks += [ ("HUD_SETTING", 50), ("EPB_STATUS", 50), ] elif CP.carFingerprint in (CAR.ODYSSEY, CAR.ODYSSEY_CHN): - signals += [("EPB_STATE", "EPB_STATUS", 0)] - checks += [("EPB_STATUS", 50)] + signals.append(("EPB_STATE", "EPB_STATUS")) + checks.append(("EPB_STATUS", 50)) # add gas interceptor reading if we are using it if CP.enableGasInterceptor: - signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0)) - signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0)) + signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR")) + signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR")) checks.append(("GAS_SENSOR", 50)) if CP.openpilotLongitudinalControl: signals += [ - ("BRAKE_ERROR_1", "STANDSTILL", 1), - ("BRAKE_ERROR_2", "STANDSTILL", 1) + ("BRAKE_ERROR_1", "STANDSTILL"), + ("BRAKE_ERROR_2", "STANDSTILL") ] - checks += [("STANDSTILL", 50)] + checks.append(("STANDSTILL", 50)) return signals, checks @@ -167,8 +162,9 @@ class CarState(CarStateBase): self.shifter_values = can_define.dv[self.gearbox_msg]["GEAR_SHIFTER"] self.steer_status_values = defaultdict(lambda: "UNKNOWN", can_define.dv["STEER_STATUS"]["STEER_STATUS"]) - self.brake_switch_prev = 0 - self.brake_switch_prev_ts = 0 + self.brake_error = False + self.brake_switch_prev = False + self.brake_switch_active = False self.cruise_setting = 0 self.v_cruise_pcm_prev = 0 @@ -207,9 +203,7 @@ class CarState(CarStateBase): # LOW_SPEED_LOCKOUT is not worth a warning ret.steerWarning = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2") - if not self.CP.openpilotLongitudinalControl: - self.brake_error = 0 - else: + if self.CP.openpilotLongitudinalControl: self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"] ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0 @@ -245,15 +239,11 @@ class CarState(CarStateBase): gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"]) ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None)) - ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"] - - # this is a hack for the interceptor. This is now only used in the simulation - # TODO: Replace tests by toyota so this can go away if self.CP.enableGasInterceptor: - user_gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2. - ret.gasPressed = user_gas > 1e-5 # this works because interceptor reads < 0 when pedal position is 0. Once calibrated, this will change + ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2. else: - ret.gasPressed = ret.gas > 1e-5 + ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"] + ret.gasPressed = ret.gas > 1e-5 ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"] ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]["MOTOR_TORQUE"] @@ -270,26 +260,28 @@ class CarState(CarStateBase): else: ret.cruiseState.speed = cp.vl["CRUISE"]["CRUISE_SPEED_PCM"] * CV.KPH_TO_MS - self.brake_switch = cp.vl["POWERTRAIN_DATA"]["BRAKE_SWITCH"] != 0 if self.CP.carFingerprint in HONDA_BOSCH_ALT_BRAKE_SIGNAL: ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0 else: # brake switch has shown some single time step noise, so only considered when # switch is on for at least 2 consecutive CAN samples - # panda safety only checks BRAKE_PRESSED signal - ret.brakePressed = bool(cp.vl["POWERTRAIN_DATA"]["BRAKE_PRESSED"] or - (self.brake_switch and self.brake_switch_prev and cp.ts["POWERTRAIN_DATA"]["BRAKE_SWITCH"] != self.brake_switch_prev_ts)) - - self.brake_switch_prev = self.brake_switch - self.brake_switch_prev_ts = cp.ts["POWERTRAIN_DATA"]["BRAKE_SWITCH"] + # brake switch rises earlier than brake pressed but is never 1 when in park + brake_switch_vals = cp.vl_all["POWERTRAIN_DATA"]["BRAKE_SWITCH"] + if len(brake_switch_vals): + brake_switch = cp.vl["POWERTRAIN_DATA"]["BRAKE_SWITCH"] != 0 + if len(brake_switch_vals) > 1: + self.brake_switch_prev = brake_switch_vals[-2] != 0 + self.brake_switch_active = brake_switch and self.brake_switch_prev + self.brake_switch_prev = brake_switch + ret.brakePressed = (cp.vl["POWERTRAIN_DATA"]["BRAKE_PRESSED"] != 0) or self.brake_switch_active ret.brake = cp.vl["VSA_STATUS"]["USER_BRAKE"] ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0 ret.cruiseState.available = bool(cp.vl[self.main_on_sig_msg]["MAIN_ON"]) # Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models - if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE): - if ret.brake > 0.05: + if self.CP.carFingerprint in (CAR.PILOT, CAR.PASSPORT, CAR.RIDGELINE): + if ret.brake > 0.1: ret.brakePressed = True # TODO: discover the CAN msg that has the imperial unit bit for all other cars @@ -334,14 +326,14 @@ class CarState(CarStateBase): ] if CP.carFingerprint not in HONDA_BOSCH: - signals += [("COMPUTER_BRAKE", "BRAKE_COMMAND", 0), - ("AEB_REQ_1", "BRAKE_COMMAND", 0), - ("FCW", "BRAKE_COMMAND", 0), - ("CHIME", "BRAKE_COMMAND", 0), - ("FCM_OFF", "ACC_HUD", 0), - ("FCM_OFF_2", "ACC_HUD", 0), - ("FCM_PROBLEM", "ACC_HUD", 0), - ("ICONS", "ACC_HUD", 0)] + signals += [("COMPUTER_BRAKE", "BRAKE_COMMAND"), + ("AEB_REQ_1", "BRAKE_COMMAND"), + ("FCW", "BRAKE_COMMAND"), + ("CHIME", "BRAKE_COMMAND"), + ("FCM_OFF", "ACC_HUD"), + ("FCM_OFF_2", "ACC_HUD"), + ("FCM_PROBLEM", "ACC_HUD"), + ("ICONS", "ACC_HUD")] checks += [ ("ACC_HUD", 10), ("BRAKE_COMMAND", 50), @@ -352,8 +344,8 @@ class CarState(CarStateBase): @staticmethod def get_body_can_parser(CP): if CP.enableBsm and CP.carFingerprint == CAR.CRV_5G: - signals = [("BSM_ALERT", "BSM_STATUS_RIGHT", 0), - ("BSM_ALERT", "BSM_STATUS_LEFT", 0)] + signals = [("BSM_ALERT", "BSM_STATUS_RIGHT"), + ("BSM_ALERT", "BSM_STATUS_LEFT")] checks = [ ("BSM_STATUS_LEFT", 3), diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 62dbb24c34..94e4305909 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -47,7 +47,6 @@ class CarInterface(CarInterfaceBase): ret.openpilotLongitudinalControl = True ret.pcmCruise = not ret.enableGasInterceptor - ret.communityFeature = ret.enableGasInterceptor if candidate == CAR.CRV_5G: ret.enableBsm = 0x12f8bfa7 in fingerprint[0] @@ -250,17 +249,7 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] - elif candidate in (CAR.PILOT, CAR.PILOT_2019): - stop_and_go = False - ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight - ret.wheelbase = 2.82 - ret.centerToFront = ret.wheelbase * 0.428 - ret.steerRatio = 17.25 # as spec - ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end - tire_stiffness_factor = 0.444 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] - - elif candidate == CAR.PASSPORT: + elif candidate in (CAR.PILOT, CAR.PASSPORT): stop_and_go = False ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight ret.wheelbase = 2.82 diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 45e015af6e..629ab01d4c 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -4,13 +4,13 @@ from opendbc.can.parser import CANParser from selfdrive.car.interfaces import RadarInterfaceBase from selfdrive.car.honda.values import DBC + def _create_nidec_can_parser(car_fingerprint): radar_messages = [0x400] + list(range(0x430, 0x43A)) + list(range(0x440, 0x446)) signals = list(zip(['RADAR_STATE'] + - ['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 + - ['REL_SPEED'] * 16, - [0x400] + radar_messages[1:] * 4, - [0] + [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16)) + ['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 + + ['REL_SPEED'] * 16, + [0x400] + radar_messages[1:] * 4)) checks = [(s[1], 20) for s in signals] return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1) diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index cbe2369b14..6e347b8da3 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -82,7 +82,6 @@ class CAR: ACURA_RDX = "ACURA RDX 2018" ACURA_RDX_3G = "ACURA RDX 2020" PILOT = "HONDA PILOT 2017" - PILOT_2019 = "HONDA PILOT 2019" PASSPORT = "HONDA PASSPORT 2021" RIDGELINE = "HONDA RIDGELINE 2017" INSIGHT = "HONDA INSIGHT 2019" @@ -990,74 +989,51 @@ FW_VERSIONS = { b'37805-RLV-C530\x00\x00', b'37805-RLV-C910\x00\x00', ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TG7-A030\x00\x00', + b'38897-TG7-A040\x00\x00', + b'38897-TG7-A110\x00\x00', + b'38897-TG7-A210\x00\x00', + ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TG7-A030\x00\x00', b'39990-TG7-A040\x00\x00', b'39990-TG7-A060\x00\x00', - ], - (Ecu.fwdCamera, 0x18dab0f1, None): [ - b'36161-TG7-A520\x00\x00', - b'36161-TG7-A720\x00\x00', - b'36161-TG7-A820\x00\x00', - b'36161-TG7-C520\x00\x00', - b'36161-TG7-D520\x00\x00', - b'36161-TG8-A520\x00\x00', - b'36161-TG8-A720\x00\x00', - ], - (Ecu.srs, 0x18da53f1, None): [ - b'77959-TG7-A110\x00\x00', - b'77959-TG7-A020\x00\x00', - ], - (Ecu.combinationMeter, 0x18da60f1, None): [ - b'78109-TG7-A040\x00\x00', - b'78109-TG7-A050\x00\x00', - b'78109-TG7-A420\x00\x00', - b'78109-TG7-A520\x00\x00', - b'78109-TG7-A720\x00\x00', - b'78109-TG7-D020\x00\x00', - b'78109-TG8-A420\x00\x00', - b'78109-TG8-A520\x00\x00', - ], - (Ecu.vsa, 0x18da28f1, None): [ - b'57114-TG7-A130\x00\x00', - b'57114-TG7-A140\x00\x00', - b'57114-TG7-A230\x00\x00', - b'57114-TG7-A240\x00\x00', - b'57114-TG8-A140\x00\x00', - b'57114-TG8-A240\x00\x00', - ], - - }, - CAR.PILOT_2019: { - (Ecu.eps, 0x18da30f1, None): [ - b'39990-TG7-A060\x00\x00', b'39990-TG7-A070\x00\x00', b'39990-TGS-A230\x00\x00', ], - (Ecu.gateway, 0x18daeff1, None): [ - b'38897-TG7-A030\x00\x00', - b'38897-TG7-A040\x00\x00', - b'38897-TG7-A110\x00\x00', - b'38897-TG7-A210\x00\x00', - ], (Ecu.fwdCamera, 0x18dab0f1, None): [ b'36161-TG7-A310\x00\x00', + b'36161-TG7-A520\x00\x00', b'36161-TG7-A630\x00\x00', + b'36161-TG7-A720\x00\x00', + b'36161-TG7-A820\x00\x00', b'36161-TG7-A930\x00\x00', + b'36161-TG7-C520\x00\x00', + b'36161-TG7-D520\x00\x00', b'36161-TG7-D630\x00\x00', b'36161-TG7-Y630\x00\x00', + b'36161-TG8-A520\x00\x00', b'36161-TG8-A630\x00\x00', + b'36161-TG8-A720\x00\x00', b'36161-TG8-A830\x00\x00', b'36161-TGS-A130\x00\x00', b'36161-TGT-A030\x00\x00', b'36161-TGT-A130\x00\x00', ], (Ecu.srs, 0x18da53f1, None): [ + b'77959-TG7-A020\x00\x00', + b'77959-TG7-A110\x00\x00', b'77959-TG7-A210\x00\x00', b'77959-TG7-Y210\x00\x00', b'77959-TGS-A010\x00\x00', ], (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TG7-A040\x00\x00', + b'78109-TG7-A050\x00\x00', + b'78109-TG7-A420\x00\x00', + b'78109-TG7-A520\x00\x00', + b'78109-TG7-A720\x00\x00', b'78109-TG7-AJ10\x00\x00', b'78109-TG7-AJ20\x00\x00', b'78109-TG7-AK10\x00\x00', @@ -1069,8 +1045,11 @@ FW_VERSIONS = { b'78109-TG7-AT20\x00\x00', b'78109-TG7-AU20\x00\x00', b'78109-TG7-AX20\x00\x00', + b'78109-TG7-D020\x00\x00', b'78109-TG7-DJ10\x00\x00', b'78109-TG7-YK20\x00\x00', + b'78109-TG8-A420\x00\x00', + b'78109-TG8-A520\x00\x00', b'78109-TG8-AJ10\x00\x00', b'78109-TG8-AJ20\x00\x00', b'78109-TG8-AK20\x00\x00', @@ -1080,8 +1059,14 @@ FW_VERSIONS = { b'78109-TGT-AK30\x00\x00', ], (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TG7-A130\x00\x00', + b'57114-TG7-A140\x00\x00', + b'57114-TG7-A230\x00\x00', + b'57114-TG7-A240\x00\x00', b'57114-TG7-A630\x00\x00', b'57114-TG7-A730\x00\x00', + b'57114-TG8-A140\x00\x00', + b'57114-TG8-A240\x00\x00', b'57114-TG8-A630\x00\x00', b'57114-TG8-A730\x00\x00', b'57114-TGS-A530\x00\x00', @@ -1371,10 +1356,9 @@ DBC = { CAR.HRV: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'), CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'), CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'), - CAR.PILOT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'), - CAR.PILOT_2019: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'), - CAR.PASSPORT: dbc_dict('honda_pilot_touring_2017_can_generated', 'acura_ilx_2016_nidec'), - CAR.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'), + CAR.PILOT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + CAR.PASSPORT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), + CAR.RIDGELINE: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'), CAR.INSIGHT: dbc_dict('honda_insight_ex_2019_can_generated', None), CAR.HONDA_E: dbc_dict('acura_rdx_2020_can_generated', None), } @@ -1387,7 +1371,7 @@ STEER_THRESHOLD = { HONDA_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY} HONDA_NIDEC_ALT_SCM_MESSAGES = {CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN, - CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE} + CAR.PILOT, CAR.PASSPORT, CAR.RIDGELINE} HONDA_BOSCH = {CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, - CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E} + CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E} HONDA_BOSCH_ALT_BRAKE_SIGNAL = {CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G} diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 6adefd26c6..f7c43bd6e3 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -46,7 +46,7 @@ class CarController(): self.last_resume_frame = 0 self.accel = 0 - def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed, + def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed, left_lane, right_lane, left_lane_depart, right_lane_depart): # Steering Torque new_steer = int(round(actuators.steer * self.p.STEER_MAX)) @@ -54,7 +54,7 @@ class CarController(): self.steer_rate_limited = new_steer != apply_steer # disable when temp fault is active, or below LKA minimum speed - lkas_active = enabled and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed + lkas_active = c.active and not CS.out.steerWarning and CS.out.vEgo >= CS.CP.minSteerSpeed if not lkas_active: apply_steer = 0 @@ -89,7 +89,7 @@ class CarController(): if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl: lead_visible = False - accel = actuators.accel if enabled else 0 + accel = actuators.accel if c.active else 0 jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index e889f24fc9..bdd49e2067 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -120,57 +120,57 @@ class CarState(CarStateBase): @staticmethod def get_can_parser(CP): signals = [ - # sig_name, sig_address, default - ("WHL_SPD_FL", "WHL_SPD11", 0), - ("WHL_SPD_FR", "WHL_SPD11", 0), - ("WHL_SPD_RL", "WHL_SPD11", 0), - ("WHL_SPD_RR", "WHL_SPD11", 0), - - ("YAW_RATE", "ESP12", 0), - - ("CF_Gway_DrvSeatBeltInd", "CGW4", 1), - - ("CF_Gway_DrvSeatBeltSw", "CGW1", 0), - ("CF_Gway_DrvDrSw", "CGW1", 0), # Driver Door - ("CF_Gway_AstDrSw", "CGW1", 0), # Passenger door - ("CF_Gway_RLDrSw", "CGW2", 0), # Rear reft door - ("CF_Gway_RRDrSw", "CGW2", 0), # Rear right door - ("CF_Gway_TurnSigLh", "CGW1", 0), - ("CF_Gway_TurnSigRh", "CGW1", 0), - ("CF_Gway_ParkBrakeSw", "CGW1", 0), - - ("CYL_PRES", "ESP12", 0), - - ("CF_Clu_CruiseSwState", "CLU11", 0), - ("CF_Clu_CruiseSwMain", "CLU11", 0), - ("CF_Clu_SldMainSW", "CLU11", 0), - ("CF_Clu_ParityBit1", "CLU11", 0), - ("CF_Clu_VanzDecimal" , "CLU11", 0), - ("CF_Clu_Vanz", "CLU11", 0), - ("CF_Clu_SPEED_UNIT", "CLU11", 0), - ("CF_Clu_DetentOut", "CLU11", 0), - ("CF_Clu_RheostatLevel", "CLU11", 0), - ("CF_Clu_CluInfo", "CLU11", 0), - ("CF_Clu_AmpInfo", "CLU11", 0), - ("CF_Clu_AliveCnt1", "CLU11", 0), - - ("ACCEnable", "TCS13", 0), - ("ACC_REQ", "TCS13", 0), - ("DriverBraking", "TCS13", 0), - ("StandStill", "TCS13", 0), - ("PBRAKE_ACT", "TCS13", 0), - - ("ESC_Off_Step", "TCS15", 0), - ("AVH_LAMP", "TCS15", 0), - - ("CR_Mdps_StrColTq", "MDPS12", 0), - ("CF_Mdps_ToiActive", "MDPS12", 0), - ("CF_Mdps_ToiUnavail", "MDPS12", 0), - ("CF_Mdps_ToiFlt", "MDPS12", 0), - ("CR_Mdps_OutTq", "MDPS12", 0), - - ("SAS_Angle", "SAS11", 0), - ("SAS_Speed", "SAS11", 0), + # sig_name, sig_address + ("WHL_SPD_FL", "WHL_SPD11"), + ("WHL_SPD_FR", "WHL_SPD11"), + ("WHL_SPD_RL", "WHL_SPD11"), + ("WHL_SPD_RR", "WHL_SPD11"), + + ("YAW_RATE", "ESP12"), + + ("CF_Gway_DrvSeatBeltInd", "CGW4"), + + ("CF_Gway_DrvSeatBeltSw", "CGW1"), + ("CF_Gway_DrvDrSw", "CGW1"), # Driver Door + ("CF_Gway_AstDrSw", "CGW1"), # Passenger door + ("CF_Gway_RLDrSw", "CGW2"), # Rear reft door + ("CF_Gway_RRDrSw", "CGW2"), # Rear right door + ("CF_Gway_TurnSigLh", "CGW1"), + ("CF_Gway_TurnSigRh", "CGW1"), + ("CF_Gway_ParkBrakeSw", "CGW1"), + + ("CYL_PRES", "ESP12"), + + ("CF_Clu_CruiseSwState", "CLU11"), + ("CF_Clu_CruiseSwMain", "CLU11"), + ("CF_Clu_SldMainSW", "CLU11"), + ("CF_Clu_ParityBit1", "CLU11"), + ("CF_Clu_VanzDecimal" , "CLU11"), + ("CF_Clu_Vanz", "CLU11"), + ("CF_Clu_SPEED_UNIT", "CLU11"), + ("CF_Clu_DetentOut", "CLU11"), + ("CF_Clu_RheostatLevel", "CLU11"), + ("CF_Clu_CluInfo", "CLU11"), + ("CF_Clu_AmpInfo", "CLU11"), + ("CF_Clu_AliveCnt1", "CLU11"), + + ("ACCEnable", "TCS13"), + ("ACC_REQ", "TCS13"), + ("DriverBraking", "TCS13"), + ("StandStill", "TCS13"), + ("PBRAKE_ACT", "TCS13"), + + ("ESC_Off_Step", "TCS15"), + ("AVH_LAMP", "TCS15"), + + ("CR_Mdps_StrColTq", "MDPS12"), + ("CF_Mdps_ToiActive", "MDPS12"), + ("CF_Mdps_ToiUnavail", "MDPS12"), + ("CF_Mdps_ToiFlt", "MDPS12"), + ("CR_Mdps_OutTq", "MDPS12"), + + ("SAS_Angle", "SAS11"), + ("SAS_Speed", "SAS11"), ] checks = [ @@ -189,11 +189,11 @@ class CarState(CarStateBase): if not CP.openpilotLongitudinalControl: signals += [ - ("MainMode_ACC", "SCC11", 0), - ("VSetDis", "SCC11", 0), - ("SCCInfoDisplay", "SCC11", 0), - ("ACC_ObjDist", "SCC11", 0), - ("ACCMode", "SCC12", 1), + ("MainMode_ACC", "SCC11"), + ("VSetDis", "SCC11"), + ("SCCInfoDisplay", "SCC11"), + ("ACC_ObjDist", "SCC11"), + ("ACCMode", "SCC12"), ] checks += [ @@ -203,39 +203,33 @@ class CarState(CarStateBase): if CP.carFingerprint in FEATURES["use_fca"]: signals += [ - ("FCA_CmdAct", "FCA11", 0), - ("CF_VSM_Warn", "FCA11", 0), + ("FCA_CmdAct", "FCA11"), + ("CF_VSM_Warn", "FCA11"), ] - checks += [("FCA11", 50)] + checks.append(("FCA11", 50)) else: signals += [ - ("AEB_CmdAct", "SCC12", 0), - ("CF_VSM_Warn", "SCC12", 0), + ("AEB_CmdAct", "SCC12"), + ("CF_VSM_Warn", "SCC12"), ] if CP.enableBsm: signals += [ - ("CF_Lca_IndLeft", "LCA11", 0), - ("CF_Lca_IndRight", "LCA11", 0), + ("CF_Lca_IndLeft", "LCA11"), + ("CF_Lca_IndRight", "LCA11"), ] - checks += [("LCA11", 50)] + checks.append(("LCA11", 50)) if CP.carFingerprint in (HYBRID_CAR | EV_CAR): if CP.carFingerprint in HYBRID_CAR: - signals += [ - ("CR_Vcu_AccPedDep_Pos", "E_EMS11", 0) - ] + signals.append(("CR_Vcu_AccPedDep_Pos", "E_EMS11")) else: - signals += [ - ("Accel_Pedal_Pos", "E_EMS11", 0) - ] - checks += [ - ("E_EMS11", 50), - ] + signals.append(("Accel_Pedal_Pos", "E_EMS11")) + checks.append(("E_EMS11", 50)) else: signals += [ - ("PV_AV_CAN", "EMS12", 0), - ("CF_Ems_AclAct", "EMS16", 0), + ("PV_AV_CAN", "EMS12"), + ("CF_Ems_AclAct", "EMS16"), ] checks += [ ("EMS12", 100), @@ -243,52 +237,39 @@ class CarState(CarStateBase): ] if CP.carFingerprint in FEATURES["use_cluster_gears"]: - signals += [ - ("CF_Clu_Gear", "CLU15", 0), - ] - checks += [ - ("CLU15", 5) - ] + signals.append(("CF_Clu_Gear", "CLU15")) + checks.append(("CLU15", 5)) elif CP.carFingerprint in FEATURES["use_tcu_gears"]: - signals += [ - ("CUR_GR", "TCU12", 0) - ] - checks += [ - ("TCU12", 100) - ] + signals.append(("CUR_GR", "TCU12")) + checks.append(("TCU12", 100)) elif CP.carFingerprint in FEATURES["use_elect_gears"]: - signals += [("Elect_Gear_Shifter", "ELECT_GEAR", 0)] - checks += [("ELECT_GEAR", 20)] + signals.append(("Elect_Gear_Shifter", "ELECT_GEAR")) + checks.append(("ELECT_GEAR", 20)) else: - signals += [ - ("CF_Lvr_Gear", "LVR12", 0) - ] - checks += [ - ("LVR12", 100) - ] + signals.append(("CF_Lvr_Gear", "LVR12")) + checks.append(("LVR12", 100)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @staticmethod def get_cam_can_parser(CP): - signals = [ - # sig_name, sig_address, default - ("CF_Lkas_LdwsActivemode", "LKAS11", 0), - ("CF_Lkas_LdwsSysState", "LKAS11", 0), - ("CF_Lkas_SysWarning", "LKAS11", 0), - ("CF_Lkas_LdwsLHWarning", "LKAS11", 0), - ("CF_Lkas_LdwsRHWarning", "LKAS11", 0), - ("CF_Lkas_HbaLamp", "LKAS11", 0), - ("CF_Lkas_FcwBasReq", "LKAS11", 0), - ("CF_Lkas_HbaSysState", "LKAS11", 0), - ("CF_Lkas_FcwOpt", "LKAS11", 0), - ("CF_Lkas_HbaOpt", "LKAS11", 0), - ("CF_Lkas_FcwSysState", "LKAS11", 0), - ("CF_Lkas_FcwCollisionWarning", "LKAS11", 0), - ("CF_Lkas_FusionState", "LKAS11", 0), - ("CF_Lkas_FcwOpt_USM", "LKAS11", 0), - ("CF_Lkas_LdwsOpt_USM", "LKAS11", 0), + # sig_name, sig_address + ("CF_Lkas_LdwsActivemode", "LKAS11"), + ("CF_Lkas_LdwsSysState", "LKAS11"), + ("CF_Lkas_SysWarning", "LKAS11"), + ("CF_Lkas_LdwsLHWarning", "LKAS11"), + ("CF_Lkas_LdwsRHWarning", "LKAS11"), + ("CF_Lkas_HbaLamp", "LKAS11"), + ("CF_Lkas_FcwBasReq", "LKAS11"), + ("CF_Lkas_HbaSysState", "LKAS11"), + ("CF_Lkas_FcwOpt", "LKAS11"), + ("CF_Lkas_HbaOpt", "LKAS11"), + ("CF_Lkas_FcwSysState", "LKAS11"), + ("CF_Lkas_FcwCollisionWarning", "LKAS11"), + ("CF_Lkas_FusionState", "LKAS11"), + ("CF_Lkas_FcwOpt_USM", "LKAS11"), + ("CF_Lkas_LdwsOpt_USM", "LKAS11"), ] checks = [ diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 89e1934cdf..463ff3569a 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -347,7 +347,7 @@ class CarInterface(CarInterfaceBase): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, + ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert, hud_control.setSpeed, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart) self.frame += 1 diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py index a7269e9283..0d22611fb5 100644 --- a/selfdrive/car/hyundai/radar_interface.py +++ b/selfdrive/car/hyundai/radar_interface.py @@ -20,11 +20,11 @@ def get_radar_can_parser(CP): for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT): msg = f"RADAR_TRACK_{addr:x}" signals += [ - ("STATE", msg, 0), - ("AZIMUTH", msg, 0), - ("LONG_DIST", msg, 0), - ("REL_ACCEL", msg, 0), - ("REL_SPEED", msg, 0), + ("STATE", msg), + ("AZIMUTH", msg), + ("LONG_DIST", msg), + ("REL_ACCEL", msg), + ("REL_SPEED", msg), ] checks += [(msg, 50)] return CANParser(DBC[CP.carFingerprint]['radar'], signals, checks, 1) @@ -81,7 +81,7 @@ class RadarInterface(RadarInterfaceBase): self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST'] self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST'] self.pts[addr].vRel = msg['REL_SPEED'] - self.pts[addr].aRel = msg['REL_ACCEL'] + self.pts[addr].aRel = msg['REL_ACCEL'] self.pts[addr].yvRel = float('nan') else: diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index d940144f86..818dbe57f6 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -463,22 +463,27 @@ FW_VERSIONS = { b'\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0', b'\xf1\x8758910-S2DA0\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0', b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0', + b'\xf1\x8758910-S1DA0\xf1\x00TM ESC \x1e 102 \x08\x08 58910-S1DA0', ], (Ecu.engine, 0x7e0, None): [ b'\xf1\x82TMBZN5TMD3XXXG2E', b'\xf1\x82TACVN5GSI3XXXH0A', + b'\xf1\x82TMCFD5MMCXXXXG0A', ], (Ecu.eps, 0x7d4, None): [ b'\xf1\x00TM MDPS C 1.00 1.02 56370-S2AA0 0B19', + b'\xf1\x00TM MDPS C 1.00 1.01 56310-S1AB0 4TSDC101', ], (Ecu.fwdCamera, 0x7c4, None): [ b'\xf1\x00TMA MFC AT MEX LHD 1.00 1.01 99211-S2500 210205', b'\xf1\x00TMA MFC AT USA LHD 1.00 1.00 99211-S2500 200720', + b'\xf1\x00TM MFC AT EUR LHD 1.00 1.03 99211-S1500 210224', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x00T02601BL T02730A1 VTMPT25XXX730NS2\xa6\x06\x88\xf7', b'\xf1\x87SDMXCA8653204GN1EVugEUuWwwwwww\x87wwwwwv/\xfb\xff\xa8\x88\x9f\xff\xa5\x9c\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00', b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02730A1 \xf1\x00T02601BL T02730A1 VTMPT25XXX730NS2\xa6\x06\x88\xf7', + b'\xf1\x87KMMYBU034207SB72x\x89\x88\x98h\x88\x98\x89\x87fhvvfWf33_\xff\x87\xff\x8f\xfa\x81\xe5\xf1\x89HT6TAF00A1\xf1\x82STM0M25GS1\x00\x00\x00\x00\x00\x00', ], }, CAR.SANTA_FE_HEV_2022: { diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 701bf7cff3..ae2644b867 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -74,8 +74,7 @@ class CarInterfaceBase(ABC): def get_std_params(candidate, fingerprint): ret = car.CarParams.new_message() ret.carFingerprint = candidate - ret.unsafeMode = 0 # see safety_declarations.h for allowed values - ret.unsafeMode = 1 if Params().get_bool("DisengageOnGas") else 0 # sets unsafeMode to 1 if DisengageOnGas is True + ret.unsafeMode = 1 if Params().get_bool("DisengageOnGas") else 0 # see safety_declarations.h for allowed values # standard ALC params ret.steerControlType = car.CarParams.SteerControlType.torque @@ -101,6 +100,7 @@ class CarInterfaceBase(ABC): ret.longitudinalTuning.kiV = [1.] ret.longitudinalActuatorDelayLowerBound = 0.15 ret.longitudinalActuatorDelayUpperBound = 0.15 + ret.steerLimitTimer = 1.0 return ret @abstractmethod diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py index c65ff72ed5..60bb620377 100644 --- a/selfdrive/car/mazda/carcontroller.py +++ b/selfdrive/car/mazda/carcontroller.py @@ -19,7 +19,7 @@ class CarController(): apply_steer = 0 self.steer_rate_limited = False - if c.enabled: + if c.active: # calculate steer and also set limits due to driver torque new_steer = int(round(c.actuators.steer * CarControllerParams.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index 7ebf60c96b..feb1147549 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -16,6 +16,7 @@ class CarState(CarStateBase): self.acc_active_last = False self.low_speed_alert = False self.lkas_allowed_speed = False + self.lkas_disabled = False def update(self, cp, cp_cam): @@ -64,12 +65,13 @@ class CarState(CarStateBase): # Either due to low speed or hands off lkas_blocked = cp.vl["STEER_RATE"]["LKAS_BLOCK"] == 1 - # LKAS is enabled at 52kph going up and disabled at 45kph going down - # wait for LKAS_BLOCK signal to clear when going up since it lags behind the speed sometimes - if speed_kph > LKAS_LIMITS.ENABLE_SPEED and not lkas_blocked: - self.lkas_allowed_speed = True - elif speed_kph < LKAS_LIMITS.DISABLE_SPEED: - self.lkas_allowed_speed = False + if self.CP.minSteerSpeed > 0: + # LKAS is enabled at 52kph going up and disabled at 45kph going down + # wait for LKAS_BLOCK signal to clear when going up since it lags behind the speed sometimes + if speed_kph > LKAS_LIMITS.ENABLE_SPEED and not lkas_blocked: + self.lkas_allowed_speed = True + elif speed_kph < LKAS_LIMITS.DISABLE_SPEED: + self.lkas_allowed_speed = False # TODO: the signal used for available seems to be the adaptive cruise signal, instead of the main on # it should be used for carState.cruiseState.nonAdaptive instead @@ -86,34 +88,35 @@ class CarState(CarStateBase): # Check if LKAS is disabled due to lack of driver torque when all other states indicate # it should be enabled (steer lockout). Don't warn until we actually get lkas active # and lose it again, i.e, after initial lkas activation - ret.steerWarning = self.lkas_allowed_speed and lkas_blocked self.acc_active_last = ret.cruiseState.enabled + self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"] + + # camera signals + self.lkas_disabled = cp_cam.vl["CAM_LANEINFO"]["LANE_LINES"] == 0 self.cam_lkas = cp_cam.vl["CAM_LKAS"] self.cam_laneinfo = cp_cam.vl["CAM_LANEINFO"] - self.crz_btns_counter = cp.vl["CRZ_BTNS"]["CTR"] ret.steerError = cp_cam.vl["CAM_LKAS"]["ERR_BIT_1"] == 1 return ret @staticmethod def get_can_parser(CP): - # this function generates lists for signal, messages and initial values signals = [ - # sig_name, sig_address, default - ("LEFT_BLINK", "BLINK_INFO", 0), - ("RIGHT_BLINK", "BLINK_INFO", 0), - ("HIGH_BEAMS", "BLINK_INFO", 0), - ("STEER_ANGLE", "STEER", 0), - ("STEER_ANGLE_RATE", "STEER_RATE", 0), - ("STEER_TORQUE_SENSOR", "STEER_TORQUE", 0), - ("STEER_TORQUE_MOTOR", "STEER_TORQUE", 0), - ("FL", "WHEEL_SPEEDS", 0), - ("FR", "WHEEL_SPEEDS", 0), - ("RL", "WHEEL_SPEEDS", 0), - ("RR", "WHEEL_SPEEDS", 0), + # sig_name, sig_address + ("LEFT_BLINK", "BLINK_INFO"), + ("RIGHT_BLINK", "BLINK_INFO"), + ("HIGH_BEAMS", "BLINK_INFO"), + ("STEER_ANGLE", "STEER"), + ("STEER_ANGLE_RATE", "STEER_RATE"), + ("STEER_TORQUE_SENSOR", "STEER_TORQUE"), + ("STEER_TORQUE_MOTOR", "STEER_TORQUE"), + ("FL", "WHEEL_SPEEDS"), + ("FR", "WHEEL_SPEEDS"), + ("RL", "WHEEL_SPEEDS"), + ("RR", "WHEEL_SPEEDS"), ] checks = [ @@ -127,26 +130,26 @@ class CarState(CarStateBase): if CP.carFingerprint in GEN1: signals += [ - ("LKAS_BLOCK", "STEER_RATE", 0), - ("LKAS_TRACK_STATE", "STEER_RATE", 0), - ("HANDS_OFF_5_SECONDS", "STEER_RATE", 0), - ("CRZ_ACTIVE", "CRZ_CTRL", 0), - ("CRZ_AVAILABLE", "CRZ_CTRL", 0), - ("CRZ_SPEED", "CRZ_EVENTS", 0), - ("STANDSTILL", "PEDALS", 0), - ("BRAKE_ON", "PEDALS", 0), - ("BRAKE_PRESSURE", "BRAKE", 0), - ("GEAR", "GEAR", 0), - ("DRIVER_SEATBELT", "SEATBELT", 0), - ("FL", "DOORS", 0), - ("FR", "DOORS", 0), - ("BL", "DOORS", 0), - ("BR", "DOORS", 0), - ("PEDAL_GAS", "ENGINE_DATA", 0), - ("SPEED", "ENGINE_DATA", 0), - ("CTR", "CRZ_BTNS", 0), - ("LEFT_BS1", "BSM", 0), - ("RIGHT_BS1", "BSM", 0), + ("LKAS_BLOCK", "STEER_RATE"), + ("LKAS_TRACK_STATE", "STEER_RATE"), + ("HANDS_OFF_5_SECONDS", "STEER_RATE"), + ("CRZ_ACTIVE", "CRZ_CTRL"), + ("CRZ_AVAILABLE", "CRZ_CTRL"), + ("CRZ_SPEED", "CRZ_EVENTS"), + ("STANDSTILL", "PEDALS"), + ("BRAKE_ON", "PEDALS"), + ("BRAKE_PRESSURE", "BRAKE"), + ("GEAR", "GEAR"), + ("DRIVER_SEATBELT", "SEATBELT"), + ("FL", "DOORS"), + ("FR", "DOORS"), + ("BL", "DOORS"), + ("BR", "DOORS"), + ("PEDAL_GAS", "ENGINE_DATA"), + ("SPEED", "ENGINE_DATA"), + ("CTR", "CRZ_BTNS"), + ("LEFT_BS1", "BSM"), + ("RIGHT_BS1", "BSM"), ] checks += [ @@ -171,26 +174,26 @@ class CarState(CarStateBase): if CP.carFingerprint in GEN1: signals += [ - # sig_name, sig_address, default - ("LKAS_REQUEST", "CAM_LKAS", 0), - ("CTR", "CAM_LKAS", 0), - ("ERR_BIT_1", "CAM_LKAS", 0), - ("LINE_NOT_VISIBLE", "CAM_LKAS", 0), - ("BIT_1", "CAM_LKAS", 1), - ("ERR_BIT_2", "CAM_LKAS", 0), - ("STEERING_ANGLE", "CAM_LKAS", 0), - ("ANGLE_ENABLED", "CAM_LKAS", 0), - ("CHKSUM", "CAM_LKAS", 0), - - ("LINE_VISIBLE", "CAM_LANEINFO", 0), - ("LINE_NOT_VISIBLE", "CAM_LANEINFO", 1), - ("LANE_LINES", "CAM_LANEINFO", 0), - ("BIT1", "CAM_LANEINFO", 0), - ("BIT2", "CAM_LANEINFO", 0), - ("BIT3", "CAM_LANEINFO", 0), - ("NO_ERR_BIT", "CAM_LANEINFO", 1), - ("S1", "CAM_LANEINFO", 0), - ("S1_HBEAM", "CAM_LANEINFO", 0), + # sig_name, sig_address + ("LKAS_REQUEST", "CAM_LKAS"), + ("CTR", "CAM_LKAS"), + ("ERR_BIT_1", "CAM_LKAS"), + ("LINE_NOT_VISIBLE", "CAM_LKAS"), + ("BIT_1", "CAM_LKAS"), + ("ERR_BIT_2", "CAM_LKAS"), + ("STEERING_ANGLE", "CAM_LKAS"), + ("ANGLE_ENABLED", "CAM_LKAS"), + ("CHKSUM", "CAM_LKAS"), + + ("LINE_VISIBLE", "CAM_LANEINFO"), + ("LINE_NOT_VISIBLE", "CAM_LANEINFO"), + ("LANE_LINES", "CAM_LANEINFO"), + ("BIT1", "CAM_LANEINFO"), + ("BIT2", "CAM_LANEINFO"), + ("BIT3", "CAM_LANEINFO"), + ("NO_ERR_BIT", "CAM_LANEINFO"), + ("S1", "CAM_LANEINFO"), + ("S1_HBEAM", "CAM_LANEINFO"), ] checks += [ diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py index a4c0ce705d..fb8edd6f42 100755 --- a/selfdrive/car/mazda/interface.py +++ b/selfdrive/car/mazda/interface.py @@ -22,14 +22,14 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)] ret.radarOffCan = True - ret.dashcamOnly = candidate not in (CAR.CX9_2021,) + ret.dashcamOnly = candidate not in (CAR.CX5_2022, CAR.CX9_2021) ret.steerActuatorDelay = 0.1 ret.steerRateCost = 1.0 ret.steerLimitTimer = 0.8 tire_stiffness_factor = 0.70 # not optimized yet - if candidate == CAR.CX5: + if candidate in (CAR.CX5, CAR.CX5_2022): ret.mass = 3655 * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.7 ret.steerRatio = 15.5 @@ -58,8 +58,8 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.19], [0.019]] ret.lateralTuning.pid.kf = 0.00006 - # No steer below disable speed - ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS + if candidate not in (CAR.CX5_2022, ): + ret.minSteerSpeed = LKAS_LIMITS.DISABLE_SPEED * CV.KPH_TO_MS ret.centerToFront = ret.wheelbase * 0.41 @@ -86,7 +86,9 @@ class CarInterface(CarInterfaceBase): # events events = self.create_common_events(ret) - if self.CS.low_speed_alert: + if self.CS.lkas_disabled: + events.add(EventName.lkasDisabled) + elif self.CS.low_speed_alert: events.add(EventName.belowSteerSpeed) ret.events = events.to_msg() diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py index c3a5de0c19..1fcf184281 100644 --- a/selfdrive/car/mazda/values.py +++ b/selfdrive/car/mazda/values.py @@ -19,7 +19,8 @@ class CAR: CX9 = "MAZDA CX-9" MAZDA3 = "MAZDA 3" MAZDA6 = "MAZDA 6" - CX9_2021 = "MAZDA CX-9 2021" # No Steer Lockout + CX9_2021 = "MAZDA CX-9 2021" + CX5_2022 = "MAZDA CX-5 2022" class LKAS_LIMITS: STEER_THRESHOLD = 15 @@ -35,6 +36,26 @@ class Buttons: FW_VERSIONS = { + CAR.CX5_2022 : { + (Ecu.eps, 0x730, None): [ + b'KSD5-3210X-C-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'PX2G-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x764, None): [ + b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.esp, 0x760, None): [ + b'KSD5-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x706, None): [ + b'GSH7-67XK2-S\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.transmission, 0x7e1, None): [ + b'PYB2-21PS1-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + ], + }, CAR.CX5: { (Ecu.eps, 0x730, None): [ b'KJ01-3210X-G-00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -115,16 +136,19 @@ FW_VERSIONS = { b'PYD7-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYD8-188K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYFM-188K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYFM-188K2-H\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x764, None): [ b'K123-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'K131-67XK2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'K131-67XK2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'TK80-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'TK80-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.esp, 0x760, None): [ b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'TK79-437K2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'TK79-437K2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'TM53-437K2-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'TN40-437K2-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', ], @@ -138,6 +162,7 @@ FW_VERSIONS = { b'PXM7-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PXM7-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYFM-21PS1-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', + b'PYFM-21PS1-D\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYD5-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYD5-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', b'PYD6-21PS1-A\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', @@ -242,10 +267,8 @@ DBC = { CAR.MAZDA3: dbc_dict('mazda_2017', None), CAR.MAZDA6: dbc_dict('mazda_2017', None), CAR.CX9_2021: dbc_dict('mazda_2017', None), + CAR.CX5_2022: dbc_dict('mazda_2017', None), } # Gen 1 hardware: same CAN messages and same camera -GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6} - -# Cars with a steering lockout -STEER_LOCKOUT_CAR = {CAR.CX5, CAR.CX9, CAR.MAZDA3, CAR.MAZDA6} +GEN1 = {CAR.CX5, CAR.CX9, CAR.CX9_2021, CAR.MAZDA3, CAR.MAZDA6, CAR.CX5_2022} diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index bc1f6dcf6b..b2e315a5f9 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -20,7 +20,6 @@ class CarInterface(CarInterfaceBase): cloudlog.debug("Using Mock Car Interface") - # TODO: subscribe to phone sensor self.sensor = messaging.sub_sock('sensorEvents') self.gps = messaging.sub_sock('gpsLocationExternal') diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 40dd5da42a..8b30c11249 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -18,22 +18,20 @@ class CarController(): self.packer = CANPacker(dbc_name) - def update(self, enabled, CS, frame, actuators, cruise_cancel, hud_alert, + def update(self, c, enabled, CS, frame, actuators, cruise_cancel, hud_alert, left_line, right_line, left_lane_depart, right_lane_depart): - """ Controls thread """ - # Send CAN commands. can_sends = [] ### STEER ### - acc_active = bool(CS.out.cruiseState.enabled) + acc_active = CS.out.cruiseState.enabled lkas_hud_msg = CS.lkas_hud_msg lkas_hud_info_msg = CS.lkas_hud_info_msg apply_angle = actuators.steeringAngleDeg steer_hud_alert = 1 if hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw) else 0 - if enabled: + if c.active: # # windup slower if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): angle_rate_lim = interp(CS.out.vEgo, CarControllerParams.ANGLE_DELTA_BP, CarControllerParams.ANGLE_DELTA_V) diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index 4c605395d4..6b030e9b45 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -119,27 +119,26 @@ class CarState(CarStateBase): @staticmethod def get_can_parser(CP): - # this function generates lists for signal, messages and initial values signals = [ - # sig_name, sig_address, default - ("WHEEL_SPEED_FL", "WHEEL_SPEEDS_FRONT", 0), - ("WHEEL_SPEED_FR", "WHEEL_SPEEDS_FRONT", 0), - ("WHEEL_SPEED_RL", "WHEEL_SPEEDS_REAR", 0), - ("WHEEL_SPEED_RR", "WHEEL_SPEEDS_REAR", 0), + # sig_name, sig_address + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS_FRONT"), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS_FRONT"), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS_REAR"), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS_REAR"), - ("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0), + ("STEER_ANGLE", "STEER_ANGLE_SENSOR"), - ("DOOR_OPEN_FR", "DOORS_LIGHTS", 1), - ("DOOR_OPEN_FL", "DOORS_LIGHTS", 1), - ("DOOR_OPEN_RR", "DOORS_LIGHTS", 1), - ("DOOR_OPEN_RL", "DOORS_LIGHTS", 1), + ("DOOR_OPEN_FR", "DOORS_LIGHTS"), + ("DOOR_OPEN_FL", "DOORS_LIGHTS"), + ("DOOR_OPEN_RR", "DOORS_LIGHTS"), + ("DOOR_OPEN_RL", "DOORS_LIGHTS"), - ("RIGHT_BLINKER", "LIGHTS", 0), - ("LEFT_BLINKER", "LIGHTS", 0), + ("RIGHT_BLINKER", "LIGHTS"), + ("LEFT_BLINKER", "LIGHTS"), - ("ESP_DISABLED", "ESP", 0), + ("ESP_DISABLED", "ESP"), - ("GEAR_SHIFTER", "GEARBOX", 0), + ("GEAR_SHIFTER", "GEARBOX"), ] checks = [ @@ -155,26 +154,26 @@ class CarState(CarStateBase): if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL, CAR.ALTIMA): signals += [ - ("USER_BRAKE_PRESSED", "DOORS_LIGHTS", 1), - - ("GAS_PEDAL", "GAS_PEDAL", 0), - ("SEATBELT_DRIVER_LATCHED", "HUD", 0), - ("SPEED_MPH", "HUD", 0), - - ("PROPILOT_BUTTON", "CRUISE_THROTTLE", 0), - ("CANCEL_BUTTON", "CRUISE_THROTTLE", 0), - ("GAS_PEDAL_INVERTED", "CRUISE_THROTTLE", 0), - ("SET_BUTTON", "CRUISE_THROTTLE", 0), - ("RES_BUTTON", "CRUISE_THROTTLE", 0), - ("FOLLOW_DISTANCE_BUTTON", "CRUISE_THROTTLE", 0), - ("NO_BUTTON_PRESSED", "CRUISE_THROTTLE", 0), - ("GAS_PEDAL", "CRUISE_THROTTLE", 0), - ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE", 0), - ("NEW_SIGNAL_2", "CRUISE_THROTTLE", 0), - ("GAS_PRESSED_INVERTED", "CRUISE_THROTTLE", 0), - ("unsure1", "CRUISE_THROTTLE", 0), - ("unsure2", "CRUISE_THROTTLE", 0), - ("unsure3", "CRUISE_THROTTLE", 0), + ("USER_BRAKE_PRESSED", "DOORS_LIGHTS"), + + ("GAS_PEDAL", "GAS_PEDAL"), + ("SEATBELT_DRIVER_LATCHED", "HUD"), + ("SPEED_MPH", "HUD"), + + ("PROPILOT_BUTTON", "CRUISE_THROTTLE"), + ("CANCEL_BUTTON", "CRUISE_THROTTLE"), + ("GAS_PEDAL_INVERTED", "CRUISE_THROTTLE"), + ("SET_BUTTON", "CRUISE_THROTTLE"), + ("RES_BUTTON", "CRUISE_THROTTLE"), + ("FOLLOW_DISTANCE_BUTTON", "CRUISE_THROTTLE"), + ("NO_BUTTON_PRESSED", "CRUISE_THROTTLE"), + ("GAS_PEDAL", "CRUISE_THROTTLE"), + ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE"), + ("NEW_SIGNAL_2", "CRUISE_THROTTLE"), + ("GAS_PRESSED_INVERTED", "CRUISE_THROTTLE"), + ("unsure1", "CRUISE_THROTTLE"), + ("unsure2", "CRUISE_THROTTLE"), + ("unsure3", "CRUISE_THROTTLE"), ] checks += [ @@ -185,17 +184,17 @@ class CarState(CarStateBase): elif CP.carFingerprint in (CAR.LEAF, CAR.LEAF_IC): signals += [ - ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE", 1), - ("GAS_PEDAL", "CRUISE_THROTTLE", 0), - ("CRUISE_AVAILABLE", "CRUISE_THROTTLE", 0), - ("SPEED_MPH", "HUD_SETTINGS", 0), - ("SEATBELT_DRIVER_LATCHED", "SEATBELT", 0), + ("USER_BRAKE_PRESSED", "CRUISE_THROTTLE"), + ("GAS_PEDAL", "CRUISE_THROTTLE"), + ("CRUISE_AVAILABLE", "CRUISE_THROTTLE"), + ("SPEED_MPH", "HUD_SETTINGS"), + ("SEATBELT_DRIVER_LATCHED", "SEATBELT"), # Copy other values, we use this to cancel - ("CANCEL_SEATBELT", "CANCEL_MSG", 0), - ("NEW_SIGNAL_1", "CANCEL_MSG", 0), - ("NEW_SIGNAL_2", "CANCEL_MSG", 0), - ("NEW_SIGNAL_3", "CANCEL_MSG", 0), + ("CANCEL_SEATBELT", "CANCEL_MSG"), + ("NEW_SIGNAL_1", "CANCEL_MSG"), + ("NEW_SIGNAL_2", "CANCEL_MSG"), + ("NEW_SIGNAL_3", "CANCEL_MSG"), ] checks += [ ("BRAKE_PEDAL", 100), @@ -207,9 +206,9 @@ class CarState(CarStateBase): if CP.carFingerprint == CAR.ALTIMA: signals += [ - ("LKAS_ENABLED", "LKAS_SETTINGS", 0), - ("CRUISE_ENABLED", "CRUISE_STATE", 0), - ("SET_SPEED", "PROPILOT_HUD", 0), + ("LKAS_ENABLED", "LKAS_SETTINGS"), + ("CRUISE_ENABLED", "CRUISE_STATE"), + ("SET_SPEED", "PROPILOT_HUD"), ] checks += [ ("CRUISE_STATE", 10), @@ -218,12 +217,8 @@ class CarState(CarStateBase): ] return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) - signals += [ - ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), - ] - checks += [ - ("STEER_TORQUE_SENSOR", 100), - ] + signals.append(("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR")) + checks.append(("STEER_TORQUE_SENSOR", 100)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @@ -233,14 +228,14 @@ class CarState(CarStateBase): if CP.carFingerprint == CAR.ALTIMA: signals = [ - ("DESIRED_ANGLE", "LKAS", 0), - ("SET_0x80_2", "LKAS", 0), - ("MAX_TORQUE", "LKAS", 0), - ("SET_0x80", "LKAS", 0), - ("COUNTER", "LKAS", 0), - ("LKA_ACTIVE", "LKAS", 0), - - ("CRUISE_ON", "PRO_PILOT", 0), + ("DESIRED_ANGLE", "LKAS"), + ("SET_0x80_2", "LKAS"), + ("MAX_TORQUE", "LKAS"), + ("SET_0x80", "LKAS"), + ("COUNTER", "LKAS"), + ("LKA_ACTIVE", "LKAS"), + + ("CRUISE_ON", "PRO_PILOT"), ] checks = [ ("LKAS", 100), @@ -248,85 +243,85 @@ class CarState(CarStateBase): ] else: signals = [ - # sig_name, sig_address, default - ("LKAS_ENABLED", "LKAS_SETTINGS", 0), + # sig_name, sig_address + ("LKAS_ENABLED", "LKAS_SETTINGS"), - ("CRUISE_ENABLED", "CRUISE_STATE", 0), + ("CRUISE_ENABLED", "CRUISE_STATE"), - ("DESIRED_ANGLE", "LKAS", 0), - ("SET_0x80_2", "LKAS", 0), - ("MAX_TORQUE", "LKAS", 0), - ("SET_0x80", "LKAS", 0), - ("COUNTER", "LKAS", 0), - ("LKA_ACTIVE", "LKAS", 0), + ("DESIRED_ANGLE", "LKAS"), + ("SET_0x80_2", "LKAS"), + ("MAX_TORQUE", "LKAS"), + ("SET_0x80", "LKAS"), + ("COUNTER", "LKAS"), + ("LKA_ACTIVE", "LKAS"), # Below are the HUD messages. We copy the stock message and modify - ("LARGE_WARNING_FLASHING", "PROPILOT_HUD", 0), - ("SIDE_RADAR_ERROR_FLASHING1", "PROPILOT_HUD", 0), - ("SIDE_RADAR_ERROR_FLASHING2", "PROPILOT_HUD", 0), - ("LEAD_CAR", "PROPILOT_HUD", 0), - ("LEAD_CAR_ERROR", "PROPILOT_HUD", 0), - ("FRONT_RADAR_ERROR", "PROPILOT_HUD", 0), - ("FRONT_RADAR_ERROR_FLASHING", "PROPILOT_HUD", 0), - ("SIDE_RADAR_ERROR_FLASHING3", "PROPILOT_HUD", 0), - ("LKAS_ERROR_FLASHING", "PROPILOT_HUD", 0), - ("SAFETY_SHIELD_ACTIVE", "PROPILOT_HUD", 0), - ("RIGHT_LANE_GREEN_FLASH", "PROPILOT_HUD", 0), - ("LEFT_LANE_GREEN_FLASH", "PROPILOT_HUD", 0), - ("FOLLOW_DISTANCE", "PROPILOT_HUD", 0), - ("AUDIBLE_TONE", "PROPILOT_HUD", 0), - ("SPEED_SET_ICON", "PROPILOT_HUD", 0), - ("SMALL_STEERING_WHEEL_ICON", "PROPILOT_HUD", 0), - ("unknown59", "PROPILOT_HUD", 0), - ("unknown55", "PROPILOT_HUD", 0), - ("unknown26", "PROPILOT_HUD", 0), - ("unknown28", "PROPILOT_HUD", 0), - ("unknown31", "PROPILOT_HUD", 0), - ("SET_SPEED", "PROPILOT_HUD", 0), - ("unknown43", "PROPILOT_HUD", 0), - ("unknown08", "PROPILOT_HUD", 0), - ("unknown05", "PROPILOT_HUD", 0), - ("unknown02", "PROPILOT_HUD", 0), - - ("NA_HIGH_ACCEL_TEMP", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_RADAR_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("LKAS_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("FRONT_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_RADAR_NA_CLEAN_REAR_CAMERA", "PROPILOT_HUD_INFO_MSG", 0), - ("NA_POOR_ROAD_CONDITIONS", "PROPILOT_HUD_INFO_MSG", 0), - ("CURRENTLY_UNAVAILABLE", "PROPILOT_HUD_INFO_MSG", 0), - ("SAFETY_SHIELD_OFF", "PROPILOT_HUD_INFO_MSG", 0), - ("FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_IMPACT_NA_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("WARNING_DO_NOT_ENTER", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_IMPACT_SYSTEM_OFF", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_IMPACT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("FRONT_COLLISION_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("SIDE_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0), - ("LKAS_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0), - ("FRONT_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG", 0), - ("PROPILOT_NA_MSGS", "PROPILOT_HUD_INFO_MSG", 0), - ("BOTTOM_MSG", "PROPILOT_HUD_INFO_MSG", 0), - ("HANDS_ON_WHEEL_WARNING", "PROPILOT_HUD_INFO_MSG", 0), - ("WARNING_STEP_ON_BRAKE_NOW", "PROPILOT_HUD_INFO_MSG", 0), - ("PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED", "PROPILOT_HUD_INFO_MSG", 0), - ("PROPILOT_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0), - ("WARNING_PROPILOT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG", 0), - ("ACC_UNAVAILABLE_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG", 0), - ("ACC_NA_FRONT_CAMERA_IMPARED", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown07", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown10", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown15", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown23", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown19", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown31", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown32", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown46", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown61", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown55", "PROPILOT_HUD_INFO_MSG", 0), - ("unknown50", "PROPILOT_HUD_INFO_MSG", 0), + ("LARGE_WARNING_FLASHING", "PROPILOT_HUD"), + ("SIDE_RADAR_ERROR_FLASHING1", "PROPILOT_HUD"), + ("SIDE_RADAR_ERROR_FLASHING2", "PROPILOT_HUD"), + ("LEAD_CAR", "PROPILOT_HUD"), + ("LEAD_CAR_ERROR", "PROPILOT_HUD"), + ("FRONT_RADAR_ERROR", "PROPILOT_HUD"), + ("FRONT_RADAR_ERROR_FLASHING", "PROPILOT_HUD"), + ("SIDE_RADAR_ERROR_FLASHING3", "PROPILOT_HUD"), + ("LKAS_ERROR_FLASHING", "PROPILOT_HUD"), + ("SAFETY_SHIELD_ACTIVE", "PROPILOT_HUD"), + ("RIGHT_LANE_GREEN_FLASH", "PROPILOT_HUD"), + ("LEFT_LANE_GREEN_FLASH", "PROPILOT_HUD"), + ("FOLLOW_DISTANCE", "PROPILOT_HUD"), + ("AUDIBLE_TONE", "PROPILOT_HUD"), + ("SPEED_SET_ICON", "PROPILOT_HUD"), + ("SMALL_STEERING_WHEEL_ICON", "PROPILOT_HUD"), + ("unknown59", "PROPILOT_HUD"), + ("unknown55", "PROPILOT_HUD"), + ("unknown26", "PROPILOT_HUD"), + ("unknown28", "PROPILOT_HUD"), + ("unknown31", "PROPILOT_HUD"), + ("SET_SPEED", "PROPILOT_HUD"), + ("unknown43", "PROPILOT_HUD"), + ("unknown08", "PROPILOT_HUD"), + ("unknown05", "PROPILOT_HUD"), + ("unknown02", "PROPILOT_HUD"), + + ("NA_HIGH_ACCEL_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("LKAS_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_RADAR_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_NA_CLEAN_REAR_CAMERA", "PROPILOT_HUD_INFO_MSG"), + ("NA_POOR_ROAD_CONDITIONS", "PROPILOT_HUD_INFO_MSG"), + ("CURRENTLY_UNAVAILABLE", "PROPILOT_HUD_INFO_MSG"), + ("SAFETY_SHIELD_OFF", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_COLLISION_NA_FRONT_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG"), + ("PEDAL_MISSAPPLICATION_SYSTEM_ACTIVATED", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_IMPACT_NA_RADAR_OBSTRUCTION", "PROPILOT_HUD_INFO_MSG"), + ("WARNING_DO_NOT_ENTER", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_IMPACT_SYSTEM_OFF", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_IMPACT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_COLLISION_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("SIDE_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), + ("LKAS_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), + ("FRONT_RADAR_MALFUNCTION2", "PROPILOT_HUD_INFO_MSG"), + ("PROPILOT_NA_MSGS", "PROPILOT_HUD_INFO_MSG"), + ("BOTTOM_MSG", "PROPILOT_HUD_INFO_MSG"), + ("HANDS_ON_WHEEL_WARNING", "PROPILOT_HUD_INFO_MSG"), + ("WARNING_STEP_ON_BRAKE_NOW", "PROPILOT_HUD_INFO_MSG"), + ("PROPILOT_NA_FRONT_CAMERA_OBSTRUCTED", "PROPILOT_HUD_INFO_MSG"), + ("PROPILOT_NA_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("WARNING_PROPILOT_MALFUNCTION", "PROPILOT_HUD_INFO_MSG"), + ("ACC_UNAVAILABLE_HIGH_CABIN_TEMP", "PROPILOT_HUD_INFO_MSG"), + ("ACC_NA_FRONT_CAMERA_IMPARED", "PROPILOT_HUD_INFO_MSG"), + ("unknown07", "PROPILOT_HUD_INFO_MSG"), + ("unknown10", "PROPILOT_HUD_INFO_MSG"), + ("unknown15", "PROPILOT_HUD_INFO_MSG"), + ("unknown23", "PROPILOT_HUD_INFO_MSG"), + ("unknown19", "PROPILOT_HUD_INFO_MSG"), + ("unknown31", "PROPILOT_HUD_INFO_MSG"), + ("unknown32", "PROPILOT_HUD_INFO_MSG"), + ("unknown46", "PROPILOT_HUD_INFO_MSG"), + ("unknown61", "PROPILOT_HUD_INFO_MSG"), + ("unknown55", "PROPILOT_HUD_INFO_MSG"), + ("unknown50", "PROPILOT_HUD_INFO_MSG"), ] checks = [ @@ -345,20 +340,11 @@ class CarState(CarStateBase): checks = [] if CP.carFingerprint in (CAR.ROGUE, CAR.XTRAIL): - signals += [ - ("CRUISE_ON", "PRO_PILOT", 0), - ] - checks += [ - ("PRO_PILOT", 100), - ] + signals.append(("CRUISE_ON", "PRO_PILOT")) + checks.append(("PRO_PILOT", 100)) elif CP.carFingerprint == CAR.ALTIMA: - signals += [ - ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), - ] - checks += [ - ("STEER_TORQUE_SENSOR", 100), - ] + signals.append(("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR")) + checks.append(("STEER_TORQUE_SENSOR", 100)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) - return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 1) diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py index 4350fb5447..c32fb13780 100644 --- a/selfdrive/car/nissan/interface.py +++ b/selfdrive/car/nissan/interface.py @@ -16,7 +16,7 @@ class CarInterface(CarInterfaceBase): ret.carName = "nissan" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)] - ret.steerLimitAlert = False + ret.steerLimitTimer = 1.0 ret.steerRateCost = 0.5 ret.steerActuatorDelay = 0.1 @@ -79,7 +79,7 @@ class CarInterface(CarInterfaceBase): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, + ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart) diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index 72b07f9192..afc91f4755 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -15,7 +15,7 @@ class CarController(): self.p = CarControllerParams(CP) self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) - def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): + def update(self, c, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line, left_lane_depart, right_lane_depart): can_sends = [] @@ -30,7 +30,7 @@ class CarController(): apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p) self.steer_rate_limited = new_steer != apply_steer - if not enabled: + if not c.active: apply_steer = 0 if CS.CP.carFingerprint in PREGLOBAL_CARS: diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index b6eb54287f..1eefb18584 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -77,28 +77,27 @@ class CarState(CarStateBase): @staticmethod def get_can_parser(CP): - # this function generates lists for signal, messages and initial values signals = [ - # sig_name, sig_address, default - ("Steer_Torque_Sensor", "Steering_Torque", 0), - ("Steering_Angle", "Steering_Torque", 0), - ("Steer_Error_1", "Steering_Torque", 0), - ("Cruise_On", "CruiseControl", 0), - ("Cruise_Activated", "CruiseControl", 0), - ("Brake_Pedal", "Brake_Pedal", 0), - ("Throttle_Pedal", "Throttle", 0), - ("LEFT_BLINKER", "Dashlights", 0), - ("RIGHT_BLINKER", "Dashlights", 0), - ("SEATBELT_FL", "Dashlights", 0), - ("FL", "Wheel_Speeds", 0), - ("FR", "Wheel_Speeds", 0), - ("RL", "Wheel_Speeds", 0), - ("RR", "Wheel_Speeds", 0), - ("DOOR_OPEN_FR", "BodyInfo", 1), - ("DOOR_OPEN_FL", "BodyInfo", 1), - ("DOOR_OPEN_RR", "BodyInfo", 1), - ("DOOR_OPEN_RL", "BodyInfo", 1), - ("Gear", "Transmission", 0), + # sig_name, sig_address + ("Steer_Torque_Sensor", "Steering_Torque"), + ("Steering_Angle", "Steering_Torque"), + ("Steer_Error_1", "Steering_Torque"), + ("Cruise_On", "CruiseControl"), + ("Cruise_Activated", "CruiseControl"), + ("Brake_Pedal", "Brake_Pedal"), + ("Throttle_Pedal", "Throttle"), + ("LEFT_BLINKER", "Dashlights"), + ("RIGHT_BLINKER", "Dashlights"), + ("SEATBELT_FL", "Dashlights"), + ("FL", "Wheel_Speeds"), + ("FR", "Wheel_Speeds"), + ("RL", "Wheel_Speeds"), + ("RR", "Wheel_Speeds"), + ("DOOR_OPEN_FR", "BodyInfo"), + ("DOOR_OPEN_FL", "BodyInfo"), + ("DOOR_OPEN_RR", "BodyInfo"), + ("DOOR_OPEN_RL", "BodyInfo"), + ("Gear", "Transmission"), ] checks = [ @@ -114,20 +113,18 @@ class CarState(CarStateBase): if CP.enableBsm: signals += [ - ("L_ADJACENT", "BSD_RCTA", 0), - ("R_ADJACENT", "BSD_RCTA", 0), - ("L_APPROACHING", "BSD_RCTA", 0), - ("R_APPROACHING", "BSD_RCTA", 0), - ] - checks += [ - ("BSD_RCTA", 17), + ("L_ADJACENT", "BSD_RCTA"), + ("R_ADJACENT", "BSD_RCTA"), + ("L_APPROACHING", "BSD_RCTA"), + ("R_APPROACHING", "BSD_RCTA"), ] + checks.append(("BSD_RCTA", 17)) if CP.carFingerprint not in PREGLOBAL_CARS: signals += [ - ("Steer_Warning", "Steering_Torque", 0), - ("Brake", "Brake_Status", 0), - ("UNITS", "Dashlights", 0), + ("Steer_Warning", "Steering_Torque"), + ("Brake", "Brake_Status"), + ("UNITS", "Dashlights"), ] checks += [ @@ -137,13 +134,9 @@ class CarState(CarStateBase): ("CruiseControl", 20), ] else: - signals += [ - ("UNITS", "Dash_State2", 0), - ] + signals.append(("UNITS", "Dash_State2")) - checks += [ - ("Dash_State2", 1), - ] + checks.append(("Dash_State2", 1)) if CP.carFingerprint == CAR.FORESTER_PREGLOBAL: checks += [ @@ -164,26 +157,26 @@ class CarState(CarStateBase): def get_cam_can_parser(CP): if CP.carFingerprint in PREGLOBAL_CARS: signals = [ - ("Cruise_Set_Speed", "ES_DashStatus", 0), - ("Not_Ready_Startup", "ES_DashStatus", 0), - - ("Cruise_Throttle", "ES_Distance", 0), - ("Signal1", "ES_Distance", 0), - ("Car_Follow", "ES_Distance", 0), - ("Signal2", "ES_Distance", 0), - ("Brake_On", "ES_Distance", 0), - ("Distance_Swap", "ES_Distance", 0), - ("Standstill", "ES_Distance", 0), - ("Signal3", "ES_Distance", 0), - ("Close_Distance", "ES_Distance", 0), - ("Signal4", "ES_Distance", 0), - ("Standstill_2", "ES_Distance", 0), - ("Cruise_Fault", "ES_Distance", 0), - ("Signal5", "ES_Distance", 0), - ("Counter", "ES_Distance", 0), - ("Signal6", "ES_Distance", 0), - ("Cruise_Button", "ES_Distance", 0), - ("Signal7", "ES_Distance", 0), + ("Cruise_Set_Speed", "ES_DashStatus"), + ("Not_Ready_Startup", "ES_DashStatus"), + + ("Cruise_Throttle", "ES_Distance"), + ("Signal1", "ES_Distance"), + ("Car_Follow", "ES_Distance"), + ("Signal2", "ES_Distance"), + ("Brake_On", "ES_Distance"), + ("Distance_Swap", "ES_Distance"), + ("Standstill", "ES_Distance"), + ("Signal3", "ES_Distance"), + ("Close_Distance", "ES_Distance"), + ("Signal4", "ES_Distance"), + ("Standstill_2", "ES_Distance"), + ("Cruise_Fault", "ES_Distance"), + ("Signal5", "ES_Distance"), + ("Counter", "ES_Distance"), + ("Signal6", "ES_Distance"), + ("Cruise_Button", "ES_Distance"), + ("Signal7", "ES_Distance"), ] checks = [ @@ -192,42 +185,42 @@ class CarState(CarStateBase): ] else: signals = [ - ("Cruise_Set_Speed", "ES_DashStatus", 0), - ("Conventional_Cruise", "ES_DashStatus", 0), - - ("Counter", "ES_Distance", 0), - ("Signal1", "ES_Distance", 0), - ("Cruise_Fault", "ES_Distance", 0), - ("Cruise_Throttle", "ES_Distance", 0), - ("Signal2", "ES_Distance", 0), - ("Car_Follow", "ES_Distance", 0), - ("Signal3", "ES_Distance", 0), - ("Cruise_Brake_Active", "ES_Distance", 0), - ("Distance_Swap", "ES_Distance", 0), - ("Cruise_EPB", "ES_Distance", 0), - ("Signal4", "ES_Distance", 0), - ("Close_Distance", "ES_Distance", 0), - ("Signal5", "ES_Distance", 0), - ("Cruise_Cancel", "ES_Distance", 0), - ("Cruise_Set", "ES_Distance", 0), - ("Cruise_Resume", "ES_Distance", 0), - ("Signal6", "ES_Distance", 0), - - ("Counter", "ES_LKAS_State", 0), - ("LKAS_Alert_Msg", "ES_LKAS_State", 0), - ("Signal1", "ES_LKAS_State", 0), - ("LKAS_ACTIVE", "ES_LKAS_State", 0), - ("LKAS_Dash_State", "ES_LKAS_State", 0), - ("Signal2", "ES_LKAS_State", 0), - ("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0), - ("LKAS_Left_Line_Enable", "ES_LKAS_State", 0), - ("LKAS_Left_Line_Light_Blink", "ES_LKAS_State", 0), - ("LKAS_Right_Line_Enable", "ES_LKAS_State", 0), - ("LKAS_Right_Line_Light_Blink", "ES_LKAS_State", 0), - ("LKAS_Left_Line_Visible", "ES_LKAS_State", 0), - ("LKAS_Right_Line_Visible", "ES_LKAS_State", 0), - ("LKAS_Alert", "ES_LKAS_State", 0), - ("Signal3", "ES_LKAS_State", 0), + ("Cruise_Set_Speed", "ES_DashStatus"), + ("Conventional_Cruise", "ES_DashStatus"), + + ("Counter", "ES_Distance"), + ("Signal1", "ES_Distance"), + ("Cruise_Fault", "ES_Distance"), + ("Cruise_Throttle", "ES_Distance"), + ("Signal2", "ES_Distance"), + ("Car_Follow", "ES_Distance"), + ("Signal3", "ES_Distance"), + ("Cruise_Brake_Active", "ES_Distance"), + ("Distance_Swap", "ES_Distance"), + ("Cruise_EPB", "ES_Distance"), + ("Signal4", "ES_Distance"), + ("Close_Distance", "ES_Distance"), + ("Signal5", "ES_Distance"), + ("Cruise_Cancel", "ES_Distance"), + ("Cruise_Set", "ES_Distance"), + ("Cruise_Resume", "ES_Distance"), + ("Signal6", "ES_Distance"), + + ("Counter", "ES_LKAS_State"), + ("LKAS_Alert_Msg", "ES_LKAS_State"), + ("Signal1", "ES_LKAS_State"), + ("LKAS_ACTIVE", "ES_LKAS_State"), + ("LKAS_Dash_State", "ES_LKAS_State"), + ("Signal2", "ES_LKAS_State"), + ("Backward_Speed_Limit_Menu", "ES_LKAS_State"), + ("LKAS_Left_Line_Enable", "ES_LKAS_State"), + ("LKAS_Left_Line_Light_Blink", "ES_LKAS_State"), + ("LKAS_Right_Line_Enable", "ES_LKAS_State"), + ("LKAS_Right_Line_Light_Blink", "ES_LKAS_State"), + ("LKAS_Left_Line_Visible", "ES_LKAS_State"), + ("LKAS_Right_Line_Visible", "ES_LKAS_State"), + ("LKAS_Alert", "ES_LKAS_State"), + ("Signal3", "ES_LKAS_State"), ] checks = [ diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index 6c8659b4c0..8c6d188643 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -123,7 +123,7 @@ class CarInterface(CarInterfaceBase): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, + ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, hud_control.leftLaneDepart, hud_control.rightLaneDepart) self.frame += 1 diff --git a/selfdrive/car/tesla/carcontroller.py b/selfdrive/car/tesla/carcontroller.py index 0b58632f0a..03f09f2407 100644 --- a/selfdrive/car/tesla/carcontroller.py +++ b/selfdrive/car/tesla/carcontroller.py @@ -12,12 +12,12 @@ class CarController(): self.pt_packer = CANPacker(DBC[CP.carFingerprint]['pt']) self.tesla_can = TeslaCAN(self.packer, self.pt_packer) - def update(self, enabled, CS, frame, actuators, cruise_cancel): + def update(self, c, enabled, CS, frame, actuators, cruise_cancel): can_sends = [] # Temp disable steering on a hands_on_fault, and allow for user override hands_on_fault = (CS.steer_warning == "EAC_ERROR_HANDS_ON" and CS.hands_on_level >= 3) - lkas_enabled = enabled and (not hands_on_fault) + lkas_enabled = c.active and (not hands_on_fault) if lkas_enabled: apply_angle = actuators.steeringAngleDeg @@ -65,4 +65,4 @@ class CarController(): new_actuators = actuators.copy() new_actuators.steeringAngleDeg = apply_angle - return actuators, can_sends + return new_actuators, can_sends diff --git a/selfdrive/car/tesla/carstate.py b/selfdrive/car/tesla/carstate.py index 39869baeae..51ae43ad1b 100644 --- a/selfdrive/car/tesla/carstate.py +++ b/selfdrive/car/tesla/carstate.py @@ -96,64 +96,64 @@ class CarState(CarStateBase): @staticmethod def get_can_parser(CP): signals = [ - # sig_name, sig_address, default - ("ESP_vehicleSpeed", "ESP_B", 0), - ("DI_pedalPos", "DI_torque1", 0), - ("DI_brakePedal", "DI_torque2", 0), - ("StW_AnglHP", "STW_ANGLHP_STAT", 0), - ("StW_AnglHP_Spd", "STW_ANGLHP_STAT", 0), - ("EPAS_handsOnLevel", "EPAS_sysStatus", 0), - ("EPAS_torsionBarTorque", "EPAS_sysStatus", 0), - ("EPAS_internalSAS", "EPAS_sysStatus", 0), - ("EPAS_eacStatus", "EPAS_sysStatus", 1), - ("EPAS_eacErrorCode", "EPAS_sysStatus", 0), - ("DI_cruiseState", "DI_state", 0), - ("DI_digitalSpeed", "DI_state", 0), - ("DI_speedUnits", "DI_state", 0), - ("DI_gear", "DI_torque2", 0), - ("DOOR_STATE_FL", "GTW_carState", 1), - ("DOOR_STATE_FR", "GTW_carState", 1), - ("DOOR_STATE_RL", "GTW_carState", 1), - ("DOOR_STATE_RR", "GTW_carState", 1), - ("DOOR_STATE_FrontTrunk", "GTW_carState", 1), - ("BOOT_STATE", "GTW_carState", 1), - ("BC_indicatorLStatus", "GTW_carState", 1), - ("BC_indicatorRStatus", "GTW_carState", 1), - ("SDM_bcklDrivStatus", "SDM1", 0), - ("driverBrakeStatus", "BrakeMessage", 0), + # sig_name, sig_address + ("ESP_vehicleSpeed", "ESP_B"), + ("DI_pedalPos", "DI_torque1"), + ("DI_brakePedal", "DI_torque2"), + ("StW_AnglHP", "STW_ANGLHP_STAT"), + ("StW_AnglHP_Spd", "STW_ANGLHP_STAT"), + ("EPAS_handsOnLevel", "EPAS_sysStatus"), + ("EPAS_torsionBarTorque", "EPAS_sysStatus"), + ("EPAS_internalSAS", "EPAS_sysStatus"), + ("EPAS_eacStatus", "EPAS_sysStatus"), + ("EPAS_eacErrorCode", "EPAS_sysStatus"), + ("DI_cruiseState", "DI_state"), + ("DI_digitalSpeed", "DI_state"), + ("DI_speedUnits", "DI_state"), + ("DI_gear", "DI_torque2"), + ("DOOR_STATE_FL", "GTW_carState"), + ("DOOR_STATE_FR", "GTW_carState"), + ("DOOR_STATE_RL", "GTW_carState"), + ("DOOR_STATE_RR", "GTW_carState"), + ("DOOR_STATE_FrontTrunk", "GTW_carState"), + ("BOOT_STATE", "GTW_carState"), + ("BC_indicatorLStatus", "GTW_carState"), + ("BC_indicatorRStatus", "GTW_carState"), + ("SDM_bcklDrivStatus", "SDM1"), + ("driverBrakeStatus", "BrakeMessage"), # We copy this whole message when spamming cancel - ("SpdCtrlLvr_Stat", "STW_ACTN_RQ", 0), - ("VSL_Enbl_Rq", "STW_ACTN_RQ", 0), - ("SpdCtrlLvrStat_Inv", "STW_ACTN_RQ", 0), - ("DTR_Dist_Rq", "STW_ACTN_RQ", 0), - ("TurnIndLvr_Stat", "STW_ACTN_RQ", 0), - ("HiBmLvr_Stat", "STW_ACTN_RQ", 0), - ("WprWashSw_Psd", "STW_ACTN_RQ", 0), - ("WprWash_R_Sw_Posn_V2", "STW_ACTN_RQ", 0), - ("StW_Lvr_Stat", "STW_ACTN_RQ", 0), - ("StW_Cond_Flt", "STW_ACTN_RQ", 0), - ("StW_Cond_Psd", "STW_ACTN_RQ", 0), - ("HrnSw_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw00_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw01_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw02_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw03_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw04_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw05_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw06_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw07_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw08_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw09_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw10_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw11_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw12_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw13_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw14_Psd", "STW_ACTN_RQ", 0), - ("StW_Sw15_Psd", "STW_ACTN_RQ", 0), - ("WprSw6Posn", "STW_ACTN_RQ", 0), - ("MC_STW_ACTN_RQ", "STW_ACTN_RQ", 0), - ("CRC_STW_ACTN_RQ", "STW_ACTN_RQ", 0), + ("SpdCtrlLvr_Stat", "STW_ACTN_RQ"), + ("VSL_Enbl_Rq", "STW_ACTN_RQ"), + ("SpdCtrlLvrStat_Inv", "STW_ACTN_RQ"), + ("DTR_Dist_Rq", "STW_ACTN_RQ"), + ("TurnIndLvr_Stat", "STW_ACTN_RQ"), + ("HiBmLvr_Stat", "STW_ACTN_RQ"), + ("WprWashSw_Psd", "STW_ACTN_RQ"), + ("WprWash_R_Sw_Posn_V2", "STW_ACTN_RQ"), + ("StW_Lvr_Stat", "STW_ACTN_RQ"), + ("StW_Cond_Flt", "STW_ACTN_RQ"), + ("StW_Cond_Psd", "STW_ACTN_RQ"), + ("HrnSw_Psd", "STW_ACTN_RQ"), + ("StW_Sw00_Psd", "STW_ACTN_RQ"), + ("StW_Sw01_Psd", "STW_ACTN_RQ"), + ("StW_Sw02_Psd", "STW_ACTN_RQ"), + ("StW_Sw03_Psd", "STW_ACTN_RQ"), + ("StW_Sw04_Psd", "STW_ACTN_RQ"), + ("StW_Sw05_Psd", "STW_ACTN_RQ"), + ("StW_Sw06_Psd", "STW_ACTN_RQ"), + ("StW_Sw07_Psd", "STW_ACTN_RQ"), + ("StW_Sw08_Psd", "STW_ACTN_RQ"), + ("StW_Sw09_Psd", "STW_ACTN_RQ"), + ("StW_Sw10_Psd", "STW_ACTN_RQ"), + ("StW_Sw11_Psd", "STW_ACTN_RQ"), + ("StW_Sw12_Psd", "STW_ACTN_RQ"), + ("StW_Sw13_Psd", "STW_ACTN_RQ"), + ("StW_Sw14_Psd", "STW_ACTN_RQ"), + ("StW_Sw15_Psd", "STW_ACTN_RQ"), + ("WprSw6Posn", "STW_ACTN_RQ"), + ("MC_STW_ACTN_RQ", "STW_ACTN_RQ"), + ("CRC_STW_ACTN_RQ", "STW_ACTN_RQ"), ] checks = [ @@ -175,8 +175,8 @@ class CarState(CarStateBase): @staticmethod def get_cam_can_parser(CP): signals = [ - # sig_name, sig_address, default - ("DAS_accState", "DAS_control", 0), + # sig_name, sig_address + ("DAS_accState", "DAS_control"), ] checks = [ # sig_address, frequency diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py index e8d6ab6854..03012bc52e 100755 --- a/selfdrive/car/tesla/interface.py +++ b/selfdrive/car/tesla/interface.py @@ -40,6 +40,7 @@ class CarInterface(CarInterfaceBase): ret.openpilotLongitudinalControl = False ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla, 0)] + ret.steerLimitTimer = 1.0 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.5 @@ -70,6 +71,6 @@ class CarInterface(CarInterfaceBase): return self.CS.out def apply(self, c): - ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel) + ret = self.CC.update(c, c.enabled, self.CS, self.frame, c.actuators, c.cruiseControl.cancel) self.frame += 1 return ret diff --git a/selfdrive/car/tesla/radar_interface.py b/selfdrive/car/tesla/radar_interface.py index f5ad12ba7e..a09f53e758 100755 --- a/selfdrive/car/tesla/radar_interface.py +++ b/selfdrive/car/tesla/radar_interface.py @@ -11,9 +11,9 @@ NUM_POINTS = len(RADAR_MSGS_A) def get_radar_can_parser(CP): # Status messages signals = [ - ('RADC_HWFail', 'TeslaRadarSguInfo', 0), - ('RADC_SGUFail', 'TeslaRadarSguInfo', 0), - ('RADC_SensorDirty', 'TeslaRadarSguInfo', 0), + ('RADC_HWFail', 'TeslaRadarSguInfo'), + ('RADC_SGUFail', 'TeslaRadarSguInfo'), + ('RADC_SensorDirty', 'TeslaRadarSguInfo'), ] checks = [ @@ -29,16 +29,16 @@ def get_radar_can_parser(CP): # There is a bunch more info in the messages, # but these are the only things actually used in openpilot signals.extend([ - ('LongDist', msg_id_a, 255), - ('LongSpeed', msg_id_a, 0), - ('LatDist', msg_id_a, 0), - ('LongAccel', msg_id_a, 0), - ('Meas', msg_id_a, 0), - ('Tracked', msg_id_a, 0), - ('Index', msg_id_a, 0), - - ('LatSpeed', msg_id_b, 0), - ('Index2', msg_id_b, 0), + ('LongDist', msg_id_a), + ('LongSpeed', msg_id_a), + ('LatDist', msg_id_a), + ('LongAccel', msg_id_a), + ('Meas', msg_id_a), + ('Tracked', msg_id_a), + ('Index', msg_id_a), + + ('LatSpeed', msg_id_b), + ('Index2', msg_id_b), ]) checks.extend([ diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 73ea2f350f..87ba0055f0 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -26,7 +26,7 @@ class CarController(): left_line, right_line, lead, left_lane_depart, right_lane_depart): # gas and brake - if CS.CP.enableGasInterceptor and enabled: + if CS.CP.enableGasInterceptor and active: MAX_INTERCEPTOR_GAS = 0.5 # RAV4 has very sensitive gas pedal if CS.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH): @@ -49,7 +49,7 @@ class CarController(): self.steer_rate_limited = new_steer != apply_steer # Cut steering while we're in a known fault state (2s) - if not enabled or CS.steer_state in (9, 25): + if not active or CS.steer_state in (9, 25): apply_steer = 0 apply_steer_req = 0 else: @@ -106,7 +106,7 @@ class CarController(): can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, frame // 2)) self.gas = interceptor_gas_cmd - # ui mesg is at 100Hz but we send asap if: + # ui mesg is at 1Hz but we send asap if: # - there is something to display # - there is something to stop displaying fcw_alert = hud_alert == VisualAlert.fcw diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 16d51bfef0..d73460ef32 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -6,7 +6,7 @@ from opendbc.can.can_define import CANDefine from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from selfdrive.config import Conversions as CV -from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR +from selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, TSS2_CAR, EPS_SCALE class CarState(CarStateBase): @@ -14,6 +14,7 @@ class CarState(CarStateBase): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) self.shifter_values = can_define.dv["GEAR_PACKET"]["GEAR"] + self.eps_torque_scale = EPS_SCALE[CP.carFingerprint] / 100. # On cars with cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] # the signal is zeroed to where the steering angle is at start. @@ -38,7 +39,9 @@ class CarState(CarStateBase): ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2. ret.gasPressed = ret.gas > 15 else: - ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"] + # TODO: find a new, common signal + msg = "GAS_PEDAL_HYBRID" if (self.CP.flags & ToyotaFlags.HYBRID) else "GAS_PEDAL" + ret.gas = cp.vl[msg]["GAS_PEDAL"] ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 ret.wheelSpeeds = self.get_wheel_speeds( @@ -61,7 +64,7 @@ class CarState(CarStateBase): if self.accurate_steer_angle_seen: # Offset seems to be invalid for large steering angles - if abs(ret.steeringAngleDeg) < 90: + if abs(ret.steeringAngleDeg) < 90 and cp.can_valid: self.angle_offset.update(torque_sensor_angle_deg - ret.steeringAngleDeg) if self.angle_offset.initialized: @@ -76,7 +79,7 @@ class CarState(CarStateBase): ret.rightBlinker = cp.vl["BLINKERS_STATE"]["TURN_SIGNALS"] == 2 ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_DRIVER"] - ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] + ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]["STEER_TORQUE_EPS"] * self.eps_torque_scale # we could use the override bit from dbc, but it's triggered at too high torque values ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD ret.steerWarning = cp.vl["EPS_STATUS"]["LKA_STATE"] not in (1, 5) @@ -124,35 +127,33 @@ class CarState(CarStateBase): @staticmethod def get_can_parser(CP): - signals = [ - # sig_name, sig_address, default - ("STEER_ANGLE", "STEER_ANGLE_SENSOR", 0), - ("GEAR", "GEAR_PACKET", 0), - ("BRAKE_PRESSED", "BRAKE_MODULE", 0), - ("GAS_PEDAL", "GAS_PEDAL", 0), - ("WHEEL_SPEED_FL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_FR", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RL", "WHEEL_SPEEDS", 0), - ("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0), - ("DOOR_OPEN_FL", "BODY_CONTROL_STATE", 1), - ("DOOR_OPEN_FR", "BODY_CONTROL_STATE", 1), - ("DOOR_OPEN_RL", "BODY_CONTROL_STATE", 1), - ("DOOR_OPEN_RR", "BODY_CONTROL_STATE", 1), - ("SEATBELT_DRIVER_UNLATCHED", "BODY_CONTROL_STATE", 1), - ("TC_DISABLED", "ESP_CONTROL", 1), - ("BRAKE_HOLD_ACTIVE", "ESP_CONTROL", 1), - ("STEER_FRACTION", "STEER_ANGLE_SENSOR", 0), - ("STEER_RATE", "STEER_ANGLE_SENSOR", 0), - ("CRUISE_ACTIVE", "PCM_CRUISE", 0), - ("CRUISE_STATE", "PCM_CRUISE", 0), - ("GAS_RELEASED", "PCM_CRUISE", 1), - ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), - ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0), - ("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0), - ("TURN_SIGNALS", "BLINKERS_STATE", 3), # 3 is no blinkers - ("LKA_STATE", "EPS_STATUS", 0), - ("AUTO_HIGH_BEAM", "LIGHT_STALK", 0), + # sig_name, sig_address + ("STEER_ANGLE", "STEER_ANGLE_SENSOR"), + ("GEAR", "GEAR_PACKET"), + ("BRAKE_PRESSED", "BRAKE_MODULE"), + ("WHEEL_SPEED_FL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_FR", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RL", "WHEEL_SPEEDS"), + ("WHEEL_SPEED_RR", "WHEEL_SPEEDS"), + ("DOOR_OPEN_FL", "BODY_CONTROL_STATE"), + ("DOOR_OPEN_FR", "BODY_CONTROL_STATE"), + ("DOOR_OPEN_RL", "BODY_CONTROL_STATE"), + ("DOOR_OPEN_RR", "BODY_CONTROL_STATE"), + ("SEATBELT_DRIVER_UNLATCHED", "BODY_CONTROL_STATE"), + ("TC_DISABLED", "ESP_CONTROL"), + ("BRAKE_HOLD_ACTIVE", "ESP_CONTROL"), + ("STEER_FRACTION", "STEER_ANGLE_SENSOR"), + ("STEER_RATE", "STEER_ANGLE_SENSOR"), + ("CRUISE_ACTIVE", "PCM_CRUISE"), + ("CRUISE_STATE", "PCM_CRUISE"), + ("GAS_RELEASED", "PCM_CRUISE"), + ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR"), + ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR"), + ("STEER_ANGLE", "STEER_TORQUE_SENSOR"), + ("TURN_SIGNALS", "BLINKERS_STATE"), + ("LKA_STATE", "EPS_STATUS"), + ("AUTO_HIGH_BEAM", "LIGHT_STALK"), ] checks = [ @@ -163,48 +164,51 @@ class CarState(CarStateBase): ("ESP_CONTROL", 3), ("EPS_STATUS", 25), ("BRAKE_MODULE", 40), - ("GAS_PEDAL", 33), ("WHEEL_SPEEDS", 80), ("STEER_ANGLE_SENSOR", 80), ("PCM_CRUISE", 33), ("STEER_TORQUE_SENSOR", 50), ] + if CP.flags & ToyotaFlags.HYBRID: + signals.append(("GAS_PEDAL", "GAS_PEDAL_HYBRID")) + checks.append(("GAS_PEDAL_HYBRID", 33)) + else: + signals.append(("GAS_PEDAL", "GAS_PEDAL")) + checks.append(("GAS_PEDAL", 33)) + if CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC): - signals.append(("MAIN_ON", "DSU_CRUISE", 0)) - signals.append(("SET_SPEED", "DSU_CRUISE", 0)) + signals.append(("MAIN_ON", "DSU_CRUISE")) + signals.append(("SET_SPEED", "DSU_CRUISE")) checks.append(("DSU_CRUISE", 5)) else: - signals.append(("MAIN_ON", "PCM_CRUISE_2", 0)) - signals.append(("SET_SPEED", "PCM_CRUISE_2", 0)) - signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2", 0)) + signals.append(("MAIN_ON", "PCM_CRUISE_2")) + signals.append(("SET_SPEED", "PCM_CRUISE_2")) + signals.append(("LOW_SPEED_LOCKOUT", "PCM_CRUISE_2")) checks.append(("PCM_CRUISE_2", 33)) # add gas interceptor reading if we are using it if CP.enableGasInterceptor: - signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0)) - signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0)) + signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR")) + signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR")) checks.append(("GAS_SENSOR", 50)) if CP.enableBsm: signals += [ - ("L_ADJACENT", "BSM", 0), - ("L_APPROACHING", "BSM", 0), - ("R_ADJACENT", "BSM", 0), - ("R_APPROACHING", "BSM", 0), - ] - checks += [ - ("BSM", 1) + ("L_ADJACENT", "BSM"), + ("L_APPROACHING", "BSM"), + ("R_ADJACENT", "BSM"), + ("R_APPROACHING", "BSM"), ] + checks.append(("BSM", 1)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0) @staticmethod def get_cam_can_parser(CP): - signals = [ - ("FORCE", "PRE_COLLISION", 0), - ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0), + ("FORCE", "PRE_COLLISION"), + ("PRECOLLISION_ACTIVE", "PRE_COLLISION"), ] # use steering message to check if panda is connected to frc @@ -214,7 +218,7 @@ class CarState(CarStateBase): ] if CP.carFingerprint in TSS2_CAR: - signals.append(("ACC_TYPE", "ACC_CONTROL", 0)) + signals.append(("ACC_TYPE", "ACC_CONTROL")) checks.append(("ACC_CONTROL", 33)) return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 2) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py old mode 100755 new mode 100644 index c95aca0b22..65c2faacab --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -2,7 +2,7 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.car.toyota.tunes import LatTunes, LongTunes, set_long_tune, set_lat_tune -from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, CarControllerParams +from selfdrive.car.toyota.values import Ecu, CAR, ToyotaFlags, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, EPS_SCALE, CarControllerParams from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config from selfdrive.car.interfaces import CarInterfaceBase @@ -20,17 +20,15 @@ class CarInterface(CarInterfaceBase): ret.carName = "toyota" ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)] + ret.safetyConfigs[0].safetyParam = EPS_SCALE[candidate] ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 - ret.stoppingControl = False # Toyota starts braking more when it thinks you want to stop - # Most cars use this default safety param - ret.safetyConfigs[0].safetyParam = 73 + stop_and_go = False if candidate == CAR.PRIUS: - ret.safetyConfigs[0].safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file stop_and_go = True ret.wheelbase = 2.70 ret.steerRatio = 15.74 # unknown end-to-end spec @@ -40,6 +38,14 @@ class CarInterface(CarInterfaceBase): set_lat_tune(ret.lateralTuning, LatTunes.INDI_PRIUS) ret.steerActuatorDelay = 0.3 + elif candidate == CAR.PRIUS_V: + stop_and_go = True + ret.wheelbase = 2.78 + ret.steerRatio = 17.4 + tire_stiffness_factor = 0.5533 + ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG + set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4) + elif candidate in (CAR.RAV4, CAR.RAV4H): stop_and_go = True if (candidate in CAR.RAV4H) else False ret.wheelbase = 2.65 @@ -49,8 +55,6 @@ class CarInterface(CarInterfaceBase): set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4) elif candidate == CAR.COROLLA: - ret.safetyConfigs[0].safetyParam = 88 - stop_and_go = False ret.wheelbase = 2.70 ret.steerRatio = 18.27 tire_stiffness_factor = 0.444 # not optimized yet @@ -99,7 +103,6 @@ class CarInterface(CarInterfaceBase): set_lat_tune(ret.lateralTuning, LatTunes.PID_G) elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2): - stop_and_go = False ret.wheelbase = 2.82 ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download tire_stiffness_factor = 0.7983 @@ -129,20 +132,12 @@ class CarInterface(CarInterfaceBase): ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG set_lat_tune(ret.lateralTuning, LatTunes.PID_D) - elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2): + elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH): stop_and_go = True ret.wheelbase = 2.8702 ret.steerRatio = 16.0 # not optimized tire_stiffness_factor = 0.444 # not optimized yet - ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.PID_D) - - elif candidate == CAR.LEXUS_ESH: - stop_and_go = True - ret.wheelbase = 2.8190 - ret.steerRatio = 16.06 - tire_stiffness_factor = 0.444 # not optimized yet - ret.mass = 3682. * CV.LB_TO_KG + STD_CARGO_KG + ret.mass = 3677. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max set_lat_tune(ret.lateralTuning, LatTunes.PID_D) elif candidate == CAR.SIENNA: @@ -153,26 +148,14 @@ class CarInterface(CarInterfaceBase): ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG set_lat_tune(ret.lateralTuning, LatTunes.PID_J) - elif candidate == CAR.LEXUS_IS: - ret.safetyConfigs[0].safetyParam = 77 - stop_and_go = False + elif candidate in (CAR.LEXUS_IS, CAR.LEXUS_RC): ret.wheelbase = 2.79908 ret.steerRatio = 13.3 tire_stiffness_factor = 0.444 ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG set_lat_tune(ret.lateralTuning, LatTunes.PID_L) - elif candidate == CAR.LEXUS_RC: - ret.safetyConfigs[0].safetyParam = 77 - stop_and_go = False - ret.wheelbase = 2.73050 - ret.steerRatio = 13.3 - tire_stiffness_factor = 0.444 - ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.PID_L) - elif candidate == CAR.LEXUS_CTH: - ret.safetyConfigs[0].safetyParam = 100 stop_and_go = True ret.wheelbase = 2.60 ret.steerRatio = 18.6 @@ -234,14 +217,13 @@ class CarInterface(CarInterfaceBase): # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected") ret.openpilotLongitudinalControl = smartDsu or ret.enableDsu or candidate in TSS2_CAR + if 0x245 in fingerprint[0]: + ret.flags |= ToyotaFlags.HYBRID.value + # min speed to enable ACC. if car can do stop and go, then set enabling speed # to a negative value, so it won't matter. ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED - # removing the DSU disables AEB and it's considered a community maintained feature - # intercepting the DSU is a community feature since it requires unofficial hardware - ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu or smartDsu - if ret.enableGasInterceptor: set_long_tune(ret.longitudinalTuning, LongTunes.PEDAL) elif candidate in TSS2_CAR: diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index a7d2ec37bb..590840851d 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -4,6 +4,7 @@ from cereal import car from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSS2_CAR from selfdrive.car.interfaces import RadarInterfaceBase + def _create_radar_can_parser(car_fingerprint): if car_fingerprint in TSS2_CAR: RADAR_A_MSGS = list(range(0x180, 0x190)) @@ -16,11 +17,10 @@ def _create_radar_can_parser(car_fingerprint): msg_b_n = len(RADAR_B_MSGS) signals = list(zip(['LONG_DIST'] * msg_a_n + ['NEW_TRACK'] * msg_a_n + ['LAT_DIST'] * msg_a_n + - ['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n, - RADAR_A_MSGS * 5 + RADAR_B_MSGS, - [255] * msg_a_n + [1] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_a_n + [0] * msg_b_n)) + ['REL_SPEED'] * msg_a_n + ['VALID'] * msg_a_n + ['SCORE'] * msg_b_n, + RADAR_A_MSGS * 5 + RADAR_B_MSGS)) - checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20]*(msg_a_n + msg_b_n))) + checks = list(zip(RADAR_A_MSGS + RADAR_B_MSGS, [20] * (msg_a_n + msg_b_n))) return CANParser(DBC[car_fingerprint]['radar'], signals, checks, 1) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 0852991611..ca58126793 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -69,17 +69,32 @@ def create_fcw_command(packer, fcw): def create_ui_command(packer, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart, enabled): values = { + "TWO_BEEPS": chime, + "LDA_ALERT": steer, "RIGHT_LINE": 3 if right_lane_depart else 1 if right_line else 2, "LEFT_LINE": 3 if left_lane_depart else 1 if left_line else 2, "BARRIERS" : 1 if enabled else 0, - "SET_ME_X0C": 0x0c, - "SET_ME_X2C": 0x2c, - "SET_ME_X38": 0x38, - "SET_ME_X02": 0x02, + + # static signals + "SET_ME_X02": 2, "SET_ME_X01": 1, - "SET_ME_X01_2": 1, + "LKAS_STATUS": 1, "REPEATED_BEEPS": 0, - "TWO_BEEPS": chime, - "LDA_ALERT": steer, + "LANE_SWAY_FLD": 7, + "LANE_SWAY_BUZZER": 0, + "LANE_SWAY_WARNING": 0, + "LDA_FRONT_CAMERA_BLOCKED": 0, + "TAKE_CONTROL": 0, + "LANE_SWAY_SENSITIVITY": 2, + "LANE_SWAY_TOGGLE": 1, + "LDA_ON_MESSAGE": 0, + "LDA_SPEED_TOO_LOW": 0, + "LDA_SA_TOGGLE": 1, + "LDA_SENSITIVITY": 2, + "LDA_UNAVAILABLE": 0, + "LDA_MALFUNCTION": 0, + "LDA_UNAVAILABLE_QUIET": 0, + "ADJUSTING_CAMERA": 0, + "LDW_EXIST": 1, } return packer.make_can_msg("LKAS_HUD", 0, values) diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 81f530ef5d..99937561a7 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -1,3 +1,6 @@ +from collections import defaultdict +from enum import IntFlag + from cereal import car from selfdrive.car import dbc_dict from selfdrive.config import Conversions as CV @@ -16,6 +19,11 @@ class CarControllerParams: STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50) STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor + +class ToyotaFlags(IntFlag): + HYBRID = 1 + + class CAR: # Toyota ALPHARD_TSS2 = "TOYOTA ALPHARD 2020" @@ -38,6 +46,7 @@ class CAR: HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018" HIGHLANDERH_TSS2 = "TOYOTA HIGHLANDER HYBRID 2020" PRIUS = "TOYOTA PRIUS 2017" + PRIUS_V = "TOYOTA PRIUS v 2017" PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021" RAV4 = "TOYOTA RAV4 2017" RAV4H = "TOYOTA RAV4 HYBRID 2017" @@ -65,25 +74,24 @@ class CAR: STATIC_DSU_MSGS = [ (0x128, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'), (0x128, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 3, b'\x03\x00\x20\x00\x00\x52'), - (0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 1, 2, b'\x00\x00\x00\x46'), - (0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), - (0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'), + (0x141, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 2, b'\x00\x00\x00\x46'), + (0x160, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 7, b'\x00\x00\x08\x12\x01\x31\x9c\x51'), + (0x161, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.AVALON, CAR.LEXUS_RX, CAR.PRIUS_V), 1, 7, b'\x00\x1e\x00\x00\x00\x80\x07'), (0X161, (CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 7, b'\x00\x1e\x00\xd4\x00\x00\x5b'), - (0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), + (0x283, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 3, b'\x00\x00\x00\x00\x00\x00\x8c'), (0x2E6, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xff\xf8\x00\x08\x7f\xe0\x00\x4e'), (0x2E7, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 3, b'\xa8\x9c\x31\x9c\x00\x00\x00\x02'), (0x33E, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH), 0, 20, b'\x0f\xff\x26\x40\x00\x1f\x00'), - (0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), + (0x344, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 5, b'\x00\x00\x01\x00\x00\x00\x00\x50'), (0x365, (CAR.PRIUS, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x00\x80\x03\x00\x08'), - (0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'), + (0x365, (CAR.RAV4, CAR.RAV4H, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 20, b'\x00\x00\x00\x80\xfc\x00\x08'), (0x366, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.HIGHLANDERH), 0, 20, b'\x00\x00\x4d\x82\x40\x02\x00'), - (0x366, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'), + (0x366, (CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 20, b'\x00\x72\x07\xff\x09\xfe\x00'), (0x470, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'), - (0x470, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH), 1, 100, b'\x00\x00\x01\x79'), - (0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), + (0x470, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.PRIUS_V), 1, 100, b'\x00\x00\x01\x79'), + (0x4CB, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_ESH, CAR.LEXUS_RX, CAR.PRIUS_V), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), ] - FW_VERSIONS = { CAR.AVALON: { (Ecu.esp, 0x7b0, None): [ @@ -360,12 +368,14 @@ FW_VERSIONS = { b'\x018966306Q5000\x00\x00\x00\x00', b'\x018966306T3100\x00\x00\x00\x00', b'\x018966306T3200\x00\x00\x00\x00', + b'\x018966306T4000\x00\x00\x00\x00', b'\x018966306T4100\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F6201200\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F0602100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', b'\x028646F0602200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', b'\x028646F3305200\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', b'\x028646F3305300\x00\x00\x00\x008646G5301200\x00\x00\x00\x00', @@ -392,6 +402,7 @@ FW_VERSIONS = { }, CAR.CHR: { (Ecu.engine, 0x700, None): [ + b'\x01896631021100\x00\x00\x00\x00', b'\x01896631017100\x00\x00\x00\x00', b'\x01896631017200\x00\x00\x00\x00', b'\x0189663F413100\x00\x00\x00\x00', @@ -575,6 +586,7 @@ FW_VERSIONS = { b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00', b'\x03312N6100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00', + b'\x02312K4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'\x018965B12350\x00\x00\x00\x00\x00\x00', @@ -586,6 +598,7 @@ FW_VERSIONS = { b'\x018965B1255000\x00\x00\x00\x00', b'8965B12361\x00\x00\x00\x00\x00\x00', b'8965B16011\x00\x00\x00\x00\x00\x00', + b'\x018965B12510\x00\x00\x00\x00\x00\x00' ], (Ecu.esp, 0x7b0, None): [ b'\x01F152602280\x00\x00\x00\x00\x00\x00', @@ -600,11 +613,13 @@ FW_VERSIONS = { b'\x01F152612B51\x00\x00\x00\x00\x00\x00', b'\x01F152612B60\x00\x00\x00\x00\x00\x00', b'\x01F152612B61\x00\x00\x00\x00\x00\x00', + b'\x01F152612B62\x00\x00\x00\x00\x00\x00', b'\x01F152612B71\x00\x00\x00\x00\x00\x00', b'\x01F152612B81\x00\x00\x00\x00\x00\x00', b'\x01F152612B90\x00\x00\x00\x00\x00\x00', b'\x01F152612C00\x00\x00\x00\x00\x00\x00', b'F152602191\x00\x00\x00\x00\x00\x00', + b'\x01F152612862\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301100\x00\x00\x00\x00', @@ -632,6 +647,7 @@ FW_VERSIONS = { b'\x01896637624000\x00\x00\x00\x00', b'\x01896637626000\x00\x00\x00\x00', b'\x01896637648000\x00\x00\x00\x00', + b'\x01896637643000\x00\x00\x00\x00', b'\x02896630ZJ5000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZN8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', b'\x02896630ZQ3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00', @@ -808,9 +824,11 @@ FW_VERSIONS = { b'\x01F15264872300\x00\x00\x00\x00', b'\x01F15264872400\x00\x00\x00\x00', b'\x01F15264872500\x00\x00\x00\x00', + b'\x01F15264873500\x00\x00\x00\x00', b'\x01F152648C6300\x00\x00\x00\x00', ], (Ecu.engine, 0x700, None): [ + b'\x01896630E67000\x00\x00\x00\x00', b'\x01896630EA1000\x00\x00\x00\x00', b'\x01896630EE4000\x00\x00\x00\x00', b'\x01896630EA1000\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00', @@ -968,6 +986,23 @@ FW_VERSIONS = { b'8646F4705200\x00\x00\x00\x00', ], }, + CAR.PRIUS_V: { + (Ecu.esp, 0x7b0, None): [ + b'F152647280\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x0234781000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'881514705100\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821F4702300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646F4703300\x00\x00\x00\x00', + ], + }, CAR.RAV4: { (Ecu.engine, 0x7e0, None): [ b'\x02342Q1000\x00\x00\x00\x00\x00\x00\x00\x0054212000\x00\x00\x00\x00\x00\x00\x00\x00', @@ -1520,20 +1555,21 @@ FW_VERSIONS = { }, CAR.LEXUS_RX_TSS2: { (Ecu.engine, 0x700, None): [ - b'\x01896630EC9000\x00\x00\x00\x00', - b'\x01896634D12000\x00\x00\x00\x00', - b'\x01896630EB0000\x00\x00\x00\x00', b'\x01896630EA9000\x00\x00\x00\x00', + b'\x01896630EB0000\x00\x00\x00\x00', + b'\x01896630EC9000\x00\x00\x00\x00', b'\x01896630ED0000\x00\x00\x00\x00', + b'\x01896630ED6000\x00\x00\x00\x00', b'\x018966348W5100\x00\x00\x00\x00', b'\x018966348W9000\x00\x00\x00\x00', + b'\x01896634D12000\x00\x00\x00\x00', b'\x01896634D12100\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ - b'\x01F152648801\x00\x00\x00\x00\x00\x00', b'\x01F15260E031\x00\x00\x00\x00\x00\x00', b'\x01F15260E041\x00\x00\x00\x00\x00\x00', b'\x01F152648781\x00\x00\x00\x00\x00\x00', + b'\x01F152648801\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B48261\x00\x00\x00\x00\x00\x00', @@ -1545,8 +1581,9 @@ FW_VERSIONS = { b'\x018821F3301400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ - b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', b'\x028646F4810100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + b'\x028646F4810300\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', ], }, CAR.LEXUS_RXH_TSS2: { @@ -1607,64 +1644,79 @@ FW_VERSIONS = { (Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F6201400\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',], }, CAR.ALPHARD_TSS2: { - (Ecu.engine, 0x7e0, None): [b'\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',], - (Ecu.eps, 0x7a1, None): [b'8965B58040\x00\x00\x00\x00\x00\x00',], - (Ecu.fwdRadar, 0x750, 0xf): [b'\x018821F3301400\x00\x00\x00\x00',], - (Ecu.fwdCamera, 0x750, 0x6d): [b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',], + (Ecu.engine, 0x7e0, None): [ + b'\x0235870000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', + b'\x0235883000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B58040\x00\x00\x00\x00\x00\x00', + b'8965B58052\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301200\x00\x00\x00\x00', + b'\x018821F3301400\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F58010C0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F5803200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00', + ], }, } STEER_THRESHOLD = 100 DBC = { - CAR.RAV4H: dbc_dict('toyota_rav4_hybrid_2017_pt_generated', 'toyota_adas'), - CAR.RAV4: dbc_dict('toyota_rav4_2017_pt_generated', 'toyota_adas'), - CAR.PRIUS: dbc_dict('toyota_prius_2017_pt_generated', 'toyota_adas'), - CAR.COROLLA: dbc_dict('toyota_corolla_2017_pt_generated', 'toyota_adas'), - CAR.LEXUS_RC: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'), - CAR.LEXUS_RX: dbc_dict('lexus_rx_350_2016_pt_generated', 'toyota_adas'), - CAR.LEXUS_RXH: dbc_dict('lexus_rx_hybrid_2017_pt_generated', 'toyota_adas'), + CAR.RAV4H: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.RAV4: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.PRIUS: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), + CAR.PRIUS_V: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.COROLLA: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.LEXUS_RC: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_RX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_RXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.LEXUS_RX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_RXH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), + CAR.LEXUS_RXH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.CHR: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.CHRH: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_adas'), + CAR.CHRH: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), CAR.CAMRY: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.CAMRYH: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_adas'), + CAR.CAMRYH: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), CAR.CAMRY_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.CAMRYH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), - CAR.HIGHLANDER: dbc_dict('toyota_highlander_2017_pt_generated', 'toyota_adas'), + CAR.CAMRYH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.HIGHLANDER: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.HIGHLANDER_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.HIGHLANDERH: dbc_dict('toyota_highlander_hybrid_2018_pt_generated', 'toyota_adas'), - CAR.HIGHLANDERH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), - CAR.AVALON: dbc_dict('toyota_avalon_2017_pt_generated', 'toyota_adas'), + CAR.HIGHLANDERH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.HIGHLANDERH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.AVALON: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.AVALON_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), - CAR.AVALONH_2019: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_adas'), + CAR.AVALONH_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'), CAR.AVALON_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), + CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.LEXUS_ES_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_ESH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'), - CAR.SIENNA: dbc_dict('toyota_sienna_xle_2018_pt_generated', 'toyota_adas'), - CAR.LEXUS_IS: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'), - CAR.LEXUS_CTH: dbc_dict('lexus_ct200h_2018_pt_generated', 'toyota_adas'), - CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), - CAR.LEXUS_NXH: dbc_dict('lexus_nx300h_2018_pt_generated', 'toyota_adas'), - CAR.LEXUS_NX: dbc_dict('lexus_nx300_2018_pt_generated', 'toyota_adas'), + CAR.LEXUS_ESH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.LEXUS_ESH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.SIENNA: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_IS: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_CTH: dbc_dict('toyota_new_mc_pt_generated', 'toyota_adas'), + CAR.RAV4H_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.LEXUS_NXH: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), + CAR.LEXUS_NX: dbc_dict('toyota_tnga_k_pt_generated', 'toyota_adas'), CAR.LEXUS_NX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), - CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), - CAR.MIRAI: dbc_dict('toyota_nodsu_hybrid_pt_generated', 'toyota_tss2_adas'), + CAR.PRIUS_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), + CAR.MIRAI: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), CAR.ALPHARD_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'), } +# These cars have non-standard EPS torque scale factors. All others are 73 +EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_IS: 77, CAR.LEXUS_RC: 77, CAR.LEXUS_CTH: 100, CAR.PRIUS_V: 100}) # Toyota/Lexus Safety Sense 2.0 and 2.5 TSS2_CAR = {CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2, - CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, - CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2} + CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2, + CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2} NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH} # no resume button press required -NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH} +NO_STOP_TIMER_CAR = TSS2_CAR | {CAR.PRIUS_V, CAR.RAV4H, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.SIENNA, CAR.LEXUS_ESH} diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index d271ecf1ea..f85a81a538 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -21,7 +21,7 @@ class CarController(): self.steer_rate_limited = False - def update(self, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): + def update(self, c, enabled, CS, frame, ext_bus, actuators, visual_alert, left_lane_visible, right_lane_visible, left_lane_depart, right_lane_depart): """ Controls thread """ can_sends = [] @@ -39,7 +39,7 @@ class CarController(): # torque value. Do that anytime we happen to have 0 torque, or failing that, # when exceeding ~1/3 the 360 second timer. - if enabled and CS.out.vEgo > CS.CP.minSteerSpeed and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning): + if c.active and CS.out.vEgo > CS.CP.minSteerSpeed and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning): new_steer = int(round(actuators.steer * P.STEER_MAX)) apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P) self.steer_rate_limited = new_steer != apply_steer diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 0e0b0dfc07..1fe8b56b90 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -33,7 +33,7 @@ class CarState(CarStateBase): # Update steering angle, rate, yaw rate, and driver input torque. VW send # the sign/direction in a separate signal so they must be recombined. - ret.steeringAngleDeg = pt_cp.vl["LH_EPS_03"]["EPS_Berechneter_LW"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_BLW"])] + ret.steeringAngleDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradwinkel"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradwinkel"])] ret.steeringRateDeg = pt_cp.vl["LWI_01"]["LWI_Lenkradw_Geschw"] * (1, -1)[int(pt_cp.vl["LWI_01"]["LWI_VZ_Lenkradw_Geschw"])] ret.steeringTorque = pt_cp.vl["LH_EPS_03"]["EPS_Lenkmoment"] * (1, -1)[int(pt_cp.vl["LH_EPS_03"]["EPS_VZ_Lenkmoment"])] ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE @@ -147,50 +147,49 @@ class CarState(CarStateBase): @staticmethod def get_can_parser(CP): - # this function generates lists for signal, messages and initial values signals = [ - # sig_name, sig_address, default - ("EPS_Berechneter_LW", "LH_EPS_03", 0), # Absolute steering angle - ("EPS_VZ_BLW", "LH_EPS_03", 0), # Steering angle sign - ("LWI_Lenkradw_Geschw", "LWI_01", 0), # Absolute steering rate - ("LWI_VZ_Lenkradw_Geschw", "LWI_01", 0), # Steering rate sign - ("ESP_VL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front left - ("ESP_VR_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, front right - ("ESP_HL_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, rear left - ("ESP_HR_Radgeschw_02", "ESP_19", 0), # ABS wheel speed, rear right - ("ESP_Gierrate", "ESP_02", 0), # Absolute yaw rate - ("ESP_VZ_Gierrate", "ESP_02", 0), # Yaw rate sign - ("ZV_FT_offen", "Gateway_72", 0), # Door open, driver - ("ZV_BT_offen", "Gateway_72", 0), # Door open, passenger - ("ZV_HFS_offen", "Gateway_72", 0), # Door open, rear left - ("ZV_HBFS_offen", "Gateway_72", 0), # Door open, rear right - ("ZV_HD_offen", "Gateway_72", 0), # Trunk or hatch open - ("Comfort_Signal_Left", "Blinkmodi_02", 0), # Left turn signal including comfort blink interval - ("Comfort_Signal_Right", "Blinkmodi_02", 0), # Right turn signal including comfort blink interval - ("AB_Gurtschloss_FA", "Airbag_02", 0), # Seatbelt status, driver - ("AB_Gurtschloss_BF", "Airbag_02", 0), # Seatbelt status, passenger - ("ESP_Fahrer_bremst", "ESP_05", 0), # Brake pedal pressed - ("ESP_Bremsdruck", "ESP_05", 0), # Brake pressure applied - ("MO_Fahrpedalrohwert_01", "Motor_20", 0), # Accelerator pedal value - ("EPS_Lenkmoment", "LH_EPS_03", 0), # Absolute driver torque input - ("EPS_VZ_Lenkmoment", "LH_EPS_03", 0), # Driver torque input sign - ("EPS_HCA_Status", "LH_EPS_03", 3), # EPS HCA control status - ("ESP_Tastung_passiv", "ESP_21", 0), # Stability control disabled - ("ESP_Haltebestaetigung", "ESP_21", 0), # ESP hold confirmation - ("KBI_MFA_v_Einheit_02", "Einheiten_01", 0), # MPH vs KMH speed display - ("KBI_Handbremse", "Kombi_01", 0), # Manual handbrake applied - ("TSK_Status", "TSK_06", 0), # ACC engagement status from drivetrain coordinator - ("GRA_Hauptschalter", "GRA_ACC_01", 0), # ACC button, on/off - ("GRA_Abbrechen", "GRA_ACC_01", 0), # ACC button, cancel - ("GRA_Tip_Setzen", "GRA_ACC_01", 0), # ACC button, set - ("GRA_Tip_Hoch", "GRA_ACC_01", 0), # ACC button, increase or accel - ("GRA_Tip_Runter", "GRA_ACC_01", 0), # ACC button, decrease or decel - ("GRA_Tip_Wiederaufnahme", "GRA_ACC_01", 0), # ACC button, resume - ("GRA_Verstellung_Zeitluecke", "GRA_ACC_01", 0), # ACC button, time gap adj - ("GRA_Typ_Hauptschalter", "GRA_ACC_01", 0), # ACC main button type - ("GRA_Tip_Stufe_2", "GRA_ACC_01", 0), # unknown related to stalk type - ("GRA_ButtonTypeInfo", "GRA_ACC_01", 0), # unknown related to stalk type - ("COUNTER", "GRA_ACC_01", 0), # GRA_ACC_01 CAN message counter + # sig_name, sig_address + ("LWI_Lenkradwinkel", "LWI_01"), # Absolute steering angle + ("LWI_VZ_Lenkradwinkel", "LWI_01"), # Steering angle sign + ("LWI_Lenkradw_Geschw", "LWI_01"), # Absolute steering rate + ("LWI_VZ_Lenkradw_Geschw", "LWI_01"), # Steering rate sign + ("ESP_VL_Radgeschw_02", "ESP_19"), # ABS wheel speed, front left + ("ESP_VR_Radgeschw_02", "ESP_19"), # ABS wheel speed, front right + ("ESP_HL_Radgeschw_02", "ESP_19"), # ABS wheel speed, rear left + ("ESP_HR_Radgeschw_02", "ESP_19"), # ABS wheel speed, rear right + ("ESP_Gierrate", "ESP_02"), # Absolute yaw rate + ("ESP_VZ_Gierrate", "ESP_02"), # Yaw rate sign + ("ZV_FT_offen", "Gateway_72"), # Door open, driver + ("ZV_BT_offen", "Gateway_72"), # Door open, passenger + ("ZV_HFS_offen", "Gateway_72"), # Door open, rear left + ("ZV_HBFS_offen", "Gateway_72"), # Door open, rear right + ("ZV_HD_offen", "Gateway_72"), # Trunk or hatch open + ("Comfort_Signal_Left", "Blinkmodi_02"), # Left turn signal including comfort blink interval + ("Comfort_Signal_Right", "Blinkmodi_02"), # Right turn signal including comfort blink interval + ("AB_Gurtschloss_FA", "Airbag_02"), # Seatbelt status, driver + ("AB_Gurtschloss_BF", "Airbag_02"), # Seatbelt status, passenger + ("ESP_Fahrer_bremst", "ESP_05"), # Brake pedal pressed + ("ESP_Bremsdruck", "ESP_05"), # Brake pressure applied + ("MO_Fahrpedalrohwert_01", "Motor_20"), # Accelerator pedal value + ("EPS_Lenkmoment", "LH_EPS_03"), # Absolute driver torque input + ("EPS_VZ_Lenkmoment", "LH_EPS_03"), # Driver torque input sign + ("EPS_HCA_Status", "LH_EPS_03"), # EPS HCA control status + ("ESP_Tastung_passiv", "ESP_21"), # Stability control disabled + ("ESP_Haltebestaetigung", "ESP_21"), # ESP hold confirmation + ("KBI_MFA_v_Einheit_02", "Einheiten_01"), # MPH vs KMH speed display + ("KBI_Handbremse", "Kombi_01"), # Manual handbrake applied + ("TSK_Status", "TSK_06"), # ACC engagement status from drivetrain coordinator + ("GRA_Hauptschalter", "GRA_ACC_01"), # ACC button, on/off + ("GRA_Abbrechen", "GRA_ACC_01"), # ACC button, cancel + ("GRA_Tip_Setzen", "GRA_ACC_01"), # ACC button, set + ("GRA_Tip_Hoch", "GRA_ACC_01"), # ACC button, increase or accel + ("GRA_Tip_Runter", "GRA_ACC_01"), # ACC button, decrease or decel + ("GRA_Tip_Wiederaufnahme", "GRA_ACC_01"), # ACC button, resume + ("GRA_Verstellung_Zeitluecke", "GRA_ACC_01"), # ACC button, time gap adj + ("GRA_Typ_Hauptschalter", "GRA_ACC_01"), # ACC main button type + ("GRA_Tip_Stufe_2", "GRA_ACC_01"), # unknown related to stalk type + ("GRA_ButtonTypeInfo", "GRA_ACC_01"), # unknown related to stalk type + ("COUNTER", "GRA_ACC_01"), # GRA_ACC_01 CAN message counter ] checks = [ @@ -212,15 +211,15 @@ class CarState(CarStateBase): ] if CP.transmissionType == TransmissionType.automatic: - signals += [("GE_Fahrstufe", "Getriebe_11", 0)] # Auto trans gear selector position - checks += [("Getriebe_11", 20)] # From J743 Auto transmission control module + signals.append(("GE_Fahrstufe", "Getriebe_11")) # Auto trans gear selector position + checks.append(("Getriebe_11", 20)) # From J743 Auto transmission control module elif CP.transmissionType == TransmissionType.direct: - signals += [("GearPosition", "EV_Gearshift", 0)] # EV gear selector position - checks += [("EV_Gearshift", 10)] # From J??? unknown EV control module + signals.append(("GearPosition", "EV_Gearshift")) # EV gear selector position + checks.append(("EV_Gearshift", 10)) # From J??? unknown EV control module elif CP.transmissionType == TransmissionType.manual: - signals += [("MO_Kuppl_schalter", "Motor_14", 0), # Clutch switch - ("BCM1_Rueckfahrlicht_Schalter", "Gateway_72", 0)] # Reverse light from BCM - checks += [("Motor_14", 10)] # From J623 Engine control module + signals += [("MO_Kuppl_schalter", "Motor_14"), # Clutch switch + ("BCM1_Rueckfahrlicht_Schalter", "Gateway_72")] # Reverse light from BCM + checks.append(("Motor_14", 10)) # From J623 Engine control module if CP.networkLocation == NetworkLocation.fwdCamera: # Radars are here on CANBUS.pt @@ -234,18 +233,17 @@ class CarState(CarStateBase): @staticmethod def get_cam_can_parser(CP): - signals = [] checks = [] if CP.networkLocation == NetworkLocation.fwdCamera: signals += [ - # sig_name, sig_address, default - ("LDW_SW_Warnung_links", "LDW_02", 0), # Blind spot in warning mode on left side due to lane departure - ("LDW_SW_Warnung_rechts", "LDW_02", 0), # Blind spot in warning mode on right side due to lane departure - ("LDW_Seite_DLCTLC", "LDW_02", 0), # Direction of most likely lane departure (left or right) - ("LDW_DLC", "LDW_02", 0), # Lane departure, distance to line crossing - ("LDW_TLC", "LDW_02", 0), # Lane departure, time to line crossing + # sig_name, sig_address + ("LDW_SW_Warnung_links", "LDW_02"), # Blind spot in warning mode on left side due to lane departure + ("LDW_SW_Warnung_rechts", "LDW_02"), # Blind spot in warning mode on right side due to lane departure + ("LDW_Seite_DLCTLC", "LDW_02"), # Direction of most likely lane departure (left or right) + ("LDW_DLC", "LDW_02"), # Lane departure, distance to line crossing + ("LDW_TLC", "LDW_02"), # Lane departure, time to line crossing ] checks += [ # sig_address, frequency @@ -264,20 +262,20 @@ class CarState(CarStateBase): class MqbExtraSignals: # Additional signal and message lists for optional or bus-portable controllers fwd_radar_signals = [ - ("ACC_Wunschgeschw", "ACC_02", 0), # ACC set speed - ("AWV2_Freigabe", "ACC_10", 0), # FCW brake jerk release - ("ANB_Teilbremsung_Freigabe", "ACC_10", 0), # AEB partial braking release - ("ANB_Zielbremsung_Freigabe", "ACC_10", 0), # AEB target braking release + ("ACC_Wunschgeschw", "ACC_02"), # ACC set speed + ("AWV2_Freigabe", "ACC_10"), # FCW brake jerk release + ("ANB_Teilbremsung_Freigabe", "ACC_10"), # AEB partial braking release + ("ANB_Zielbremsung_Freigabe", "ACC_10"), # AEB target braking release ] fwd_radar_checks = [ ("ACC_10", 50), # From J428 ACC radar control module ("ACC_02", 17), # From J428 ACC radar control module ] bsm_radar_signals = [ - ("SWA_Infostufe_SWA_li", "SWA_01", 0), # Blind spot object info, left - ("SWA_Warnung_SWA_li", "SWA_01", 0), # Blind spot object warning, left - ("SWA_Infostufe_SWA_re", "SWA_01", 0), # Blind spot object info, right - ("SWA_Warnung_SWA_re", "SWA_01", 0), # Blind spot object warning, right + ("SWA_Infostufe_SWA_li", "SWA_01"), # Blind spot object info, left + ("SWA_Warnung_SWA_li", "SWA_01"), # Blind spot object warning, left + ("SWA_Infostufe_SWA_re", "SWA_01"), # Blind spot object info, right + ("SWA_Warnung_SWA_re", "SWA_01"), # Blind spot object warning, right ] bsm_radar_checks = [ ("SWA_01", 20), # From J1086 Lane Change Assist diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index bb67a27262..961cfed7fe 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -45,7 +45,7 @@ class CarInterface(CarInterfaceBase): # Global lateral tuning defaults, can be overridden per-vehicle - ret.steerActuatorDelay = 0.05 + ret.steerActuatorDelay = 0.1 ret.steerRateCost = 1.0 ret.steerLimitTimer = 0.4 ret.steerRatio = 15.6 # Let the params learner figure this out @@ -174,12 +174,6 @@ class CarInterface(CarInterfaceBase): ret.canValid = self.cp.can_valid and self.cp_cam.can_valid ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - # TODO: add a field for this to carState, car interface code shouldn't write params - # Update the device metric configuration to match the car at first startup, - # or if there's been a change. - #if self.CS.displayMetricUnits != self.displayMetricUnitsPrev: - # put_nonblocking("IsMetric", "1" if self.CS.displayMetricUnits else "0") - # Check for and process state-change events (button press or release) from # the turn stalk switch or ACC steering wheel/control stalk buttons. for button in self.CS.buttonStates: @@ -217,7 +211,7 @@ class CarInterface(CarInterfaceBase): def apply(self, c): hud_control = c.hudControl - ret = self.CC.update(c.enabled, self.CS, self.frame, self.ext_bus, c.actuators, + ret = self.CC.update(c, c.enabled, self.CS, self.frame, self.ext_bus, c.actuators, hud_control.visualAlert, hud_control.leftLaneVisible, hud_control.rightLaneVisible, diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 4806754fe9..241338c2c5 100755 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -165,6 +165,7 @@ FW_VERSIONS = { b'\xf1\x8704E906027HD\xf1\x893742', b'\xf1\x8704E906027MA\xf1\x894958', b'\xf1\x8704L906021DT\xf1\x895520', + b'\xf1\x8704L906021DT\xf1\x898127', b'\xf1\x8704L906021N \xf1\x895518', b'\xf1\x8704L906026BP\xf1\x897608', b'\xf1\x8704L906026NF\xf1\x899528', @@ -211,6 +212,7 @@ FW_VERSIONS = { b'\xf1\x870D9300040A \xf1\x893613', b'\xf1\x870D9300040S \xf1\x894311', b'\xf1\x870D9300041H \xf1\x895220', + b'\xf1\x870D9300041P \xf1\x894507', b'\xf1\x870DD300045K \xf1\x891120', b'\xf1\x870DD300046F \xf1\x891601', b'\xf1\x870GC300012A \xf1\x891403', @@ -469,13 +471,16 @@ FW_VERSIONS = { }, CAR.TRANSPORTER_T61: { (Ecu.engine, 0x7e0, None): [ + b'\xf1\x8704L906057AP\xf1\x891186', b'\xf1\x8704L906057N \xf1\x890413', ], (Ecu.transmission, 0x7e1, None): [ b'\xf1\x870BT300012G \xf1\x893102', + b'\xf1\x870BT300012E \xf1\x893105', ], (Ecu.srs, 0x715, None): [ b'\xf1\x872Q0959655AE\xf1\x890506\xf1\x82\02316170411110411--04041704161611152S1411', + b'\xf1\x872Q0959655AF\xf1\x890506\xf1\x82\x1316171111110411--04041711121211152S1413', ], (Ecu.eps, 0x712, None): [ b'\xf1\x877LA909144F \xf1\x897150\xf1\x82\005323A5519A2', diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript index 21f609865b..8dfeecbdd7 100644 --- a/selfdrive/common/SConscript +++ b/selfdrive/common/SConscript @@ -7,6 +7,7 @@ else: common_libs = [ 'params.cc', + 'statlog.cc', 'swaglog.cc', 'util.cc', 'gpio.cc', diff --git a/selfdrive/common/clutil.cc b/selfdrive/common/clutil.cc index 313978525e..4e952a2a88 100644 --- a/selfdrive/common/clutil.cc +++ b/selfdrive/common/clutil.cc @@ -49,7 +49,7 @@ void cl_print_build_errors(cl_program program, cl_device_id device) { std::string log(log_size, '\0'); clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_LOG, log_size, &log[0], NULL); - std::cout << "build failed; status=" << status << ", log:" << std::endl << log << std::endl; + std::cout << "build failed; status=" << status << ", log:" << std::endl << log << std::endl; } } // namespace diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index e425fda816..25062b4975 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -91,7 +91,6 @@ std::unordered_map keys = { {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"CarParamsCache", CLEAR_ON_MANAGER_START}, {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, - {"CommunityFeaturesToggle", PERSISTENT}, {"CompletedTrainingVersion", PERSISTENT}, {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON}, @@ -117,7 +116,6 @@ std::unordered_map keys = { {"GsmRoaming", PERSISTENT}, {"HardwareSerial", PERSISTENT}, {"HasAcceptedTerms", PERSISTENT}, - {"HasPrime", PERSISTENT}, {"IMEI", PERSISTENT}, {"InstallDate", PERSISTENT}, {"IsDriverViewEnabled", CLEAR_ON_MANAGER_START}, @@ -144,8 +142,10 @@ std::unordered_map keys = { {"NavdRender", PERSISTENT}, {"OpenpilotEnabledToggle", PERSISTENT}, {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF}, + {"PandaSignatures", CLEAR_ON_MANAGER_START}, {"Passive", PERSISTENT}, {"PrimeRedirected", PERSISTENT}, + {"PrimeType", PERSISTENT}, {"RecordFront", PERSISTENT}, {"RecordFrontLock", PERSISTENT}, // for the internal fleet {"ReleaseNotes", PERSISTENT}, diff --git a/selfdrive/common/statlog.cc b/selfdrive/common/statlog.cc new file mode 100644 index 0000000000..27dfc2ca9d --- /dev/null +++ b/selfdrive/common/statlog.cc @@ -0,0 +1,43 @@ +#ifndef _GNU_SOURCE +#define _GNU_SOURCE +#endif + +#include "selfdrive/common/statlog.h" +#include "selfdrive/common/util.h" + +#include +#include +#include + +class StatlogState : public LogState { + public: + StatlogState() : LogState("ipc:///tmp/stats") {} +}; + +static StatlogState s = {}; + +static void log(const char* metric_type, const char* metric, const char* fmt, ...) { + char* value_buf = nullptr; + va_list args; + va_start(args, fmt); + int ret = vasprintf(&value_buf, fmt, args); + va_end(args); + + if (ret > 0 && value_buf) { + char* line_buf = nullptr; + ret = asprintf(&line_buf, "%s:%s|%s", metric, value_buf, metric_type); + if (ret > 0 && line_buf) { + zmq_send(s.sock, line_buf, ret, ZMQ_NOBLOCK); + free(line_buf); + } + free(value_buf); + } +} + +void statlog_log(const char* metric_type, const char* metric, int value) { + log(metric_type, metric, "%d", value); +} + +void statlog_log(const char* metric_type, const char* metric, float value) { + log(metric_type, metric, "%f", value); +} diff --git a/selfdrive/common/statlog.h b/selfdrive/common/statlog.h new file mode 100644 index 0000000000..5d223bb666 --- /dev/null +++ b/selfdrive/common/statlog.h @@ -0,0 +1,10 @@ +#pragma once + +#define STATLOG_GAUGE "g" +#define STATLOG_SAMPLE "sa" + +void statlog_log(const char* metric_type, const char* metric, int value); +void statlog_log(const char* metric_type, const char* metric, float value); + +#define statlog_gauge(metric, value) statlog_log(STATLOG_GAUGE, metric, value) +#define statlog_sample(metric, value) statlog_log(STATLOG_SAMPLE, metric, value) diff --git a/selfdrive/common/swaglog.cc b/selfdrive/common/swaglog.cc index aae4fb6e70..1fe700415d 100644 --- a/selfdrive/common/swaglog.cc +++ b/selfdrive/common/swaglog.cc @@ -16,71 +16,53 @@ #include "selfdrive/common/version.h" #include "selfdrive/hardware/hw.h" -class LogState { +class SwaglogState : public LogState { public: - LogState() = default; - ~LogState(); - std::mutex lock; - bool inited; - json11::Json::object ctx_j; - void *zctx; - void *sock; - int print_level; -}; - -LogState::~LogState() { - zmq_close(sock); - zmq_ctx_destroy(zctx); -} + SwaglogState() : LogState("ipc:///tmp/logmessage") {} -static LogState s = {}; - -static void cloudlog_init() { - if (s.inited) return; - s.ctx_j = json11::Json::object {}; - s.zctx = zmq_ctx_new(); - s.sock = zmq_socket(s.zctx, ZMQ_PUSH); - - int timeout = 100; // 100 ms timeout on shutdown for messages to be received by logmessaged - zmq_setsockopt(s.sock, ZMQ_LINGER, &timeout, sizeof(timeout)); + bool initialized = false; + json11::Json::object ctx_j; - zmq_connect(s.sock, "ipc:///tmp/logmessage"); + inline void initialize() { + ctx_j = json11::Json::object {}; + print_level = CLOUDLOG_WARNING; + const char* print_lvl = getenv("LOGPRINT"); + if (print_lvl) { + if (strcmp(print_lvl, "debug") == 0) { + print_level = CLOUDLOG_DEBUG; + } else if (strcmp(print_lvl, "info") == 0) { + print_level = CLOUDLOG_INFO; + } else if (strcmp(print_lvl, "warning") == 0) { + print_level = CLOUDLOG_WARNING; + } + } - s.print_level = CLOUDLOG_WARNING; - const char* print_level = getenv("LOGPRINT"); - if (print_level) { - if (strcmp(print_level, "debug") == 0) { - s.print_level = CLOUDLOG_DEBUG; - } else if (strcmp(print_level, "info") == 0) { - s.print_level = CLOUDLOG_INFO; - } else if (strcmp(print_level, "warning") == 0) { - s.print_level = CLOUDLOG_WARNING; + // openpilot bindings + char* dongle_id = getenv("DONGLE_ID"); + if (dongle_id) { + ctx_j["dongle_id"] = dongle_id; + } + char* daemon_name = getenv("MANAGER_DAEMON"); + if (daemon_name) { + ctx_j["daemon"] = daemon_name; + } + ctx_j["version"] = COMMA_VERSION; + ctx_j["dirty"] = !getenv("CLEAN"); + + // device type + if (Hardware::EON()) { + ctx_j["device"] = "eon"; + } else if (Hardware::TICI()) { + ctx_j["device"] = "tici"; + } else { + ctx_j["device"] = "pc"; } - } - // openpilot bindings - char* dongle_id = getenv("DONGLE_ID"); - if (dongle_id) { - s.ctx_j["dongle_id"] = dongle_id; - } - char* daemon_name = getenv("MANAGER_DAEMON"); - if (daemon_name) { - s.ctx_j["daemon"] = daemon_name; - } - s.ctx_j["version"] = COMMA_VERSION; - s.ctx_j["dirty"] = !getenv("CLEAN"); - - // device type - if (Hardware::EON()) { - s.ctx_j["device"] = "eon"; - } else if (Hardware::TICI()) { - s.ctx_j["device"] = "tici"; - } else { - s.ctx_j["device"] = "pc"; + initialized = true; } +}; - s.inited = true; -} +static SwaglogState s = {}; static void log(int levelnum, const char* filename, int lineno, const char* func, const char* msg, const std::string& log_s) { if (levelnum >= s.print_level) { @@ -101,7 +83,8 @@ void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func if (ret <= 0 || !msg_buf) return; std::lock_guard lk(s.lock); - cloudlog_init(); + + if (!s.initialized) s.initialize(); json11::Json log_j = json11::Json::object { {"msg", msg_buf}, diff --git a/selfdrive/common/swaglog.h b/selfdrive/common/swaglog.h index a508bcac1a..6403820ac8 100644 --- a/selfdrive/common/swaglog.h +++ b/selfdrive/common/swaglog.h @@ -8,6 +8,8 @@ #define CLOUDLOG_ERROR 40 #define CLOUDLOG_CRITICAL 50 + + void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func, const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/; diff --git a/selfdrive/common/tests/test_swaglog.cc b/selfdrive/common/tests/test_swaglog.cc index 025ee51dbf..1d00def63d 100644 --- a/selfdrive/common/tests/test_swaglog.cc +++ b/selfdrive/common/tests/test_swaglog.cc @@ -12,29 +12,31 @@ const char *SWAGLOG_ADDR = "ipc:///tmp/logmessage"; std::string daemon_name = "testy"; std::string dongle_id = "test_dongle_id"; +int LINE_NO = 0; -void log_thread(int msg, int msg_cnt) { +void log_thread(int thread_id, int msg_cnt) { for (int i = 0; i < msg_cnt; ++i) { - LOGD("%d", msg); + LOGD("%d", thread_id); + LINE_NO = __LINE__ - 1; usleep(1); } } -void send_stop_msg(void *zctx) { - void *sock = zmq_socket(zctx, ZMQ_PUSH); - zmq_connect(sock, SWAGLOG_ADDR); - zmq_send(sock, "", 0, ZMQ_NOBLOCK); - zmq_close(sock); -} - -void recv_log(void *zctx, int thread_cnt, int thread_msg_cnt) { +void recv_log(int thread_cnt, int thread_msg_cnt) { + void *zctx = zmq_ctx_new(); void *sock = zmq_socket(zctx, ZMQ_PULL); zmq_bind(sock, SWAGLOG_ADDR); std::vector thread_msgs(thread_cnt); + int total_count = 0; - while (true) { + for (auto start = std::chrono::steady_clock::now(), now = start; + now < start + std::chrono::seconds{1} && total_count < (thread_cnt * thread_msg_cnt); + now = std::chrono::steady_clock::now()) { char buf[4096] = {}; - if (zmq_recv(sock, buf, sizeof(buf), 0) == 0) break; + if (zmq_recv(sock, buf, sizeof(buf), ZMQ_DONTWAIT) <= 0) { + if (errno == EAGAIN || errno == EINTR || errno == EFSM) continue; + break; + } REQUIRE(buf[0] == CLOUDLOG_DEBUG); std::string err; @@ -44,13 +46,16 @@ void recv_log(void *zctx, int thread_cnt, int thread_msg_cnt) { REQUIRE(msg["levelnum"].int_value() == CLOUDLOG_DEBUG); REQUIRE_THAT(msg["filename"].string_value(), Catch::Contains("test_swaglog.cc")); REQUIRE(msg["funcname"].string_value() == "log_thread"); - REQUIRE(msg["lineno"].int_value() == 18); // TODO: do this automatically + REQUIRE(msg["lineno"].int_value() == LINE_NO); auto ctx = msg["ctx"]; + REQUIRE(ctx["daemon"].string_value() == daemon_name); REQUIRE(ctx["dongle_id"].string_value() == dongle_id); - REQUIRE(ctx["version"].string_value() == COMMA_VERSION); REQUIRE(ctx["dirty"].bool_value() == true); + + REQUIRE(ctx["version"].string_value() == COMMA_VERSION); + std::string device = "pc"; if (Hardware::EON()) { device = "eon"; @@ -62,11 +67,14 @@ void recv_log(void *zctx, int thread_cnt, int thread_msg_cnt) { int thread_id = atoi(msg["msg"].string_value().c_str()); REQUIRE((thread_id >= 0 && thread_id < thread_cnt)); thread_msgs[thread_id]++; + total_count++; } for (int i = 0; i < thread_cnt; ++i) { + INFO("thread :" << i); REQUIRE(thread_msgs[i] == thread_msg_cnt); } zmq_close(sock); + zmq_ctx_destroy(zctx); } TEST_CASE("swaglog") { @@ -76,14 +84,11 @@ TEST_CASE("swaglog") { const int thread_cnt = 5; const int thread_msg_cnt = 100; - void *zctx = zmq_ctx_new(); - send_stop_msg(zctx); std::vector log_threads; for (int i = 0; i < thread_cnt; ++i) { log_threads.push_back(std::thread(log_thread, i, thread_msg_cnt)); } - for (auto &t : log_threads) t.join(); - recv_log(zctx, thread_cnt, thread_msg_cnt); - zmq_ctx_destroy(zctx); + + recv_log(thread_cnt, thread_msg_cnt); } diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h index 89eacf69a2..bf0df3bcaa 100644 --- a/selfdrive/common/util.h +++ b/selfdrive/common/util.h @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -11,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -163,3 +165,26 @@ void update_max_atomic(std::atomic& max, T const& value) { T prev = max; while(prev < value && !max.compare_exchange_weak(prev, value)) {} } + +class LogState { + public: + std::mutex lock; + void *zctx; + void *sock; + int print_level; + + LogState(const char* endpoint) { + zctx = zmq_ctx_new(); + sock = zmq_socket(zctx, ZMQ_PUSH); + + // Timeout on shutdown for messages to be received by the logging process + int timeout = 100; + zmq_setsockopt(sock, ZMQ_LINGER, &timeout, sizeof(timeout)); + + zmq_connect(sock, endpoint); + }; + ~LogState() { + zmq_close(sock); + zmq_ctx_destroy(zctx); + } +}; diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 55257a4378..f9260762ed 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -31,8 +31,6 @@ from selfdrive.manager.process_config import managed_processes SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS LANE_DEPARTURE_THRESHOLD = 0.1 -STEER_ANGLE_SATURATION_TIMEOUT = 1.0 / DT_CTRL -STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees REPLAY = "REPLAY" in os.environ SIMULATION = "SIMULATION" in os.environ @@ -100,7 +98,6 @@ class Controls: # read params self.is_metric = params.get_bool("IsMetric") self.is_ldw_enabled = params.get_bool("IsLdwEnabled") - community_feature_toggle = params.get_bool("CommunityFeaturesToggle") openpilot_enabled_toggle = params.get_bool("OpenpilotEnabledToggle") passive = params.get_bool("Passive") or not openpilot_enabled_toggle @@ -110,11 +107,7 @@ class Controls: car_recognized = self.CP.carName != 'mock' controller_available = self.CI.CC is not None and not passive and not self.CP.dashcamOnly - community_feature = self.CP.communityFeature or \ - self.CP.fingerprintSource == car.CarParams.FingerprintSource.can - community_feature_disallowed = community_feature and (not community_feature_toggle) - self.read_only = not car_recognized or not controller_available or \ - self.CP.dashcamOnly or community_feature_disallowed + self.read_only = not car_recognized or not controller_available or self.CP.dashcamOnly if self.read_only: safety_config = car.CarParams.SafetyConfig.new_message() safety_config.safetyModel = car.CarParams.SafetyModel.noOutput @@ -133,13 +126,13 @@ class Controls: self.VM = VehicleModel(self.CP) if self.CP.steerControlType == car.CarParams.SteerControlType.angle: - self.LaC = LatControlAngle(self.CP) + self.LaC = LatControlAngle(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'pid': self.LaC = LatControlPID(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'indi': - self.LaC = LatControlINDI(self.CP) + self.LaC = LatControlINDI(self.CP, self.CI) elif self.CP.lateralTuning.which() == 'lqr': - self.LaC = LatControlLQR(self.CP) + self.LaC = LatControlLQR(self.CP, self.CI) self.initialized = False self.state = State.disabled @@ -153,7 +146,6 @@ class Controls: self.cruise_mismatch_counter = 0 self.can_rcv_error_counter = 0 self.last_blinker_frame = 0 - self.saturated_count = 0 self.distance_traveled = 0 self.last_functional_fan_frame = 0 self.events_prev = [] @@ -169,8 +161,6 @@ class Controls: if not sounds_available: self.events.add(EventName.soundsUnavailable, static=True) - if community_feature_disallowed and car_recognized and not self.CP.dashcamOnly: - self.events.add(EventName.communityFeatureDisallowed, static=True) if not car_recognized: self.events.add(EventName.carUnrecognized, static=True) if len(self.CP.carFw) > 0: @@ -191,10 +181,8 @@ class Controls: """Compute carEvents from carState""" self.events.clear() - self.events.add_from_msg(CS.events) - self.events.add_from_msg(self.sm['driverMonitoringState'].events) - # Handle startup event + # Add startup event if self.startup_event is not None: self.events.add(self.startup_event) self.startup_event = None @@ -204,6 +192,9 @@ class Controls: self.events.add(EventName.controlsInitializing) return + self.events.add_from_msg(CS.events) + self.events.add_from_msg(self.sm['driverMonitoringState'].events) + # Create events for battery, temperature, disk space, and memory if EON and (self.sm['peripheralState'].pandaType != PandaType.uno) and \ self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError: @@ -260,9 +251,12 @@ class Controls: for i, pandaState in enumerate(self.sm['pandaStates']): # All pandas must match the list of safetyConfigs, and if outside this list, must be silent or noOutput if i < len(self.CP.safetyConfigs): - safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam + safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or \ + pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam or \ + pandaState.unsafeMode != self.CP.unsafeMode else: safety_mismatch = pandaState.safetyModel not in IGNORED_SAFETY_MODES + if safety_mismatch or self.mismatch_counter >= 200: self.events.add(EventName.controlsMismatch) @@ -279,7 +273,7 @@ class Controls: if not self.logged_comm_issue: invalid = [s for s, valid in self.sm.valid.items() if not valid] not_alive = [s for s, alive in self.sm.alive.items() if not alive] - cloudlog.event("commIssue", invalid=invalid, not_alive=not_alive) + cloudlog.event("commIssue", invalid=invalid, not_alive=not_alive, can_error=self.can_rcv_error, error=True) self.logged_comm_issue = True else: self.logged_comm_issue = False @@ -311,7 +305,7 @@ class Controls: self.events.add(EventName.fcw) if TICI: - for m in messaging.drain_sock(self.log_sock, wait_for_one=False): + for m in messaging.drain_sock(self.log_sock, wait_for_one=False): try: msg = m.androidLog.message if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")): @@ -365,6 +359,10 @@ class Controls: if not self.read_only: self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan']) self.initialized = True + + if REPLAY and self.sm['pandaStates'][0].controlsAllowed: + self.state = State.enabled + Params().put_bool("ControlsReady", True) # Check for CAN timeout @@ -432,7 +430,7 @@ class Controls: # no more soft disabling condition, so go back to ENABLED self.state = State.enabled - elif self.events.any(ET.SOFT_DISABLE) and self.soft_disable_timer > 0: + elif self.soft_disable_timer > 0: self.current_alert_types.append(ET.SOFT_DISABLE) elif self.soft_disable_timer <= 0: @@ -518,19 +516,8 @@ class Controls: lac_log.output = steer lac_log.saturated = abs(steer) >= 0.9 - # Check for difference between desired angle and angle for angle based control - angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \ - abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD - - if angle_control_saturated and not CS.steeringPressed and self.active: - self.saturated_count += 1 - else: - self.saturated_count = 0 - # Send a "steering required alert" if saturation count has reached the limit - if (lac_log.saturated and not CS.steeringPressed) or \ - (self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT): - + if lac_log.active and lac_log.saturated and not CS.steeringPressed: dpath_points = lat_plan.dPathPoints if len(dpath_points): # Check if we deviated from the path @@ -593,7 +580,7 @@ class Controls: ldw_allowed = self.is_ldw_enabled and CS.vEgo > LDW_MIN_SPEED and not recent_blinker \ and not self.active and self.sm['liveCalibration'].calStatus == Calibration.CALIBRATED - model_v2 = self.sm['modelV2'] + model_v2 = self.sm['modelV2'] desire_prediction = model_v2.meta.desirePrediction if len(desire_prediction) and ldw_allowed: right_lane_visible = self.sm['lateralPlan'].rProb > 0.5 @@ -611,10 +598,15 @@ class Controls: if hudControl.rightLaneDepart or hudControl.leftLaneDepart: self.events.add(EventName.ldw) - clear_event = ET.WARNING if ET.WARNING not in self.current_alert_types else None + clear_event_types = set() + if ET.WARNING not in self.current_alert_types: + clear_event_types.add(ET.WARNING) + if self.enabled: + clear_event_types.add(ET.NO_ENTRY) + alerts = self.events.create_alerts(self.current_alert_types, [self.CP, self.sm, self.is_metric, self.soft_disable_timer]) self.AM.add_many(self.sm.frame, alerts) - current_alert = self.AM.process_alerts(self.sm.frame, clear_event) + current_alert = self.AM.process_alerts(self.sm.frame, clear_event_types) if current_alert: hudControl.visualAlert = current_alert.visual_alert diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py index 4694d74bff..2dad05e214 100644 --- a/selfdrive/controls/lib/alertmanager.py +++ b/selfdrive/controls/lib/alertmanager.py @@ -47,13 +47,13 @@ class AlertManager: min_end_frame = entry.start_frame + alert.duration entry.end_frame = max(frame + 1, min_end_frame) - def process_alerts(self, frame: int, clear_event_type=None) -> Optional[Alert]: + def process_alerts(self, frame: int, clear_event_types: set) -> Optional[Alert]: current_alert = AlertEntry() for v in self.alerts.values(): if not v.alert: continue - if clear_event_type and v.alert.event_type == clear_event_type: + if v.alert.event_type in clear_event_types: v.end_frame = -1 # sort by priority first and then by start_frame diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json index 4affe0cfe3..aa54f582f6 100644 --- a/selfdrive/controls/lib/alerts_offroad.json +++ b/selfdrive/controls/lib/alerts_offroad.json @@ -38,7 +38,7 @@ "severity": 1 }, "Offroad_StorageMissing": { - "text": "Storage drive not mounted.", + "text": "NVMe drive not mounted.", "severity": 1 }, "Offroad_CarUnrecognized": { diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py new file mode 100644 index 0000000000..c34d143a5a --- /dev/null +++ b/selfdrive/controls/lib/desire_helper.py @@ -0,0 +1,113 @@ +from cereal import log +from common.realtime import DT_MDL +from selfdrive.config import Conversions as CV + +LaneChangeState = log.LateralPlan.LaneChangeState +LaneChangeDirection = log.LateralPlan.LaneChangeDirection + +LANE_CHANGE_SPEED_MIN = 30 * CV.MPH_TO_MS +LANE_CHANGE_TIME_MAX = 10. + +DESIRES = { + LaneChangeDirection.none: { + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.none, + }, + LaneChangeDirection.left: { + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft, + }, + LaneChangeDirection.right: { + LaneChangeState.off: log.LateralPlan.Desire.none, + LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight, + LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight, + }, +} + + +class DesireHelper: + def __init__(self): + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none + self.lane_change_timer = 0.0 + self.lane_change_ll_prob = 1.0 + self.keep_pulse_timer = 0.0 + self.prev_one_blinker = False + self.desire = log.LateralPlan.Desire.none + + def update(self, carstate, active, lane_change_prob): + v_ego = carstate.vEgo + one_blinker = carstate.leftBlinker != carstate.rightBlinker + below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN + + if not active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: + self.lane_change_state = LaneChangeState.off + self.lane_change_direction = LaneChangeDirection.none + else: + # LaneChangeState.off + if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: + self.lane_change_state = LaneChangeState.preLaneChange + self.lane_change_ll_prob = 1.0 + + # LaneChangeState.preLaneChange + elif self.lane_change_state == LaneChangeState.preLaneChange: + # Set lane change direction + self.lane_change_direction = LaneChangeDirection.left if \ + carstate.leftBlinker else LaneChangeDirection.right + + torque_applied = carstate.steeringPressed and \ + ((carstate.steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or + (carstate.steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) + + blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or + (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) + + if not one_blinker or below_lane_change_speed: + self.lane_change_state = LaneChangeState.off + elif torque_applied and not blindspot_detected: + self.lane_change_state = LaneChangeState.laneChangeStarting + + # LaneChangeState.laneChangeStarting + elif self.lane_change_state == LaneChangeState.laneChangeStarting: + # fade out over .5s + self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0) + + # 98% certainty + if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: + self.lane_change_state = LaneChangeState.laneChangeFinishing + + # LaneChangeState.laneChangeFinishing + elif self.lane_change_state == LaneChangeState.laneChangeFinishing: + # fade in laneline over 1s + self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) + + if self.lane_change_ll_prob > 0.99: + self.lane_change_direction = LaneChangeDirection.none + if one_blinker: + self.lane_change_state = LaneChangeState.preLaneChange + else: + self.lane_change_state = LaneChangeState.off + + if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange): + self.lane_change_timer = 0.0 + else: + self.lane_change_timer += DT_MDL + + self.prev_one_blinker = one_blinker + + self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] + + # Send keep pulse once per second during LaneChangeStart.preLaneChange + if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting): + self.keep_pulse_timer = 0.0 + elif self.lane_change_state == LaneChangeState.preLaneChange: + self.keep_pulse_timer += DT_MDL + if self.keep_pulse_timer > 1.0: + self.keep_pulse_timer = 0.0 + elif self.desire in (log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight): + self.desire = log.LateralPlan.Desire.none diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 4d693b0fbd..fbce9460a8 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -264,8 +264,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { EventName.stockFcw: {}, - EventName.lkasDisabled: {}, - # ********** events only containing alerts displayed in all states ********** EventName.joystickDebug: { @@ -316,14 +314,6 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { #ET.PERMANENT: ImmediateDisableAlert("openpilot failed to cancel cruise"), }, - # Some features or cars are marked as community features. If openpilot - # detects the use of a community feature it switches to dashcam mode - # until these features are allowed using a toggle in settings. - EventName.communityFeatureDisallowed: { - ET.PERMANENT: NormalPermanentAlert("openpilot Unavailable", - "Enable Community Features in Settings"), - }, - # openpilot doesn't recognize the car. This switches openpilot into a # read-only mode. This can be solved by adding your fingerprint. # See https://github.com/commaai/openpilot/wiki/Fingerprinting for more information @@ -516,7 +506,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { # current GPS position. This alert is thrown when the localizer is reset # more often than expected. EventName.localizerMalfunction: { - ET.PERMANENT: NormalPermanentAlert("Sensor Malfunction", "Contact Support"), + # ET.PERMANENT: NormalPermanentAlert("Sensor Malfunction", "Contact Support"), }, # ********** events that affect controls state transitions ********** @@ -831,4 +821,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.NO_ENTRY: NoEntryAlert("Cruise Fault: Restart the Car"), }, + EventName.lkasDisabled: { + ET.PERMANENT: NormalPermanentAlert("LKAS Disabled: Enable LKAS to engage"), + ET.NO_ENTRY: NoEntryAlert("LKAS Disabled"), + }, + } diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py new file mode 100644 index 0000000000..eb16aca2e8 --- /dev/null +++ b/selfdrive/controls/lib/latcontrol.py @@ -0,0 +1,28 @@ +from abc import abstractmethod, ABC + +from common.realtime import DT_CTRL +from common.numpy_fast import clip + +MIN_STEER_SPEED = 0.3 + + +class LatControl(ABC): + def __init__(self, CP, CI): + self.sat_count_rate = 1.0 * DT_CTRL + self.sat_limit = CP.steerLimitTimer + self.sat_count = 0. + + @abstractmethod + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): + pass + + def reset(self): + self.sat_count = 0. + + def _check_saturation(self, saturated, CS): + if saturated and CS.vEgo > 10. and not CS.steeringRateLimited and not CS.steeringPressed: + self.sat_count += self.sat_count_rate + else: + self.sat_count -= self.sat_count_rate + self.sat_count = clip(self.sat_count, 0.0, self.sat_limit) + return self.sat_count > (self.sat_limit - 1e-3) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index dc184cf58b..c935356311 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -1,19 +1,16 @@ import math from cereal import log +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED +STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees -class LatControlAngle(): - def __init__(self, CP): - pass - - def reset(self): - pass +class LatControlAngle(LatControl): def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): angle_log = log.ControlsState.LateralAngleState.new_message() - if CS.vEgo < 0.3 or not active: + if CS.vEgo < MIN_STEER_SPEED or not active: angle_log.active = False angle_steers_des = float(CS.steeringAngleDeg) else: @@ -21,8 +18,8 @@ class LatControlAngle(): angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) angle_steers_des += params.angleOffsetDeg - angle_log.saturated = False + angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD + angle_log.saturated = self._check_saturation(angle_control_saturated, CS) angle_log.steeringAngleDeg = float(CS.steeringAngleDeg) angle_log.steeringAngleDesiredDeg = angle_steers_des - return 0, float(angle_steers_des), angle_log diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 75b27ac8c1..dc1b31bad9 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -5,13 +5,13 @@ from cereal import log from common.filter_simple import FirstOrderFilter from common.numpy_fast import clip, interp from common.realtime import DT_CTRL -from selfdrive.car import apply_toyota_steer_torque_limits -from selfdrive.car.toyota.values import CarControllerParams from selfdrive.controls.lib.drive_helpers import get_steer_max +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED -class LatControlINDI(): - def __init__(self, CP): +class LatControlINDI(LatControl): + def __init__(self, CP, CI): + super().__init__(CP, CI) self.angle_steers_des = 0. A = np.array([[1.0, DT_CTRL, 0.0], @@ -35,15 +35,11 @@ class LatControlINDI(): self.A_K = A - np.dot(K, C) self.x = np.array([[0.], [0.], [0.]]) - self.enforce_rate_limit = CP.carName == "toyota" - self._RC = (CP.lateralTuning.indi.timeConstantBP, CP.lateralTuning.indi.timeConstantV) self._G = (CP.lateralTuning.indi.actuatorEffectivenessBP, CP.lateralTuning.indi.actuatorEffectivenessV) self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV) self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP, CP.lateralTuning.indi.innerLoopGainV) - self.sat_count_rate = 1.0 * DT_CTRL - self.sat_limit = CP.steerLimitTimer self.steer_filter = FirstOrderFilter(0., self.RC, DT_CTRL) self.reset() @@ -65,24 +61,11 @@ class LatControlINDI(): return interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1]) def reset(self): + super().reset() self.steer_filter.x = 0. - self.output_steer = 0. - self.sat_count = 0. self.speed = 0. - def _check_saturation(self, control, check_saturation, limit): - saturated = abs(control) == limit - - if saturated and check_saturation: - self.sat_count += self.sat_count_rate - else: - self.sat_count -= self.sat_count_rate - - self.sat_count = clip(self.sat_count, 0.0, 1.0) - - return self.sat_count > self.sat_limit - - def update(self, active, CS, CP, VM, params, last_actuators, curvature, curvature_rate): + def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): self.speed = CS.vEgo # Update Kalman filter y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]]) @@ -93,21 +76,21 @@ class LatControlINDI(): indi_log.steeringRateDeg = math.degrees(self.x[1]) indi_log.steeringAccelDeg = math.degrees(self.x[2]) - steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo, params.roll) + steers_des = VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll) steers_des += math.radians(params.angleOffsetDeg) indi_log.steeringAngleDesiredDeg = math.degrees(steers_des) - rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo, 0) + rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0) indi_log.steeringRateDesiredDeg = math.degrees(rate_des) - if CS.vEgo < 0.3 or not active: + if CS.vEgo < MIN_STEER_SPEED or not active: indi_log.active = False - self.output_steer = 0.0 self.steer_filter.x = 0.0 + output_steer = 0 else: # Expected actuator value self.steer_filter.update_alpha(self.RC) - self.steer_filter.update(self.output_steer) + self.steer_filter.update(last_actuators.steer) # Compute acceleration error rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des @@ -119,21 +102,13 @@ class LatControlINDI(): delta_u = g_inv * accel_error # If steering pressed, only allow wind down - if CS.steeringPressed and (delta_u * self.output_steer > 0): + if CS.steeringPressed and (delta_u * last_actuators.steer > 0): delta_u = 0 - # Enforce rate limit - if self.enforce_rate_limit: - steer_max = float(CarControllerParams.STEER_MAX) - new_output_steer_cmd = steer_max * (self.steer_filter.x + delta_u) - prev_output_steer_cmd = steer_max * self.output_steer - new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, CarControllerParams) - self.output_steer = new_output_steer_cmd / steer_max - else: - self.output_steer = self.steer_filter.x + delta_u + output_steer = self.steer_filter.x + delta_u steers_max = get_steer_max(CP, CS.vEgo) - self.output_steer = clip(self.output_steer, -steers_max, steers_max) + output_steer = clip(output_steer, -steers_max, steers_max) indi_log.active = True indi_log.rateSetPoint = float(rate_sp) @@ -141,9 +116,7 @@ class LatControlINDI(): indi_log.accelError = float(accel_error) indi_log.delayedOutput = float(self.steer_filter.x) indi_log.delta = float(delta_u) - indi_log.output = float(self.output_steer) - - check_saturation = (CS.vEgo > 10.) and not CS.steeringRateLimited and not CS.steeringPressed - indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) + indi_log.output = float(output_steer) + indi_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS) - return float(self.output_steer), float(steers_des), indi_log + return float(output_steer), float(steers_des), indi_log diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index 5777cde8e8..5c273a45be 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -5,10 +5,12 @@ from common.numpy_fast import clip from common.realtime import DT_CTRL from cereal import log from selfdrive.controls.lib.drive_helpers import get_steer_max +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED -class LatControlLQR(): - def __init__(self, CP): +class LatControlLQR(LatControl): + def __init__(self, CP, CI): + super().__init__(CP, CI) self.scale = CP.lateralTuning.lqr.scale self.ki = CP.lateralTuning.lqr.ki @@ -23,26 +25,11 @@ class LatControlLQR(): self.i_unwind_rate = 0.3 * DT_CTRL self.i_rate = 1.0 * DT_CTRL - self.sat_count_rate = 1.0 * DT_CTRL - self.sat_limit = CP.steerLimitTimer - self.reset() def reset(self): + super().reset() self.i_lqr = 0.0 - self.sat_count = 0.0 - - def _check_saturation(self, control, check_saturation, limit): - saturated = abs(control) == limit - - if saturated and check_saturation: - self.sat_count += self.sat_count_rate - else: - self.sat_count -= self.sat_count_rate - - self.sat_count = clip(self.sat_count, 0.0, 1.0) - - return self.sat_count > self.sat_limit def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): lqr_log = log.ControlsState.LateralLQRState.new_message() @@ -64,7 +51,7 @@ class LatControlLQR(): e = steering_angle_no_offset - angle_steers_k self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e) - if CS.vEgo < 0.3 or not active: + if CS.vEgo < MIN_STEER_SPEED or not active: lqr_log.active = False lqr_output = 0. output_steer = 0. @@ -91,12 +78,9 @@ class LatControlLQR(): output_steer = lqr_output + self.i_lqr output_steer = clip(output_steer, -steers_max, steers_max) - check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed - saturated = self._check_saturation(output_steer, check_saturation, steers_max) - lqr_log.steeringAngleDeg = angle_steers_k lqr_log.i = self.i_lqr lqr_log.output = output_steer lqr_log.lqrOutput = lqr_output - lqr_log.saturated = saturated + lqr_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS) return output_steer, desired_angle, lqr_log diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index bcb8ccba56..f5ff5a95e5 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -2,18 +2,20 @@ import math from selfdrive.controls.lib.pid import PIController from selfdrive.controls.lib.drive_helpers import get_steer_max +from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED from cereal import log -class LatControlPID(): +class LatControlPID(LatControl): def __init__(self, CP, CI): + super().__init__(CP, CI) self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), - k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0, - sat_limit=CP.steerLimitTimer) + k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0) self.get_steer_feedforward = CI.get_steer_feedforward_function() def reset(self): + super().reset() self.pid.reset() def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): @@ -26,7 +28,7 @@ class LatControlPID(): pid_log.steeringAngleDesiredDeg = angle_steers_des pid_log.angleError = angle_steers_des - CS.steeringAngleDeg - if CS.vEgo < 0.3 or not active: + if CS.vEgo < MIN_STEER_SPEED or not active: output_steer = 0.0 pid_log.active = False self.pid.reset() @@ -40,14 +42,13 @@ class LatControlPID(): deadzone = 0.0 - check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed - output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed, + output_steer = self.pid.update(angle_steers_des, CS.steeringAngleDeg, override=CS.steeringPressed, feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p pid_log.i = self.pid.i pid_log.f = self.pid.f pid_log.output = output_steer - pid_log.saturated = bool(self.pid.saturated) + pid_log.saturated = self._check_saturation(steers_max - abs(output_steer) < 1e-3, CS) return output_steer, angle_steers_des, pid_log diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index 6162cf68a5..eeda25627a 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -3,6 +3,8 @@ import os import numpy as np from casadi import SX, vertcat, sin, cos + +from common.realtime import sec_since_boot from selfdrive.controls.lib.drive_helpers import LAT_MPC_N as N from selfdrive.controls.lib.drive_helpers import T_IDXS @@ -15,7 +17,8 @@ else: LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__)) EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code") JSON_FILE = "acados_ocp_lat.json" -X_DIM = 6 +X_DIM = 4 +P_DIM = 2 def gen_lat_model(): model = AcadosModel() @@ -26,9 +29,12 @@ def gen_lat_model(): y_ego = SX.sym('y_ego') psi_ego = SX.sym('psi_ego') curv_ego = SX.sym('curv_ego') + model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego) + + # parameters v_ego = SX.sym('v_ego') rotation_radius = SX.sym('rotation_radius') - model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego, v_ego, rotation_radius) + model.p = vertcat(v_ego, rotation_radius) # controls curv_rate = SX.sym('curv_rate') @@ -39,18 +45,14 @@ def gen_lat_model(): y_ego_dot = SX.sym('y_ego_dot') psi_ego_dot = SX.sym('psi_ego_dot') curv_ego_dot = SX.sym('curv_ego_dot') - v_ego_dot = SX.sym('v_ego_dot') - rotation_radius_dot = SX.sym('rotation_radius_dot') - model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot, - v_ego_dot, rotation_radius_dot) + + model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot) # dynamics model f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego), v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego), v_ego * curv_ego, - curv_rate, - 0.0, - 0.0) + curv_rate) model.f_impl_expr = model.xdot - f_expl model.f_expl_expr = f_expl return model @@ -77,8 +79,9 @@ def gen_lat_mpc_solver(): y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2] curv_rate = ocp.model.u[0] - v_ego = ocp.model.x[4] + v_ego = ocp.model.p[0] + ocp.parameter_values = np.zeros((P_DIM, )) ocp.cost.yref = np.zeros((3, )) ocp.cost.yref_e = np.zeros((2, )) @@ -94,7 +97,7 @@ def gen_lat_mpc_solver(): ocp.constraints.idxbx = np.array([2,3]) ocp.constraints.ubx = np.array([np.radians(90), np.radians(50)]) ocp.constraints.lbx = np.array([-np.radians(90), -np.radians(50)]) - x0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + x0 = np.zeros((X_DIM,)) ocp.constraints.x0 = x0 ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' @@ -102,7 +105,7 @@ def gen_lat_mpc_solver(): ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.nlp_solver_type = 'SQP_RTI' ocp.solver_options.qp_solver_iter_max = 1 - ocp.solver_options.qp_solver_cond_N = N//4 + ocp.solver_options.qp_solver_cond_N = 1 # set prediction horizon ocp.solver_options.tf = Tf @@ -128,10 +131,12 @@ class LateralMpc(): # Somehow needed for stable init for i in range(N+1): self.solver.set(i, 'x', np.zeros(X_DIM)) + self.solver.set(i, 'p', np.zeros(P_DIM)) self.solver.constraints_set(0, "lbx", x0) self.solver.constraints_set(0, "ubx", x0) self.solver.solve() self.solution_status = 0 + self.solve_time = 0.0 self.cost = 0 def set_weights(self, path_weight, heading_weight, steer_rate_weight): @@ -141,17 +146,25 @@ class LateralMpc(): #TODO hacky weights to keep behavior the same self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2]) - def run(self, x0, v_ego, car_rotation_radius, y_pts, heading_pts): + def run(self, x0, p, y_pts, heading_pts): x0_cp = np.copy(x0) + p_cp = np.copy(p) self.solver.constraints_set(0, "lbx", x0_cp) self.solver.constraints_set(0, "ubx", x0_cp) self.yref[:,0] = y_pts + v_ego = p_cp[0] + # rotation_radius = p_cp[1] self.yref[:,1] = heading_pts*(v_ego+5.0) for i in range(N): self.solver.cost_set(i, "yref", self.yref[i]) + self.solver.set(i, "p", p_cp) + self.solver.set(N, "p", p_cp) self.solver.cost_set(N, "yref", self.yref[N][:2]) + t = sec_since_boot() self.solution_status = self.solver.solve() + self.solve_time = sec_since_boot() - t + for i in range(N+1): self.x_sol[i] = self.solver.get(i, 'x') for i in range(N): diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 110c6def67..a9c6411394 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -5,54 +5,20 @@ from selfdrive.swaglog import cloudlog from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N, CAR_ROTATION_RADIUS from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE -from selfdrive.config import Conversions as CV +from selfdrive.controls.lib.desire_helper import DesireHelper import cereal.messaging as messaging from cereal import log -LaneChangeState = log.LateralPlan.LaneChangeState -LaneChangeDirection = log.LateralPlan.LaneChangeDirection - -LANE_CHANGE_SPEED_MIN = 30 * CV.MPH_TO_MS -LANE_CHANGE_TIME_MAX = 10. - -DESIRES = { - LaneChangeDirection.none: { - LaneChangeState.off: log.LateralPlan.Desire.none, - LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.none, - LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.none, - }, - LaneChangeDirection.left: { - LaneChangeState.off: log.LateralPlan.Desire.none, - LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeLeft, - LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeLeft, - }, - LaneChangeDirection.right: { - LaneChangeState.off: log.LateralPlan.Desire.none, - LaneChangeState.preLaneChange: log.LateralPlan.Desire.none, - LaneChangeState.laneChangeStarting: log.LateralPlan.Desire.laneChangeRight, - LaneChangeState.laneChangeFinishing: log.LateralPlan.Desire.laneChangeRight, - }, -} - class LateralPlanner: def __init__(self, CP, use_lanelines=True, wide_camera=False): self.use_lanelines = use_lanelines self.LP = LanePlanner(wide_camera) + self.DH = DesireHelper() self.last_cloudlog_t = 0 self.steer_rate_cost = CP.steerRateCost - self.solution_invalid_cnt = 0 - self.lane_change_state = LaneChangeState.off - self.lane_change_direction = LaneChangeDirection.none - self.lane_change_timer = 0.0 - self.lane_change_ll_prob = 1.0 - self.keep_pulse_timer = 0.0 - self.prev_one_blinker = False - self.desire = log.LateralPlan.Desire.none self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3)) self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3)) @@ -61,19 +27,19 @@ class LateralPlanner: self.y_pts = np.zeros(TRAJECTORY_SIZE) self.lat_mpc = LateralMpc() - self.reset_mpc(np.zeros(6)) + self.reset_mpc(np.zeros(4)) - def reset_mpc(self, x0=np.zeros(6)): + def reset_mpc(self, x0=np.zeros(4)): self.x0 = x0 self.lat_mpc.reset(x0=self.x0) def update(self, sm): v_ego = sm['carState'].vEgo - active = sm['controlsState'].active measured_curvature = sm['controlsState'].curvature + # Parse model predictions md = sm['modelV2'] - self.LP.parse_model(sm['modelV2']) + self.LP.parse_model(md) if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE: self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z]) self.t_idxs = np.array(md.position.t) @@ -82,84 +48,15 @@ class LateralPlanner: self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd]) # Lane change logic - one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker - below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN - - if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX): - self.lane_change_state = LaneChangeState.off - self.lane_change_direction = LaneChangeDirection.none - else: - # LaneChangeState.off - if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: - self.lane_change_state = LaneChangeState.preLaneChange - self.lane_change_ll_prob = 1.0 - - # LaneChangeState.preLaneChange - elif self.lane_change_state == LaneChangeState.preLaneChange: - # Set lane change direction - if sm['carState'].leftBlinker: - self.lane_change_direction = LaneChangeDirection.left - elif sm['carState'].rightBlinker: - self.lane_change_direction = LaneChangeDirection.right - else: # If there are no blinkers we will go back to LaneChangeState.off - self.lane_change_direction = LaneChangeDirection.none - - torque_applied = sm['carState'].steeringPressed and \ - ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or - (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) - - blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or - (sm['carState'].rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) - - if not one_blinker or below_lane_change_speed: - self.lane_change_state = LaneChangeState.off - elif torque_applied and not blindspot_detected: - self.lane_change_state = LaneChangeState.laneChangeStarting - - # LaneChangeState.laneChangeStarting - elif self.lane_change_state == LaneChangeState.laneChangeStarting: - # fade out over .5s - self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0) - - # 98% certainty - lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob - if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: - self.lane_change_state = LaneChangeState.laneChangeFinishing - - # LaneChangeState.laneChangeFinishing - elif self.lane_change_state == LaneChangeState.laneChangeFinishing: - # fade in laneline over 1s - self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) - if self.lane_change_ll_prob > 0.99: - self.lane_change_direction = LaneChangeDirection.none - if one_blinker: - self.lane_change_state = LaneChangeState.preLaneChange - else: - self.lane_change_state = LaneChangeState.off - - if self.lane_change_state in (LaneChangeState.off, LaneChangeState.preLaneChange): - self.lane_change_timer = 0.0 - else: - self.lane_change_timer += DT_MDL - - self.prev_one_blinker = one_blinker - - self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] - - # Send keep pulse once per second during LaneChangeStart.preLaneChange - if self.lane_change_state in (LaneChangeState.off, LaneChangeState.laneChangeStarting): - self.keep_pulse_timer = 0.0 - elif self.lane_change_state == LaneChangeState.preLaneChange: - self.keep_pulse_timer += DT_MDL - if self.keep_pulse_timer > 1.0: - self.keep_pulse_timer = 0.0 - elif self.desire in (log.LateralPlan.Desire.keepLeft, log.LateralPlan.Desire.keepRight): - self.desire = log.LateralPlan.Desire.none + lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob + self.DH.update(sm['carState'], sm['controlsState'].active, lane_change_prob) # Turn off lanes during lane change - if self.desire == log.LateralPlan.Desire.laneChangeRight or self.desire == log.LateralPlan.Desire.laneChangeLeft: - self.LP.lll_prob *= self.lane_change_ll_prob - self.LP.rll_prob *= self.lane_change_ll_prob + if self.DH.desire == log.LateralPlan.Desire.laneChangeRight or self.DH.desire == log.LateralPlan.Desire.laneChangeLeft: + self.LP.lll_prob *= self.DH.lane_change_ll_prob + self.LP.rll_prob *= self.DH.lane_change_ll_prob + + # Calculate final driving path and set MPC costs if self.use_lanelines: d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz) self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost) @@ -169,16 +66,17 @@ class LateralPlanner: # Heading cost is useful at low speed, otherwise end of plan can be off-heading heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0]) self.lat_mpc.set_weights(path_cost, heading_cost, self.steer_rate_cost) + y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1]) heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw) self.y_pts = y_pts assert len(y_pts) == LAT_MPC_N + 1 assert len(heading_pts) == LAT_MPC_N + 1 - self.x0[4] = v_ego + # self.x0[4] = v_ego + p = np.array([v_ego, CAR_ROTATION_RADIUS]) self.lat_mpc.run(self.x0, - v_ego, - CAR_ROTATION_RADIUS, + p, y_pts, heading_pts) # init state for next @@ -215,10 +113,11 @@ class LateralPlanner: lateralPlan.dProb = float(self.LP.d_prob) lateralPlan.mpcSolutionValid = bool(plan_solution_valid) + lateralPlan.solverExecutionTime = self.lat_mpc.solve_time - lateralPlan.desire = self.desire + lateralPlan.desire = self.DH.desire lateralPlan.useLaneLines = self.use_lanelines - lateralPlan.laneChangeState = self.lane_change_state - lateralPlan.laneChangeDirection = self.lane_change_direction + lateralPlan.laneChangeState = self.DH.lane_change_state + lateralPlan.laneChangeDirection = self.DH.lane_change_direction pm.send('lateralPlan', plan_send) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index b14ffdc6c8..21c34aa2d6 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -44,8 +44,7 @@ class LongControl(): self.long_control_state = LongCtrlState.off # initialized to off self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), - rate=1/DT_CTRL, - sat_limit=0.8) + rate=1 / DT_CTRL) self.v_pid = 0.0 self.last_output_accel = 0.0 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index c07110eefa..8625df745a 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -215,6 +215,7 @@ class LongitudinalMpc: self.status = False self.crash_cnt = 0.0 self.solution_status = 0 + self.solve_time = 0.0 self.x0 = np.zeros(X_DIM) self.set_weights() @@ -356,7 +357,11 @@ class LongitudinalMpc: self.solver.set(i, 'p', self.params[i]) self.solver.constraints_set(0, "lbx", self.x0) self.solver.constraints_set(0, "ubx", self.x0) + + t = sec_since_boot() self.solution_status = self.solver.solve() + self.solve_time = sec_since_boot() - t + for i in range(N+1): self.x_sol[i] = self.solver.get(i, 'x') for i in range(N): @@ -368,7 +373,6 @@ class LongitudinalMpc: self.prev_a = np.interp(T_IDXS + 0.05, T_IDXS, self.a_solution) - t = sec_since_boot() if self.solution_status != 0: if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 9dc6ab9ee5..8c79404058 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -120,4 +120,6 @@ class Planner: longitudinalPlan.longitudinalPlanSource = self.mpc.source longitudinalPlan.fcw = self.fcw + longitudinalPlan.solverExecutionTime = self.mpc.solve_time + pm.send('longitudinalPlan', plan_send) diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index e384aaaedf..28c6438191 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -13,7 +13,7 @@ def apply_deadzone(error, deadzone): return error class PIController(): - def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8): + def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100): self._k_p = k_p # proportional gain self._k_i = k_i # integral gain self.k_f = k_f # feedforward gain @@ -25,10 +25,8 @@ class PIController(): self.pos_limit = pos_limit self.neg_limit = neg_limit - self.sat_count_rate = 1.0 / rate self.i_unwind_rate = 0.3 / rate self.i_rate = 1.0 / rate - self.sat_limit = sat_limit self.reset() @@ -40,27 +38,13 @@ class PIController(): def k_i(self): return interp(self.speed, self._k_i[0], self._k_i[1]) - def _check_saturation(self, control, check_saturation, error): - saturated = (control < self.neg_limit) or (control > self.pos_limit) - - if saturated and check_saturation and abs(error) > 0.1: - self.sat_count += self.sat_count_rate - else: - self.sat_count -= self.sat_count_rate - - self.sat_count = clip(self.sat_count, 0.0, 1.0) - - return self.sat_count > self.sat_limit - def reset(self): self.p = 0.0 self.i = 0.0 self.f = 0.0 - self.sat_count = 0.0 - self.saturated = False self.control = 0 - def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False): + def update(self, setpoint, measurement, speed=0.0, override=False, feedforward=0., deadzone=0., freeze_integrator=False): self.speed = speed error = float(apply_deadzone(setpoint - measurement, deadzone)) @@ -81,7 +65,6 @@ class PIController(): self.i = i control = self.p + self.f + self.i - self.saturated = self._check_saturation(control, check_saturation, error) self.control = clip(control, self.neg_limit, self.pos_limit) return self.control diff --git a/selfdrive/controls/lib/tests/test_alertmanager.py b/selfdrive/controls/lib/tests/test_alertmanager.py index 2b606390c7..6c1b6fc4a2 100755 --- a/selfdrive/controls/lib/tests/test_alertmanager.py +++ b/selfdrive/controls/lib/tests/test_alertmanager.py @@ -34,7 +34,7 @@ class TestAlertManager(unittest.TestCase): for frame in range(duration+10): if frame < add_duration: AM.add_many(frame, [alert, ]) - current_alert = AM.process_alerts(frame) + current_alert = AM.process_alerts(frame, {}) shown = current_alert is not None should_show = frame <= show_duration diff --git a/selfdrive/controls/lib/tests/test_latcontrol.py b/selfdrive/controls/lib/tests/test_latcontrol.py new file mode 100755 index 0000000000..8345840eca --- /dev/null +++ b/selfdrive/controls/lib/tests/test_latcontrol.py @@ -0,0 +1,44 @@ +#!/usr/bin/env python3 +import unittest + +from parameterized import parameterized + +from cereal import car, log +from selfdrive.car.car_helpers import interfaces +from selfdrive.car.honda.values import CAR as HONDA +from selfdrive.car.toyota.values import CAR as TOYOTA +from selfdrive.car.nissan.values import CAR as NISSAN +from selfdrive.controls.lib.latcontrol_pid import LatControlPID +from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR +from selfdrive.controls.lib.latcontrol_indi import LatControlINDI +from selfdrive.controls.lib.latcontrol_angle import LatControlAngle +from selfdrive.controls.lib.vehicle_model import VehicleModel + + +class TestLatControl(unittest.TestCase): + + @parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)]) + def test_saturation(self, car_name, controller): + CarInterface, CarController, CarState = interfaces[car_name] + CP = CarInterface.get_params(car_name) + CI = CarInterface(CP, CarController, CarState) + VM = VehicleModel(CP) + + controller = controller(CP, CI) + + + CS = car.CarState.new_message() + CS.vEgo = 30 + + last_actuators = car.CarControl.Actuators.new_message() + + params = log.LiveParametersData.new_message() + + for _ in range(1000): + _, _, lac_log = controller.update(True, CS, CP, VM, params, last_actuators, 1, 0) + + self.assertTrue(lac_log.saturated) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index d4b0733692..65f8480c7c 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -172,7 +172,7 @@ class RadarD(): radarState.carStateMonoTime = sm.logMonoTime['carState'] if enable_lead: - leads_v3 = sm['modelV2'].leadsV3 + leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True) radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False) diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 7c4fe7b6ad..4630e28a61 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -14,12 +14,12 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur y_pts = poly_shift * np.ones(LAT_MPC_N + 1) heading_pts = np.zeros(LAT_MPC_N + 1) - x0 = np.array([x_init, y_init, psi_init, curvature_init, v_ref, CAR_ROTATION_RADIUS]) + x0 = np.array([x_init, y_init, psi_init, curvature_init]) + p = np.array([v_ref, CAR_ROTATION_RADIUS]) # converge in no more than 10 iterations for _ in range(10): - lat_mpc.run(x0, v_ref, - CAR_ROTATION_RADIUS, + lat_mpc.run(x0, p, y_pts, heading_pts) return lat_mpc.x_sol diff --git a/selfdrive/controls/tests/test_startup.py b/selfdrive/controls/tests/test_startup.py index 658adf499e..9d13453045 100755 --- a/selfdrive/controls/tests/test_startup.py +++ b/selfdrive/controls/tests/test_startup.py @@ -42,31 +42,27 @@ class TestStartup(unittest.TestCase): # TODO: test EventName.startup for release branches # officially supported car - (EventName.startupMaster, TOYOTA.COROLLA, False, COROLLA_FW_VERSIONS), - (EventName.startupMaster, TOYOTA.COROLLA, True, COROLLA_FW_VERSIONS), - - # DSU unplugged - (EventName.startupMaster, TOYOTA.COROLLA, True, COROLLA_FW_VERSIONS_NO_DSU), - (EventName.communityFeatureDisallowed, TOYOTA.COROLLA, False, COROLLA_FW_VERSIONS_NO_DSU), + (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS), + (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS), # dashcamOnly car - (EventName.startupNoControl, MAZDA.CX5, True, CX5_FW_VERSIONS), - (EventName.startupNoControl, MAZDA.CX5, False, CX5_FW_VERSIONS), + (EventName.startupNoControl, MAZDA.CX5, CX5_FW_VERSIONS), + (EventName.startupNoControl, MAZDA.CX5, CX5_FW_VERSIONS), # unrecognized car with no fw - (EventName.startupNoFw, None, True, None), - (EventName.startupNoFw, None, False, None), + (EventName.startupNoFw, None, None), + (EventName.startupNoFw, None, None), # unrecognized car - (EventName.startupNoCar, None, True, COROLLA_FW_VERSIONS[:1]), - (EventName.startupNoCar, None, False, COROLLA_FW_VERSIONS[:1]), + (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1]), + (EventName.startupNoCar, None, COROLLA_FW_VERSIONS[:1]), # fuzzy match - (EventName.startupMaster, TOYOTA.COROLLA, True, COROLLA_FW_VERSIONS_FUZZY), - (EventName.startupMaster, TOYOTA.COROLLA, False, COROLLA_FW_VERSIONS_FUZZY), + (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS_FUZZY), + (EventName.startupMaster, TOYOTA.COROLLA, COROLLA_FW_VERSIONS_FUZZY), ]) @with_processes(['controlsd']) - def test_startup_alert(self, expected_event, car_model, toggle_enabled, fw_versions): + def test_startup_alert(self, expected_event, car_model, fw_versions): # TODO: this should be done without any real sockets controls_sock = messaging.sub_sock("controlsState") @@ -76,7 +72,6 @@ class TestStartup(unittest.TestCase): params.clear_all() params.put_bool("Passive", False) params.put_bool("OpenpilotEnabledToggle", True) - params.put_bool("CommunityFeaturesToggle", toggle_enabled) # Build capnn version of FW array if fw_versions is not None: diff --git a/selfdrive/debug/README.md b/selfdrive/debug/README.md index abb0c3240a..d49db25d09 100644 --- a/selfdrive/debug/README.md +++ b/selfdrive/debug/README.md @@ -34,3 +34,26 @@ optional arguments: --addr ADDR --values VALUES values to monitor (instead of entire event) ``` + +## [vw_mqb_config.py](vw_mqb_config.py) + +``` +usage: vw_mqb_config.py [-h] [--debug] {enable,show,disable} + +Shows Volkswagen EPS software and coding info, and enables or disables Heading Control +Assist (Lane Assist). Useful for enabling HCA on cars without factory Lane Assist that want +to use openpilot integrated at the CAN gateway (J533). + +positional arguments: + {enable,show,disable} + show or modify current EPS HCA config + +optional arguments: + -h, --help show this help message and exit + --debug enable ISO-TP/UDS stack debugging output + +This tool is meant to run directly on a vehicle-installed comma two or comma three, with +the openpilot/tmux processes stopped. It should also work on a separate PC with a USB- +attached comma panda. Vehicle ignition must be on. Recommend engine not be running when +making changes. Must turn ignition off and on again for any changes to take effect. +``` diff --git a/selfdrive/debug/adb.sh b/selfdrive/debug/adb.sh index 82a3ddbb94..8a04d4aefd 100755 --- a/selfdrive/debug/adb.sh +++ b/selfdrive/debug/adb.sh @@ -1,8 +1,16 @@ #!/usr/bin/bash +set -e -# then, connect to computer: -# adb connect 192.168.5.11:5555 +PORT=5555 -setprop service.adb.tcp.port 5555 -stop adbd -start adbd +setprop service.adb.tcp.port $PORT +if [ -f /EON ]; then + stop adbd + start adbd +else + sudo systemctl start adbd +fi + +IP=$(echo $SSH_CONNECTION | awk '{ print $3}') +echo "then, connect on your computer:" +echo "adb connect $IP:$PORT" diff --git a/selfdrive/debug/profiling/snapdragon/.gitignore b/selfdrive/debug/profiling/snapdragon/.gitignore new file mode 100644 index 0000000000..5d3033efb3 --- /dev/null +++ b/selfdrive/debug/profiling/snapdragon/.gitignore @@ -0,0 +1 @@ +SnapdragonProfiler/ diff --git a/selfdrive/debug/profiling/snapdragon/README b/selfdrive/debug/profiling/snapdragon/README new file mode 100644 index 0000000000..ee826b413a --- /dev/null +++ b/selfdrive/debug/profiling/snapdragon/README @@ -0,0 +1,7 @@ +snapdragon profiler +-------- + +* download from https://developer.qualcomm.com/software/snapdragon-profiler +* unzip to selfdrive/debug/profiling/snapdragon/SnapdragonProfiler +* run ./setup-agnos.sh +* run selfdrive/debug/adb.sh on device diff --git a/selfdrive/debug/profiling/snapdragon/setup-agnos.sh b/selfdrive/debug/profiling/snapdragon/setup-agnos.sh new file mode 100755 index 0000000000..5099fb09cb --- /dev/null +++ b/selfdrive/debug/profiling/snapdragon/setup-agnos.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +# TODO: there's probably a better way to do this + +cd SnapdragonProfiler/service +mv android real_android +ln -s iot_rb5_lu/ android diff --git a/selfdrive/debug/vw_mqb_config.py b/selfdrive/debug/vw_mqb_config.py new file mode 100755 index 0000000000..1dd28d371f --- /dev/null +++ b/selfdrive/debug/vw_mqb_config.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 + +import argparse +import struct +from enum import IntEnum +from panda import Panda +from panda.python.uds import UdsClient, MessageTimeoutError, NegativeResponseError, SESSION_TYPE,\ + DATA_IDENTIFIER_TYPE, ACCESS_TYPE + +# TODO: extend UDS library to allow custom/vendor-defined data identifiers without ignoring type checks +class VOLKSWAGEN_DATA_IDENTIFIER_TYPE(IntEnum): + CODING = 0x0600 + +# TODO: extend UDS library security_access() to take an access level offset per ISO 14229-1:2020 10.4 and remove this +class ACCESS_TYPE_LEVEL_1(IntEnum): + REQUEST_SEED = ACCESS_TYPE.REQUEST_SEED + 2 + SEND_KEY = ACCESS_TYPE.SEND_KEY + 2 + +MQB_EPS_CAN_ADDR = 0x712 +RX_OFFSET = 0x6a + +if __name__ == "__main__": + desc_text = "Shows Volkswagen EPS software and coding info, and enables or disables Heading Control Assist " + \ + "(Lane Assist). Useful for enabling HCA on cars without factory Lane Assist that want to use " + \ + "openpilot integrated at the CAN gateway (J533)." + epilog_text = "This tool is meant to run directly on a vehicle-installed comma two or comma three, with the " + \ + "openpilot/tmux processes stopped. It should also work on a separate PC with a USB-attached comma " + \ + "panda. Vehicle ignition must be on. Recommend engine not be running when making changes. Must " + \ + "turn ignition off and on again for any changes to take effect." + parser = argparse.ArgumentParser(description=desc_text, epilog=epilog_text) + parser.add_argument("--debug", action="store_true", help="enable ISO-TP/UDS stack debugging output") + parser.add_argument("action", choices={"show", "enable", "disable"}, help="show or modify current EPS HCA config") + args = parser.parse_args() + + panda = Panda() + panda.set_safety_mode(Panda.SAFETY_ELM327) + bus = 1 if panda.has_obd() else 0 + uds_client = UdsClient(panda, MQB_EPS_CAN_ADDR, MQB_EPS_CAN_ADDR + RX_OFFSET, bus, timeout=0.2, debug=args.debug) + + try: + uds_client.diagnostic_session_control(SESSION_TYPE.EXTENDED_DIAGNOSTIC) + except MessageTimeoutError: + print("Timeout opening session with EPS") + quit() + + odx_file, current_coding = None, None + try: + hw_pn = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_HARDWARE_NUMBER).decode("utf-8") + sw_pn = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER).decode("utf-8") + sw_ver = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_ECU_SOFTWARE_VERSION_NUMBER).decode("utf-8") + component = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.SYSTEM_NAME_OR_ENGINE_TYPE).decode("utf-8") + odx_file = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.ODX_FILE).decode("utf-8") + current_coding = uds_client.read_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING) # type: ignore + coding_text = current_coding.hex() + + print("\nEPS diagnostic data\n") + print(f" Part No HW: {hw_pn}") + print(f" Part No SW: {sw_pn}") + print(f" SW Version: {sw_ver}") + print(f" Component: {component}") + print(f" Coding: {coding_text}") + print(f" ASAM Dataset: {odx_file}") + except NegativeResponseError: + print("Error fetching data from EPS") + quit() + except MessageTimeoutError: + print("Timeout fetching data from EPS") + quit() + + coding_variant, current_coding_array = None, None + # EV_SteerAssisMQB covers the majority of MQB racks (EPS_MQB_ZFLS) + # APA racks (MQB_PP_APA) have a different coding layout, which should + # be easy to support once we identify the specific config bit + if odx_file == "EV_SteerAssisMQB\x00": + coding_variant = "ZF" + current_coding_array = struct.unpack("!4B", current_coding) + hca_enabled = (current_coding_array[0] & (1 << 4) != 0) + hca_text = ("DISABLED", "ENABLED")[hca_enabled] + print(f" Lane Assist: {hca_text}") + else: + print("Configuration changes not yet supported on this EPS!") + quit() + + try: + params = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.APPLICATION_DATA_IDENTIFICATION).decode("utf-8") + param_version_system_params = params[1:3] + param_vehicle_type = params[3:5] + param_index_char_curve = params[5:7] + param_version_char_values = params[7:9] + param_version_memory_map = params[9:11] + print("\nEPS parameterization (per-vehicle calibration) data\n") + print(f" Version of system parameters: {param_version_system_params}") + print(f" Vehicle type: {param_vehicle_type}") + print(f" Index of characteristic curve: {param_index_char_curve}") + print(f" Version of characteristic values: {param_version_char_values}") + print(f" Version of memory map: {param_version_memory_map}") + except (NegativeResponseError, MessageTimeoutError): + print("Error fetching parameterization data from EPS!") + quit() + + if args.action in ["enable", "disable"]: + print("\nAttempting configuration update") + + assert(coding_variant == "ZF") # revisit when we have the APA rack coding bit + # ZF EPS config coding length can be anywhere from 1 to 4 bytes, but the + # bit we care about is always in the same place in the first byte + if args.action == "enable": + new_byte_0 = current_coding_array[0] | (1 << 4) + else: + new_byte_0 = current_coding_array[0] & ~(1 << 4) + new_coding = new_byte_0.to_bytes(1, "little") + current_coding[1:] + + try: + seed = uds_client.security_access(ACCESS_TYPE_LEVEL_1.REQUEST_SEED) # type: ignore + key = struct.unpack("!I", seed)[0] + 28183 # yeah, it's like that + uds_client.security_access(ACCESS_TYPE_LEVEL_1.SEND_KEY, struct.pack("!I", key)) # type: ignore + except (NegativeResponseError, MessageTimeoutError): + print("Security access failed!") + quit() + + try: + # Programming date and tester number must be written before making + # a change, or write to CODING will fail with request sequence error + # Encoding on tester is unclear, it contains the workshop code in the + # last two bytes, but not the VZ/importer or tester serial number + # Can't seem to read it back, but we can read the calibration tester, + # so fib a little and say that same tester did the programming + # TODO: encode the actual current date + prog_date = b'\x22\x02\x08' + uds_client.write_data_by_identifier(DATA_IDENTIFIER_TYPE.PROGRAMMING_DATE, prog_date) + tester_num = uds_client.read_data_by_identifier(DATA_IDENTIFIER_TYPE.CALIBRATION_REPAIR_SHOP_CODE_OR_CALIBRATION_EQUIPMENT_SERIAL_NUMBER) + uds_client.write_data_by_identifier(DATA_IDENTIFIER_TYPE.REPAIR_SHOP_CODE_OR_TESTER_SERIAL_NUMBER, tester_num) + uds_client.write_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING, new_coding) # type: ignore + except (NegativeResponseError, MessageTimeoutError): + print("Writing new configuration failed!") + quit() + + try: + # Read back result just to make 100% sure everything worked + current_coding_text = uds_client.read_data_by_identifier(VOLKSWAGEN_DATA_IDENTIFIER_TYPE.CODING).hex() # type: ignore + print(f" New coding: {current_coding_text}") + except (NegativeResponseError, MessageTimeoutError): + print("Reading back updated coding failed!") + quit() + print("EPS configuration successfully updated") diff --git a/selfdrive/hardware/base.py b/selfdrive/hardware/base.py index 77ddcbc2d4..b551138ce3 100644 --- a/selfdrive/hardware/base.py +++ b/selfdrive/hardware/base.py @@ -67,6 +67,10 @@ class HardwareBase(ABC): def get_network_strength(self, network_type): pass + @staticmethod + def set_bandwidth_limit(upload_speed_kbps: int, download_speed_kbps: int) -> None: + pass + @abstractmethod def get_battery_capacity(self): pass diff --git a/selfdrive/hardware/eon/androidd.py b/selfdrive/hardware/eon/androidd.py index b836eb0129..3d91468b90 100755 --- a/selfdrive/hardware/eon/androidd.py +++ b/selfdrive/hardware/eon/androidd.py @@ -4,13 +4,14 @@ import time import psutil from typing import Optional +import cereal.messaging as messaging from common.realtime import set_core_affinity, set_realtime_priority from selfdrive.swaglog import cloudlog MAX_MODEM_CRASHES = 3 MODEM_PATH = "/sys/devices/soc/2080000.qcom,mss/subsys5" -WATCHED_PROCS = ["zygote", "zygote64", "/system/bin/servicemanager", "/system/bin/surfaceflinger"] +WATCHED_PROCS = ["zygote", "zygote64", "system_server", "/system/bin/servicemanager", "/system/bin/surfaceflinger"] def get_modem_crash_count() -> Optional[int]: @@ -37,6 +38,8 @@ def main(): crash_count = 0 modem_killed = False modem_state = "ONLINE" + androidLog = messaging.sub_sock('androidLog') + while True: # check critical android services if any(p is None or not p.is_running() for p in procs.values()) or not len(procs): @@ -49,9 +52,18 @@ def main(): if len(procs): for p in WATCHED_PROCS: if cur[p] != procs[p]: - cloudlog.event("android service pid changed", proc=p, cur=cur[p], prev=procs[p]) + cloudlog.event("android service pid changed", proc=p, cur=cur[p], prev=procs[p], error=True) procs.update(cur) + # log caught NetworkPolicy exceptions + msgs = messaging.drain_sock(androidLog) + for m in msgs: + try: + if m.androidLog.tag == "NetworkPolicy" and m.androidLog.message.startswith("problem with advise persist threshold"): + cloudlog.event("network policy exception caught", androidLog=m.androidLog, error=True) + except UnicodeDecodeError: + pass + if os.path.exists(MODEM_PATH): # check modem state state = get_modem_state() @@ -68,7 +80,7 @@ def main(): # handle excessive modem crashes if crash_count > MAX_MODEM_CRASHES and not modem_killed: - cloudlog.event("killing modem") + cloudlog.event("killing modem", error=True) with open("/sys/kernel/debug/msm_subsys/modem", "w") as f: f.write("put") modem_killed = True diff --git a/selfdrive/hardware/eon/neos.json b/selfdrive/hardware/eon/neos.json index 228029e9fb..4010f7126a 100644 --- a/selfdrive/hardware/eon/neos.json +++ b/selfdrive/hardware/eon/neos.json @@ -1,7 +1,7 @@ { - "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-5dc2575d713977666a8e14ae1b43a04d7f63123934c80fa10751d949a107653e.zip", - "ota_hash": "5dc2575d713977666a8e14ae1b43a04d7f63123934c80fa10751d949a107653e", - "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-f01a55c9ba52ca57668d1684c6bf4118efd31916b04f8c1fcd8495013d3677eb.img", + "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-50da8800caa5cbc224acbaa21f3a83d21802a31d89cccfc62a898903a8eb19e7.zip", + "ota_hash": "50da8800caa5cbc224acbaa21f3a83d21802a31d89cccfc62a898903a8eb19e7", + "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-fe76739438d28ea6111853a737b616afdf340ba75c01c0ee99e6a28c19ecc29f.img", "recovery_len": 15222060, - "recovery_hash": "f01a55c9ba52ca57668d1684c6bf4118efd31916b04f8c1fcd8495013d3677eb" + "recovery_hash": "fe76739438d28ea6111853a737b616afdf340ba75c01c0ee99e6a28c19ecc29f" } diff --git a/selfdrive/hardware/eon/shutdownd.py b/selfdrive/hardware/eon/shutdownd.py index e4ffa79ef3..15112e7d5e 100755 --- a/selfdrive/hardware/eon/shutdownd.py +++ b/selfdrive/hardware/eon/shutdownd.py @@ -8,18 +8,25 @@ from selfdrive.hardware.eon.hardware import getprop from selfdrive.swaglog import cloudlog def main(): + prev = b"" params = Params() while True: - # 0 for shutdown, 1 for reboot - prop = getprop("sys.shutdown.requested") - if prop is not None and len(prop) > 0: - os.system("pkill -9 loggerd") - params.put("LastSystemShutdown", f"'{prop}' {datetime.datetime.now()}") - print("shutdown detected", repr(prop)) + with open("/dev/__properties__", 'rb') as f: + cur = f.read() - time.sleep(120) - cloudlog.error('shutdown false positive') - break + if cur != prev: + prev = cur + + # 0 for shutdown, 1 for reboot + prop = getprop("sys.shutdown.requested") + if prop is not None and len(prop) > 0: + os.system("pkill -9 loggerd") + params.put("LastSystemShutdown", f"'{prop}' {datetime.datetime.now()}") + os.sync() + + time.sleep(120) + cloudlog.error('shutdown false positive') + break time.sleep(0.1) diff --git a/selfdrive/hardware/hw.h b/selfdrive/hardware/hw.h index d7a6c8d1d5..f18ede01ec 100644 --- a/selfdrive/hardware/hw.h +++ b/selfdrive/hardware/hw.h @@ -20,17 +20,16 @@ public: #endif namespace Path { -inline static std::string HOME = util::getenv("HOME"); inline std::string log_root() { if (const char *env = getenv("LOG_ROOT")) { return env; } - return Hardware::PC() ? HOME + "/.comma/media/0/realdata" : "/data/media/0/realdata"; + return Hardware::PC() ? util::getenv("HOME") + "/.comma/media/0/realdata" : "/data/media/0/realdata"; } inline std::string params() { - return Hardware::PC() ? HOME + "/.comma/params" : "/data/params"; + return Hardware::PC() ? util::getenv("HOME") + "/.comma/params" : "/data/params"; } inline std::string rsa_file() { - return Hardware::PC() ? HOME + "/.comma/persist/comma/id_rsa" : "/persist/comma/id_rsa"; + return Hardware::PC() ? util::getenv("HOME") + "/.comma/persist/comma/id_rsa" : "/persist/comma/id_rsa"; } } // namespace Path diff --git a/selfdrive/hardware/tici/agnos.json b/selfdrive/hardware/tici/agnos.json index f22575a335..6674ddaa5b 100644 --- a/selfdrive/hardware/tici/agnos.json +++ b/selfdrive/hardware/tici/agnos.json @@ -1,10 +1,10 @@ [ { "name": "boot", - "url": "https://commadist.azureedge.net/agnosupdate/boot-5ad783213b7de18400f5fd3609fe75677fec80780ae31cbdf5a8ee7106675d7c.img.xz", - "hash": "5ad783213b7de18400f5fd3609fe75677fec80780ae31cbdf5a8ee7106675d7c", - "hash_raw": "5ad783213b7de18400f5fd3609fe75677fec80780ae31cbdf5a8ee7106675d7c", - "size": 14768128, + "url": "https://commadist.azureedge.net/agnosupdate/boot-0a86272196cb54607f8ba082f412fed4607baaf6ce3eb8dc8e950b4a1d763954.img.xz", + "hash": "0a86272196cb54607f8ba082f412fed4607baaf6ce3eb8dc8e950b4a1d763954", + "hash_raw": "0a86272196cb54607f8ba082f412fed4607baaf6ce3eb8dc8e950b4a1d763954", + "size": 14776320, "sparse": false, "full_check": true, "has_ab": true @@ -41,9 +41,9 @@ }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-0fee88a42385d067756e9b25d57a80228835310deb7b5eef7b7bed5c22c45515.img.xz", - "hash": "a043cba1ae08ca6d17704a8a0978b1e27e5bc79abb85b97efd35203ae26ae1ea", - "hash_raw": "0fee88a42385d067756e9b25d57a80228835310deb7b5eef7b7bed5c22c45515", + "url": "https://commadist.azureedge.net/agnosupdate/system-9b0b534ed0c35c25850dbb73d3f052611f2e2c9db32410edc25d75fbcfc6c15e.img.xz", + "hash": "b1f622f00037bbdb28c0a6016c0e42fa7f87e99591ff2417c757a67ff559b526", + "hash_raw": "9b0b534ed0c35c25850dbb73d3f052611f2e2c9db32410edc25d75fbcfc6c15e", "size": 10737418240, "sparse": true, "full_check": false, diff --git a/selfdrive/hardware/tici/hardware.py b/selfdrive/hardware/tici/hardware.py index 31bfa736c0..41ae316d3c 100644 --- a/selfdrive/hardware/tici/hardware.py +++ b/selfdrive/hardware/tici/hardware.py @@ -218,6 +218,65 @@ class Tici(HardwareBase): return network_strength + @staticmethod + def set_bandwidth_limit(upload_speed_kbps: int, download_speed_kbps: int) -> None: + upload_speed_kbps = int(upload_speed_kbps) # Ensure integer value + download_speed_kbps = int(download_speed_kbps) # Ensure integer value + + adapter = "wwan0" + ifb = "ifb0" + + sudo = ["sudo"] + tc = sudo + ["tc"] + + # check, cmd + cleanup = [ + # Clean up old rules + (False, tc + ["qdisc", "del", "dev", adapter, "root"]), + (False, tc + ["qdisc", "del", "dev", ifb, "root"]), + (False, tc + ["qdisc", "del", "dev", adapter, "ingress"]), + (False, tc + ["qdisc", "del", "dev", ifb, "ingress"]), + + # Bring ifb0 down + (False, sudo + ["ip", "link", "set", "dev", ifb, "down"]), + ] + + upload = [ + # Create root Hierarchy Token Bucket that sends all trafic to 1:20 + (True, tc + ["qdisc", "add", "dev", adapter, "root", "handle", "1:", "htb", "default", "20"]), + + # Create class 1:20 with specified rate limit + (True, tc + ["class", "add", "dev", adapter, "parent", "1:", "classid", "1:20", "htb", "rate", f"{upload_speed_kbps}kbit"]), + + # Create universal 32 bit filter on adapter that sends all outbound ip traffic through the class + (True, tc + ["filter", "add", "dev", adapter, "parent", "1:", "protocol", "ip", "prio", "10", "u32", "match", "ip", "dst", "0.0.0.0/0", "flowid", "1:20"]), + ] + + download = [ + # Bring ifb0 up + (True, sudo + ["ip", "link", "set", "dev", ifb, "up"]), + + # Redirect ingress (incoming) to egress ifb0 + (True, tc + ["qdisc", "add", "dev", adapter, "handle", "ffff:", "ingress"]), + (True, tc + ["filter", "add", "dev", adapter, "parent", "ffff:", "protocol", "ip", "u32", "match", "u32", "0", "0", "action", "mirred", "egress", "redirect", "dev", ifb]), + + # Add class and rules for virtual interface + (True, tc + ["qdisc", "add", "dev", ifb, "root", "handle", "2:", "htb"]), + (True, tc + ["class", "add", "dev", ifb, "parent", "2:", "classid", "2:1", "htb", "rate", f"{download_speed_kbps}kbit"]), + + # Add filter to rule for IP address + (True, tc + ["filter", "add", "dev", ifb, "protocol", "ip", "parent", "2:", "prio", "1", "u32", "match", "ip", "src", "0.0.0.0/0", "flowid", "2:1"]), + ] + + commands = cleanup + if upload_speed_kbps != -1: + commands += upload + if download_speed_kbps != -1: + commands += download + + for check, cmd in commands: + subprocess.run(cmd, check=check) + def get_modem_version(self): try: modem = self.get_modem() diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index ae191bbebb..ae314e38c4 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -6,17 +6,20 @@ While the roll calibration is a real value that can be estimated, here we assume and the image input into the neural network is not corrected for roll. ''' +import gc import os -from typing import NoReturn import numpy as np -import cereal.messaging as messaging +from typing import NoReturn + from cereal import log -from selfdrive.hardware import TICI +import cereal.messaging as messaging from common.params import Params, put_nonblocking +from common.realtime import set_realtime_priority from common.transformations.model import model_height from common.transformations.camera import get_view_frame_from_road_frame from common.transformations.orientation import rot_from_euler, euler_from_rot from selfdrive.config import Conversions as CV +from selfdrive.hardware import TICI from selfdrive.swaglog import cloudlog MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS @@ -191,6 +194,9 @@ class Calibrator(): def calibrationd_thread(sm=None, pm=None) -> NoReturn: + gc.disable() + set_realtime_priority(1) + if sm is None: sm = messaging.SubMaster(['cameraOdometry', 'carState'], poll=['cameraOdometry']) diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 031c56ceb9..75534efa5a 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -71,11 +71,11 @@ class CarKalman(KalmanFilter): P_initial = Q.copy() obs_noise: Dict[int, Any] = { - ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.01)**2), + ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2), ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2), ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2), ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2), - ObservationKind.STIFFNESS: np.atleast_2d(5.0**2), + ObservationKind.STIFFNESS: np.atleast_2d(0.5**2), ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2), } @@ -106,9 +106,9 @@ class CarKalman(KalmanFilter): state = sp.Matrix(state_sym) # Vehicle model constants - x = state[States.STIFFNESS, :][0, 0] + sf = state[States.STIFFNESS, :][0, 0] - cF, cR = x * cF_orig, x * cR_orig + cF, cR = sf * cF_orig, sf * cR_orig angle_offset = state[States.ANGLE_OFFSET, :][0, 0] angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0] theta = state[States.ROAD_ROLL, :][0, 0] @@ -154,7 +154,7 @@ class CarKalman(KalmanFilter): [sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None], [sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None], [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None], - [sp.Matrix([x]), ObservationKind.STIFFNESS, None], + [sp.Matrix([sf]), ObservationKind.STIFFNESS, None], [sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None], ] diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 9557568ba2..0efb4d04be 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -45,7 +45,7 @@ class ParamsLearner: yaw_rate_std = msg.angularVelocityCalibrated.std[2] localizer_roll = msg.orientationNED.value[0] - localizer_roll_std = msg.orientationNED.std[0] + localizer_roll_std = np.radians(1) if np.isnan(msg.orientationNED.std[0]) else msg.orientationNED.std[0] roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX if roll_valid: roll = localizer_roll @@ -76,6 +76,14 @@ class ParamsLearner: np.array([np.atleast_2d(roll_std**2)])) self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]])) + # We observe the current stiffness and steer ratio (with a high observation noise) to bound + # the respective estimate STD. Otherwise the STDs keep increasing, causing rapid changes in the + # states in longer routes (especially straight stretches). + stiffness = float(self.kf.x[States.STIFFNESS]) + steer_ratio = float(self.kf.x[States.STEER_RATIO]) + self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, np.array([[stiffness]])) + self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, np.array([[steer_ratio]])) + elif which == 'carState': self.steering_angle = msg.steeringAngleDeg self.steering_pressed = msg.steeringPressed diff --git a/selfdrive/logcatd/logcatd_android.cc b/selfdrive/logcatd/logcatd_android.cc index 4452e2f093..8c2524d94a 100644 --- a/selfdrive/logcatd/logcatd_android.cc +++ b/selfdrive/logcatd/logcatd_android.cc @@ -1,74 +1,51 @@ -#include -#include - #include #include #include +#include + +#include #include "cereal/messaging/messaging.h" -#include "selfdrive/common/util.h" + +#undef LOG_ID_KERNEL +#define LOG_ID_KERNEL 5 int main() { + std::signal(SIGINT, exit); + std::signal(SIGTERM, exit); setpriority(PRIO_PROCESS, 0, -15); - ExitHandler do_exit; - PubMaster pm({"androidLog"}); - - struct timespec cur_time; - clock_gettime(CLOCK_REALTIME, &cur_time); - log_time last_log_time(cur_time); - logger_list *logger_list = nullptr; - - while (!do_exit) { - // setup android logging - if (!logger_list) { - logger_list = android_logger_list_alloc_time(ANDROID_LOG_RDONLY | ANDROID_LOG_NONBLOCK, last_log_time, 0); - } - assert(logger_list); - - struct logger *main_logger = android_logger_open(logger_list, LOG_ID_MAIN); - assert(main_logger); - struct logger *radio_logger = android_logger_open(logger_list, LOG_ID_RADIO); - assert(radio_logger); - struct logger *system_logger = android_logger_open(logger_list, LOG_ID_SYSTEM); - assert(system_logger); - struct logger *crash_logger = android_logger_open(logger_list, LOG_ID_CRASH); - assert(crash_logger); - struct logger *kernel_logger = android_logger_open(logger_list, (log_id_t)5); // LOG_ID_KERNEL - assert(kernel_logger); - - while (!do_exit) { - log_msg log_msg; - int err = android_logger_list_read(logger_list, &log_msg); - if (err <= 0) break; - - AndroidLogEntry entry; - err = android_log_processLogBuffer(&log_msg.entry_v1, &entry); - if (err < 0) continue; - last_log_time.tv_sec = entry.tv_sec; - last_log_time.tv_nsec = entry.tv_nsec; - - MessageBuilder msg; - auto androidEntry = msg.initEvent().initAndroidLog(); - androidEntry.setId(log_msg.id()); - androidEntry.setTs(entry.tv_sec * 1000000000ULL + entry.tv_nsec); - androidEntry.setPriority(entry.priority); - androidEntry.setPid(entry.pid); - androidEntry.setTid(entry.tid); - androidEntry.setTag(entry.tag); - androidEntry.setMessage(entry.message); - - pm.send("androidLog", msg); - } - - android_logger_list_free(logger_list); - logger_list = NULL; - util::sleep_for(500); + // setup android logging + logger_list *logger_list = android_logger_list_alloc(ANDROID_LOG_RDONLY, 0, 0); + assert(logger_list); + for (auto log_id : {LOG_ID_MAIN, LOG_ID_RADIO, LOG_ID_SYSTEM, LOG_ID_CRASH, (log_id_t)LOG_ID_KERNEL}) { + logger *logger = android_logger_open(logger_list, log_id); + assert(logger); } - if (logger_list) { - android_logger_list_free(logger_list); + PubMaster pm({"androidLog"}); + + while (true) { + log_msg log_msg; + int err = android_logger_list_read(logger_list, &log_msg); + if (err <= 0) break; + + AndroidLogEntry entry; + err = android_log_processLogBuffer(&log_msg.entry_v1, &entry); + if (err < 0) continue; + + MessageBuilder msg; + auto androidEntry = msg.initEvent().initAndroidLog(); + androidEntry.setId(log_msg.id()); + androidEntry.setTs(entry.tv_sec * NS_PER_SEC + entry.tv_nsec); + androidEntry.setPriority(entry.priority); + androidEntry.setPid(entry.pid); + androidEntry.setTid(entry.tid); + androidEntry.setTag(entry.tag); + androidEntry.setMessage(entry.message); + pm.send("androidLog", msg); } + android_logger_list_free(logger_list); return 0; } diff --git a/selfdrive/logcatd/tests/test_logcatd_android.py b/selfdrive/logcatd/tests/test_logcatd_android.py index 5b908d3e87..4e0c903dfa 100755 --- a/selfdrive/logcatd/tests/test_logcatd_android.py +++ b/selfdrive/logcatd/tests/test_logcatd_android.py @@ -4,6 +4,7 @@ import random import string import time import unittest +import uuid import cereal.messaging as messaging from selfdrive.test.helpers import with_processes @@ -18,34 +19,27 @@ class TestLogcatdAndroid(unittest.TestCase): time.sleep(1) messaging.drain_sock(sock) + sent_msgs = {} for _ in range(random.randint(2, 10)): # write some log messages - sent_msgs = {} for __ in range(random.randint(5, 50)): + tag = uuid.uuid4().hex msg = ''.join(random.choice(string.ascii_letters) for _ in range(random.randrange(2, 50))) - if msg in sent_msgs: - continue - sent_msgs[msg] = ''.join(random.choice(string.ascii_letters) for _ in range(random.randrange(2, 20))) - os.system(f"log -t '{sent_msgs[msg]}' '{msg}'") + sent_msgs[tag] = {'recv_cnt': 0, 'msg': msg} + os.system(f"log -t '{tag}' '{msg}'") time.sleep(1) msgs = messaging.drain_sock(sock) for m in msgs: self.assertTrue(m.valid) self.assertLess(time.monotonic() - (m.logMonoTime / 1e9), 30) + tag = m.androidLog.tag + if tag in sent_msgs: + sent_msgs[tag]['recv_cnt'] += 1 + self.assertEqual(m.androidLog.message.strip(), sent_msgs[tag]['msg']) - recv_msg = m.androidLog.message.strip() - if recv_msg not in sent_msgs: - continue - - # see https://android.googlesource.com/platform/system/core/+/android-2.1_r1/liblog/logd_write.c#144 - radio_msg = m.androidLog.id == 1 and m.androidLog.tag.startswith("use-Rlog/RLOG-") - if m.androidLog.tag == sent_msgs[recv_msg] or radio_msg: - del sent_msgs[recv_msg] - - # ensure we received all the logs we sent - self.assertEqual(len(sent_msgs), 0) - + for v in sent_msgs.values(): + self.assertEqual(v['recv_cnt'], 1) if __name__ == "__main__": unittest.main() diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript index 2adcfb846c..76fbcae6c4 100644 --- a/selfdrive/loggerd/SConscript +++ b/selfdrive/loggerd/SConscript @@ -1,13 +1,12 @@ Import('env', 'arch', 'cereal', 'messaging', 'common', 'visionipc', 'gpucommon') -logger_lib = env.Library('logger', ["logger.cc"]) -libs = [logger_lib, common, cereal, messaging, visionipc, +libs = [common, cereal, messaging, visionipc, 'zmq', 'capnp', 'kj', 'z', 'avformat', 'avcodec', 'swscale', 'avutil', 'yuv', 'bz2', 'OpenCL'] -src = ['loggerd.cc'] +src = ['logger.cc', 'loggerd.cc'] if arch in ["aarch64", "larch64"]: src += ['omx_encoder.cc'] libs += ['OmxCore', 'gsl', 'CB'] + gpucommon @@ -24,8 +23,11 @@ if arch == "Darwin": del libs[libs.index('OpenCL')] env['FRAMEWORKS'] = ['OpenCL'] -env.Program('loggerd', ['main.cc'] + src, LIBS=libs) +logger_lib = env.Library('logger', src) +libs.insert(0, logger_lib) + +env.Program('loggerd', ['main.cc'], LIBS=libs) env.Program('bootlog.cc', LIBS=libs) if GetOption('test'): - env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_loggerd.cc', 'tests/test_logger.cc', env.Object('logger_util', '#/selfdrive/ui/replay/util.cc')] + src, LIBS=[libs] + ['curl', 'crypto', 'bz2']) + env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_loggerd.cc', 'tests/test_logger.cc', env.Object('logger_util', '#/selfdrive/ui/replay/util.cc')], LIBS=libs + ['curl', 'crypto']) diff --git a/selfdrive/loggerd/bootlog.cc b/selfdrive/loggerd/bootlog.cc index 478eb16852..c5897091dc 100644 --- a/selfdrive/loggerd/bootlog.cc +++ b/selfdrive/loggerd/bootlog.cc @@ -23,20 +23,13 @@ static kj::Array build_boot_log() { std::string pstore = "/sys/fs/pstore"; std::map pstore_map = util::read_files_in_dir(pstore); - const std::vector log_keywords = {"Kernel panic"}; - auto lpstore = boot.initPstore().initEntries(pstore_map.size()); int i = 0; + auto lpstore = boot.initPstore().initEntries(pstore_map.size()); for (auto& kv : pstore_map) { auto lentry = lpstore[i]; lentry.setKey(kv.first); lentry.setValue(capnp::Data::Reader((const kj::byte*)kv.second.data(), kv.second.size())); i++; - - for (auto &k : log_keywords) { - if (kv.second.find(k) != std::string::npos) { - LOGE("%s: found '%s'", kv.first.c_str(), k.c_str()); - } - } } // Gather output of commands diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 37f03ef4e5..d402c5787f 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -11,14 +11,14 @@ bool sync_encoders(LoggerdState *s, CameraType cam_type, uint32_t frame_id) { update_max_atomic(s->start_frame_id, frame_id + 2); if (std::exchange(s->camera_ready[cam_type], true) == false) { ++s->encoders_ready; - LOGE("camera %d encoder ready", cam_type); + LOGD("camera %d encoder ready", cam_type); } return false; } else { if (s->max_waiting == 1) update_max_atomic(s->start_frame_id, frame_id); bool synced = frame_id >= s->start_frame_id; s->camera_synced[cam_type] = synced; - if (!synced) LOGE("camera %d waiting for frame %d, cur %d", cam_type, (int)s->start_frame_id, frame_id); + if (!synced) LOGD("camera %d waiting for frame %d, cur %d", cam_type, (int)s->start_frame_id, frame_id); return synced; } } @@ -244,7 +244,7 @@ void loggerd_thread() { count++; if (count >= 200) { - LOGE("large volume of '%s' messages", qs.name.c_str()); + LOGD("large volume of '%s' messages", qs.name.c_str()); break; } } diff --git a/selfdrive/loggerd/loggerd.h b/selfdrive/loggerd/loggerd.h index bdf5ef8f9d..caacbc591f 100644 --- a/selfdrive/loggerd/loggerd.h +++ b/selfdrive/loggerd/loggerd.h @@ -80,7 +80,7 @@ const LogCameraInfo cameras_logged[] = { .downscale = false, .has_qcamera = false, .trigger_rotate = Hardware::TICI(), - .enable = !Hardware::PC(), + .enable = true, .record = Params().getBool("RecordFront"), }, { diff --git a/selfdrive/loggerd/raw_logger.cc b/selfdrive/loggerd/raw_logger.cc index b0f9f7a9c8..f103822a13 100644 --- a/selfdrive/loggerd/raw_logger.cc +++ b/selfdrive/loggerd/raw_logger.cc @@ -11,6 +11,8 @@ #define __STDC_CONSTANT_MACROS +#include "libyuv.h" + extern "C" { #include #include @@ -55,6 +57,10 @@ RawLogger::RawLogger(const char* filename, int width, int height, int fps, frame->linesize[0] = width; frame->linesize[1] = width/2; frame->linesize[2] = width/2; + + if (downscale) { + downscale_buf.resize(width * height * 3 / 2); + } } RawLogger::~RawLogger() { @@ -64,7 +70,7 @@ RawLogger::~RawLogger() { } void RawLogger::encoder_open(const char* path) { - vid_path = util::string_format("%s/%s.mkv", path, filename); + vid_path = util::string_format("%s/%s", path, filename); // create camera lock file lock_path = util::string_format("%s/%s.lock", path, filename); @@ -76,7 +82,7 @@ void RawLogger::encoder_open(const char* path) { close(lock_fd); format_ctx = NULL; - avformat_alloc_output_context2(&format_ctx, NULL, NULL, vid_path.c_str()); + avformat_alloc_output_context2(&format_ctx, NULL, "matroska", vid_path.c_str()); assert(format_ctx); stream = avformat_new_stream(format_ctx, codec); @@ -124,9 +130,27 @@ int RawLogger::encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const ui pkt.data = NULL; pkt.size = 0; - frame->data[0] = (uint8_t*)y_ptr; - frame->data[1] = (uint8_t*)u_ptr; - frame->data[2] = (uint8_t*)v_ptr; + if (downscale_buf.size() > 0) { + uint8_t *out_y = downscale_buf.data(); + uint8_t *out_u = out_y + codec_ctx->width * codec_ctx->height; + uint8_t *out_v = out_u + (codec_ctx->width / 2) * (codec_ctx->height / 2); + libyuv::I420Scale(y_ptr, in_width, + u_ptr, in_width/2, + v_ptr, in_width/2, + in_width, in_height, + out_y, codec_ctx->width, + out_u, codec_ctx->width/2, + out_v, codec_ctx->width/2, + codec_ctx->width, codec_ctx->height, + libyuv::kFilterNone); + frame->data[0] = out_y; + frame->data[1] = out_u; + frame->data[2] = out_v; + } else { + frame->data[0] = (uint8_t*)y_ptr; + frame->data[1] = (uint8_t*)u_ptr; + frame->data[2] = (uint8_t*)v_ptr; + } frame->pts = counter; int ret = counter; diff --git a/selfdrive/loggerd/raw_logger.h b/selfdrive/loggerd/raw_logger.h index 9cef7ddcab..75d906784d 100644 --- a/selfdrive/loggerd/raw_logger.h +++ b/selfdrive/loggerd/raw_logger.h @@ -39,4 +39,5 @@ private: AVFormatContext *format_ctx = NULL; AVFrame *frame = NULL; + std::vector downscale_buf; }; diff --git a/selfdrive/loggerd/tests/test_loggerd.py b/selfdrive/loggerd/tests/test_loggerd.py index 20cd61bad5..13e0b720d8 100755 --- a/selfdrive/loggerd/tests/test_loggerd.py +++ b/selfdrive/loggerd/tests/test_loggerd.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import numpy as np import os import random import string @@ -14,12 +15,14 @@ from cereal.services import service_list from common.basedir import BASEDIR from common.params import Params from common.timeout import Timeout -from selfdrive.hardware import PC, TICI +from selfdrive.hardware import TICI from selfdrive.loggerd.config import ROOT from selfdrive.manager.process_config import managed_processes -from selfdrive.test.helpers import with_processes from selfdrive.version import get_version from tools.lib.logreader import LogReader +from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error +from common.transformations.camera import eon_f_frame_size, tici_f_frame_size, \ + eon_d_frame_size, tici_d_frame_size, tici_e_frame_size SentinelType = log.Sentinel.SentinelType @@ -28,12 +31,6 @@ CEREAL_SERVICES = [f for f in log.Event.schema.union_fields if f in service_list class TestLoggerd(unittest.TestCase): - # TODO: all tests should work on PC - @classmethod - def setUpClass(cls): - if PC: - raise unittest.SkipTest - def _get_latest_log_dir(self): log_dirs = sorted(Path(ROOT).iterdir(), key=lambda f: f.stat().st_mtime) return log_dirs[-1] @@ -107,25 +104,41 @@ class TestLoggerd(unittest.TestCase): for _, k, v in fake_params: self.assertEqual(getattr(initData, k), v) - # TODO: this shouldn't need camerad - @with_processes(['camerad']) def test_rotation(self): os.environ["LOGGERD_TEST"] = "1" Params().put("RecordFront", "1") + expected_files = {"rlog.bz2", "qlog.bz2", "qcamera.ts", "fcamera.hevc", "dcamera.hevc"} + streams = [(VisionStreamType.VISION_STREAM_ROAD, tici_f_frame_size if TICI else eon_f_frame_size, "roadCameraState"), + (VisionStreamType.VISION_STREAM_DRIVER, tici_d_frame_size if TICI else eon_d_frame_size, "driverCameraState")] if TICI: expected_files.add("ecamera.hevc") + streams.append((VisionStreamType.VISION_STREAM_WIDE_ROAD, tici_e_frame_size, "wideRoadCameraState")) - # give camerad time to start - time.sleep(3) + pm = messaging.PubMaster(["roadCameraState", "driverCameraState", "wideRoadCameraState"]) + vipc_server = VisionIpcServer("camerad") + for stream_type, frame_size, _ in streams: + vipc_server.create_buffers(stream_type, 40, False, *(frame_size)) + vipc_server.start_listener() for _ in range(5): num_segs = random.randint(2, 5) length = random.randint(1, 3) os.environ["LOGGERD_SEGMENT_LENGTH"] = str(length) - managed_processes["loggerd"].start() - time.sleep(num_segs*length + 1) + + fps = 20.0 + for n in range(1, int(num_segs*length*fps)+1): + for stream_type, frame_size, state in streams: + dat = np.empty(int(frame_size[0]*frame_size[1]*3/2), dtype=np.uint8) + vipc_server.send(stream_type, dat[:].flatten().tobytes(), n, n/fps, n/fps) + + camera_state = messaging.new_message(state) + frame = getattr(camera_state, state) + frame.frameId = n + pm.send(state, camera_state) + time.sleep(1.0/fps) + managed_processes["loggerd"].stop() route_path = str(self._get_latest_log_dir()).rsplit("--", 1)[0] diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index c75b34e9cb..2013a522bc 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -165,16 +165,14 @@ class Uploader(): return self.last_resp - def upload(self, key, fn): + def upload(self, key, fn, network_type): try: sz = os.path.getsize(fn) except OSError: cloudlog.exception("upload: getsize failed") return False - cloudlog.event("upload", key=key, fn=fn, sz=sz) - - cloudlog.debug("checking %r with size %r", key, sz) + cloudlog.event("upload_start", key=key, fn=fn, sz=sz, network_type=network_type) if sz == 0: try: @@ -185,10 +183,8 @@ class Uploader(): success = True else: start_time = time.monotonic() - cloudlog.debug("uploading %r", fn) stat = self.normal_upload(key, fn) if stat is not None and stat.status_code in (200, 201, 403, 412): - cloudlog.event("upload_success" if stat.status_code != 412 else "upload_ignored", key=key, fn=fn, sz=sz, debug=True) try: # tag file as uploaded setxattr(fn, UPLOAD_ATTR_NAME, UPLOAD_ATTR_VALUE) @@ -199,9 +195,10 @@ class Uploader(): self.last_time = time.monotonic() - start_time self.last_speed = (sz / 1e6) / self.last_time success = True + cloudlog.event("upload_success" if stat.status_code != 412 else "upload_ignored", key=key, fn=fn, sz=sz, network_type=network_type) else: - cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz, debug=True) success = False + cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz, network_type=network_type) return success @@ -248,8 +245,7 @@ def uploader_fn(exit_event): key, fn = d - cloudlog.debug("upload %r over %s", d, network_type) - success = uploader.upload(key, fn) + success = uploader.upload(key, fn, sm['deviceState'].networkType.raw) if success: backoff = 0.1 elif allow_sleep: @@ -258,7 +254,6 @@ def uploader_fn(exit_event): backoff = min(backoff*2, 120) pm.send("uploaderState", uploader.get_msg()) - cloudlog.info("upload done, success=%r", success) def main(): uploader_fn(threading.Event()) diff --git a/selfdrive/loggerd/xattr_cache.py b/selfdrive/loggerd/xattr_cache.py index aa97f0c777..e721692500 100644 --- a/selfdrive/loggerd/xattr_cache.py +++ b/selfdrive/loggerd/xattr_cache.py @@ -1,13 +1,15 @@ +from typing import Dict, Tuple + from common.xattr import getxattr as getattr1 from common.xattr import setxattr as setattr1 -cached_attributes = {} -def getxattr(path, attr_name): +cached_attributes: Dict[Tuple, bytes] = {} +def getxattr(path: str, attr_name: bytes) -> bytes: if (path, attr_name) not in cached_attributes: response = getattr1(path, attr_name) cached_attributes[(path, attr_name)] = response return cached_attributes[(path, attr_name)] -def setxattr(path, attr_name, attr_value): +def setxattr(path: str, attr_name: str, attr_value: bytes) -> None: cached_attributes.pop((path, attr_name), None) return setattr1(path, attr_name, attr_value) diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index ba2ce91435..95a2146892 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -121,8 +121,8 @@ def manager_thread() -> None: params = Params() - ignore = [] - if params.get("DongleId", encoding='utf8') == UNREGISTERED_DONGLE_ID: + ignore: List[str] = [] + if params.get("DongleId", encoding='utf8') in (None, UNREGISTERED_DONGLE_ID): ignore += ["manage_athenad", "uploader"] if os.getenv("NOBOARD") is not None: ignore.append("pandad") @@ -217,6 +217,11 @@ if __name__ == "__main__": add_file_handler(cloudlog) cloudlog.exception("Manager failed to start") + try: + managed_processes['ui'].stop() + except Exception: + pass + # Show last 3 lines of traceback error = traceback.format_exc(-3) error = "Manager failed to start\n\n" + error diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py index 5165d6da78..ebdd2a90bf 100644 --- a/selfdrive/manager/process.py +++ b/selfdrive/manager/process.py @@ -34,8 +34,9 @@ def launcher(proc: str, name: str) -> None: # create new context since we forked messaging.context = messaging.Context() - # add daemon name to cloudlog ctx + # add daemon name tag to logs cloudlog.bind(daemon=name) + sentry.set_tag("daemon", name) # exec the process getattr(mod, 'main')() diff --git a/selfdrive/monitoring/dmonitoringd.py b/selfdrive/monitoring/dmonitoringd.py index 1728cc9e79..f1b0c42bd9 100755 --- a/selfdrive/monitoring/dmonitoringd.py +++ b/selfdrive/monitoring/dmonitoringd.py @@ -1,13 +1,19 @@ #!/usr/bin/env python3 +import gc + +import cereal.messaging as messaging from cereal import car from common.params import Params -import cereal.messaging as messaging +from common.realtime import set_realtime_priority from selfdrive.controls.lib.events import Events -from selfdrive.monitoring.driver_monitor import DriverStatus from selfdrive.locationd.calibrationd import Calibration +from selfdrive.monitoring.driver_monitor import DriverStatus def dmonitoringd_thread(sm=None, pm=None): + gc.disable() + set_realtime_priority(2) + if pm is None: pm = messaging.PubMaster(['driverMonitoringState']) @@ -38,8 +44,6 @@ def dmonitoringd_thread(sm=None, pm=None): v_cruise != v_cruise_last or \ sm['carState'].steeringPressed or \ sm['carState'].gasPressed - if driver_engaged: - driver_status.update(Events(), True, sm['controlsState'].enabled, sm['carState'].standstill) v_cruise_last = v_cruise if sm.updated['modelV2']: @@ -77,8 +81,10 @@ def dmonitoringd_thread(sm=None, pm=None): } pm.send('driverMonitoringState', dat) + def main(sm=None, pm=None): dmonitoringd_thread(sm, pm) + if __name__ == '__main__': main() diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py index 3df2c1ec24..1f9fddab7a 100755 --- a/selfdrive/pandad.py +++ b/selfdrive/pandad.py @@ -75,9 +75,12 @@ def panda_sort_cmp(a: Panda, b: Panda): def main() -> NoReturn: first_run = True + params = Params() while True: try: + params.delete("PandaSignatures") + # Flash all Pandas in DFU mode for p in PandaDFU.list(): cloudlog.info(f"Panda in DFU mode found, flashing recovery {p}") @@ -99,7 +102,7 @@ def main() -> NoReturn: for panda in pandas: health = panda.health() if health["heartbeat_lost"]: - Params().put_bool("PandaHeartbeatLost", True) + params.put_bool("PandaHeartbeatLost", True) cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial()) if first_run: @@ -110,6 +113,9 @@ def main() -> NoReturn: pandas.sort(key=cmp_to_key(panda_sort_cmp)) panda_serials = list(map(lambda p: p.get_usb_serial(), pandas)) + # log panda fw versions + params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas)) + # close all pandas for p in pandas: p.close() diff --git a/selfdrive/sentry.py b/selfdrive/sentry.py index 85c97a9873..5f22bf18e0 100644 --- a/selfdrive/sentry.py +++ b/selfdrive/sentry.py @@ -4,7 +4,8 @@ from enum import Enum from sentry_sdk.integrations.threading import ThreadingIntegration from common.params import Params -from selfdrive.hardware import HARDWARE +from selfdrive.athena.registration import is_registered_device +from selfdrive.hardware import HARDWARE, PC from selfdrive.swaglog import cloudlog from selfdrive.version import get_branch, get_commit, get_origin, get_version, \ is_comma_remote, is_dirty, is_tested_branch @@ -12,9 +13,9 @@ from selfdrive.version import get_branch, get_commit, get_origin, get_version, \ class SentryProject(Enum): # python project - SELFDRIVE = "https://a8dc76b5bfb34908a601d67e2aa8bcf9@o33823.ingest.sentry.io/77924" + SELFDRIVE = "https://6f3c7076c1e14b2aa10f5dde6dda0cc4@o33823.ingest.sentry.io/77924" # native project - SELFDRIVE_NATIVE = "https://a40f22e13cbc4261873333c125fc9d38@o33823.ingest.sentry.io/157615" + SELFDRIVE_NATIVE = "https://3e4b586ed21a4479ad5d85083b639bc6@o33823.ingest.sentry.io/157615" def report_tombstone(fn: str, message: str, contents: str) -> None: @@ -37,12 +38,17 @@ def capture_exception(*args, **kwargs) -> None: cloudlog.exception("sentry exception") +def set_tag(key: str, value: str) -> None: + sentry_sdk.set_tag(key, value) + + def init(project: SentryProject) -> None: # forks like to mess with this, so double check - if not (is_comma_remote() and "commaai" in get_origin(default="")): + comma_remote = is_comma_remote() and "commaai" in get_origin(default="") + if not comma_remote or not is_registered_device() or PC: return - env = "production" if is_tested_branch() else "master" + env = "release" if is_tested_branch() else "master" dongle_id = Params().get("DongleId", encoding='utf-8') integrations = [] diff --git a/selfdrive/statsd.py b/selfdrive/statsd.py index feb1c16337..2e62e32536 100755 --- a/selfdrive/statsd.py +++ b/selfdrive/statsd.py @@ -4,6 +4,8 @@ import zmq import time from pathlib import Path from datetime import datetime, timezone +from typing import NoReturn + from common.params import Params from cereal.messaging import SubMaster from selfdrive.swaglog import cloudlog @@ -20,14 +22,14 @@ class StatLog: def __init__(self): self.pid = None - def connect(self): + def connect(self) -> None: self.zctx = zmq.Context() self.sock = self.zctx.socket(zmq.PUSH) self.sock.setsockopt(zmq.LINGER, 10) self.sock.connect(STATS_SOCKET) self.pid = os.getpid() - def _send(self, metric: str): + def _send(self, metric: str) -> None: if os.getpid() != self.pid: self.connect() @@ -37,16 +39,17 @@ class StatLog: # drop :/ pass - def gauge(self, name: str, value: float): + def gauge(self, name: str, value: float) -> None: self._send(f"{name}:{value}|{METRIC_TYPE.GAUGE}") -def main(): - def get_influxdb_line(measurement: str, value: float, timestamp: datetime, tags: dict): +def main() -> NoReturn: + dongle_id = Params().get("DongleId", encoding='utf-8') + def get_influxdb_line(measurement: str, value: float, timestamp: datetime, tags: dict) -> str: res = f"{measurement}" for k, v in tags.items(): res += f",{k}={str(v)}" - res += f" value={value} {int(timestamp.timestamp() * 1e9)}\n" + res += f" value={value},dongle_id=\"{dongle_id}\" {int(timestamp.timestamp() * 1e9)}\n" return res # open statistics socket @@ -59,7 +62,6 @@ def main(): # initialize tags tags = { - 'dongleId': Params().get("DongleId", encoding='utf-8'), 'started': False, 'version': get_short_version(), 'branch': get_short_branch(), diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py index 0bb475c929..5abc0d964f 100644 --- a/selfdrive/test/helpers.py +++ b/selfdrive/test/helpers.py @@ -13,7 +13,6 @@ def set_params_enabled(): params.put("HasAcceptedTerms", terms_version) params.put("CompletedTrainingVersion", training_version) params.put_bool("OpenpilotEnabledToggle", True) - params.put_bool("CommunityFeaturesToggle", True) params.put_bool("Passive", False) diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 35b9f32e68..60aaeb724d 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -120,7 +120,6 @@ class LongitudinalControl(unittest.TestCase): params.clear_all() params.put_bool("Passive", bool(os.getenv("PASSIVE"))) params.put_bool("OpenpilotEnabledToggle", True) - params.put_bool("CommunityFeaturesToggle", True) # hack def test_longitudinal_setup(self): diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index 9cf8e3fa9a..de734e52fb 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -85,11 +85,14 @@ def compare_logs(log1, log2, ignore_fields=None, ignore_msgs=None, tolerance=Non # Dictdiffer only supports relative tolerance, we also want to check for absolute # TODO: add this to dictdiffer def outside_tolerance(diff): - if diff[0] == "change": - a, b = diff[2] - finite = math.isfinite(a) and math.isfinite(b) - if finite and isinstance(a, numbers.Number) and isinstance(b, numbers.Number): - return abs(a - b) > max(tolerance, tolerance * max(abs(a), abs(b))) + try: + if diff[0] == "change": + a, b = diff[2] + finite = math.isfinite(a) and math.isfinite(b) + if finite and isinstance(a, numbers.Number) and isinstance(b, numbers.Number): + return abs(a - b) > max(tolerance, tolerance * max(abs(a), abs(b))) + except TypeError: + pass return True dd = list(filter(outside_tolerance, dd)) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 69d79cdcce..da9c717285 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -25,7 +25,7 @@ NUMPY_TOLERANCE = 1e-7 CI = "CI" in os.environ TIMEOUT = 15 -ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback', 'tolerance', 'fake_pubsubmaster']) +ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback', 'tolerance', 'fake_pubsubmaster', 'submaster_config'], defaults=({},)) def wait_for_event(evt): @@ -88,8 +88,8 @@ class DumbSocket: class FakeSubMaster(messaging.SubMaster): - def __init__(self, services): - super().__init__(services, addr=None) + def __init__(self, services, ignore_alive=None, ignore_avg_freq=None): + super().__init__(services, ignore_alive=ignore_alive, ignore_avg_freq=ignore_avg_freq, addr=None) self.sock = {s: DumbSocket(s) for s in services} self.update_called = threading.Event() self.update_ready = threading.Event() @@ -172,7 +172,6 @@ def fingerprint(msgs, fsm, can_sock, fingerprint): can_sock.data = [] fsm.update_ready.set() - print("finished fingerprinting") def get_car_params(msgs, fsm, can_sock, fingerprint): @@ -241,13 +240,14 @@ CONFIGS = [ pub_sub={ "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], "deviceState": [], "pandaStates": [], "peripheralState": [], "liveCalibration": [], "driverMonitoringState": [], "longitudinalPlan": [], "lateralPlan": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [], - "modelV2": [], "driverCameraState": [], "roadCameraState": [], "ubloxRaw": [], "managerState": [], + "modelV2": [], "driverCameraState": [], "roadCameraState": [], "managerState": [], }, ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], init_callback=fingerprint, should_recv_callback=controlsd_rcv_callback, tolerance=NUMPY_TOLERANCE, fake_pubsubmaster=True, + submaster_config={'ignore_avg_freq': ['radarState', 'longitudinalPlan']} ), ProcessConfig( proc_name="radard", @@ -267,7 +267,7 @@ CONFIGS = [ "modelV2": ["lateralPlan", "longitudinalPlan"], "carState": [], "controlsState": [], "radarState": [], }, - ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay"], + ignore=["logMonoTime", "valid", "longitudinalPlan.processingDelay", "longitudinalPlan.solverExecutionTime", "lateralPlan.solverExecutionTime"], init_callback=get_car_params, should_recv_callback=None, tolerance=NUMPY_TOLERANCE, @@ -341,20 +341,25 @@ def replay_process(cfg, lr, fingerprint=None): else: return cpp_replay_process(cfg, lr, fingerprint) -def setup_env(): +def setup_env(simulation=False): params = Params() params.clear_all() params.put_bool("OpenpilotEnabledToggle", True) params.put_bool("Passive", False) - os.environ['NO_RADAR_SLEEP'] = "1" - os.environ["SIMULATION"] = "1" + os.environ["NO_RADAR_SLEEP"] = "1" + os.environ["REPLAY"] = "1" + + if simulation: + os.environ["SIMULATION"] = "1" + elif "SIMULATION" in os.environ: + del os.environ["SIMULATION"] def python_replay_process(cfg, lr, fingerprint=None): sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub] pub_sockets = [s for s in cfg.pub_sub.keys() if s != 'can'] - fsm = FakeSubMaster(pub_sockets) + fsm = FakeSubMaster(pub_sockets, **cfg.submaster_config) fpm = FakePubMaster(sub_sockets) args = (fsm, fpm) if 'can' in list(cfg.pub_sub.keys()): @@ -425,7 +430,7 @@ def python_replay_process(cfg, lr, fingerprint=None): msg_queue.append(msg.as_builder()) if should_recv: - fsm.update_msgs(0, msg_queue) + fsm.update_msgs(msg.logMonoTime / 1e9, msg_queue) msg_queue = [] recv_cnt = len(recv_socks) @@ -447,7 +452,8 @@ def cpp_replay_process(cfg, lr, fingerprint=None): pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())] log_msgs = [] - setup_env() + # We need to fake SubMaster alive since we can't inject a fake clock + setup_env(simulation=True) managed_processes[cfg.proc_name].prepare() managed_processes[cfg.proc_name].start() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 2d3c904da9..50a0c30f83 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -280a712ece99c231ea036c3b66d6aafa55548211 \ No newline at end of file +0c4da879ace9c1517c2324b35da7ff05a4744dd9 \ No newline at end of file diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index 5326075240..16ee5e9a28 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -11,8 +11,8 @@ import cereal.messaging as messaging from cereal.services import service_list from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error from common.params import Params -from common.realtime import Ratekeeper, DT_MDL, DT_DMON -from common.transformations.camera import eon_f_frame_size, eon_d_frame_size +from common.realtime import Ratekeeper, DT_MDL, DT_DMON, sec_since_boot +from common.transformations.camera import eon_f_frame_size, eon_d_frame_size, tici_f_frame_size, tici_d_frame_size from selfdrive.car.fingerprints import FW_VERSIONS from selfdrive.manager.process import ensure_running from selfdrive.manager.process_config import managed_processes @@ -26,32 +26,107 @@ process_replay_dir = os.path.dirname(os.path.abspath(__file__)) FAKEDATA = os.path.join(process_replay_dir, "fakedata/") +def replay_panda_states(s, msgs): + pm = messaging.PubMaster([s, 'peripheralState']) + rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) + smsgs = [m for m in msgs if m.which() in ['pandaStates', 'pandaStateDEPRECATED']] + + # Migrate safety param base on carState + cp = [m for m in msgs if m.which() == 'carParams'][0].carParams + if len(cp.safetyConfigs): + safety_param = cp.safetyConfigs[0].safetyParam + else: + safety_param = cp.safetyParamDEPRECATED + + while True: + for m in smsgs: + if m.which() == 'pandaStateDEPRECATED': + new_m = messaging.new_message('pandaStates', 1) + new_m.pandaStates[0] = m.pandaStateDEPRECATED + new_m.pandaStates[0].safetyParam = safety_param + pm.send(s, new_m) + else: + new_m = m.as_builder() + new_m.logMonoTime = int(sec_since_boot() * 1e9) + pm.send(s, new_m) + + new_m = messaging.new_message('peripheralState') + pm.send('peripheralState', new_m) + + rk.keep_time() + + +def replay_manager_state(s, msgs): + pm = messaging.PubMaster([s, ]) + rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) + + while True: + new_m = messaging.new_message('managerState') + new_m.managerState.processes = [{'name': name, 'running': True} for name in managed_processes] + pm.send(s, new_m) + rk.keep_time() + + +def replay_device_state(s, msgs): + pm = messaging.PubMaster([s, ]) + rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) + smsgs = [m for m in msgs if m.which() == s] + while True: + for m in smsgs: + new_m = m.as_builder() + new_m.logMonoTime = int(sec_since_boot() * 1e9) + new_m.deviceState.freeSpacePercent = 50 + new_m.deviceState.memoryUsagePercent = 50 + pm.send(s, new_m) + rk.keep_time() + + +def replay_sensor_events(s, msgs): + pm = messaging.PubMaster([s, ]) + rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) + smsgs = [m for m in msgs if m.which() == s] + while True: + for m in smsgs: + new_m = m.as_builder() + new_m.logMonoTime = int(sec_since_boot() * 1e9) + + for evt in new_m.sensorEvents: + evt.timestamp = new_m.logMonoTime + + pm.send(s, new_m) + rk.keep_time() + + def replay_service(s, msgs): pm = messaging.PubMaster([s, ]) rk = Ratekeeper(service_list[s].frequency, print_delay_threshold=None) smsgs = [m for m in msgs if m.which() == s] while True: for m in smsgs: - # TODO: use logMonoTime - pm.send(s, m.as_builder()) + new_m = m.as_builder() + new_m.logMonoTime = int(sec_since_boot() * 1e9) + pm.send(s, new_m) rk.keep_time() -vs = None + def replay_cameras(lr, frs): - cameras = [ + eon_cameras = [ ("roadCameraState", DT_MDL, eon_f_frame_size, VisionStreamType.VISION_STREAM_ROAD), ("driverCameraState", DT_DMON, eon_d_frame_size, VisionStreamType.VISION_STREAM_DRIVER), ] + tici_cameras = [ + ("roadCameraState", DT_MDL, tici_f_frame_size, VisionStreamType.VISION_STREAM_ROAD), + ("driverCameraState", DT_MDL, tici_d_frame_size, VisionStreamType.VISION_STREAM_DRIVER), + ] - def replay_camera(s, stream, dt, vipc_server, fr, size): + def replay_camera(s, stream, dt, vipc_server, frames, size): pm = messaging.PubMaster([s, ]) rk = Ratekeeper(1 / dt, print_delay_threshold=None) img = b"\x00" * int(size[0]*size[1]*3/2) while True: - if fr is not None: - img = fr.get(rk.frame % fr.frame_count, pix_fmt='yuv420p')[0] - img = img.flatten().tobytes() + if frames is not None: + img = frames[rk.frame % len(frames)] rk.keep_time() @@ -62,24 +137,34 @@ def replay_cameras(lr, frs): vipc_server.send(stream, img, msg.frameId, msg.timestampSof, msg.timestampEof) + init_data = [m for m in lr if m.which() == 'initData'][0] + cameras = tici_cameras if (init_data.initData.deviceType == 'tici') else eon_cameras + # init vipc server and cameras p = [] - global vs vs = VisionIpcServer("camerad") for (s, dt, size, stream) in cameras: fr = frs.get(s, None) + + frames = None + if fr is not None: + print(f"Decomressing frames {s}") + frames = [] + for i in tqdm(range(fr.frame_count)): + img = fr.get(i, pix_fmt='yuv420p')[0] + frames.append(img.flatten().tobytes()) + vs.create_buffers(stream, 40, False, size[0], size[1]) p.append(multiprocessing.Process(target=replay_camera, - args=(s, stream, dt, vs, fr, size))) + args=(s, stream, dt, vs, frames, size))) # hack to make UI work vs.create_buffers(VisionStreamType.VISION_STREAM_RGB_BACK, 4, True, eon_f_frame_size[0], eon_f_frame_size[1]) vs.start_listener() - return p + return vs, p def regen_segment(lr, frs=None, outdir=FAKEDATA): - lr = list(lr) if frs is None: frs = dict() @@ -89,53 +174,48 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA): params.clear_all() params.put_bool("Passive", False) params.put_bool("OpenpilotEnabledToggle", True) - params.put_bool("CommunityFeaturesToggle", True) - params.put_bool("CommunityFeaturesToggle", True) - cal = messaging.new_message('liveCalibration') - cal.liveCalibration.validBlocks = 20 - cal.liveCalibration.rpyCalib = [0.0, 0.0, 0.0] - params.put("CalibrationParams", cal.to_bytes()) os.environ["LOG_ROOT"] = outdir - os.environ["SIMULATION"] = "1" + os.environ["REPLAY"] = "1" os.environ['SKIP_FW_QUERY'] = "" os.environ['FINGERPRINT'] = "" + + # TODO: remove after getting new route for mazda + migration = { + "Mazda CX-9 2021": "MAZDA CX-9 2021", + } + for msg in lr: if msg.which() == 'carParams': - car_fingerprint = msg.carParams.carFingerprint + car_fingerprint = migration.get(msg.carParams.carFingerprint, msg.carParams.carFingerprint) if len(msg.carParams.carFw) and (car_fingerprint in FW_VERSIONS): params.put("CarParamsCache", msg.carParams.as_builder().to_bytes()) else: os.environ['SKIP_FW_QUERY'] = "1" os.environ['FINGERPRINT'] = car_fingerprint + elif msg.which() == 'liveCalibration': + params.put("CalibrationParams", msg.as_builder().to_bytes()) - #TODO: init car, make sure starts engaged when segment is engaged + vs, cam_procs = replay_cameras(lr, frs) fake_daemons = { 'sensord': [ - multiprocessing.Process(target=replay_service, args=('sensorEvents', lr)), + multiprocessing.Process(target=replay_sensor_events, args=('sensorEvents', lr)), ], 'pandad': [ multiprocessing.Process(target=replay_service, args=('can', lr)), - multiprocessing.Process(target=replay_service, args=('pandaStates', lr)), + multiprocessing.Process(target=replay_service, args=('ubloxRaw', lr)), + multiprocessing.Process(target=replay_panda_states, args=('pandaStates', lr)), + ], + 'managerState': [ + multiprocessing.Process(target=replay_manager_state, args=('managerState', lr)), ], - #'managerState': [ - # multiprocessing.Process(target=replay_service, args=('managerState', lr)), - #], 'thermald': [ - multiprocessing.Process(target=replay_service, args=('deviceState', lr)), + multiprocessing.Process(target=replay_device_state, args=('deviceState', lr)), ], 'camerad': [ - *replay_cameras(lr, frs), - ], - - # TODO: fix these and run them - 'paramsd': [ - multiprocessing.Process(target=replay_service, args=('liveParameters', lr)), - ], - 'locationd': [ - multiprocessing.Process(target=replay_service, args=('liveLocationKalman', lr)), + *cam_procs, ], } @@ -162,11 +242,13 @@ def regen_segment(lr, frs=None, outdir=FAKEDATA): for p in procs: p.terminate() + del vs + r = params.get("CurrentRoute", encoding='utf-8') return os.path.join(outdir, r + "--0") -def regen_and_save(route, sidx, upload=False, use_route_meta=True): +def regen_and_save(route, sidx, upload=False, use_route_meta=False): if use_route_meta: r = Route(args.route) lr = LogReader(r.log_paths()[args.seg]) @@ -175,6 +257,11 @@ def regen_and_save(route, sidx, upload=False, use_route_meta=True): lr = LogReader(f"cd:/{route.replace('|', '/')}/{sidx}/rlog.bz2") fr = FrameReader(f"cd:/{route.replace('|', '/')}/{sidx}/fcamera.hevc") rpath = regen_segment(lr, {'roadCameraState': fr}) + + lr = LogReader(os.path.join(rpath, 'rlog.bz2')) + controls_state_active = [m.controlsState.active for m in lr if m.which() == 'controlsState'] + assert any(controls_state_active), "Segment did not engage" + relr = os.path.relpath(rpath) print("\n\n", "*"*30, "\n\n") diff --git a/selfdrive/test/process_replay/regen_all.py b/selfdrive/test/process_replay/regen_all.py old mode 100644 new mode 100755 index 047c83f465..4f057bbf50 --- a/selfdrive/test/process_replay/regen_all.py +++ b/selfdrive/test/process_replay/regen_all.py @@ -7,12 +7,13 @@ if __name__ == "__main__": for segment in segments: route = segment[1].rsplit('--', 1)[0] sidx = int(segment[1].rsplit('--', 1)[1]) + print("Regen", route, sidx) relr = regen_and_save(route, sidx, upload=True, use_route_meta=False) print("\n\n", "*"*30, "\n\n") print("New route:", relr, "\n") relr = relr.replace('/', '|') - new_segments.append(f'("{segment[0]}", "{relr}"), ') + new_segments.append(f' ("{segment[0]}", "{relr}"), ') print() print() print() diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 6c33b46224..f7dae5c9fb 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -30,18 +30,18 @@ original_segments = [ ] segments = [ - ("HYUNDAI", "fakedata|2021-10-07--15-56-26--0"), - ("TOYOTA", "fakedata|2021-10-07--15-57-47--0"), - ("TOYOTA2", "fakedata|2021-10-07--15-59-03--0"), - ("TOYOTA3", "fakedata|2021-10-07--15-53-21--0"), - ("HONDA", "fakedata|2021-10-07--16-00-19--0"), - ("HONDA2", "fakedata|2021-10-07--16-01-35--0"), - ("CHRYSLER", "fakedata|2021-10-07--16-02-52--0"), - ("SUBARU", "fakedata|2021-10-07--16-04-09--0"), - ("GM", "fakedata|2021-10-07--16-05-26--0"), - ("NISSAN", "fakedata|2021-10-07--16-09-53--0"), - ("VOLKSWAGEN", "fakedata|2021-10-07--16-11-11--0"), - ("MAZDA", "bd6a637565e91581|2021-10-30--15-14-53--2"), + ("HYUNDAI", "fakedata|2022-01-20--17-49-04--0"), + ("TOYOTA", "fakedata|2022-01-20--17-50-51--0"), + ("TOYOTA2", "fakedata|2022-01-20--17-52-36--0"), + ("TOYOTA3", "fakedata|2022-01-20--17-54-50--0"), + ("HONDA", "fakedata|2022-01-20--17-56-40--0"), + ("HONDA2", "fakedata|2022-01-20--17-58-25--0"), + ("CHRYSLER", "fakedata|2022-01-20--18-00-11--0"), + ("SUBARU", "fakedata|2022-01-20--18-01-57--0"), + ("GM", "fakedata|2022-01-20--18-03-41--0"), + ("NISSAN", "fakedata|2022-01-20--18-05-29--0"), + ("VOLKSWAGEN", "fakedata|2022-01-20--18-07-15--0"), + ("MAZDA", "fakedata|2022-01-20--18-09-32--0"), ] # dashcamOnly makes don't need to be tested until a full port is done @@ -65,9 +65,7 @@ def test_process(cfg, lr, cmp_log_fn, ignore_fields=None, ignore_msgs=None): log_msgs = replay_process(cfg, lr) # check to make sure openpilot is engaged in the route - # TODO: update routes so enable check can run - # failed enable check: honda bosch, hyundai, chrysler, and subaru - if cfg.proc_name == "controlsd" and FULL_TEST and False: + if cfg.proc_name == "controlsd": for msg in log_msgs: if msg.which() == "controlsState": if msg.controlsState.active: diff --git a/selfdrive/test/test_fingerprints.py b/selfdrive/test/test_fingerprints.py index 4ea60a76a8..01c7d753c3 100755 --- a/selfdrive/test/test_fingerprints.py +++ b/selfdrive/test/test_fingerprints.py @@ -1,6 +1,8 @@ #!/usr/bin/env python3 import os import sys +from typing import Dict, List + from common.basedir import BASEDIR # messages reserved for CAN based ignition (see can_ignition_hook function in panda/board/drivers/can) @@ -62,7 +64,7 @@ def check_can_ignition_conflicts(fingerprints, brands): if __name__ == "__main__": fingerprints = _get_fingerprints() - fingerprints_flat = [] + fingerprints_flat: List[Dict] = [] car_names = [] brand_names = [] for brand in fingerprints: diff --git a/selfdrive/test/test_models.py b/selfdrive/test/test_models.py index 2a4160c274..6b9595b6c6 100755 --- a/selfdrive/test/test_models.py +++ b/selfdrive/test/test_models.py @@ -4,15 +4,19 @@ import os import importlib import unittest from collections import defaultdict, Counter +from typing import List, Optional, Tuple from parameterized import parameterized_class from cereal import log, car from common.params import Params +from common.realtime import DT_CTRL +from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp from selfdrive.car.fingerprints import all_known_cars from selfdrive.car.car_helpers import interfaces from selfdrive.car.gm.values import CAR as GM -from selfdrive.car.honda.values import HONDA_BOSCH, CAR as HONDA +from selfdrive.car.honda.values import CAR as HONDA from selfdrive.car.hyundai.values import CAR as HYUNDAI +from selfdrive.car.toyota.values import CAR as TOYOTA from selfdrive.test.test_routes import routes, non_tested_cars from selfdrive.test.openpilotci import get_url from tools.lib.logreader import LogReader @@ -25,8 +29,6 @@ PandaType = log.PandaState.PandaType NUM_JOBS = int(os.environ.get("NUM_JOBS", "1")) JOB_ID = int(os.environ.get("JOB_ID", "0")) -ROUTES = {rt.car_fingerprint: rt.route for rt in routes} - # TODO: get updated routes for these cars ignore_can_valid = [ HYUNDAI.SANTA_FE, @@ -37,25 +39,34 @@ ignore_addr_checks_valid = [ HYUNDAI.GENESIS_G70_2020, ] -@parameterized_class(('car_model'), [(car,) for i, car in enumerate(sorted(all_known_cars())) if i % NUM_JOBS == JOB_ID]) +# build list of test cases +routes_by_car = defaultdict(set) +for r in routes: + routes_by_car[r.car_fingerprint].add(r.route) + +test_cases: List[Tuple[str, Optional[str]]] = [] +for i, c in enumerate(sorted(all_known_cars())): + if i % NUM_JOBS == JOB_ID: + test_cases.extend((c, r) for r in routes_by_car.get(c, (None, ))) + + +@parameterized_class(('car_model', 'route'), test_cases) class TestCarModel(unittest.TestCase): @classmethod def setUpClass(cls): - if cls.car_model not in ROUTES: - # TODO: get routes for missing cars and remove this + if cls.route is None: if cls.car_model in non_tested_cars: print(f"Skipping tests for {cls.car_model}: missing route") raise unittest.SkipTest - else: - raise Exception(f"missing test route for car {cls.car_model}") + raise Exception(f"missing test route for {cls.car_model}") params = Params() params.clear_all() for seg in [2, 1, 0]: try: - lr = LogReader(get_url(ROUTES[cls.car_model], seg)) + lr = LogReader(get_url(cls.route, seg)) except Exception: continue @@ -71,16 +82,17 @@ class TestCarModel(unittest.TestCase): if msg.carParams.openpilotLongitudinalControl: params.put_bool("DisableRadar", True) - if len(can_msgs): + if len(can_msgs) > int(50 / DT_CTRL): break else: - raise Exception("Route not found or no CAN msgs found. Is it uploaded?") + raise Exception(f"Route {repr(cls.route)} not found or no CAN msgs found. Is it uploaded?") cls.can_msgs = sorted(can_msgs, key=lambda msg: msg.logMonoTime) cls.CarInterface, cls.CarController, cls.CarState = interfaces[cls.car_model] cls.CP = cls.CarInterface.get_params(cls.car_model, fingerprint, []) assert cls.CP + assert cls.CP.carFingerprint == cls.car_model def setUp(self): self.CI = self.CarInterface(self.CP, self.CarController, self.CarState) @@ -174,50 +186,54 @@ class TestCarModel(unittest.TestCase): if self.CP.dashcamOnly: self.skipTest("no need to check panda safety for dashcamOnly") - checks = defaultdict(lambda: 0) CC = car.CarControl.new_message() + + # warm up pass, as initial states may be different + for can in self.can_msgs[:300]: + for msg in can_capnp_to_can_list(can.can, src_filter=range(64)): + to_send = package_can_msg(msg) + self.safety.safety_rx_hook(to_send) + self.CI.update(CC, (can_list_to_can_capnp([msg, ]), )) + + checks = defaultdict(lambda: 0) for can in self.can_msgs: - for msg in can.can: - if msg.src >= 64: - continue - to_send = package_can_msg([msg.address, 0, msg.dat, msg.src]) + CS = self.CI.update(CC, (can.as_builder().to_bytes(), )) + for msg in can_capnp_to_can_list(can.can, src_filter=range(64)): + to_send = package_can_msg(msg) ret = self.safety.safety_rx_hook(to_send) self.assertEqual(1, ret, f"safety rx failed ({ret=}): {to_send}") - CS = self.CI.update(CC, (can.as_builder().to_bytes(),)) - # TODO: check steering state - # check that openpilot and panda safety agree on the car's state - checks['gasPressed'] += CS.gasPressed != self.safety.get_gas_pressed_prev() - checks['brakePressed'] += CS.brakePressed != self.safety.get_brake_pressed_prev() + # TODO: check rest of panda's carstate (steering, ACC main on, etc.) + + # TODO: make the interceptor thresholds in openpilot and panda match, then remove this exception + gas_pressed = CS.gasPressed + if self.CP.enableGasInterceptor and gas_pressed and not self.safety.get_gas_pressed_prev(): + # panda intentionally has a higher threshold + if self.CP.carName == "toyota" and 15 < CS.gas < 15*1.5: + gas_pressed = False + if self.CP.carName == "honda": + gas_pressed = False + checks['gasPressed'] += gas_pressed != self.safety.get_gas_pressed_prev() + + # TODO: remove this exception once this mismatch is resolved + brake_pressed = CS.brakePressed + if CS.brakePressed and not self.safety.get_brake_pressed_prev(): + if self.CP.carFingerprint in (HONDA.PILOT, HONDA.PASSPORT, HONDA.RIDGELINE) and CS.brake > 0.05: + brake_pressed = False + checks['brakePressed'] += brake_pressed != self.safety.get_brake_pressed_prev() + if self.CP.pcmCruise: checks['controlsAllowed'] += not CS.cruiseState.enabled and self.safety.get_controls_allowed() - # TODO: extend this to all cars if self.CP.carName == "honda": checks['mainOn'] += CS.cruiseState.available != self.safety.get_acc_main_on() - # TODO: reduce tolerance to 0 - failed_checks = {k: v for k, v in checks.items() if v > 25} - - # TODO: the panda and openpilot interceptor thresholds should match - skip_gas_check = self.CP.carName == 'chrysler' - if "gasPressed" in failed_checks and (self.CP.enableGasInterceptor or skip_gas_check): - if failed_checks['gasPressed'] < 150 or skip_gas_check: - del failed_checks['gasPressed'] - - # TODO: honda nidec: do same checks in carState and panda - if "brakePressed" in failed_checks and self.CP.carName == 'honda' and \ - (self.car_model not in HONDA_BOSCH or self.car_model in [HONDA.CRV_HYBRID, HONDA.HONDA_E]): - if failed_checks['brakePressed'] < 150: - del failed_checks['brakePressed'] - - # TODO: use the same signal in panda and carState - # tolerate a small delay between the button press and PCM entering a cruise state - if self.car_model == HONDA.ACCORD: - if failed_checks['controlsAllowed'] < 500: - del failed_checks['controlsAllowed'] + # TODO: add flag to toyota safety + if self.CP.carFingerprint == TOYOTA.SIENNA and checks['brakePressed'] < 25: + checks['brakePressed'] = 0 - self.assertFalse(len(failed_checks), f"panda safety doesn't agree with CarState: {failed_checks}") + failed_checks = {k: v for k, v in checks.items() if v > 0} + self.assertFalse(len(failed_checks), f"panda safety doesn't agree with openpilot: {failed_checks}") if __name__ == "__main__": unittest.main() diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py index 2f85605b83..d0a1ad956f 100755 --- a/selfdrive/test/test_onroad.py +++ b/selfdrive/test/test_onroad.py @@ -22,7 +22,7 @@ from tools.lib.logreader import LogReader # Baseline CPU usage by process PROCS = { - "selfdrive.controls.controlsd": 50.0, + "selfdrive.controls.controlsd": 55.0, "./loggerd": 45.0, "./locationd": 9.1, "selfdrive.controls.plannerd": 22.6, @@ -30,11 +30,11 @@ PROCS = { "selfdrive.locationd.paramsd": 9.1, "./camerad": 7.07, "./_sensord": 6.17, - "selfdrive.controls.radard": 5.67, + "selfdrive.controls.radard": 7.0, "./_modeld": 4.48, "./boardd": 3.63, "./_dmonitoringmodeld": 2.67, - "selfdrive.thermald.thermald": 7.73, + "selfdrive.thermald.thermald": 5.36, "selfdrive.locationd.calibrationd": 2.0, "./_soundd": 1.0, "selfdrive.monitoring.dmonitoringd": 1.90, @@ -49,19 +49,20 @@ PROCS = { if EON: PROCS.update({ "selfdrive.hardware.eon.androidd": 0.4, + "selfdrive.hardware.eon.shutdownd": 0.4, }) if TICI: PROCS.update({ "./loggerd": 70.0, - "selfdrive.controls.controlsd": 28.0, + "selfdrive.controls.controlsd": 31.0, "./camerad": 31.0, "./_ui": 33.0, "selfdrive.controls.plannerd": 11.7, "./_dmonitoringmodeld": 10.0, "selfdrive.locationd.paramsd": 5.0, - "selfdrive.controls.radard": 3.6, - "selfdrive.thermald.thermald": 4.75, + "selfdrive.controls.radard": 4.5, + "selfdrive.thermald.thermald": 3.87, }) @@ -204,6 +205,22 @@ class TestOnroad(unittest.TestCase): cpu_ok = check_cpu_usage(proclogs[0], proclogs[-1]) self.assertTrue(cpu_ok) + def test_mpc_execution_timings(self): + result = "\n" + result += "------------------------------------------------\n" + result += "----------------- MPC Timing ------------------\n" + result += "------------------------------------------------\n" + + cfgs = [("lateralPlan", 0.05, 0.05), ("longitudinalPlan", 0.05, 0.05)] + for (s, instant_max, avg_max) in cfgs: + ts = [getattr(getattr(m, s), "solverExecutionTime") for m in self.lr if m.which() == s] + self.assertLess(min(ts), instant_max, f"high '{s}' execution time: {min(ts)}") + self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}") + result += f"'{s}' execution time: {min(ts)}\n" + result += f"'{s}' avg execution time: {np.mean(ts)}\n" + result += "------------------------------------------------\n" + print(result) + def test_model_execution_timings(self): result = "\n" result += "------------------------------------------------\n" diff --git a/selfdrive/test/test_routes.py b/selfdrive/test/test_routes.py old mode 100755 new mode 100644 index 053d2d4bfb..02bcd36b19 --- a/selfdrive/test/test_routes.py +++ b/selfdrive/test/test_routes.py @@ -2,7 +2,6 @@ from collections import namedtuple from selfdrive.car.chrysler.values import CAR as CHRYSLER -from selfdrive.car.ford.values import CAR as FORD from selfdrive.car.gm.values import CAR as GM from selfdrive.car.honda.values import CAR as HONDA from selfdrive.car.hyundai.values import CAR as HYUNDAI @@ -34,7 +33,7 @@ routes = [ TestRoute("8190c7275a24557b|2020-01-29--08-33-58", CHRYSLER.PACIFICA_2019_HYBRID), TestRoute("3d84727705fecd04|2021-05-25--08-38-56", CHRYSLER.PACIFICA_2020), - TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION), + #TestRoute("f1b4c567731f4a1b|2018-04-30--10-15-35", FORD.FUSION), TestRoute("7cc2a8365b4dd8a9|2018-12-02--12-10-44", GM.ACADIA), TestRoute("aa20e335f61ba898|2019-02-05--16-59-04", GM.BUICK_REGAL), @@ -61,7 +60,6 @@ routes = [ TestRoute("d83f36766f8012a5|2020-02-05--18-42-21", HONDA.CIVIC_BOSCH_DIESEL), TestRoute("f0890d16a07a236b|2021-05-25--17-27-22", HONDA.INSIGHT), TestRoute("07d37d27996096b6|2020-03-04--21-57-27", HONDA.PILOT), - TestRoute("fa1cd231131ca137|2021-05-22--07-59-57", HONDA.PILOT_2019), TestRoute("684e8f96bd491a0e|2021-11-03--11-08-42", HONDA.PASSPORT), TestRoute("0a78dfbacc8504ef|2020-03-04--13-29-55", HONDA.CIVIC_BOSCH), TestRoute("f34a60d68d83b1e5|2020-10-06--14-35-55", HONDA.ACURA_RDX), @@ -116,8 +114,6 @@ routes = [ TestRoute("5f5afb36036506e4|2019-05-14--02-09-54", TOYOTA.COROLLA_TSS2), TestRoute("5ceff72287a5c86c|2019-10-19--10-59-02", TOYOTA.COROLLAH_TSS2), TestRoute("d2525c22173da58b|2021-04-25--16-47-04", TOYOTA.PRIUS), - TestRoute("b0f5a01cf604185c|2017-12-18--20-32-32", TOYOTA.RAV4), - TestRoute("b0c9d2329ad1606b|2019-04-02--13-24-43", TOYOTA.RAV4), TestRoute("b14c5b4742e6fc85|2020-07-28--19-50-11", TOYOTA.RAV4), TestRoute("32a7df20486b0f70|2020-02-06--16-06-50", TOYOTA.RAV4H), TestRoute("cdf2f7de565d40ae|2019-04-25--03-53-41", TOYOTA.RAV4_TSS2), @@ -145,6 +141,7 @@ routes = [ TestRoute("9b36accae406390e|2021-03-30--10-41-38", TOYOTA.MIRAI), TestRoute("cd9cff4b0b26c435|2021-05-13--15-12-39", TOYOTA.CHR), TestRoute("57858ede0369a261|2021-05-18--20-34-20", TOYOTA.CHRH), + TestRoute("14623aae37e549f3|2021-10-24--01-20-49", TOYOTA.PRIUS_V), TestRoute("202c40641158a6e5|2021-09-21--09-43-24", VOLKSWAGEN.ARTEON_MK1), TestRoute("2c68dda277d887ac|2021-05-11--15-22-20", VOLKSWAGEN.ATLAS_MK1), @@ -194,23 +191,8 @@ routes = [ TestRoute("74f1038827005090|2020-08-26--20-05-50", MAZDA.MAZDA3), TestRoute("fb53c640f499b73d|2021-06-01--04-17-56", MAZDA.MAZDA6), TestRoute("f6d5b1a9d7a1c92e|2021-07-08--06-56-59", MAZDA.CX9_2021), + TestRoute("a4af1602d8e668ac|2022-02-03--12-17-07", MAZDA.CX5_2022), TestRoute("6c14ee12b74823ce|2021-06-30--11-49-02", TESLA.AP1_MODELS), TestRoute("bb50caf5f0945ab1|2021-06-19--17-20-18", TESLA.AP2_MODELS), ] - -forced_dashcam_routes = [ - # Ford fusion - "f1b4c567731f4a1b|2018-04-18--11-29-37", - "f1b4c567731f4a1b|2018-04-30--10-15-35", - # Mazda CX5 - "32a319f057902bb3|2020-04-27--15-18-58", - # Mazda CX9 - "10b5a4b380434151|2020-08-26--17-11-45", - # Mazda3 - "74f1038827005090|2020-08-26--20-05-50", - # Mazda6 - "fb53c640f499b73d|2021-06-01--04-17-56", - # CX-9 2021 - "f6d5b1a9d7a1c92e|2021-07-08--06-56-59", -] diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index efec437d0f..113ffaa04c 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -1,28 +1,31 @@ #!/usr/bin/env python3 import datetime import os +import queue +import threading +import time +from collections import OrderedDict, namedtuple from pathlib import Path -from typing import Dict, NoReturn, Optional, Tuple -from collections import namedtuple, OrderedDict +from typing import Dict, Optional, Tuple import psutil from smbus2 import SMBus import cereal.messaging as messaging from cereal import log +from common.dict_helpers import strip_deprecated_keys from common.filter_simple import FirstOrderFilter from common.numpy_fast import interp from common.params import Params from common.realtime import DT_TRML, sec_since_boot -from common.dict_helpers import strip_deprecated_keys from selfdrive.controls.lib.alertmanager import set_offroad_alert from selfdrive.controls.lib.pid import PIController -from selfdrive.hardware import EON, TICI, PC, HARDWARE +from selfdrive.hardware import EON, HARDWARE, PC, TICI from selfdrive.loggerd.config import get_available_percent +from selfdrive.statsd import statlog from selfdrive.swaglog import cloudlog from selfdrive.thermald.power_monitoring import PowerMonitoring from selfdrive.version import terms_version, training_version -from selfdrive.statsd import statlog ThermalStatus = log.DeviceState.ThermalStatus NetworkType = log.DeviceState.NetworkType @@ -30,8 +33,10 @@ NetworkStrength = log.DeviceState.NetworkStrength CURRENT_TAU = 15. # 15s time constant TEMP_TAU = 5. # 5s time constant DISCONNECT_TIMEOUT = 5. # wait 5 seconds before going offroad after disconnect so you get an alert +PANDA_STATES_TIMEOUT = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp']) +HardwareState = namedtuple("HardwareState", ['network_type', 'network_strength', 'network_info', 'nvme_temps', 'modem_temps']) # List of thermal bands. We will stay within this region as long as we are within the bounds. # When exiting the bounds, we'll jump to the lower or higher band. Bands are ordered in the dict. @@ -152,13 +157,50 @@ def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_tex set_offroad_alert(offroad_alert, show_alert, extra_text) -def thermald_thread() -> NoReturn: +def hw_state_thread(end_event, hw_queue): + """Handles non critical hardware state, and sends over queue""" + count = 0 + registered_count = 0 - pm = messaging.PubMaster(['deviceState']) + while not end_event.is_set(): + # these are expensive calls. update every 10s + if (count % int(10. / DT_TRML)) == 0: + try: + network_type = HARDWARE.get_network_type() + + hw_state = HardwareState( + network_type=network_type, + network_strength=HARDWARE.get_network_strength(network_type), + network_info=HARDWARE.get_network_info(), + nvme_temps=HARDWARE.get_nvme_temperatures(), + modem_temps=HARDWARE.get_modem_temperatures(), + ) + + try: + hw_queue.put_nowait(hw_state) + except queue.Full: + pass + + if TICI and (hw_state.network_info is not None) and (hw_state.network_info.get('state', None) == "REGISTERED"): + registered_count += 1 + else: + registered_count = 0 + + if registered_count > 10: + cloudlog.warning(f"Modem stuck in registered state {hw_state.network_info}. nmcli conn up lte") + os.system("nmcli conn up lte") + registered_count = 0 + + except Exception: + cloudlog.exception("Error getting network status") + + count += 1 + time.sleep(DT_TRML) - pandaState_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency - pandaState_sock = messaging.sub_sock('pandaStates', timeout=pandaState_timeout) - sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState"]) + +def thermald_thread(end_event, hw_queue): + pm = messaging.PubMaster(['deviceState']) + sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "controlsState", "pandaStates"], poll=["pandaStates"]) fan_speed = 0 count = 0 @@ -175,12 +217,13 @@ def thermald_thread() -> NoReturn: thermal_status = ThermalStatus.green usb_power = True - network_type = NetworkType.none - network_strength = NetworkStrength.unknown - network_info = None - registered_count = 0 - nvme_temps = None - modem_temps = None + last_hw_state = HardwareState( + network_type=NetworkType.none, + network_strength=NetworkStrength.unknown, + network_info=None, + nvme_temps=[], + modem_temps=[], + ) current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML) @@ -199,19 +242,20 @@ def thermald_thread() -> NoReturn: # TODO: use PI controller for UNO controller = PIController(k_p=0, k_i=2e-3, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML)) - while True: - pandaStates = messaging.recv_sock(pandaState_sock, wait=True) + while not end_event.is_set(): + sm.update(PANDA_STATES_TIMEOUT) - sm.update(0) + pandaStates = sm['pandaStates'] peripheralState = sm['peripheralState'] msg = read_thermal(thermal_config) - if pandaStates is not None and len(pandaStates.pandaStates) > 0: - pandaState = pandaStates.pandaStates[0] + if sm.updated['pandaStates'] and len(pandaStates) > 0: + + # Set ignition based on any panda connected + onroad_conditions["ignition"] = any(ps.ignitionLine or ps.ignitionCan for ps in pandaStates if ps.pandaType != log.PandaState.PandaType.unknown) - if pandaState.pandaType != log.PandaState.PandaType.unknown: - onroad_conditions["ignition"] = pandaState.ignitionLine or pandaState.ignitionCan + pandaState = pandaStates[0] in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected usb_power = peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client @@ -231,44 +275,23 @@ def thermald_thread() -> NoReturn: setup_eon_fan() handle_fan = handle_fan_eon - # these are expensive calls. update every 10s - if (count % int(10. / DT_TRML)) == 0: - try: - network_type = HARDWARE.get_network_type() - network_strength = HARDWARE.get_network_strength(network_type) - network_info = HARDWARE.get_network_info() # pylint: disable=assignment-from-none - nvme_temps = HARDWARE.get_nvme_temperatures() - modem_temps = HARDWARE.get_modem_temperatures() - - if TICI and (network_info.get('state', None) == "REGISTERED"): - registered_count += 1 - else: - registered_count = 0 - - if registered_count > 10: - cloudlog.warning(f"Modem stuck in registered state {network_info}. nmcli conn up lte") - os.system("nmcli conn up lte") - registered_count = 0 - - except Exception: - cloudlog.exception("Error getting network status") + try: + last_hw_state = hw_queue.get_nowait() + except queue.Empty: + pass msg.deviceState.freeSpacePercent = get_available_percent(default=100.0) msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent)) msg.deviceState.cpuUsagePercent = [int(round(n)) for n in psutil.cpu_percent(percpu=True)] msg.deviceState.gpuUsagePercent = int(round(HARDWARE.get_gpu_usage_percent())) - msg.deviceState.networkType = network_type - msg.deviceState.networkStrength = network_strength - if network_info is not None: - msg.deviceState.networkInfo = network_info - if nvme_temps is not None: - msg.deviceState.nvmeTempC = nvme_temps - for i, temp in enumerate(nvme_temps): - statlog.gauge(f"nvme_temperature{i}", temp) - if modem_temps is not None: - msg.deviceState.modemTempC = modem_temps - for i, temp in enumerate(modem_temps): - statlog.gauge(f"modem_temperature{i}", temp) + + msg.deviceState.networkType = last_hw_state.network_type + msg.deviceState.networkStrength = last_hw_state.network_strength + if last_hw_state.network_info is not None: + msg.deviceState.networkInfo = last_hw_state.network_info + + msg.deviceState.nvmeTempC = last_hw_state.nvme_temps + msg.deviceState.modemTempC = last_hw_state.modem_temps msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness() msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity() @@ -320,7 +343,8 @@ def thermald_thread() -> NoReturn: set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not onroad_conditions["device_temp_good"])) if TICI: - set_offroad_alert_if_changed("Offroad_StorageMissing", (not Path("/data/media").is_mount())) + missing = (not Path("/data/media").is_mount()) and (not os.path.isfile("/persist/comma/living-in-the-moment")) + set_offroad_alert_if_changed("Offroad_StorageMissing", missing) # Handle offroad/onroad transition should_start = all(onroad_conditions.values()) @@ -343,7 +367,7 @@ def thermald_thread() -> NoReturn: try: with open('/dev/kmsg', 'w') as kmsg: - kmsg.write(f"[thermald] engaged: {engaged}") + kmsg.write(f"<3>[thermald] engaged: {engaged}\n") except Exception: pass @@ -392,7 +416,7 @@ def thermald_thread() -> NoReturn: should_start_prev = should_start startup_conditions_prev = startup_conditions.copy() - # log more stats + # Log to statsd statlog.gauge("free_space_percent", msg.deviceState.freeSpacePercent) statlog.gauge("gpu_usage_percent", msg.deviceState.gpuUsagePercent) statlog.gauge("memory_usage_percent", msg.deviceState.memoryUsagePercent) @@ -406,6 +430,10 @@ def thermald_thread() -> NoReturn: statlog.gauge("ambient_temperature", msg.deviceState.ambientTempC) for i, temp in enumerate(msg.deviceState.pmicTempC): statlog.gauge(f"pmic{i}_temperature", temp) + for i, temp in enumerate(last_hw_state.nvme_temps): + statlog.gauge(f"nvme_temperature{i}", temp) + for i, temp in enumerate(last_hw_state.modem_temps): + statlog.gauge(f"modem_temperature{i}", temp) statlog.gauge("fan_speed_percent_desired", msg.deviceState.fanSpeedPercentDesired) statlog.gauge("screen_brightness_percent", msg.deviceState.screenBrightnessPercent) @@ -416,7 +444,7 @@ def thermald_thread() -> NoReturn: cloudlog.event("STATUS_PACKET", count=count, - pandaStates=(strip_deprecated_keys(pandaStates.to_dict()) if pandaStates else None), + pandaStates=[strip_deprecated_keys(p.to_dict()) for p in pandaStates], peripheralState=strip_deprecated_keys(peripheralState.to_dict()), location=(strip_deprecated_keys(sm["gpsLocationExternal"].to_dict()) if sm.alive["gpsLocationExternal"] else None), deviceState=strip_deprecated_keys(msg.to_dict())) @@ -424,8 +452,28 @@ def thermald_thread() -> NoReturn: count += 1 -def main() -> NoReturn: - thermald_thread() +def main(): + hw_queue = queue.Queue(maxsize=1) + end_event = threading.Event() + + threads = [ + threading.Thread(target=hw_state_thread, args=(end_event, hw_queue)), + threading.Thread(target=thermald_thread, args=(end_event, hw_queue)), + ] + + for t in threads: + t.start() + + try: + while True: + time.sleep(1) + if not all(t.is_alive() for t in threads): + break + finally: + end_event.set() + + for t in threads: + t.join() if __name__ == "__main__": diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 62f93c13a9..00d528e63d 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -47,7 +47,7 @@ asset_obj = qt_env.Object("assets", assets) qt_env.Program("soundd/_soundd", ["soundd/main.cc", "soundd/sound.cc"], LIBS=qt_libs) if GetOption('test'): qt_env.Program("tests/playsound", "tests/playsound.cc", LIBS=base_libs) - qt_env.Program('tests/test_sound', ['tests/test_runner.cc', 'soundd/sound.cc', 'tests/test_sound.cc'], LIBS=[base_libs]) + qt_env.Program('tests/test_sound', ['tests/test_runner.cc', 'soundd/sound.cc', 'tests/test_sound.cc'], LIBS=qt_libs) qt_env.SharedLibrary("qt/python_helpers", ["qt/qt_window.cc"], LIBS=qt_libs) @@ -115,10 +115,10 @@ if GetOption('extras'): if arch in ['x86_64', 'Darwin'] or GetOption('extras'): qt_env['CXXFLAGS'] += ["-Wno-deprecated-declarations"] - replay_lib_src = ["replay/replay.cc", "replay/camera.cc", "replay/filereader.cc", "replay/logreader.cc", "replay/framereader.cc", "replay/route.cc", "replay/util.cc"] + replay_lib_src = ["replay/replay.cc", "replay/consoleui.cc", "replay/camera.cc", "replay/filereader.cc", "replay/logreader.cc", "replay/framereader.cc", "replay/route.cc", "replay/util.cc"] replay_lib = qt_env.Library("qt_replay", replay_lib_src, LIBS=base_libs) - replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv'] + qt_libs + replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv', 'ncurses'] + qt_libs qt_env.Program("replay/replay", ["replay/main.cc"], LIBS=replay_libs) qt_env.Program("watch3", ["watch3.cc"], LIBS=qt_libs + ['common', 'json11', 'zmq', 'visionipc', 'messaging']) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 1f284877c4..4058bd405f 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -53,12 +53,6 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { "Display speed in km/h instead of mph.", "../assets/offroad/icon_metric.png", }, - { - "CommunityFeaturesToggle", - "Enable Community Features", - "Use features, such as community supported hardware, from the open source community that are not maintained or supported by comma.ai and have not been confirmed to meet the standard safety model. Be extra cautious when using these features", - "../assets/offroad/icon_shell.png", - }, { "RecordFront", "Record and Upload Driver Camera", @@ -170,6 +164,10 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) { power_layout->addWidget(poweroff_btn); QObject::connect(poweroff_btn, &QPushButton::clicked, this, &DevicePanel::poweroff); + if (Hardware::TICI()) { + connect(uiState(), &UIState::offroadTransition, poweroff_btn, &QPushButton::setVisible); + } + setStyleSheet(R"( #reboot_btn { height: 120px; border-radius: 15px; background-color: #393939; } #reboot_btn:pressed { background-color: #4a4a4a; } diff --git a/selfdrive/ui/qt/offroad/wifiManager.cc b/selfdrive/ui/qt/offroad/wifiManager.cc index 40973a240f..1628f35d99 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.cc +++ b/selfdrive/ui/qt/offroad/wifiManager.cc @@ -1,5 +1,8 @@ #include "selfdrive/ui/qt/offroad/wifiManager.h" +#include "selfdrive/ui/ui.h" +#include "selfdrive/ui/qt/widgets/prime.h" + #include "selfdrive/common/params.h" #include "selfdrive/common/swaglog.h" #include "selfdrive/ui/qt/util.h" @@ -31,6 +34,12 @@ T call(const QString &path, const QString &interface, const QString &method, Arg return T(); } +template +QDBusPendingCall asyncCall(const QString &path, const QString &interface, const QString &method, Args &&...args) { + QDBusInterface nm = QDBusInterface(NM_DBUS_SERVICE, path, interface, QDBusConnection::systemBus()); + return nm.asyncCall(method, args...); +} + WifiManager::WifiManager(QObject *parent) : QObject(parent) { qDBusRegisterMetaType(); qDBusRegisterMetaType(); @@ -78,39 +87,39 @@ void WifiManager::stop() { void WifiManager::refreshNetworks() { if (adapter.isEmpty() || !timer.isActive()) return; + QDBusPendingCall pending_call = asyncCall(adapter, NM_DBUS_INTERFACE_DEVICE_WIRELESS, "GetAllAccessPoints"); + QDBusPendingCallWatcher *watcher = new QDBusPendingCallWatcher(pending_call); + QObject::connect(watcher, &QDBusPendingCallWatcher::finished, this, &WifiManager::refreshFinished); +} + +void WifiManager::refreshFinished(QDBusPendingCallWatcher *watcher) { + ipv4_address = getIp4Address(); seenNetworks.clear(); - ipv4_address = get_ipv4_address(); - QDBusReply> response = call(adapter, NM_DBUS_INTERFACE_DEVICE_WIRELESS, "GetAllAccessPoints"); - for (const QDBusObjectPath &path : response.value()) { - const QByteArray &ssid = get_property(path.path(), "Ssid"); - unsigned int strength = get_ap_strength(path.path()); - if (ssid.isEmpty() || (seenNetworks.contains(ssid) && - strength <= seenNetworks.value(ssid).strength)) { - continue; - } - SecurityType security = getSecurityType(path.path()); - ConnectedType ctype; - QString activeSsid = (activeAp != "" && activeAp != "/") ? get_property(activeAp, "Ssid") : ""; - if (ssid != activeSsid) { - ctype = ConnectedType::DISCONNECTED; - } else { - if (ssid == connecting_to_network) { - ctype = ConnectedType::CONNECTING; - } else { - ctype = ConnectedType::CONNECTED; - } + const QDBusReply> wather_reply = *watcher; + for (const QDBusObjectPath &path : wather_reply.value()) { + QDBusReply replay = call(path.path(), NM_DBUS_INTERFACE_PROPERTIES, "GetAll", NM_DBUS_INTERFACE_ACCESS_POINT); + auto properties = replay.value(); + + const QByteArray ssid = properties["Ssid"].toByteArray(); + uint32_t strength = properties["Strength"].toUInt(); + if (ssid.isEmpty() || (seenNetworks.contains(ssid) && strength <= seenNetworks[ssid].strength)) continue; + + SecurityType security = getSecurityType(properties); + ConnectedType ctype = ConnectedType::DISCONNECTED; + if (path.path() == activeAp) { + ctype = (ssid == connecting_to_network) ? ConnectedType::CONNECTING : ConnectedType::CONNECTED; } - Network network = {ssid, strength, ctype, security}; - seenNetworks[ssid] = network; + seenNetworks[ssid] = {ssid, strength, ctype, security}; } + emit refreshSignal(); + watcher->deleteLater(); } -QString WifiManager::get_ipv4_address() { - if (raw_adapter_state != NM_DEVICE_STATE_ACTIVATED) { - return ""; - } +QString WifiManager::getIp4Address() { + if (raw_adapter_state != NM_DEVICE_STATE_ACTIVATED) return ""; + for (const auto &p : getActiveConnections()) { QString type = call(p.path(), NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACTIVE_CONNECTION, "Type"); if (type == "802-11-wireless") { @@ -129,10 +138,10 @@ QString WifiManager::get_ipv4_address() { return ""; } -SecurityType WifiManager::getSecurityType(const QString &path) { - int sflag = get_property(path, "Flags").toInt(); - int wpaflag = get_property(path, "WpaFlags").toInt(); - int rsnflag = get_property(path, "RsnFlags").toInt(); +SecurityType WifiManager::getSecurityType(const QVariantMap &properties) { + int sflag = properties["Flags"].toUInt(); + int wpaflag = properties["WpaFlags"].toUInt(); + int rsnflag = properties["RsnFlags"].toUInt(); int wpa_props = wpaflag | rsnflag; // obtained by looking at flags of networks in the office as reported by an Android phone @@ -181,13 +190,14 @@ void WifiManager::deactivateConnectionBySsid(const QString &ssid) { QString Ssid = get_property(pth.path(), "Ssid"); if (Ssid == ssid) { deactivateConnection(active_connection); + return; } } } } void WifiManager::deactivateConnection(const QDBusObjectPath &path) { - call(NM_DBUS_PATH, NM_DBUS_INTERFACE, "DeactivateConnection", QVariant::fromValue(path)); + asyncCall(NM_DBUS_PATH, NM_DBUS_INTERFACE, "DeactivateConnection", QVariant::fromValue(path)); } QVector WifiManager::getActiveConnections() { @@ -219,17 +229,15 @@ uint WifiManager::getAdapterType(const QDBusObjectPath &path) { } void WifiManager::requestScan() { - call(adapter, NM_DBUS_INTERFACE_DEVICE_WIRELESS, "RequestScan", QVariantMap()); + if (!adapter.isEmpty()) { + asyncCall(adapter, NM_DBUS_INTERFACE_DEVICE_WIRELESS, "RequestScan", QVariantMap()); + } } QByteArray WifiManager::get_property(const QString &network_path , const QString &property) { return call(network_path, NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACCESS_POINT, property); } -unsigned int WifiManager::get_ap_strength(const QString &network_path) { - return call(network_path, NM_DBUS_INTERFACE_PROPERTIES, "Get", NM_DBUS_INTERFACE_ACCESS_POINT, "Strength"); -} - QString WifiManager::getAdapter(const uint adapter_type) { QDBusReply> response = call(NM_DBUS_PATH, NM_DBUS_INTERFACE, "GetDevices"); for (const QDBusObjectPath &path : response.value()) { @@ -281,19 +289,8 @@ void WifiManager::newConnection(const QDBusObjectPath &path) { } } -void WifiManager::disconnect() { - if (activeAp != "" && activeAp != "/") { - deactivateConnectionBySsid(get_property(activeAp, "Ssid")); - } -} - QDBusObjectPath WifiManager::getConnectionPath(const QString &ssid) { - for (const QString &conn_ssid : knownConnections) { - if (ssid == conn_ssid) { - return knownConnections.key(conn_ssid); - } - } - return QDBusObjectPath(); + return knownConnections.key(ssid); } Connection WifiManager::getConnectionSettings(const QDBusObjectPath &path) { @@ -312,18 +309,19 @@ void WifiManager::initConnections() { } } -void WifiManager::activateWifiConnection(const QString &ssid) { +std::optional WifiManager::activateWifiConnection(const QString &ssid) { const QDBusObjectPath &path = getConnectionPath(ssid); if (!path.path().isEmpty()) { connecting_to_network = ssid; - call(NM_DBUS_PATH, NM_DBUS_INTERFACE, "ActivateConnection", QVariant::fromValue(path), QVariant::fromValue(QDBusObjectPath(adapter)), QVariant::fromValue(QDBusObjectPath("/"))); + return asyncCall(NM_DBUS_PATH, NM_DBUS_INTERFACE, "ActivateConnection", QVariant::fromValue(path), QVariant::fromValue(QDBusObjectPath(adapter)), QVariant::fromValue(QDBusObjectPath("/"))); } + return std::nullopt; } void WifiManager::activateModemConnection(const QDBusObjectPath &path) { QString modem = getAdapter(NM_DEVICE_TYPE_MODEM); if (!path.path().isEmpty() && !modem.isEmpty()) { - call(NM_DBUS_PATH, NM_DBUS_INTERFACE, "ActivateConnection", QVariant::fromValue(path), QVariant::fromValue(QDBusObjectPath(modem)), QVariant::fromValue(QDBusObjectPath("/"))); + asyncCall(NM_DBUS_PATH, NM_DBUS_INTERFACE, "ActivateConnection", QVariant::fromValue(path), QVariant::fromValue(QDBusObjectPath(modem)), QVariant::fromValue(QDBusObjectPath("/"))); } } @@ -409,12 +407,32 @@ void WifiManager::addTetheringConnection() { call(NM_DBUS_PATH_SETTINGS, NM_DBUS_INTERFACE_SETTINGS, "AddConnection", QVariant::fromValue(connection)); } +void WifiManager::tetheringActivated(QDBusPendingCallWatcher *call) { + int prime_type = uiState()->prime_type; + int ipv4_forward = (prime_type == PrimeType::NONE || prime_type == PrimeType::LITE); + + if (!ipv4_forward) { + QTimer::singleShot(5000, this, [=] { + qWarning() << "net.ipv4.ip_forward = 0"; + std::system("sudo sysctl net.ipv4.ip_forward=0"); + }); + } + call->deleteLater(); +} + void WifiManager::setTetheringEnabled(bool enabled) { if (enabled) { if (!isKnownConnection(tethering_ssid)) { addTetheringConnection(); } - activateWifiConnection(tethering_ssid); + + auto pending_call = activateWifiConnection(tethering_ssid); + + if (pending_call) { + QDBusPendingCallWatcher *watcher = new QDBusPendingCallWatcher(*pending_call); + QObject::connect(watcher, &QDBusPendingCallWatcher::finished, this, &WifiManager::tetheringActivated); + } + } else { deactivateConnectionBySsid(tethering_ssid); } diff --git a/selfdrive/ui/qt/offroad/wifiManager.h b/selfdrive/ui/qt/offroad/wifiManager.h index 8ab9e0e27c..07b982c2c2 100644 --- a/selfdrive/ui/qt/offroad/wifiManager.h +++ b/selfdrive/ui/qt/offroad/wifiManager.h @@ -1,5 +1,6 @@ #pragma once +#include #include #include @@ -37,29 +38,24 @@ class WifiManager : public QObject { Q_OBJECT public: - explicit WifiManager(QObject* parent); - void start(); - void stop(); - void requestScan(); QMap seenNetworks; QMap knownConnections; - QDBusObjectPath lteConnectionPath; QString ipv4_address; - void refreshNetworks(); + explicit WifiManager(QObject* parent); + void start(); + void stop(); + void requestScan(); void forgetConnection(const QString &ssid); bool isKnownConnection(const QString &ssid); - void activateWifiConnection(const QString &ssid); - void activateModemConnection(const QDBusObjectPath &path); + std::optional activateWifiConnection(const QString &ssid); NetworkType currentNetworkType(); void updateGsmSettings(bool roaming, QString apn); void connect(const Network &ssid, const QString &password = {}, const QString &username = {}); - void disconnect(); // Tethering functions void setTetheringEnabled(bool enabled); bool isTetheringEnabled(); - void addTetheringConnection(); void changeTetheringPassword(const QString &newPassword); QString getTetheringPassword(); @@ -70,23 +66,26 @@ private: QString connecting_to_network; QString tethering_ssid; const QString defaultTetheringPassword = "swagswagcomma"; + QString activeAp; + QDBusObjectPath lteConnectionPath; QString getAdapter(const uint = NM_DEVICE_TYPE_WIFI); uint getAdapterType(const QDBusObjectPath &path); bool isWirelessAdapter(const QDBusObjectPath &path); - QString get_ipv4_address(); + QString getIp4Address(); void connect(const QByteArray &ssid, const QString &username, const QString &password, SecurityType security_type); - QString activeAp; void deactivateConnectionBySsid(const QString &ssid); void deactivateConnection(const QDBusObjectPath &path); QVector getActiveConnections(); QByteArray get_property(const QString &network_path, const QString &property); - unsigned int get_ap_strength(const QString &network_path); - SecurityType getSecurityType(const QString &path); + SecurityType getSecurityType(const QVariantMap &properties); QDBusObjectPath getConnectionPath(const QString &ssid); Connection getConnectionSettings(const QDBusObjectPath &path); void initConnections(); void setup(); + void refreshNetworks(); + void activateModemConnection(const QDBusObjectPath &path); + void addTetheringConnection(); signals: void wrongPassword(const QString &ssid); @@ -98,4 +97,6 @@ private slots: void deviceAdded(const QDBusObjectPath &path); void connectionRemoved(const QDBusObjectPath &path); void newConnection(const QDBusObjectPath &path); + void refreshFinished(QDBusPendingCallWatcher *call); + void tetheringActivated(QDBusPendingCallWatcher *call); }; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index e394d1248a..f7e2dc9bf8 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -78,7 +78,7 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { void OnroadWindow::offroadTransition(bool offroad) { #ifdef ENABLE_MAPS if (!offroad) { - if (map == nullptr && (uiState()->has_prime || !MAPBOX_TOKEN.isEmpty())) { + if (map == nullptr && (uiState()->prime_type || !MAPBOX_TOKEN.isEmpty())) { MapWindow * m = new MapWindow(get_mapbox_settings()); m->setFixedWidth(topWidget(this)->width() / 2); m->offroadTransition(offroad); @@ -338,7 +338,7 @@ void NvgWindow::drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV float g_xo = sz / 5; float g_yo = sz / 10; - QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_xo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}}; + QPointF glow[] = {{x + (sz * 1.35) + g_xo, y + sz + g_yo}, {x, y - g_yo}, {x - (sz * 1.35) - g_xo, y + sz + g_yo}}; painter.setBrush(QColor(218, 202, 37, 255)); painter.drawPolygon(glow, std::size(glow)); diff --git a/selfdrive/ui/qt/setup/reset.cc b/selfdrive/ui/qt/setup/reset.cc index aa000f9359..9ffcf7f6cf 100644 --- a/selfdrive/ui/qt/setup/reset.cc +++ b/selfdrive/ui/qt/setup/reset.cc @@ -9,15 +9,12 @@ #include "selfdrive/ui/qt/setup/reset.h" #define NVME "/dev/nvme0n1" -#define SDCARD "/dev/mmcblk0" #define USERDATA "/dev/disk/by-partlabel/userdata" void Reset::doReset() { // best effort to wipe nvme and sd card std::system("sudo umount " NVME); std::system("yes | sudo mkfs.ext4 " NVME); - std::system("sudo umount " SDCARD); - std::system("yes | sudo mkfs.ext4 " SDCARD); // we handle two cases here // * user-prompted factory reset diff --git a/selfdrive/ui/qt/setup/setup.cc b/selfdrive/ui/qt/setup/setup.cc index bd494327cc..17304665c7 100644 --- a/selfdrive/ui/qt/setup/setup.cc +++ b/selfdrive/ui/qt/setup/setup.cc @@ -10,13 +10,14 @@ #include +#include "selfdrive/common/util.h" #include "selfdrive/hardware/hw.h" #include "selfdrive/ui/qt/api.h" #include "selfdrive/ui/qt/qt_window.h" #include "selfdrive/ui/qt/offroad/networking.h" #include "selfdrive/ui/qt/widgets/input.h" -const char* USER_AGENT = "AGNOSSetup-0.1"; +const std::string USER_AGENT = "AGNOSSetup-"; const QString DASHCAM_URL = "https://dashcam.comma.ai"; void Setup::download(QString url) { @@ -26,6 +27,8 @@ void Setup::download(QString url) { return; } + auto version = util::read_file("/VERSION"); + char tmpfile[] = "/tmp/installer_XXXXXX"; FILE *fp = fdopen(mkstemp(tmpfile), "w"); @@ -34,18 +37,21 @@ void Setup::download(QString url) { curl_easy_setopt(curl, CURLOPT_WRITEDATA, fp); curl_easy_setopt(curl, CURLOPT_VERBOSE, 1L); curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1L); - curl_easy_setopt(curl, CURLOPT_USERAGENT, USER_AGENT); + curl_easy_setopt(curl, CURLOPT_USERAGENT, (USER_AGENT + version).c_str()); int ret = curl_easy_perform(curl); - if (ret != CURLE_OK) { + + long res_status = 0; + curl_easy_getinfo(curl, CURLINFO_RESPONSE_CODE, &res_status); + if (ret == CURLE_OK && res_status == 200) { + rename(tmpfile, "/tmp/installer"); + emit finished(true); + } else { emit finished(false); - return; } + curl_easy_cleanup(curl); fclose(fp); - - rename(tmpfile, "/tmp/installer"); - emit finished(true); } QWidget * Setup::low_voltage() { diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index a16923894d..fbd425b025 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -299,12 +299,12 @@ void CameraViewWidget::vipcThread() { std::lock_guard lk(lock); if (!Hardware::EON()) { void *texture_buffer = gl_buffer->map(QOpenGLBuffer::WriteOnly); - + if (texture_buffer == nullptr) { LOGE("gl_buffer->map returned nullptr"); continue; } - + memcpy(texture_buffer, buf->addr, buf->len); gl_buffer->unmap(); diff --git a/selfdrive/ui/qt/widgets/controls.cc b/selfdrive/ui/qt/widgets/controls.cc index d4e48fc64f..89c95843fb 100644 --- a/selfdrive/ui/qt/widgets/controls.cc +++ b/selfdrive/ui/qt/widgets/controls.cc @@ -19,7 +19,7 @@ QFrame *horizontal_line(QWidget *parent) { AbstractControl::AbstractControl(const QString &title, const QString &desc, const QString &icon, QWidget *parent) : QFrame(parent) { setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum); - + QVBoxLayout *main_layout = new QVBoxLayout(this); main_layout->setMargin(0); diff --git a/selfdrive/ui/qt/widgets/prime.cc b/selfdrive/ui/qt/widgets/prime.cc index 2e9a9e6af9..27d472535b 100644 --- a/selfdrive/ui/qt/widgets/prime.cc +++ b/selfdrive/ui/qt/widgets/prime.cc @@ -307,19 +307,19 @@ void SetupWidget::replyFinished(const QString &response, bool success) { } QJsonObject json = doc.object(); + int prime_type = json["prime_type"].toInt(); + + if (uiState()->prime_type != prime_type) { + uiState()->prime_type = prime_type; + Params().put("PrimeType", std::to_string(prime_type)); + } + if (!json["is_paired"].toBool()) { mainLayout->setCurrentIndex(0); } else { popup->reject(); - bool prime = json["prime"].toBool(); - - if (uiState()->has_prime != prime) { - uiState()->has_prime = prime; - Params().putBool("HasPrime", prime); - } - - if (prime) { + if (prime_type) { mainLayout->setCurrentWidget(primeUser); } else { mainLayout->setCurrentWidget(primeAd); diff --git a/selfdrive/ui/qt/widgets/prime.h b/selfdrive/ui/qt/widgets/prime.h index f7470fe441..566238fc33 100644 --- a/selfdrive/ui/qt/widgets/prime.h +++ b/selfdrive/ui/qt/widgets/prime.h @@ -7,6 +7,12 @@ #include "selfdrive/ui/qt/widgets/input.h" +enum PrimeType { + NONE = 0, + MAGENTA, + LITE, +}; + // pairing QR code class PairingQRWidget : public QWidget { Q_OBJECT diff --git a/selfdrive/ui/replay/camera.cc b/selfdrive/ui/replay/camera.cc index 1912c9b581..9bcc00583e 100644 --- a/selfdrive/ui/replay/camera.cc +++ b/selfdrive/ui/replay/camera.cc @@ -1,7 +1,7 @@ #include "selfdrive/ui/replay/camera.h" +#include "selfdrive/ui/replay/util.h" #include -#include CameraServer::CameraServer(std::pair camera_size[MAX_CAMERAS], bool send_yuv) : send_yuv(send_yuv) { for (int i = 0; i < MAX_CAMERAS; ++i) { @@ -24,7 +24,7 @@ void CameraServer::startVipcServer() { vipc_server_.reset(new VisionIpcServer("camerad")); for (auto &cam : cameras_) { if (cam.width > 0 && cam.height > 0) { - std::cout << "camera[" << cam.type << "] frame size " << cam.width << "x" << cam.height << std::endl; + rInfo("camera[%d] frame size %dx%d", cam.type, cam.width, cam.height); vipc_server_->create_buffers(cam.rgb_type, UI_BUF_COUNT, true, cam.width, cam.height); if (send_yuv) { vipc_server_->create_buffers(cam.yuv_type, YUV_BUFFER_COUNT, false, cam.width, cam.height); @@ -61,7 +61,7 @@ void CameraServer::cameraThread(Camera &cam) { if (rgb) vipc_server_->send(rgb, &extra, false); if (yuv) vipc_server_->send(yuv, &extra, false); } else { - std::cout << "camera[" << cam.type << "] failed to get frame:" << eidx.getSegmentId() << std::endl; + rError("camera[%d] failed to get frame:", cam.type, eidx.getSegmentId()); } cam.cached_id = id + 1; diff --git a/selfdrive/ui/replay/camera.h b/selfdrive/ui/replay/camera.h index 21e02292d5..340a120e8c 100644 --- a/selfdrive/ui/replay/camera.h +++ b/selfdrive/ui/replay/camera.h @@ -18,7 +18,7 @@ public: protected: struct Camera { CameraType type; - VisionStreamType rgb_type; + VisionStreamType rgb_type; VisionStreamType yuv_type; int width; int height; diff --git a/selfdrive/ui/replay/consoleui.cc b/selfdrive/ui/replay/consoleui.cc new file mode 100644 index 0000000000..72e2fcf802 --- /dev/null +++ b/selfdrive/ui/replay/consoleui.cc @@ -0,0 +1,353 @@ +#include "selfdrive/ui/replay/consoleui.h" + +#include +#include + +#include "selfdrive/common/version.h" + +namespace { + +const int BORDER_SIZE = 3; + +const std::initializer_list> keyboard_shortcuts[] = { + { + {"s", "+10s"}, + {"shift+s", "-10s"}, + {"m", "+60s"}, + {"shift+m", "-60s"}, + {"space", "Pause/Resume"}, + {"e", "Next Engagement"}, + {"d", "Next Disengagement"}, + }, + { + {"enter", "Enter seek request"}, + {"x", "+/-Replay speed"}, + {"q", "Exit"}, + }, +}; + +enum Color { + Default, + Debug, + Yellow, + Green, + Red, + BrightWhite, + Engaged, + Disengaged, +}; + +void add_str(WINDOW *w, const char *str, Color color = Color::Default, bool bold = false) { + if (color != Color::Default) wattron(w, COLOR_PAIR(color)); + if (bold) wattron(w, A_BOLD); + waddstr(w, str); + if (bold) wattroff(w, A_BOLD); + if (color != Color::Default) wattroff(w, COLOR_PAIR(color)); +} + +std::string format_seconds(int s) { + int total_minutes = s / 60; + int seconds = s % 60; + int hours = total_minutes / 60; + int minutes = total_minutes % 60; + return util::string_format("%02d:%02d:%02d", hours, minutes, seconds); +} + +} // namespace + +ConsoleUI::ConsoleUI(Replay *replay, QObject *parent) : replay(replay), sm({"carState", "liveParameters"}), QObject(parent) { + // Initialize curses + initscr(); + clear(); + curs_set(false); + cbreak(); // Line buffering disabled. pass on everything + noecho(); + keypad(stdscr, true); + nodelay(stdscr, true); // non-blocking getchar() + + // Initialize all the colors. https://www.ditig.com/256-colors-cheat-sheet + start_color(); + init_pair(Color::Debug, 246, COLOR_BLACK); // #949494 + init_pair(Color::Yellow, 184, COLOR_BLACK); + init_pair(Color::Red, COLOR_RED, COLOR_BLACK); + init_pair(Color::BrightWhite, 15, COLOR_BLACK); + init_pair(Color::Disengaged, COLOR_BLUE, COLOR_BLUE); + init_pair(Color::Engaged, 28, 28); + init_pair(Color::Green, 34, COLOR_BLACK); + + initWindows(); + + qRegisterMetaType("uint64_t"); + qRegisterMetaType("ReplyMsgType"); + installMessageHandler([this](ReplyMsgType type, const std::string msg) { + emit logMessageSignal(type, QString::fromStdString(msg)); + }); + installDownloadProgressHandler([this](uint64_t cur, uint64_t total, bool success) { + emit updateProgressBarSignal(cur, total, success); + }); + + QObject::connect(replay, &Replay::streamStarted, this, &ConsoleUI::updateSummary); + QObject::connect(¬ifier, SIGNAL(activated(int)), SLOT(readyRead())); + QObject::connect(this, &ConsoleUI::updateProgressBarSignal, this, &ConsoleUI::updateProgressBar); + QObject::connect(this, &ConsoleUI::logMessageSignal, this, &ConsoleUI::logMessage); + + sm_timer.callOnTimeout(this, &ConsoleUI::updateStatus); + sm_timer.start(100); + getch_timer.start(1000, this); + readyRead(); +} + +ConsoleUI::~ConsoleUI() { + endwin(); +} + +void ConsoleUI::initWindows() { + getmaxyx(stdscr, max_height, max_width); + w.fill(nullptr); + w[Win::Title] = newwin(1, max_width, 0, 0); + w[Win::Stats] = newwin(2, max_width - 2 * BORDER_SIZE, 2, BORDER_SIZE); + w[Win::Timeline] = newwin(4, max_width - 2 * BORDER_SIZE, 5, BORDER_SIZE); + w[Win::TimelineDesc] = newwin(1, 100, 10, BORDER_SIZE); + w[Win::CarState] = newwin(3, 100, 12, BORDER_SIZE); + w[Win::DownloadBar] = newwin(1, 100, 16, BORDER_SIZE); + if (int log_height = max_height - 27; log_height > 4) { + w[Win::LogBorder] = newwin(log_height, max_width - 2 * (BORDER_SIZE - 1), 17, BORDER_SIZE - 1); + box(w[Win::LogBorder], 0, 0); + w[Win::Log] = newwin(log_height - 2, max_width - 2 * BORDER_SIZE, 18, BORDER_SIZE); + scrollok(w[Win::Log], true); + } + w[Win::Help] = newwin(5, max_width - (2 * BORDER_SIZE), max_height - 6, BORDER_SIZE); + + // set the title bar + wbkgd(w[Win::Title], A_REVERSE); + mvwprintw(w[Win::Title], 0, 3, "openpilot replay %s", COMMA_VERSION); + + // show windows on the real screen + refresh(); + displayTimelineDesc(); + displayHelp(); + updateSummary(); + updateTimeline(); + for (auto win : w) { + if (win) wrefresh(win); + } +} + +void ConsoleUI::timerEvent(QTimerEvent *ev) { + if (ev->timerId() != getch_timer.timerId()) return; + + if (is_term_resized(max_height, max_width)) { + for (auto win : w) { + if (win) delwin(win); + } + endwin(); + clear(); + refresh(); + initWindows(); + rWarning("resize term %dx%d", max_height, max_width); + } + updateTimeline(); +} + +void ConsoleUI::updateStatus() { + auto write_item = [this](int y, int x, const char *key, const std::string &value, const char *unit, + bool bold = false, Color color = Color::BrightWhite) { + auto win = w[Win::CarState]; + wmove(win, y, x); + add_str(win, key); + add_str(win, value.c_str(), color, bold); + add_str(win, unit); + }; + static const std::pair status_text[] = { + {"loading...", Color::Red}, + {"playing", Color::Green}, + {"paused...", Color::Yellow}, + }; + + sm.update(0); + + if (status != Status::Paused) { + status = (sm.updated("carState") || sm.updated("liveParameters")) ? Status::Playing : Status::Waiting; + } + auto [status_str, status_color] = status_text[status]; + write_item(0, 0, "STATUS: ", status_str, " ", false, status_color); + std::string suffix = util::string_format(" / %s [%d/%d] ", format_seconds(replay->totalSeconds()).c_str(), + replay->currentSeconds() / 60, replay->route()->segments().size()); + write_item(0, 25, "TIME: ", format_seconds(replay->currentSeconds()), suffix.c_str(), true); + + auto p = sm["liveParameters"].getLiveParameters(); + write_item(1, 0, "STIFFNESS: ", util::string_format("%.2f %%", p.getStiffnessFactor() * 100), " "); + write_item(1, 25, "SPEED: ", util::string_format("%.2f", sm["carState"].getCarState().getVEgo()), " m/s"); + write_item(2, 0, "STEER RATIO: ", util::string_format("%.2f", p.getSteerRatio()), ""); + auto angle_offsets = util::string_format("%.2f|%.2f", p.getAngleOffsetAverageDeg(), p.getAngleOffsetDeg()); + write_item(2, 25, "ANGLE OFFSET(AVG|INSTANT): ", angle_offsets, " deg"); + + wrefresh(w[Win::CarState]); +} + +void ConsoleUI::displayHelp() { + for (int i = 0; i < std::size(keyboard_shortcuts); ++i) { + wmove(w[Win::Help], i * 2, 0); + for (auto &[key, desc] : keyboard_shortcuts[i]) { + wattron(w[Win::Help], A_REVERSE); + waddstr(w[Win::Help], (' ' + key + ' ').c_str()); + wattroff(w[Win::Help], A_REVERSE); + waddstr(w[Win::Help], (' ' + desc + ' ').c_str()); + } + } + wrefresh(w[Win::Help]); +} + +void ConsoleUI::displayTimelineDesc() { + std::tuple indicators[]{ + {Color::Engaged, " Engaged ", false}, + {Color::Disengaged, " Disengaged ", false}, + {Color::Green, " Info ", true}, + {Color::Yellow, " Warning ", true}, + {Color::Red, " Critical ", true}, + }; + for (auto [color, name, bold] : indicators) { + add_str(w[Win::TimelineDesc], "__", color, bold); + add_str(w[Win::TimelineDesc], name); + } +} + +void ConsoleUI::logMessage(ReplyMsgType type, const QString &msg) { + if (auto win = w[Win::Log]) { + Color color = Color::Default; + if (type == ReplyMsgType::Debug) { + color = Color::Debug; + } else if (type == ReplyMsgType::Warning) { + color = Color::Yellow; + } else if (type == ReplyMsgType::Critical) { + color = Color::Red; + } + add_str(win, qPrintable(msg + "\n"), color); + wrefresh(win); + } +} + +void ConsoleUI::updateProgressBar(uint64_t cur, uint64_t total, bool success) { + werase(w[Win::DownloadBar]); + if (success && cur < total) { + const int width = 35; + const float progress = cur / (double)total; + const int pos = width * progress; + wprintw(w[Win::DownloadBar], "Downloading [%s>%s] %d%% %s", std::string(pos, '=').c_str(), + std::string(width - pos, ' ').c_str(), int(progress * 100.0), formattedDataSize(total).c_str()); + } + wrefresh(w[Win::DownloadBar]); +} + +void ConsoleUI::updateSummary() { + const auto &route = replay->route(); + mvwprintw(w[Win::Stats], 0, 0, "Route: %s, %d segments", qPrintable(route->name()), route->segments().size()); + mvwprintw(w[Win::Stats], 1, 0, "Car Fingerprint: %s", replay->carFingerprint().c_str()); + wrefresh(w[Win::Stats]); +} + +void ConsoleUI::updateTimeline() { + auto win = w[Win::Timeline]; + int width = getmaxx(win); + werase(win); + + wattron(win, COLOR_PAIR(Color::Disengaged)); + mvwhline(win, 1, 0, ' ', width); + mvwhline(win, 2, 0, ' ', width); + wattroff(win, COLOR_PAIR(Color::Disengaged)); + + const int total_sec = replay->totalSeconds(); + for (auto [begin, end, type] : replay->getTimeline()) { + int start_pos = ((double)begin / total_sec) * width; + int end_pos = ((double)end / total_sec) * width; + if (type == TimelineType::Engaged) { + mvwchgat(win, 1, start_pos, end_pos - start_pos + 1, A_COLOR, Color::Engaged, NULL); + mvwchgat(win, 2, start_pos, end_pos - start_pos + 1, A_COLOR, Color::Engaged, NULL); + } else { + auto color_id = Color::Green; + if (type != TimelineType::AlertInfo) { + color_id = type == TimelineType::AlertWarning ? Color::Yellow : Color::Red; + } + mvwchgat(win, 3, start_pos, end_pos - start_pos + 1, ACS_S3, color_id, NULL); + } + } + + int cur_pos = ((double)replay->currentSeconds() / total_sec) * width; + wattron(win, COLOR_PAIR(Color::BrightWhite)); + mvwaddch(win, 0, cur_pos, ACS_VLINE); + mvwaddch(win, 3, cur_pos, ACS_VLINE); + wattroff(win, COLOR_PAIR(Color::BrightWhite)); + wrefresh(win); +} + +void ConsoleUI::readyRead() { + int c; + while ((c = getch()) != ERR) { + handleKey(c); + } +} + +void ConsoleUI::pauseReplay(bool pause) { + replay->pause(pause); + status = pause ? Status::Paused : Status::Waiting; +} + +void ConsoleUI::handleKey(char c) { + if (c == '\n') { + // pause the replay and blocking getchar() + pauseReplay(true); + updateStatus(); + getch_timer.stop(); + curs_set(true); + nodelay(stdscr, false); + + // Wait for user input + rWarning("Waiting for input..."); + int y = getmaxy(stdscr) - 9; + move(y, BORDER_SIZE); + add_str(stdscr, "Enter seek request: ", Color::BrightWhite, true); + refresh(); + + // Seek to choice + echo(); + int choice = 0; + scanw((char *)"%d", &choice); + noecho(); + pauseReplay(false); + replay->seekTo(choice, false); + + // Clean up and turn off the blocking mode + move(y, 0); + clrtoeol(); + nodelay(stdscr, true); + curs_set(false); + refresh(); + getch_timer.start(1000, this); + + } else if (c == 'x') { + if (replay->hasFlag(REPLAY_FLAG_FULL_SPEED)) { + replay->removeFlag(REPLAY_FLAG_FULL_SPEED); + rWarning("replay at normal speed"); + } else { + replay->addFlag(REPLAY_FLAG_FULL_SPEED); + rWarning("replay at full speed"); + } + } else if (c == 'e') { + replay->seekToFlag(FindFlag::nextEngagement); + } else if (c == 'd') { + replay->seekToFlag(FindFlag::nextDisEngagement); + } else if (c == 'm') { + replay->seekTo(+60, true); + } else if (c == 'M') { + replay->seekTo(-60, true); + } else if (c == 's') { + replay->seekTo(+10, true); + } else if (c == 'S') { + replay->seekTo(-10, true); + } else if (c == ' ') { + pauseReplay(!replay->isPaused()); + } else if (c == 'q' || c == 'Q') { + replay->stop(); + qApp->exit(); + } +} diff --git a/selfdrive/ui/replay/consoleui.h b/selfdrive/ui/replay/consoleui.h new file mode 100644 index 0000000000..bce1146d46 --- /dev/null +++ b/selfdrive/ui/replay/consoleui.h @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include +#include +#include +#include + +#include "selfdrive/ui/replay/replay.h" +#include + +class ConsoleUI : public QObject { + Q_OBJECT + +public: + ConsoleUI(Replay *replay, QObject *parent = 0); + ~ConsoleUI(); + +private: + void initWindows(); + void handleKey(char c); + void displayHelp(); + void displayTimelineDesc(); + void updateTimeline(); + void updateSummary(); + void updateStatus(); + void pauseReplay(bool pause); + + enum Status { Waiting, Playing, Paused }; + enum Win { Title, Stats, Log, LogBorder, DownloadBar, Timeline, TimelineDesc, Help, CarState, Max}; + std::array w{}; + SubMaster sm; + Replay *replay; + QBasicTimer getch_timer; + QTimer sm_timer; + QSocketNotifier notifier{0, QSocketNotifier::Read, this}; + int max_width, max_height; + Status status = Status::Waiting; + +signals: + void updateProgressBarSignal(uint64_t cur, uint64_t total, bool success); + void logMessageSignal(ReplyMsgType type, const QString &msg); + +private slots: + void readyRead(); + void timerEvent(QTimerEvent *ev); + void updateProgressBar(uint64_t cur, uint64_t total, bool success); + void logMessage(ReplyMsgType type, const QString &msg); +}; diff --git a/selfdrive/ui/replay/filereader.cc b/selfdrive/ui/replay/filereader.cc index 1585bb42d2..b57a62d501 100644 --- a/selfdrive/ui/replay/filereader.cc +++ b/selfdrive/ui/replay/filereader.cc @@ -1,7 +1,6 @@ #include "selfdrive/ui/replay/filereader.h" #include -#include #include "selfdrive/common/util.h" #include "selfdrive/ui/replay/util.h" @@ -35,13 +34,12 @@ std::string FileReader::read(const std::string &file, std::atomic *abort) std::string FileReader::download(const std::string &url, std::atomic *abort) { for (int i = 0; i <= max_retries_ && !(abort && *abort); ++i) { + if (i > 0) rWarning("download failed, retrying %d", i); + std::string result = httpGet(url, chunk_size_, abort); if (!result.empty()) { return result; } - if (i != max_retries_ && !(abort && *abort)) { - std::cout << "download failed, retrying " << i + 1 << std::endl; - } } return {}; } diff --git a/selfdrive/ui/replay/framereader.cc b/selfdrive/ui/replay/framereader.cc index 59e43cb58f..ef90c28355 100644 --- a/selfdrive/ui/replay/framereader.cc +++ b/selfdrive/ui/replay/framereader.cc @@ -1,4 +1,5 @@ #include "selfdrive/ui/replay/framereader.h" +#include "selfdrive/ui/replay/util.h" #include #include "libyuv.h" @@ -29,7 +30,7 @@ enum AVPixelFormat get_hw_format(AVCodecContext *ctx, const enum AVPixelFormat * for (const enum AVPixelFormat *p = pix_fmts; *p != -1; p++) { if (*p == *hw_pix_fmt) return *p; } - printf("Please run replay with the --no-cuda flag!\n"); + rWarning("Please run replay with the --no-cuda flag!"); // fallback to YUV420p *hw_pix_fmt = AV_PIX_FMT_NONE; return AV_PIX_FMT_YUV420P; @@ -37,7 +38,9 @@ enum AVPixelFormat get_hw_format(AVCodecContext *ctx, const enum AVPixelFormat * } // namespace -FrameReader::FrameReader() {} +FrameReader::FrameReader() { + av_log_set_level(AV_LOG_QUIET); +} FrameReader::~FrameReader() { for (AVPacket *pkt : packets) { @@ -81,13 +84,13 @@ bool FrameReader::load(const std::byte *data, size_t size, bool no_cuda, std::at if (ret != 0) { char err_str[1024] = {0}; av_strerror(ret, err_str, std::size(err_str)); - printf("Error loading video - %s\n", err_str); + rError("Error loading video - %s", err_str); return false; } ret = avformat_find_stream_info(input_ctx, nullptr); if (ret < 0) { - printf("cannot find a video stream in the input file\n"); + rError("cannot find a video stream in the input file"); return false; } @@ -105,7 +108,7 @@ bool FrameReader::load(const std::byte *data, size_t size, bool no_cuda, std::at if (has_cuda_device && !no_cuda) { if (!initHardwareDecoder(AV_HWDEVICE_TYPE_CUDA)) { - printf("No CUDA capable device was found. fallback to CPU decoding.\n"); + rWarning("No CUDA capable device was found. fallback to CPU decoding."); } else { nv12toyuv_buffer.resize(getYUVSize()); } @@ -135,8 +138,8 @@ bool FrameReader::initHardwareDecoder(AVHWDeviceType hw_device_type) { for (int i = 0;; i++) { const AVCodecHWConfig *config = avcodec_get_hw_config(decoder_ctx->codec, i); if (!config) { - printf("decoder %s does not support hw device type %s.\n", - decoder_ctx->codec->name, av_hwdevice_get_type_name(hw_device_type)); + rWarning("decoder %s does not support hw device type %s.", decoder_ctx->codec->name, + av_hwdevice_get_type_name(hw_device_type)); return false; } if (config->methods & AV_CODEC_HW_CONFIG_METHOD_HW_DEVICE_CTX && config->device_type == hw_device_type) { @@ -149,7 +152,7 @@ bool FrameReader::initHardwareDecoder(AVHWDeviceType hw_device_type) { if (ret < 0) { hw_pix_fmt = AV_PIX_FMT_NONE; has_cuda_device = false; - printf("Failed to create specified HW device %d.\n", ret); + rWarning("Failed to create specified HW device %d.", ret); return false; } @@ -192,20 +195,21 @@ bool FrameReader::decode(int idx, uint8_t *rgb, uint8_t *yuv) { AVFrame *FrameReader::decodeFrame(AVPacket *pkt) { int ret = avcodec_send_packet(decoder_ctx, pkt); if (ret < 0) { - printf("Error sending a packet for decoding\n"); + rError("Error sending a packet for decoding: %d", ret); return nullptr; } av_frame_.reset(av_frame_alloc()); ret = avcodec_receive_frame(decoder_ctx, av_frame_.get()); if (ret != 0) { + rError("avcodec_receive_frame error: %d", ret); return nullptr; } if (av_frame_->format == hw_pix_fmt) { hw_frame.reset(av_frame_alloc()); if ((ret = av_hwframe_transfer_data(hw_frame.get(), av_frame_.get(), 0)) < 0) { - printf("error transferring the data from GPU to CPU\n"); + rError("error transferring the data from GPU to CPU"); return nullptr; } return hw_frame.get(); diff --git a/selfdrive/ui/replay/logreader.cc b/selfdrive/ui/replay/logreader.cc index d836ef11f8..579fe50644 100644 --- a/selfdrive/ui/replay/logreader.cc +++ b/selfdrive/ui/replay/logreader.cc @@ -1,7 +1,6 @@ #include "selfdrive/ui/replay/logreader.h" #include -#include #include "selfdrive/ui/replay/util.h" Event::Event(const kj::ArrayPtr &amsg, bool frame) : reader(amsg), frame(frame) { @@ -59,7 +58,7 @@ bool LogReader::load(const std::byte *data, size_t size, std::atomic *abor raw_ = decompressBZ2(data, size, abort); if (raw_.empty()) { if (!(abort && *abort)) { - std::cout << "failed to decompress log" << std::endl; + rWarning("failed to decompress log"); } return false; } @@ -92,9 +91,9 @@ bool LogReader::load(const std::byte *data, size_t size, std::atomic *abor events.push_back(evt); } } catch (const kj::Exception &e) { - std::cout << "failed to parse log : " << e.getDescription().cStr() << std::endl; + rWarning("failed to parse log : %s", e.getDescription().cStr()); if (!events.empty()) { - std::cout << "read " << events.size() << " events from corrupt log" << std::endl; + rWarning("read %zu events from corrupt log", events.size()); } } diff --git a/selfdrive/ui/replay/logreader.h b/selfdrive/ui/replay/logreader.h index 33d7ea82f2..7ada20605e 100644 --- a/selfdrive/ui/replay/logreader.h +++ b/selfdrive/ui/replay/logreader.h @@ -34,7 +34,7 @@ public: return mbr->allocate(size); } void operator delete(void *ptr) { - // No-op. memory used by EventMemoryPool increases monotonically until the logReader is destroyed. + // No-op. memory used by EventMemoryPool increases monotonically until the logReader is destroyed. } #endif diff --git a/selfdrive/ui/replay/main.cc b/selfdrive/ui/replay/main.cc index 180aecc7e1..a7d2f54042 100644 --- a/selfdrive/ui/replay/main.cc +++ b/selfdrive/ui/replay/main.cc @@ -1,109 +1,13 @@ -#include - #include #include -#include -#include -#include -#include +#include "selfdrive/ui/replay/consoleui.h" #include "selfdrive/ui/replay/replay.h" const QString DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"; -struct termios oldt = {}; -Replay *replay = nullptr; - -void sigHandler(int s) { - std::signal(s, SIG_DFL); - if (oldt.c_lflag) { - tcsetattr(STDIN_FILENO, TCSANOW, &oldt); - } - replay->stop(); - qApp->quit(); -} - -int getch() { - int ch; - struct termios newt; - - tcgetattr(STDIN_FILENO, &oldt); - newt = oldt; - newt.c_lflag &= ~(ICANON | ECHO); - - tcsetattr(STDIN_FILENO, TCSANOW, &newt); - ch = getchar(); - tcsetattr(STDIN_FILENO, TCSANOW, &oldt); - - return ch; -} - -void keyboardThread(Replay *replay_) { - char c; - while (true) { - c = getch(); - if (c == '\n') { - printf("Enter seek request: "); - std::string r; - std::cin >> r; - - try { - if (r[0] == '#') { - r.erase(0, 1); - replay_->seekTo(std::stoi(r) * 60, false); - } else { - replay_->seekTo(std::stoi(r), false); - } - } catch (std::invalid_argument) { - qDebug() << "invalid argument"; - } - getch(); // remove \n from entering seek - } else if (c == 'e') { - replay_->seekToFlag(FindFlag::nextEngagement); - } else if (c == 'd') { - replay_->seekToFlag(FindFlag::nextDisEngagement); - } else if (c == 'm') { - replay_->seekTo(+60, true); - } else if (c == 'M') { - replay_->seekTo(-60, true); - } else if (c == 's') { - replay_->seekTo(+10, true); - } else if (c == 'S') { - replay_->seekTo(-10, true); - } else if (c == 'G') { - replay_->seekTo(0, true); - } else if (c == 'x') { - if (replay_->hasFlag(REPLAY_FLAG_FULL_SPEED)) { - replay_->removeFlag(REPLAY_FLAG_FULL_SPEED); - qInfo() << "replay at normal speed"; - } else { - replay_->addFlag(REPLAY_FLAG_FULL_SPEED); - qInfo() << "replay at full speed"; - } - } else if (c == ' ') { - replay_->pause(!replay_->isPaused()); - } - } -} - -void replayMessageOutput(QtMsgType type, const QMessageLogContext &context, const QString &msg) { - QByteArray localMsg = msg.toLocal8Bit(); - if (type == QtDebugMsg) { - std::cout << "\033[38;5;248m" << localMsg.constData() << "\033[00m" << std::endl; - } else if (type == QtWarningMsg) { - std::cout << "\033[38;5;227m" << localMsg.constData() << "\033[00m" << std::endl; - } else if (type == QtCriticalMsg) { - std::cout << "\033[38;5;196m" << localMsg.constData() << "\033[00m" << std::endl; - } else { - std::cout << localMsg.constData() << std::endl; - } -} int main(int argc, char *argv[]) { - qInstallMessageHandler(replayMessageOutput); - QApplication app(argc, argv); - std::signal(SIGINT, sigHandler); - std::signal(SIGTERM, sigHandler); const std::tuple flags[] = { {"dcam", REPLAY_FLAG_DCAM, "load driver camera"}, @@ -145,16 +49,12 @@ int main(int argc, char *argv[]) { replay_flags |= flag; } } - replay = new Replay(route, allow, block, nullptr, replay_flags, parser.value("data_dir"), &app); + Replay *replay = new Replay(route, allow, block, nullptr, replay_flags, parser.value("data_dir"), &app); if (!replay->load()) { return 0; } - replay->start(parser.value("start").toInt()); - // start keyboard control thread - QThread *t = new QThread(); - QObject::connect(t, &QThread::started, [=]() { keyboardThread(replay); }); - QObject::connect(t, &QThread::finished, t, &QThread::deleteLater); - t->start(); + ConsoleUI console_ui(replay); + replay->start(parser.value("start").toInt()); return app.exec(); } diff --git a/selfdrive/ui/replay/replay.cc b/selfdrive/ui/replay/replay.cc index 98e211fb53..fd1a4b1990 100644 --- a/selfdrive/ui/replay/replay.cc +++ b/selfdrive/ui/replay/replay.cc @@ -1,7 +1,7 @@ #include "selfdrive/ui/replay/replay.h" -#include #include +#include #include #include "cereal/services.h" @@ -23,6 +23,7 @@ Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *s } } qDebug() << "services " << s; + qDebug() << "loading route " << route; if (sm == nullptr) { pm = std::make_unique(s); @@ -30,22 +31,16 @@ Replay::Replay(QString route, QStringList allow, QStringList block, SubMaster *s route_ = std::make_unique(route, data_dir); events_ = std::make_unique>(); new_events_ = std::make_unique>(); - - qRegisterMetaType("FindFlag"); - connect(this, &Replay::seekTo, this, &Replay::doSeek); - connect(this, &Replay::seekToFlag, this, &Replay::doSeekToFlag); - connect(this, &Replay::stop, this, &Replay::doStop); - connect(this, &Replay::segmentChanged, this, &Replay::queueSegment); } Replay::~Replay() { - doStop(); + stop(); } -void Replay::doStop() { +void Replay::stop() { if (!stream_thread_ && segments_.empty()) return; - qDebug() << "shutdown: in progress..."; + rInfo("shutdown: in progress..."); if (stream_thread_ != nullptr) { exit_ = updating_events_ = true; stream_cv_.notify_one(); @@ -55,12 +50,14 @@ void Replay::doStop() { } segments_.clear(); camera_server_.reset(nullptr); - qDebug() << "shutdown: done"; + timeline_future.waitForFinished(); + rInfo("shutdown: done"); } bool Replay::load() { if (!route_->load()) { - qCritical() << "failed to load route" << route_->name() << "from server"; + qCritical() << "failed to load route" << route_->name() + << "from" << (route_->dir().isEmpty() ? "server" : route_->dir()); return false; } @@ -75,7 +72,7 @@ bool Replay::load() { qCritical() << "no valid segments in route" << route_->name(); return false; } - qInfo() << "load route" << route_->name() << "with" << segments_.size() << "valid segments"; + rInfo("load route %s with %zu valid segments", qPrintable(route_->name()), segments_.size()); return true; } @@ -94,21 +91,17 @@ void Replay::updateEvents(const std::function &lambda) { stream_cv_.notify_one(); } -void Replay::doSeek(int seconds, bool relative) { - if (segments_.empty()) return; - +void Replay::seekTo(int seconds, bool relative) { + seconds = relative ? seconds + currentSeconds() : seconds; updateEvents([&]() { - if (relative) { - seconds += currentSeconds(); - } seconds = std::max(0, seconds); int seg = seconds / 60; if (segments_.find(seg) == segments_.end()) { - qWarning() << "can't seek to" << seconds << "s, segment" << seg << "is invalid"; + rWarning("can't seek to %d s segment %d is invalid", seconds, seg); return true; } - qInfo() << "seeking to" << seconds << "s, segment" << seg; + rInfo("seeking to %d s, segment %d", seconds, seg); current_segment_ = seg; cur_mono_time_ = route_start_ts_ + seconds * 1e9; return isSegmentMerged(seg); @@ -116,61 +109,68 @@ void Replay::doSeek(int seconds, bool relative) { queueSegment(); } -void Replay::doSeekToFlag(FindFlag flag) { - if (flag == FindFlag::nextEngagement) { - qInfo() << "seeking to the next engagement..."; - } else if (flag == FindFlag::nextDisEngagement) { - qInfo() << "seeking to the disengagement..."; +void Replay::seekToFlag(FindFlag flag) { + if (auto next = find(flag)) { + seekTo(*next - 2, false); // seek to 2 seconds before next } - - updateEvents([&]() { - if (auto next = find(flag)) { - uint64_t tm = *next - 2 * 1e9; // seek to 2 seconds before next - if (tm <= cur_mono_time_) { - return true; - } - - cur_mono_time_ = tm; - current_segment_ = currentSeconds() / 60; - return isSegmentMerged(current_segment_); - } else { - qWarning() << "seeking failed"; - return true; - } - }); - - queueSegment(); } -std::optional Replay::find(FindFlag flag) { - // Search in all segments - for (const auto &[n, _] : segments_) { - if (n < current_segment_) continue; +void Replay::buildTimeline() { + uint64_t engaged_begin = 0; + uint64_t alert_begin = 0; + TimelineType alert_type = TimelineType::None; + for (int i = 0; i < segments_.size() && !exit_; ++i) { LogReader log; - bool cache_to_local = true; // cache qlog to local for fast seek - if (!log.load(route_->at(n).qlog.toStdString(), nullptr, cache_to_local, 0, 3)) continue; + if (!log.load(route_->at(i).qlog.toStdString(), &exit_, !hasFlag(REPLAY_FLAG_NO_FILE_CACHE), 0, 3)) continue; for (const Event *e : log.events) { - if (e->mono_time > cur_mono_time_ && e->which == cereal::Event::Which::CONTROLS_STATE) { - const auto cs = e->event.getControlsState(); - if (flag == FindFlag::nextEngagement && cs.getEnabled()) { - return e->mono_time; - } else if (flag == FindFlag::nextDisEngagement && !cs.getEnabled()) { - return e->mono_time; + if (e->which == cereal::Event::Which::CONTROLS_STATE) { + auto cs = e->event.getControlsState(); + + if (!engaged_begin && cs.getEnabled()) { + engaged_begin = e->mono_time; + } else if (engaged_begin && !cs.getEnabled()) { + std::lock_guard lk(timeline_lock); + timeline.push_back({toSeconds(engaged_begin), toSeconds(e->mono_time), TimelineType::Engaged}); + engaged_begin = 0; + } + + if (!alert_begin && cs.getAlertType().size() > 0) { + alert_begin = e->mono_time; + alert_type = TimelineType::AlertInfo; + if (cs.getAlertStatus() != cereal::ControlsState::AlertStatus::NORMAL) { + alert_type = cs.getAlertStatus() == cereal::ControlsState::AlertStatus::USER_PROMPT + ? TimelineType::AlertWarning + : TimelineType::AlertCritical; + } + } else if (alert_begin && cs.getAlertType().size() == 0) { + std::lock_guard lk(timeline_lock); + timeline.push_back({toSeconds(alert_begin), toSeconds(e->mono_time), alert_type}); + alert_begin = 0; } } } } +} + +std::optional Replay::find(FindFlag flag) { + int cur_ts = currentSeconds(); + for (auto [start_ts, end_ts, type] : getTimeline()) { + if (type == TimelineType::Engaged) { + if (flag == FindFlag::nextEngagement && start_ts > cur_ts) { + return start_ts; + } else if (flag == FindFlag::nextDisEngagement && end_ts > cur_ts) { + return end_ts; + } + } + } return std::nullopt; } void Replay::pause(bool pause) { updateEvents([=]() { - qInfo() << (pause ? "paused..." : "resuming"); - if (pause) { - qInfo() << "at " << currentSeconds() << "s"; - } + rWarning("%s at %d s", pause ? "paused..." : "resuming", currentSeconds()); paused_ = pause; return true; }); @@ -178,14 +178,14 @@ void Replay::pause(bool pause) { void Replay::setCurrentSegment(int n) { if (current_segment_.exchange(n) != n) { - emit segmentChanged(); + QMetaObject::invokeMethod(this, &Replay::queueSegment, Qt::QueuedConnection); } } void Replay::segmentLoadFinished(bool success) { if (!success) { Segment *seg = qobject_cast(sender()); - qWarning() << "failed to load segment " << seg->seg_num << ", removing it from current replay list"; + rWarning("failed to load segment %d, removing it from current replay list", seg->seg_num); segments_.erase(seg->seg_num); } queueSegment(); @@ -201,19 +201,18 @@ void Replay::queueSegment() { } // load one segment at a time for (auto it = cur; it != end; ++it) { - if (!it->second) { - if (it == cur || std::prev(it)->second->isLoaded()) { - auto &[n, seg] = *it; + auto &[n, seg] = *it; + if ((seg && !seg->isLoaded()) || !seg) { + if (!seg) { + rDebug("loading segment %d...", n); seg = std::make_unique(n, route_->at(n), flags_); QObject::connect(seg.get(), &Segment::loadFinished, this, &Replay::segmentLoadFinished); - qDebug() << "loading segment" << n << "..."; } break; } } - const auto &cur_segment = cur->second; - enableHttpLogging(!cur_segment->isLoaded()); + const auto &cur_segment = cur->second; // merge the previous adjacent segment if it's loaded auto begin = segments_.find(cur_segment->seg_num - 1); if (begin == segments_.end() || !(begin->second && begin->second->isLoaded())) { @@ -228,6 +227,7 @@ void Replay::queueSegment() { // start stream thread if (stream_thread_ == nullptr && cur_segment->isLoaded()) { startStream(cur_segment.get()); + emit streamStarted(); } } @@ -235,13 +235,18 @@ void Replay::mergeSegments(const SegmentMap::iterator &begin, const SegmentMap:: // merge 3 segments in sequence. std::vector segments_need_merge; size_t new_events_size = 0; - for (auto it = begin; it != end && it->second->isLoaded() && segments_need_merge.size() < 3; ++it) { + for (auto it = begin; it != end && it->second && it->second->isLoaded() && segments_need_merge.size() < 3; ++it) { segments_need_merge.push_back(it->first); new_events_size += it->second->log->events.size(); } if (segments_need_merge != segments_merged_) { - qDebug() << "merge segments" << segments_need_merge; + std::string s; + for (int i = 0; i < segments_need_merge.size(); ++i) { + s += std::to_string(segments_need_merge[i]); + if (i != segments_need_merge.size() - 1) s += ", "; + } + rDebug("merge segments %s", s.c_str()); new_events_->clear(); new_events_->reserve(new_events_size); for (int n : segments_need_merge) { @@ -269,10 +274,11 @@ void Replay::startStream(const Segment *cur_segment) { // write CarParams it = std::find_if(events.begin(), events.end(), [](auto e) { return e->which == cereal::Event::Which::CAR_PARAMS; }); if (it != events.end()) { + car_fingerprint_ = (*it)->event.getCarParams().getCarFingerprint(); auto bytes = (*it)->bytes(); Params().put("CarParams", (const char *)bytes.begin(), bytes.size()); } else { - qWarning() << "failed to read CarParams from current segment"; + rWarning("failed to read CarParams from current segment"); } // start camera server @@ -291,6 +297,8 @@ void Replay::startStream(const Segment *cur_segment) { QObject::connect(stream_thread_, &QThread::started, [=]() { stream(); }); QObject::connect(stream_thread_, &QThread::finished, stream_thread_, &QThread::deleteLater); stream_thread_->start(); + + timeline_future = QtConcurrent::run(this, &Replay::buildTimeline); } void Replay::publishMessage(const Event *e) { @@ -298,7 +306,7 @@ void Replay::publishMessage(const Event *e) { auto bytes = e->bytes(); int ret = pm->send(sockets_[e->which], (capnp::byte *)bytes.begin(), bytes.size()); if (ret == -1) { - qDebug() << "stop publishing" << sockets_[e->which] << "due to multiple publishers error"; + rWarning("stop publishing %s due to multiple publishers error", sockets_[e->which]); sockets_[e->which] = nullptr; } } else { @@ -324,9 +332,7 @@ void Replay::publishFrame(const Event *e) { } void Replay::stream() { - float last_print = 0; cereal::Event::Which cur_which = cereal::Event::Which::INIT_DATA; - std::unique_lock lk(stream_lock_); while (true) { @@ -337,7 +343,7 @@ void Replay::stream() { Event cur_event(cur_which, cur_mono_time_); auto eit = std::upper_bound(events_->begin(), events_->end(), &cur_event, Event::lessThan()); if (eit == events_->end()) { - qDebug() << "waiting for events..."; + rInfo("waiting for events..."); continue; } @@ -348,12 +354,7 @@ void Replay::stream() { const Event *evt = (*eit); cur_which = evt->which; cur_mono_time_ = evt->mono_time; - const int current_ts = currentSeconds(); - if (last_print > current_ts || (current_ts - last_print) > 5.0) { - last_print = current_ts; - qInfo() << "at " << current_ts << "s"; - } - setCurrentSegment(current_ts / 60); + setCurrentSegment(toSeconds(cur_mono_time_) / 60); // migration for pandaState -> pandaStates to keep UI working for old segments if (cur_which == cereal::Event::Which::PANDA_STATE_D_E_P_R_E_C_A_T_E_D) { @@ -396,8 +397,8 @@ void Replay::stream() { if (eit == events_->end() && !hasFlag(REPLAY_FLAG_NO_LOOP)) { int last_segment = segments_.rbegin()->first; if (current_segment_ >= last_segment && isSegmentMerged(last_segment)) { - qInfo() << "reaches the end of route, restart from beginning"; - emit seekTo(0, false); + rInfo("reaches the end of route, restart from beginning"); + QMetaObject::invokeMethod(this, std::bind(&Replay::seekTo, this, 0, false), Qt::QueuedConnection); } } } diff --git a/selfdrive/ui/replay/replay.h b/selfdrive/ui/replay/replay.h index 4f97990506..d20d5bb920 100644 --- a/selfdrive/ui/replay/replay.h +++ b/selfdrive/ui/replay/replay.h @@ -28,6 +28,8 @@ enum class FindFlag { nextDisEngagement }; +enum class TimelineType { None, Engaged, AlertInfo, AlertWarning, AlertCritical }; + class Replay : public QObject { Q_OBJECT @@ -37,23 +39,28 @@ public: ~Replay(); bool load(); void start(int seconds = 0); + void stop(); void pause(bool pause); - bool isPaused() const { return paused_; } + void seekToFlag(FindFlag flag); + void seekTo(int seconds, bool relative); + inline bool isPaused() const { return paused_; } inline bool hasFlag(REPLAY_FLAGS flag) const { return flags_ & flag; } inline void addFlag(REPLAY_FLAGS flag) { flags_ |= flag; } inline void removeFlag(REPLAY_FLAGS flag) { flags_ &= ~flag; } + inline const Route* route() const { return route_.get(); } + inline int currentSeconds() const { return (cur_mono_time_ - route_start_ts_) / 1e9; } + inline int toSeconds(uint64_t mono_time) const { return (mono_time - route_start_ts_) / 1e9; } + inline int totalSeconds() const { return segments_.size() * 60; } + inline const std::string &carFingerprint() const { return car_fingerprint_; } + inline const std::vector> getTimeline() { + std::lock_guard lk(timeline_lock); + return timeline; + } signals: - void segmentChanged(); - void seekTo(int seconds, bool relative); - void seekToFlag(FindFlag flag); - void stop(); + void streamStarted(); protected slots: - void queueSegment(); - void doStop(); - void doSeek(int seconds, bool relative); - void doSeekToFlag(FindFlag flag); void segmentLoadFinished(bool sucess); protected: @@ -62,11 +69,12 @@ protected: void startStream(const Segment *cur_segment); void stream(); void setCurrentSegment(int n); + void queueSegment(); void mergeSegments(const SegmentMap::iterator &begin, const SegmentMap::iterator &end); void updateEvents(const std::function& lambda); void publishMessage(const Event *e); void publishFrame(const Event *e); - inline int currentSeconds() const { return (cur_mono_time_ - route_start_ts_) / 1e9; } + void buildTimeline(); inline bool isSegmentMerged(int n) { return std::find(segments_merged_.begin(), segments_merged_.end(), n) != segments_merged_.end(); } @@ -80,7 +88,7 @@ protected: std::atomic current_segment_ = 0; SegmentMap segments_; // the following variables must be protected with stream_lock_ - bool exit_ = false; + std::atomic exit_ = false; bool paused_ = false; bool events_updated_ = false; uint64_t route_start_ts_ = 0; @@ -96,4 +104,9 @@ protected: std::unique_ptr route_; std::unique_ptr camera_server_; std::atomic flags_ = REPLAY_FLAG_NONE; + + std::mutex timeline_lock; + QFuture timeline_future; + std::vector> timeline; + std::string car_fingerprint_; }; diff --git a/selfdrive/ui/replay/route.cc b/selfdrive/ui/replay/route.cc index fe6e21a91a..e6a6177149 100644 --- a/selfdrive/ui/replay/route.cc +++ b/selfdrive/ui/replay/route.cc @@ -26,7 +26,7 @@ RouteIdentifier Route::parseRoute(const QString &str) { bool Route::load() { if (route_.str.isEmpty()) { - qInfo() << "invalid route format"; + rInfo("invalid route format"); return false; } return data_dir_.isEmpty() ? loadFromServer() : loadFromLocal(); diff --git a/selfdrive/ui/replay/route.h b/selfdrive/ui/replay/route.h index c39eef7d92..ac8fba75ca 100644 --- a/selfdrive/ui/replay/route.h +++ b/selfdrive/ui/replay/route.h @@ -27,6 +27,7 @@ public: Route(const QString &route, const QString &data_dir = {}); bool load(); inline const QString &name() const { return route_.str; } + inline const QString &dir() const { return data_dir_; } inline const RouteIdentifier &identifier() const { return route_; } inline const std::map &segments() const { return segments_; } inline const SegmentFile &at(int n) { return segments_.at(n); } diff --git a/selfdrive/ui/replay/tests/test_replay.cc b/selfdrive/ui/replay/tests/test_replay.cc index 6063dfe7d5..3cafa9b534 100644 --- a/selfdrive/ui/replay/tests/test_replay.cc +++ b/selfdrive/ui/replay/tests/test_replay.cc @@ -1,3 +1,6 @@ +#include +#include + #include #include @@ -10,6 +13,16 @@ const QString DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"; const std::string TEST_RLOG_URL = "https://commadataci.blob.core.windows.net/openpilotci/0c94aa1e1296d7c6/2021-05-05--19-48-37/0/rlog.bz2"; const std::string TEST_RLOG_CHECKSUM = "5b966d4bb21a100a8c4e59195faeb741b975ccbe268211765efd1763d892bfb3"; +bool donload_to_file(const std::string &url, const std::string &local_file, int chunk_size = 5 * 1024 * 1024, int retries = 3) { + do { + if (httpDownload(url, local_file, chunk_size)) { + return true; + } + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } while (--retries >= 0); + return false; +} + TEST_CASE("httpMultiPartDownload") { char filename[] = "/tmp/XXXXXX"; close(mkstemp(filename)); @@ -17,16 +30,13 @@ TEST_CASE("httpMultiPartDownload") { const size_t chunk_size = 5 * 1024 * 1024; std::string content; SECTION("download to file") { - bool ret = false; - for (int i = 0; i < 3 && !ret; ++i) { - ret = httpDownload(TEST_RLOG_URL, filename, chunk_size); - } - REQUIRE(ret); + REQUIRE(donload_to_file(TEST_RLOG_URL, filename, chunk_size)); content = util::read_file(filename); } SECTION("download to buffer") { for (int i = 0; i < 3 && content.empty(); ++i) { content = httpGet(TEST_RLOG_URL, chunk_size); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); } REQUIRE(!content.empty()); } @@ -67,14 +77,9 @@ TEST_CASE("LogReader") { } } -TEST_CASE("Segment") { - auto flags = GENERATE(REPLAY_FLAG_DCAM | REPLAY_FLAG_ECAM, REPLAY_FLAG_QCAMERA); - Route demo_route(DEMO_ROUTE); - REQUIRE(demo_route.load()); - REQUIRE(demo_route.segments().size() == 11); - +void read_segment(int n, const SegmentFile &segment_file, uint32_t flags) { QEventLoop loop; - Segment segment(0, demo_route.at(0), flags); + Segment segment(n, segment_file, flags); QObject::connect(&segment, &Segment::loadFinished, [&]() { REQUIRE(segment.isLoaded() == true); REQUIRE(segment.log != nullptr); @@ -99,8 +104,8 @@ TEST_CASE("Segment") { } std::unique_ptr rgb_buf = std::make_unique(fr->getRGBSize()); std::unique_ptr yuv_buf = std::make_unique(fr->getYUVSize()); - // sequence get 50 frames - for (int i = 0; i < 50; ++i) { + // sequence get 100 frames + for (int i = 0; i < 100; ++i) { REQUIRE(fr->get(i, rgb_buf.get(), yuv_buf.get())); } } @@ -110,6 +115,43 @@ TEST_CASE("Segment") { loop.exec(); } +TEST_CASE("Route") { + // Create a local route from remote for testing + Route remote_route(DEMO_ROUTE); + REQUIRE(remote_route.load()); + char tmp_path[] = "/tmp/root_XXXXXX"; + const std::string data_dir = mkdtemp(tmp_path); + const std::string route_name = DEMO_ROUTE.mid(17).toStdString(); + for (int i = 0; i < 2; ++i) { + std::string log_path = util::string_format("%s/%s--%d/", data_dir.c_str(), route_name.c_str(), i); + util::create_directories(log_path, 0755); + REQUIRE(donload_to_file(remote_route.at(i).rlog.toStdString(), log_path + "rlog.bz2")); + REQUIRE(donload_to_file(remote_route.at(i).road_cam.toStdString(), log_path + "fcamera.hevc")); + REQUIRE(donload_to_file(remote_route.at(i).driver_cam.toStdString(), log_path + "dcamera.hevc")); + REQUIRE(donload_to_file(remote_route.at(i).wide_road_cam.toStdString(), log_path + "ecamera.hevc")); + REQUIRE(donload_to_file(remote_route.at(i).qcamera.toStdString(), log_path + "qcamera.ts")); + } + + SECTION("Local route") { + auto flags = GENERATE(REPLAY_FLAG_DCAM | REPLAY_FLAG_ECAM, REPLAY_FLAG_QCAMERA); + Route route(DEMO_ROUTE, QString::fromStdString(data_dir)); + REQUIRE(route.load()); + REQUIRE(route.segments().size() == 2); + for (int i = 0; i < route.segments().size(); ++i) { + read_segment(i, route.at(i), flags); + } + }; + SECTION("Remote route") { + auto flags = GENERATE(REPLAY_FLAG_DCAM | REPLAY_FLAG_ECAM, REPLAY_FLAG_QCAMERA); + Route route(DEMO_ROUTE); + REQUIRE(route.load()); + REQUIRE(route.segments().size() == 11); + for (int i = 0; i < 2; ++i) { + read_segment(i, route.at(i), flags); + } + }; +} + // helper class for unit tests class TestReplay : public Replay { public: diff --git a/selfdrive/ui/replay/util.cc b/selfdrive/ui/replay/util.cc index d6eba2e2a8..5f06a6c834 100644 --- a/selfdrive/ui/replay/util.cc +++ b/selfdrive/ui/replay/util.cc @@ -15,6 +15,40 @@ #include "selfdrive/common/timing.h" #include "selfdrive/common/util.h" +ReplayMessageHandler message_handler = nullptr; +DownloadProgressHandler download_progress_handler = nullptr; + +void installMessageHandler(ReplayMessageHandler handler) { message_handler = handler; } +void installDownloadProgressHandler(DownloadProgressHandler handler) { download_progress_handler = handler; } + +void logMessage(ReplyMsgType type, const char *fmt, ...) { + static std::mutex lock; + std::lock_guard lk(lock); + + char *msg_buf = nullptr; + va_list args; + va_start(args, fmt); + int ret = vasprintf(&msg_buf, fmt, args); + va_end(args); + if (ret <= 0 || !msg_buf) return; + + if (message_handler) { + message_handler(type, msg_buf); + } else { + if (type == ReplyMsgType::Debug) { + std::cout << "\033[38;5;248m" << msg_buf << "\033[00m" << std::endl; + } else if (type == ReplyMsgType::Warning) { + std::cout << "\033[38;5;227m" << msg_buf << "\033[00m" << std::endl; + } else if (type == ReplyMsgType::Critical) { + std::cout << "\033[38;5;196m" << msg_buf << "\033[00m" << std::endl; + } else { + std::cout << msg_buf << std::endl; + } + } + + free(msg_buf); +} + namespace { struct CURLGlobalInitializer { @@ -23,7 +57,6 @@ struct CURLGlobalInitializer { }; static CURLGlobalInitializer curl_initializer; -static std::atomic enable_http_logging = false; template struct MultiPartWriter { @@ -57,6 +90,38 @@ size_t write_cb(char *data, size_t size, size_t count, void *userp) { size_t dumy_write_cb(char *data, size_t size, size_t count, void *userp) { return size * count; } +struct DownloadStats { + void add(const std::string &url, uint64_t total_bytes) { + std::lock_guard lk(lock); + items[url] = {0, total_bytes}; + } + + void remove(const std::string &url) { + std::lock_guard lk(lock); + items.erase(url); + } + + void update(const std::string &url, uint64_t downloaded, bool success = true) { + std::lock_guard lk(lock); + items[url].first = downloaded; + + auto stat = std::accumulate(items.begin(), items.end(), std::pair{}, [=](auto &a, auto &b){ + return std::pair{a.first + b.second.first, a.second + b.second.second}; + }); + double tm = millis_since_boot(); + if (download_progress_handler && ((tm - prev_tm) > 500 || !success || stat.first >= stat.second)) { + download_progress_handler(stat.first, stat.second, success); + prev_tm = tm; + } + } + + std::mutex lock; + std::map> items; + double prev_tm = 0; +}; + +} // namespace + std::string formattedDataSize(size_t size) { if (size < 1024) { return std::to_string(size) + " B"; @@ -67,8 +132,6 @@ std::string formattedDataSize(size_t size) { } } -} // namespace - size_t getRemoteFileSize(const std::string &url, std::atomic *abort) { CURL *curl = curl_easy_init(); if (!curl) return -1; @@ -99,12 +162,11 @@ std::string getUrlWithoutQuery(const std::string &url) { return (idx == std::string::npos ? url : url.substr(0, idx)); } -void enableHttpLogging(bool enable) { - enable_http_logging = enable; -} - template bool httpDownload(const std::string &url, T &buf, size_t chunk_size, size_t content_length, std::atomic *abort) { + static DownloadStats download_stats; + download_stats.add(url, content_length); + int parts = 1; if (chunk_size > 0 && content_length > 10 * 1024 * 1024) { parts = std::nearbyint(content_length / (float)chunk_size); @@ -134,23 +196,11 @@ bool httpDownload(const std::string &url, T &buf, size_t chunk_size, size_t cont curl_multi_add_handle(cm, eh); } - size_t prev_written = 0; - double last_print = millis_since_boot(); int still_running = 1; while (still_running > 0 && !(abort && *abort)) { curl_multi_wait(cm, nullptr, 0, 1000, nullptr); curl_multi_perform(cm, &still_running); - - if (enable_http_logging) { - if (double ts = millis_since_boot(); (ts - last_print) > 2 * 1000) { - size_t average = (written - prev_written) / ((ts - last_print) / 1000.); - int progress = std::min(100, 100.0 * (double)written / (double)content_length); - std::cout << "downloading " << getUrlWithoutQuery(url) << " - " << progress - << "% (" << formattedDataSize(average) << "/s)" << std::endl; - last_print = ts; - prev_written = written; - } - } + download_stats.update(url, written); } CURLMsg *msg; @@ -164,21 +214,25 @@ bool httpDownload(const std::string &url, T &buf, size_t chunk_size, size_t cont if (res_status == 206) { complete++; } else { - std::cout << "Download failed: http error code: " << res_status << std::endl; + rWarning("Download failed: http error code: %d", res_status); } } else { - std::cout << "Download failed: connection failure: " << msg->data.result << std::endl; + rWarning("Download failed: connection failure: %d", msg->data.result); } } } + bool success = complete == parts; + download_stats.update(url, written, success); + download_stats.remove(url); + for (const auto &[e, w] : writers) { curl_multi_remove_handle(cm, e); curl_easy_cleanup(e); } curl_multi_cleanup(cm); - return complete == parts; + return success; } std::string httpGet(const std::string &url, size_t chunk_size, std::atomic *abort) { @@ -221,7 +275,7 @@ std::string decompressBZ2(const std::byte *in, size_t in_size, std::atomic if (bzerror == BZ_OK && prev_write_pos == strm.next_out) { // content is corrupt bzerror = BZ_STREAM_END; - std::cout << "decompressBZ2 error : content is corrupt" << std::endl; + rWarning("decompressBZ2 error : content is corrupt"); break; } diff --git a/selfdrive/ui/replay/util.h b/selfdrive/ui/replay/util.h index cd4b179cdc..6c808095e8 100644 --- a/selfdrive/ui/replay/util.h +++ b/selfdrive/ui/replay/util.h @@ -1,14 +1,34 @@ #pragma once #include +#include #include +enum class ReplyMsgType { + Info, + Debug, + Warning, + Critical +}; + +typedef std::function ReplayMessageHandler; +void installMessageHandler(ReplayMessageHandler); +void logMessage(ReplyMsgType type, const char* fmt, ...); + +#define rInfo(fmt, ...) ::logMessage(ReplyMsgType::Info, fmt, ## __VA_ARGS__) +#define rDebug(fmt, ...) ::logMessage(ReplyMsgType::Debug, fmt, ## __VA_ARGS__) +#define rWarning(fmt, ...) ::logMessage(ReplyMsgType::Warning, fmt, ## __VA_ARGS__) +#define rError(fmt, ...) ::logMessage(ReplyMsgType::Critical , fmt, ## __VA_ARGS__) + std::string sha256(const std::string &str); void precise_nano_sleep(long sleep_ns); std::string decompressBZ2(const std::string &in, std::atomic *abort = nullptr); std::string decompressBZ2(const std::byte *in, size_t in_size, std::atomic *abort = nullptr); -void enableHttpLogging(bool enable); std::string getUrlWithoutQuery(const std::string &url); size_t getRemoteFileSize(const std::string &url, std::atomic *abort = nullptr); std::string httpGet(const std::string &url, size_t chunk_size = 0, std::atomic *abort = nullptr); + +typedef std::function DownloadProgressHandler; +void installDownloadProgressHandler(DownloadProgressHandler); bool httpDownload(const std::string &url, const std::string &file, size_t chunk_size = 0, std::atomic *abort = nullptr); +std::string formattedDataSize(size_t size); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index c33b61c19e..007dc36d07 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -109,12 +109,6 @@ static void update_state(UIState *s) { SubMaster &sm = *(s->sm); UIScene &scene = s->scene; - if (sm.updated("modelV2")) { - update_model(s, sm["modelV2"].getModelV2()); - } - if (sm.updated("radarState") && sm.rcv_frame("modelV2") >= s->scene.started_frame) { - update_leads(s, sm["radarState"].getRadarState(), sm["modelV2"].getModelV2().getPosition()); - } if (sm.updated("liveCalibration")) { auto rpy_list = sm["liveCalibration"].getLiveCalibration().getRpyCalib(); Eigen::Vector3d rpy; @@ -131,6 +125,14 @@ static void update_state(UIState *s) { } } } + if (s->worldObjectsVisible()) { + if (sm.updated("modelV2")) { + update_model(s, sm["modelV2"].getModelV2()); + } + if (sm.updated("radarState") && sm.rcv_frame("modelV2") > s->scene.started_frame) { + update_leads(s, sm["radarState"].getRadarState(), sm["modelV2"].getModelV2().getPosition()); + } + } if (sm.updated("pandaStates")) { auto pandaStates = sm["pandaStates"].getPandaStates(); if (pandaStates.size() > 0) { @@ -164,16 +166,22 @@ static void update_state(UIState *s) { } } } - if (sm.updated("roadCameraState")) { + if (!Hardware::TICI() && sm.updated("roadCameraState")) { auto camera_state = sm["roadCameraState"].getRoadCameraState(); float max_lines = Hardware::EON() ? 5408 : 1904; float max_gain = Hardware::EON() ? 1.0: 10.0; float max_ev = max_lines * max_gain; - if (Hardware::TICI()) { - max_ev /= 6; - } + float ev = camera_state.getGain() * float(camera_state.getIntegLines()); + + scene.light_sensor = std::clamp(1.0 - (ev / max_ev), 0.0, 1.0); + } else if (Hardware::TICI() && sm.updated("wideRoadCameraState")) { + auto camera_state = sm["wideRoadCameraState"].getWideRoadCameraState(); + + float max_lines = 1904; + float max_gain = 10.0; + float max_ev = max_lines * max_gain / 6; float ev = camera_state.getGain() * float(camera_state.getIntegLines()); @@ -218,11 +226,12 @@ UIState::UIState(QObject *parent) : QObject(parent) { sm = std::make_unique>({ "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "roadCameraState", "pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman", + "wideRoadCameraState", }); Params params; wide_camera = Hardware::TICI() ? params.getBool("EnableWideCamera") : false; - has_prime = params.getBool("HasPrime"); + prime_type = std::atoi(params.get("PrimeType").c_str()); // update timer timer = new QTimer(this); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index bc15257a08..cd7ea446ec 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -115,7 +115,7 @@ class UIState : public QObject { public: UIState(QObject* parent = 0); void updateStatus(); - inline bool worldObjectsVisible() const { + inline bool worldObjectsVisible() const { return sm->rcv_frame("liveCalibration") > scene.started_frame; }; @@ -127,7 +127,7 @@ public: UIScene scene = {}; bool awake; - bool has_prime = false; + int prime_type = 0; QTransform car_space_transform; bool wide_camera; diff --git a/selfdrive/version.py b/selfdrive/version.py index d94a098e8b..f2570cd305 100644 --- a/selfdrive/version.py +++ b/selfdrive/version.py @@ -111,10 +111,6 @@ def is_dirty() -> bool: pass dirty = (subprocess.call(["git", "diff-index", "--quiet", branch, "--"]) != 0) - - dirty = dirty or (not is_comma_remote()) - dirty = dirty or ('master' in branch) - except subprocess.CalledProcessError: cloudlog.exception("git subprocess failed while checking dirty") dirty = True diff --git a/tools/plotjuggler/juggle.py b/tools/plotjuggler/juggle.py index 7cafaf60ac..af69bd7c8b 100755 --- a/tools/plotjuggler/juggle.py +++ b/tools/plotjuggler/juggle.py @@ -23,6 +23,7 @@ juggle_dir = os.path.dirname(os.path.realpath(__file__)) DEMO_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36" RELEASES_URL="https://github.com/commaai/PlotJuggler/releases/download/latest" INSTALL_DIR = os.path.join(juggle_dir, "bin") +PLOTJUGGLER_BIN = os.path.join(juggle_dir, "bin/plotjuggler") def install(): @@ -70,7 +71,7 @@ def start_juggler(fn=None, dbc=None, layout=None): if layout is not None: extra_args += f" -l {layout}" - cmd = f'plotjuggler --plugin_folders {INSTALL_DIR}{extra_args}' + cmd = f'{PLOTJUGGLER_BIN} --plugin_folders {INSTALL_DIR}{extra_args}' subprocess.call(cmd, shell=True, env=env, cwd=juggle_dir) @@ -92,7 +93,7 @@ def juggle_route(route_or_segment_name, segment_count, qlog, can, layout): r = Route(route_or_segment_name.route_name.canonical_name) logs = r.qlog_paths() if qlog else r.log_paths() - segment_end = segment_start + segment_count if segment_count else -1 + segment_end = segment_start + segment_count if segment_count else None logs = logs[segment_start:segment_end] if None in logs: @@ -149,6 +150,10 @@ if __name__ == "__main__": install() sys.exit() + if not os.path.exists(PLOTJUGGLER_BIN): + print("PlotJuggler is missing. Downloading...") + install() + if args.stream: start_juggler(layout=args.layout) else: diff --git a/tools/plotjuggler/layouts/controls_mismatch_debug.xml b/tools/plotjuggler/layouts/controls_mismatch_debug.xml new file mode 100644 index 0000000000..e50b5ffb34 --- /dev/null +++ b/tools/plotjuggler/layouts/controls_mismatch_debug.xml @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tools/sim/lib/can.py b/tools/sim/lib/can.py index 4873947d4b..af0f415bbc 100755 --- a/tools/sim/lib/can.py +++ b/tools/sim/lib/can.py @@ -12,11 +12,11 @@ rpacker = CANPacker("acura_ilx_2016_nidec") def get_car_can_parser(): dbc_f = 'honda_civic_touring_2016_can_generated' signals = [ - ("STEER_TORQUE", 0xe4, 0), - ("STEER_TORQUE_REQUEST", 0xe4, 0), - ("COMPUTER_BRAKE", 0x1fa, 0), - ("COMPUTER_BRAKE_REQUEST", 0x1fa, 0), - ("GAS_COMMAND", 0x200, 0), + ("STEER_TORQUE", 0xe4), + ("STEER_TORQUE_REQUEST", 0xe4), + ("COMPUTER_BRAKE", 0x1fa), + ("COMPUTER_BRAKE_REQUEST", 0x1fa), + ("GAS_COMMAND", 0x200), ] checks = [ (0xe4, 100),