diff --git a/.github/workflows/selfdrive_tests.yaml b/.github/workflows/selfdrive_tests.yaml index fca9d2f9a9..04ae841f77 100644 --- a/.github/workflows/selfdrive_tests.yaml +++ b/.github/workflows/selfdrive_tests.yaml @@ -362,6 +362,44 @@ jobs: CI=1 AZURE_TOKEN='$AZURE_TOKEN' python selfdrive/test/process_replay/test_processes.py -j$(nproc) --upload-only" - name: "Upload coverage to Codecov" uses: codecov/codecov-action@v2 + + model_replay_onnx: + name: model replay onnx + runs-on: ubuntu-20.04 + timeout-minutes: 50 + steps: + - uses: actions/checkout@v3 + with: + submodules: true + - name: Pull LFS + run: git lfs pull + - name: Cache dependencies + id: dependency-cache + uses: actions/cache@v2 + with: + path: /tmp/comma_download_cache + key: ${{ hashFiles('.github/workflows/selfdrive_tests.yaml', 'selfdrive/test/process_replay/model_replay.py') }} + - name: Cache scons + id: scons-cache + # TODO: Change the version to the released version when https://github.com/actions/cache/pull/489 (or 571) is merged. + uses: actions/cache@03e00da99d75a2204924908e1cca7902cafce66b + env: + CACHE_SKIP_SAVE: true + with: + path: /tmp/scons_cache + key: scons-${{ hashFiles('.github/workflows/selfdrive_tests.yaml') }}- + restore-keys: | + scons-${{ hashFiles('.github/workflows/selfdrive_tests.yaml') }}- + scons- + - name: Build Docker image + # Sim docker is needed to get the intel OPENCL drivers + run: eval "$BUILD_CL" + - name: Run replay + run: | + ${{ env.RUN_CL }} "scons -j$(nproc) && \ + ONNXCPU=1 FILEREADER_CACHE=1 CI=1 coverage run \ + selfdrive/test/process_replay/model_replay.py -j$(nproc) && \ + coverage xml" test_longitudinal: name: longitudinal diff --git a/Dockerfile.openpilot_base_cl b/Dockerfile.openpilot_base_cl index d172d55899..7652b7e4e6 100644 --- a/Dockerfile.openpilot_base_cl +++ b/Dockerfile.openpilot_base_cl @@ -1,7 +1,18 @@ FROM ghcr.io/commaai/openpilot-base:latest RUN apt-get update && apt-get install -y --no-install-recommends\ + apt-utils \ alien \ + unzip \ + tar \ + curl \ + xz-utils \ + dbus \ + gcc-arm-none-eabi \ + tmux \ + vim \ + lsb-core \ + libx11-6 \ && rm -rf /var/lib/apt/lists/* # Intel OpenCL driver @@ -18,3 +29,9 @@ RUN echo INTEL_DRIVER is $INTEL_DRIVER && \ mkdir -p /etc/OpenCL/vendors && \ echo /opt/intel/opencl_compilers_and_libraries_18.1.0.015/linux/compiler/lib/intel64_lin/libintelocl.so > /etc/OpenCL/vendors/intel.icd && \ rm -rf /tmp/opencl-driver-intel +ENV NVIDIA_VISIBLE_DEVICES all +ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute +ENV QTWEBENGINE_DISABLE_SANDBOX 1 + +RUN dbus-uuidgen > /etc/machine-id + diff --git a/cereal b/cereal index 6ad4ba689c..7cbb4f1c8c 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 6ad4ba689c557ed241862d10aef4235d3e96331f +Subproject commit 7cbb4f1c8cb7cfc798a058c611801442b40feb52 diff --git a/common/params.cc b/common/params.cc index 2338969600..330eddc1a9 100644 --- a/common/params.cc +++ b/common/params.cc @@ -104,7 +104,6 @@ std::unordered_map keys = { {"DoReboot", CLEAR_ON_MANAGER_START}, {"DoShutdown", CLEAR_ON_MANAGER_START}, {"DoUninstall", CLEAR_ON_MANAGER_START}, - {"EndToEndToggle", PERSISTENT}, {"ForcePowerDown", CLEAR_ON_MANAGER_START}, {"GitBranch", PERSISTENT}, {"GitCommit", PERSISTENT}, diff --git a/docs/CARS.md b/docs/CARS.md index 87be32f1d0..7f259012f0 100644 --- a/docs/CARS.md +++ b/docs/CARS.md @@ -51,13 +51,7 @@ How We Rate The Cars |Lexus|ES 2019-21|All|||||| |Lexus|ES Hybrid 2019-22|All|||||| |Lexus|NX 2020|All|||||| -|Lexus|RX 2020-22|All|||||| -|Lexus|RX Hybrid 2020-21|All|||||| |Lexus|UX Hybrid 2019-21|All|||||| -|Toyota|Alphard 2019-20|All|||||| -|Toyota|Alphard Hybrid 2021|All|||||| -|Toyota|Avalon 2022|All|||||| -|Toyota|Avalon Hybrid 2022|All|||||| |Toyota|Camry 2021-22|All||[4](#footnotes)|||| |Toyota|Camry Hybrid 2021-22|All|||||| |Toyota|Corolla 2020-22|All|||||| @@ -75,12 +69,7 @@ How We Rate The Cars |Make|Model|Supported Package|openpilot ACC|Stop and Go|Steer to 0|Steering Torque|Actively Maintained| |---|---|---|:---:|:---:|:---:|:---:|:---:| -|Audi|A3 2014-19|ACC + Lane Assist|||||| -|Audi|A3 Sportback e-tron 2017-18|ACC + Lane Assist|||||| |Audi|Q2 2018|ACC + Lane Assist|||||| -|Audi|Q3 2020-21|ACC + Lane Assist|||||| -|Audi|RS3 2018|ACC + Lane Assist|||||| -|Audi|S3 2015-17|ACC + Lane Assist|||||| |Genesis|G70 2018|All|||||| |Genesis|G80 2018|All|||||| |Hyundai|Elantra 2021-22|SCC + LKAS|||||| @@ -94,7 +83,6 @@ How We Rate The Cars |Hyundai|Santa Fe 2021-22|All|||||| |Hyundai|Santa Fe Hybrid 2022|All|||||| |Hyundai|Santa Fe Plug-in Hybrid 2022|All|||||| -|Hyundai|Sonata 2018-19|SCC + LKAS|||||| |Hyundai|Tucson Diesel 2019|SCC + LKAS|||||| |Kia|Ceed 2019|SCC + LKAS|||||| |Kia|Forte 2018|SCC + LKAS|||||| @@ -107,18 +95,15 @@ How We Rate The Cars |Kia|Sorento 2018|SCC + LKAS|||||| |Kia|Sorento 2019|SCC + LKAS|||||| |Kia|Stinger 2018|SCC + LKAS|||||| -|Lexus|CT Hybrid 2017-18|LSS|[3](#footnotes)||||| |Lexus|ES Hybrid 2017-18|LSS|[3](#footnotes)||||| |Lexus|NX 2018-19|All|[3](#footnotes)||||| |Lexus|NX Hybrid 2018-19|All|[3](#footnotes)||||| -|Lexus|RX 2016-18|All|[3](#footnotes)||||| -|Lexus|RX Hybrid 2016-19|All|[3](#footnotes)||||| +|Lexus|RX 2020-22|All|||||| +|Lexus|RX Hybrid 2020-21|All|||||| |Mazda|CX-5 2022|All|||||| |SEAT|Ateca 2018|Driver Assistance|||||| |SEAT|Leon 2014-20|Driver Assistance|||||| -|Subaru|Crosstrek 2018-19|EyeSight|||||| |Subaru|Forester 2019-21|EyeSight|||||| -|Subaru|Impreza 2017-19|EyeSight|||||| |Škoda|Kamiq 2021[6](#footnotes)|Driver Assistance|||||| |Škoda|Karoq 2019|Driver Assistance|||||| |Škoda|Kodiaq 2018-19|Driver Assistance|||||| @@ -126,36 +111,20 @@ How We Rate The Cars |Škoda|Octavia RS 2016|Driver Assistance|||||| |Škoda|Scala 2020|Driver Assistance|||||| |Škoda|Superb 2015-18|Driver Assistance|||||| -|Toyota|Avalon 2019-21|TSS-P|[3](#footnotes)||||| -|Toyota|Avalon Hybrid 2019-21|TSS-P|[3](#footnotes)||||| -|Toyota|C-HR 2017-21|All|||||| -|Toyota|C-HR Hybrid 2017-19|All|||||| +|Toyota|Alphard 2019-20|All|||||| +|Toyota|Alphard Hybrid 2021|All|||||| +|Toyota|Avalon 2022|All|||||| +|Toyota|Avalon Hybrid 2022|All|||||| |Toyota|Camry 2018-20|All||[4](#footnotes)|||| |Toyota|Camry Hybrid 2018-20|All||[4](#footnotes)|||| -|Toyota|Highlander 2017-19|All|[3](#footnotes)||||| -|Toyota|Highlander Hybrid 2017-19|All|[3](#footnotes)||||| |Toyota|RAV4 2022|All|||||| -|Toyota|RAV4 Hybrid 2016-18|TSS-P|[3](#footnotes)||||| |Toyota|RAV4 Hybrid 2022|All|||||| -|Toyota|Sienna 2018-20|All|[3](#footnotes)||||| |Volkswagen|Arteon 2018, 2021[8](#footnotes)|Driver Assistance|||||| -|Volkswagen|Atlas 2018-19, 2022[8](#footnotes)|Driver Assistance|||||| -|Volkswagen|e-Golf 2014, 2018-20|Driver Assistance|||||| -|Volkswagen|Golf 2015-20|Driver Assistance|||||| -|Volkswagen|Golf Alltrack 2017-18|Driver Assistance|||||| -|Volkswagen|Golf GTE 2016|Driver Assistance|||||| -|Volkswagen|Golf GTI 2018-21|Driver Assistance|||||| -|Volkswagen|Golf R 2016-19|Driver Assistance|||||| -|Volkswagen|Golf SportsVan 2016|Driver Assistance|||||| -|Volkswagen|Golf SportWagen 2015|Driver Assistance|||||| -|Volkswagen|Jetta 2018-21|Driver Assistance|||||| -|Volkswagen|Jetta GLI 2021|Driver Assistance|||||| |Volkswagen|Passat 2015-18[7](#footnotes)|Driver Assistance|||||| |Volkswagen|Polo 2020|Driver Assistance|||||| |Volkswagen|T-Cross 2021[8](#footnotes)|Driver Assistance|||||| |Volkswagen|T-Roc 2021[8](#footnotes)|Driver Assistance|||||| |Volkswagen|Taos 2022[8](#footnotes)|Driver Assistance|||||| -|Volkswagen|Tiguan 2019-22[8](#footnotes)|Driver Assistance|||||| |Volkswagen|Touran 2017|Driver Assistance|||||| ## Bronze Cars @@ -165,6 +134,11 @@ How We Rate The Cars |Acura|ILX 2016-19|AcuraWatch Plus|||||| |Acura|RDX 2016-18|AcuraWatch Plus|||||| |Acura|RDX 2019-21|All|||||| +|Audi|A3 2014-19|ACC + Lane Assist|||||| +|Audi|A3 Sportback e-tron 2017-18|ACC + Lane Assist|||||| +|Audi|Q3 2020-21|ACC + Lane Assist|||||| +|Audi|RS3 2018|ACC + Lane Assist|||||| +|Audi|S3 2015-17|ACC + Lane Assist|||||| |Cadillac|Escalade ESV 2016[1](#footnotes)|ACC + LKAS|||||| |Chevrolet|Volt 2017-18[1](#footnotes)|Adaptive Cruise|||||| |Chrysler|Pacifica 2017-18|Adaptive Cruise|||||| @@ -196,29 +170,55 @@ How We Rate The Cars |Hyundai|Ioniq Electric 2019|SCC + LKAS|||||| |Hyundai|Ioniq Hybrid 2017-19|SCC + LKAS|||||| |Hyundai|Ioniq Plug-in Hybrid 2019|SCC + LKAS|||||| +|Hyundai|Sonata 2018-19|SCC + LKAS|||||| |Hyundai|Veloster 2019-20|SCC + LKAS|||||| |Jeep|Grand Cherokee 2016-18|Adaptive Cruise|||||| |Jeep|Grand Cherokee 2019-20|Adaptive Cruise|||||| |Kia|Niro Plug-in Hybrid 2019|SCC + LKAS|||||| |Kia|Optima 2017|SCC + LKAS|||||| -|Lexus|IS 2017-19|All|||||| -|Lexus|RC 2020|All|||||| +|Lexus|CT Hybrid 2017-18|LSS|[3](#footnotes)||||| +|Lexus|IS 2017-19|All|||||| +|Lexus|RC 2020|All|||||| +|Lexus|RX 2016-18|All|[3](#footnotes)||||| +|Lexus|RX Hybrid 2016-19|All|[3](#footnotes)||||| |Mazda|CX-9 2021|All|||||| |Nissan|Altima 2019-20|ProPILOT|||||| |Nissan|Leaf 2018-22|ProPILOT|||||| |Nissan|Rogue 2018-20|ProPILOT|||||| |Nissan|X-Trail 2017|ProPILOT|||||| |Subaru|Ascent 2019-20|EyeSight|||||| +|Subaru|Crosstrek 2018-19|EyeSight|||||| |Subaru|Crosstrek 2020-21|EyeSight|||||| +|Subaru|Impreza 2017-19|EyeSight|||||| |Subaru|Impreza 2020-21|EyeSight|||||| -|Toyota|Avalon 2016-18|TSS-P|[3](#footnotes)||||| +|Toyota|Avalon 2016-18|TSS-P|[3](#footnotes)||||| +|Toyota|Avalon 2019-21|TSS-P|[3](#footnotes)||||| +|Toyota|Avalon Hybrid 2019-21|TSS-P|[3](#footnotes)||||| +|Toyota|C-HR 2017-21|All|||||| +|Toyota|C-HR Hybrid 2017-19|All|||||| |Toyota|Corolla 2017-19|All|[3](#footnotes)||||| +|Toyota|Highlander 2017-19|All|[3](#footnotes)||||| +|Toyota|Highlander Hybrid 2017-19|All|[3](#footnotes)||||| |Toyota|Prius 2016-20|TSS-P|[3](#footnotes)|||[5](#footnotes)|| |Toyota|Prius Prime 2017-20|All|[3](#footnotes)|||[5](#footnotes)|| |Toyota|Prius v 2017|TSS-P|[3](#footnotes)|||[5](#footnotes)|| -|Toyota|RAV4 2016-18|TSS-P|[3](#footnotes)||||| +|Toyota|RAV4 2016-18|TSS-P|[3](#footnotes)||||| +|Toyota|RAV4 Hybrid 2016-18|TSS-P|[3](#footnotes)||||| +|Toyota|Sienna 2018-20|All|[3](#footnotes)||||| +|Volkswagen|Atlas 2018-19, 2022[8](#footnotes)|Driver Assistance|||||| |Volkswagen|California 2021[8](#footnotes)|Driver Assistance|||||| |Volkswagen|Caravelle 2020[8](#footnotes)|Driver Assistance|||||| +|Volkswagen|e-Golf 2014, 2018-20|Driver Assistance|||||| +|Volkswagen|Golf 2015-20|Driver Assistance|||||| +|Volkswagen|Golf Alltrack 2017-18|Driver Assistance|||||| +|Volkswagen|Golf GTE 2016|Driver Assistance|||||| +|Volkswagen|Golf GTI 2018-21|Driver Assistance|||||| +|Volkswagen|Golf R 2016-19|Driver Assistance|||||| +|Volkswagen|Golf SportsVan 2016|Driver Assistance|||||| +|Volkswagen|Golf SportWagen 2015|Driver Assistance|||||| +|Volkswagen|Jetta 2018-21|Driver Assistance|||||| +|Volkswagen|Jetta GLI 2021|Driver Assistance|||||| +|Volkswagen|Tiguan 2019-22[8](#footnotes)|Driver Assistance|||||| diff --git a/launch_env.sh b/launch_env.sh index 0c318bf658..769613bc79 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -7,7 +7,7 @@ export OPENBLAS_NUM_THREADS=1 export VECLIB_MAXIMUM_THREADS=1 if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="5" + export AGNOS_VERSION="5.1" fi if [ -z "$PASSIVE" ]; then diff --git a/panda b/panda index 6d19b46ef5..475a9a3124 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 6d19b46ef525b0ae224e0bf7db71918e96f9be66 +Subproject commit 475a9a312410908abcaa32f2e48140fcbfc2362f diff --git a/selfdrive/car/body/interface.py b/selfdrive/car/body/interface.py index 2b44200397..3d6bd5e22a 100644 --- a/selfdrive/car/body/interface.py +++ b/selfdrive/car/body/interface.py @@ -17,6 +17,7 @@ class CarInterface(CarInterfaceBase): ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.body)] ret.minSteerSpeed = -math.inf + ret.maxLateralAccel = math.inf # TODO: set to a reasonable value ret.steerRatio = 0.5 ret.steerRateCost = 0.5 ret.steerLimitTimer = 1.0 diff --git a/selfdrive/car/body/values.py b/selfdrive/car/body/values.py index a009b40bc9..0caf93b619 100644 --- a/selfdrive/car/body/values.py +++ b/selfdrive/car/body/values.py @@ -18,7 +18,7 @@ class CAR: BODY = "COMMA BODY" CAR_INFO: Dict[str, CarInfo] = { - CAR.BODY: CarInfo("comma body", package="All", good_torque=True, harness=Harness.none), + CAR.BODY: CarInfo("comma body", package="All", harness=Harness.none), } FW_VERSIONS = { diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index fc441ac2e7..7863f0d882 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -1,9 +1,11 @@ import os -from typing import Any, Dict, List +from typing import Dict, List +from cereal import car from common.params import Params from common.basedir import BASEDIR from selfdrive.version import is_comma_remote, is_tested_branch +from selfdrive.car.interfaces import get_interface_attr from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars from selfdrive.car.vin import get_vin, VIN_UNKNOWN from selfdrive.car.fw_versions import get_fw_versions, match_fw_to_car @@ -11,7 +13,6 @@ from selfdrive.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.car import gen_empty_fingerprint -from cereal import car EventName = car.CarEvent.EventName @@ -59,19 +60,6 @@ def load_interfaces(brand_names): return ret -def get_interface_attr(attr: str) -> Dict[str, Any]: - # returns given attribute from each interface - brand_names = {} - for car_folder in sorted([x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]): - try: - brand_name = car_folder.split('/')[-1] - attr_data = getattr(__import__(f'selfdrive.car.{brand_name}.values', fromlist=[attr]), attr, None) - brand_names[brand_name] = attr_data - except (ImportError, OSError): - pass - return brand_names - - def _get_interface_names() -> Dict[str, List[str]]: # returns a dict of brand name and its respective models brand_names = {} diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 55672dd4ab..e47b085533 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -23,6 +23,14 @@ class CarInterface(CarInterfaceBase): ret.steerRateCost = 0.7 ret.steerLimitTimer = 0.4 + # set max lateral acceleration + if candidate in (CAR.PACIFICA_2018, CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019): + ret.maxLateralAccel = 1.6 + if candidate in (CAR.PACIFICA_2018_HYBRID,): + ret.maxLateralAccel = 1.4 + if candidate in (CAR.PACIFICA_2017_HYBRID,): + ret.maxLateralAccel = 1.2 + if candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019): ret.wheelbase = 2.91 # in meters ret.steerRatio = 12.7 diff --git a/selfdrive/car/docs.py b/selfdrive/car/docs.py index fad110689c..899f2c0878 100755 --- a/selfdrive/car/docs.py +++ b/selfdrive/car/docs.py @@ -15,9 +15,8 @@ from selfdrive.car.tests.routes import non_tested_cars def get_all_footnotes() -> Dict[Enum, int]: all_footnotes = [] - for _, footnotes in get_interface_attr("Footnote").items(): - if footnotes is not None: - all_footnotes += footnotes + for footnotes in get_interface_attr("Footnote", ignore_none=True).values(): + all_footnotes += footnotes return {fn: idx + 1 for idx, fn in enumerate(all_footnotes)} @@ -28,21 +27,20 @@ CARS_MD_TEMPLATE = os.path.join(BASEDIR, "selfdrive", "car", "CARS_template.md") def get_all_car_info() -> List[CarInfo]: all_car_info: List[CarInfo] = [] - for models in get_interface_attr("CAR_INFO").values(): - for model, car_info in models.items(): - # Hyundai exception: those with radar have openpilot longitudinal - fingerprint = {0: {}, 1: {HKG_RADAR_START_ADDR: 8}, 2: {}, 3: {}} - CP = interfaces[model][0].get_params(model, fingerprint=fingerprint, disable_radar=True) + for model, car_info in get_interface_attr("CAR_INFO", combine_brands=True).items(): + # Hyundai exception: those with radar have openpilot longitudinal + fingerprint = {0: {}, 1: {HKG_RADAR_START_ADDR: 8}, 2: {}, 3: {}} + CP = interfaces[model][0].get_params(model, fingerprint=fingerprint, disable_radar=True) - if CP.dashcamOnly or car_info is None: - continue + if CP.dashcamOnly or car_info is None: + continue - # A platform can include multiple car models - if not isinstance(car_info, list): - car_info = (car_info,) + # A platform can include multiple car models + if not isinstance(car_info, list): + car_info = (car_info,) - for _car_info in car_info: - all_car_info.append(_car_info.init(CP, non_tested_cars, ALL_FOOTNOTES)) + for _car_info in car_info: + all_car_info.append(_car_info.init(CP, non_tested_cars, ALL_FOOTNOTES)) # Sort cars by make and model + year sorted_cars: List[CarInfo] = natsorted(all_car_info, key=lambda car: (car.make + car.model).lower()) diff --git a/selfdrive/car/docs_definitions.py b/selfdrive/car/docs_definitions.py index 3046f5c9e3..381bcfa7f8 100644 --- a/selfdrive/car/docs_definitions.py +++ b/selfdrive/car/docs_definitions.py @@ -1,9 +1,13 @@ +import math + from cereal import car from collections import namedtuple from dataclasses import dataclass from enum import Enum from typing import Dict, List, Optional, Union, no_type_check +STEERING_TORQUE_THRESHOLD = 2.0 # m/s^2 + class Tier(Enum): GOLD = "The best openpilot experience. Great highway driving and beyond." @@ -66,6 +70,11 @@ class CarInfo: if self.min_enable_speed is not None: min_enable_speed = self.min_enable_speed + # TODO: remove hardcoded good torque and just use maxLateralAccel + good_torque = self.good_torque + if not math.isnan(CP.maxLateralAccel): + good_torque = CP.maxLateralAccel >= STEERING_TORQUE_THRESHOLD + self.car_name = CP.carName self.make, self.model = self.name.split(' ', 1) self.row = { @@ -76,7 +85,7 @@ class CarInfo: Column.LONGITUDINAL: CP.openpilotLongitudinalControl and not CP.radarOffCan, Column.FSR_LONGITUDINAL: min_enable_speed <= 0., Column.FSR_STEERING: min_steer_speed <= 0., - Column.STEERING_TORQUE: self.good_torque, + Column.STEERING_TORQUE: good_torque, Column.MAINTAINED: CP.carFingerprint not in non_tested_cars and self.harness is not Harness.none, } diff --git a/selfdrive/car/fingerprints.py b/selfdrive/car/fingerprints.py index 9b280f3b45..1a9bb8c4e7 100644 --- a/selfdrive/car/fingerprints.py +++ b/selfdrive/car/fingerprints.py @@ -1,44 +1,12 @@ -import os -from common.basedir import BASEDIR +from selfdrive.car.interfaces import get_interface_attr -def get_attr_from_cars(attr, result=dict, combine_brands=True): - # read all the folders in selfdrive/car and return a dict where: - # - keys are all the car models - # - values are attr values from all car folders - result = result() - - for car_folder in [x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]: - try: - car_name = car_folder.split('/')[-1] - values = __import__(f'selfdrive.car.{car_name}.values', fromlist=[attr]) - if hasattr(values, attr): - attr_values = getattr(values, attr) - else: - continue - - if isinstance(attr_values, dict): - for f, v in attr_values.items(): - if combine_brands: - result[f] = v - else: - if car_name not in result: - result[car_name] = {} - result[car_name][f] = v - elif isinstance(attr_values, list): - result += attr_values - - except (ImportError, OSError): - pass - - return result - - -FW_VERSIONS = get_attr_from_cars('FW_VERSIONS') -_FINGERPRINTS = get_attr_from_cars('FINGERPRINTS') +FW_VERSIONS = get_interface_attr('FW_VERSIONS', combine_brands=True, ignore_none=True) +_FINGERPRINTS = get_interface_attr('FINGERPRINTS', combine_brands=True, ignore_none=True) _DEBUG_ADDRESS = {1880: 8} # reserved for debug purposes + def is_valid_for_fingerprint(msg, car_fingerprint): adr = msg.address # ignore addresses that are more than 11 bits diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 120174c451..69b7c5f452 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -1,15 +1,15 @@ #!/usr/bin/env python3 import struct import traceback -from typing import Any, List from collections import defaultdict -from dataclasses import dataclass - +from dataclasses import dataclass, field +from typing import Any, List from tqdm import tqdm import panda.python.uds as uds from cereal import car -from selfdrive.car.fingerprints import FW_VERSIONS, get_attr_from_cars +from selfdrive.car.interfaces import get_interface_attr +from selfdrive.car.fingerprints import FW_VERSIONS from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery from selfdrive.car.toyota.values import CAR as TOYOTA from selfdrive.swaglog import cloudlog @@ -96,9 +96,11 @@ class Request: brand: str request: List[bytes] response: List[bytes] + whitelist_ecus: List[int] = field(default_factory=list) rx_offset: int = DEFAULT_RX_OFFSET bus: int = 1 + REQUESTS: List[Request] = [ # Subaru Request( @@ -303,7 +305,7 @@ def get_fw_versions(logcan, sendcan, extra=None, timeout=0.1, debug=False, progr addrs = [] parallel_addrs = [] - versions = get_attr_from_cars('FW_VERSIONS', combine_brands=False) + versions = get_interface_attr('FW_VERSIONS', ignore_none=True) if extra is not None: versions.update(extra) @@ -328,23 +330,26 @@ def get_fw_versions(logcan, sendcan, extra=None, timeout=0.1, debug=False, progr for addr_chunk in chunks(addr): for r in REQUESTS: try: - addrs = [(a, s) for (b, a, s) in addr_chunk if b in (r.brand, 'any')] + addrs = [(a, s) for (b, a, s) in addr_chunk if b in (r.brand, 'any') and + (len(r.whitelist_ecus) == 0 or ecu_types[(a, s)] in r.whitelist_ecus)] if addrs: query = IsoTpParallelQuery(sendcan, logcan, r.bus, addrs, r.request, r.response, r.rx_offset, debug=debug) t = 2 * timeout if i == 0 else timeout - fw_versions.update(query.get_data(t)) + fw_versions.update({addr: (version, r.request, r.rx_offset) for addr, version in query.get_data(t).items()}) except Exception: cloudlog.warning(f"FW query exception: {traceback.format_exc()}") # Build capnp list to put into CarParams car_fw = [] - for addr, version in fw_versions.items(): + for addr, (version, request, rx_offset) in fw_versions.items(): f = car.CarParams.CarFw.new_message() f.ecu = ecu_types[addr] f.fwVersion = version f.address = addr[0] + f.responseAddress = addr[0] + rx_offset + f.request = request if addr[1] is not None: f.subAddress = addr[1] diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index b4265fc0bd..ce14faec5e 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -75,6 +75,7 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.45 # Volt Gen 1, TODO corner weigh + ret.maxLateralAccel = 1.6 ret.lateralTuning.pid.kpBP = [0., 40.] ret.lateralTuning.pid.kpV = [0., 0.17] @@ -108,6 +109,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.4 # end to end is 13.46 ret.steerRatioRear = 0. ret.centerToFront = ret.wheelbase * 0.4 + ret.maxLateralAccel = 1.4 ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_acadia() elif candidate == CAR.BUICK_REGAL: diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 84437b4f40..ad98f8863f 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -100,6 +100,7 @@ class CarInterface(CarInterfaceBase): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]] else: + ret.maxLateralAccel = 1.7 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]] tire_stiffness_factor = 1. @@ -112,6 +113,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.38 # 10.93 is end-to-end 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 = 1. + ret.maxLateralAccel = 1.6 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] elif candidate in (CAR.ACCORD, CAR.ACCORDH): @@ -126,6 +128,7 @@ class CarInterface(CarInterfaceBase): if eps_modified: ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]] else: + ret.maxLateralAccel = 1.6 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] elif candidate == CAR.ACURA_ILX: @@ -162,6 +165,7 @@ class CarInterface(CarInterfaceBase): ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]] else: + ret.maxLateralAccel = 1.7 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]] tire_stiffness_factor = 0.677 @@ -186,6 +190,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.06 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.75 + ret.maxLateralAccel = 1.7 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] elif candidate == CAR.FREED: @@ -216,8 +221,9 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.68 ret.centerToFront = ret.wheelbase * 0.38 ret.steerRatio = 15.0 # as spec - ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.444 + ret.maxLateralAccel = 0.9 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] elif candidate == CAR.ACURA_RDX_3G: @@ -226,6 +232,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.75 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 11.95 # as spec + ret.maxLateralAccel = 1.2 ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]] tire_stiffness_factor = 0.677 @@ -238,6 +245,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.35 # 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.82 + ret.maxLateralAccel = 1.5 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]] elif candidate == CAR.ODYSSEY_CHN: @@ -258,6 +266,7 @@ class CarInterface(CarInterfaceBase): 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.maxLateralAccel = 1.5 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] elif candidate == CAR.RIDGELINE: @@ -268,6 +277,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.59 # 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.maxLateralAccel = 1.3 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] elif candidate == CAR.INSIGHT: @@ -278,6 +288,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.0 # 12.58 is spec end-to-end 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.82 + ret.maxLateralAccel = 1.4 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] elif candidate == CAR.HONDA_E: diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 673b304e5a..e051079dc4 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -60,6 +60,7 @@ class CarInterface(CarInterfaceBase): # Values from optimizer ret.steerRatio = 16.55 # 13.8 is spec end-to-end tire_stiffness_factor = 0.82 + ret.maxLateralAccel = 3.2 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[9., 22.], [9., 22.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.35], [0.05, 0.09]] elif candidate in (CAR.SONATA, CAR.SONATA_HYBRID): @@ -68,6 +69,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.84 ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable tire_stiffness_factor = 0.65 + ret.maxLateralAccel = 2.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.SONATA_LF: @@ -75,6 +77,7 @@ class CarInterface(CarInterfaceBase): ret.mass = 4497. * CV.LB_TO_KG ret.wheelbase = 2.804 ret.steerRatio = 13.27 * 1.15 # 15% higher at the center seems reasonable + ret.maxLateralAccel = 1.8 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.PALISADE: @@ -83,6 +86,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.90 ret.steerRatio = 15.6 * 1.15 tire_stiffness_factor = 0.63 + ret.maxLateralAccel = 2.5 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]] elif candidate in (CAR.ELANTRA, CAR.ELANTRA_GT_I30): @@ -139,6 +143,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.7 ret.steerRatio = 13.73 # Spec tire_stiffness_factor = 0.385 + ret.maxLateralAccel = 3.0 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] if candidate not in (CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022): @@ -188,6 +193,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.7 ret.steerRatio = 13.9 if CAR.KIA_NIRO_HEV_2021 else 13.73 # Spec tire_stiffness_factor = 0.385 + ret.maxLateralAccel = 2.9 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] if candidate == CAR.KIA_NIRO_HEV: @@ -219,6 +225,7 @@ class CarInterface(CarInterfaceBase): ret.mass = 1825. + STD_CARGO_KG ret.wheelbase = 2.78 ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable + ret.maxLateralAccel = 2.4 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_FORTE: @@ -244,6 +251,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.85 ret.steerRatio = 13.27 # 2021 Kia K5 Steering Ratio (all trims) tire_stiffness_factor = 0.5 + ret.maxLateralAccel = 2.1 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] elif candidate == CAR.KIA_EV6: @@ -254,8 +262,8 @@ class CarInterface(CarInterfaceBase): get_safety_config(car.CarParams.SafetyModel.hyundaiHDA2)] tire_stiffness_factor = 0.65 - max_lat_accel = 2. - set_torque_tune(ret.lateralTuning, max_lat_accel, 0.01) + ret.maxLateralAccel = 2. + set_torque_tune(ret.lateralTuning, ret.maxLateralAccel, 0.01) # Genesis elif candidate == CAR.GENESIS_G70: diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 1ee237fe36..a2c11bf023 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -1,13 +1,14 @@ import os import time from abc import abstractmethod, ABC -from typing import Dict, Tuple, List +from typing import Any, Dict, Tuple, List from cereal import car +from common.basedir import BASEDIR +from common.conversions import Conversions as CV from common.kalman.simple_kalman import KF1D from common.realtime import DT_CTRL from selfdrive.car import gen_empty_fingerprint -from common.conversions import Conversions as CV from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX from selfdrive.controls.lib.events import Events from selfdrive.controls.lib.vehicle_model import VehicleModel @@ -22,7 +23,6 @@ ACCEL_MIN = -3.5 # generic car and radar interfaces - class CarInterfaceBase(ABC): def __init__(self, CP, CarController, CarState): self.CP = CP @@ -81,6 +81,7 @@ class CarInterfaceBase(ABC): ret.steerControlType = car.CarParams.SteerControlType.torque ret.minSteerSpeed = 0. ret.wheelSpeedFactor = 1.0 + ret.maxLateralAccel = float('nan') ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this @@ -293,3 +294,31 @@ class CarStateBase(ABC): @staticmethod def get_loopback_can_parser(CP): return None + + +# interface-specific helpers + +def get_interface_attr(attr: str, combine_brands: bool = False, ignore_none: bool = False) -> Dict[str, Any]: + # read all the folders in selfdrive/car and return a dict where: + # - keys are all the car models or brand names + # - values are attr values from all car folders + result = {} + for car_folder in sorted([x[0] for x in os.walk(BASEDIR + '/selfdrive/car')]): + try: + brand_name = car_folder.split('/')[-1] + brand_values = __import__(f'selfdrive.car.{brand_name}.values', fromlist=[attr]) + if hasattr(brand_values, attr) or not ignore_none: + attr_data = getattr(brand_values, attr, None) + else: + continue + + if combine_brands: + if isinstance(attr_data, dict): + for f, v in attr_data.items(): + result[f] = v + else: + result[brand_name] = attr_data + except (ImportError, OSError): + pass + + return result diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py index e8bba727c5..8230548586 100644 --- a/selfdrive/car/isotp_parallel_query.py +++ b/selfdrive/car/isotp_parallel_query.py @@ -139,7 +139,7 @@ class IsoTpParallelQuery: cloudlog.warning(f"iso-tp query response pending: {hex(tx_addr)}") else: request_done[tx_addr] = True - cloudlog.warning(f"iso-tp query bad response: 0x{dat.hex()}") + cloudlog.warning(f"iso-tp query bad response: {tx_addr} - 0x{dat.hex()}") cur_time = time.monotonic() if cur_time - max(response_timeouts.values()) > 0: diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index d0d8e91ce1..edd26a4449 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -51,6 +51,7 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.5 ret.steerRatio = 17 # learned, 14 stock ret.steerActuatorDelay = 0.1 + ret.maxLateralAccel = 1.3 ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.045, 0.042, 0.20], [0.04, 0.035, 0.045]] @@ -61,6 +62,7 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.5 ret.steerRatio = 17 # learned, 14 stock ret.steerActuatorDelay = 0.1 + ret.maxLateralAccel = 3.2 ret.lateralTuning.pid.kf = 0.000038 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 14., 23.], [0., 14., 23.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]] diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py index 6e81fcb1b3..412333edcd 100644 --- a/selfdrive/car/subaru/values.py +++ b/selfdrive/car/subaru/values.py @@ -43,14 +43,14 @@ class SubaruCarInfo(CarInfo): CAR_INFO: Dict[str, Union[SubaruCarInfo, List[SubaruCarInfo]]] = { CAR.ASCENT: SubaruCarInfo("Subaru Ascent 2019-20"), CAR.IMPREZA: [ - SubaruCarInfo("Subaru Impreza 2017-19", good_torque=True), - SubaruCarInfo("Subaru Crosstrek 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26", good_torque=True), + SubaruCarInfo("Subaru Impreza 2017-19"), + SubaruCarInfo("Subaru Crosstrek 2018-19", video_link="https://youtu.be/Agww7oE1k-s?t=26"), ], CAR.IMPREZA_2020: [ SubaruCarInfo("Subaru Impreza 2020-21"), SubaruCarInfo("Subaru Crosstrek 2020-21"), ], - CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21", good_torque=True), + CAR.FORESTER: SubaruCarInfo("Subaru Forester 2019-21"), CAR.FORESTER_PREGLOBAL: SubaruCarInfo("Subaru Forester 2017-18"), CAR.LEGACY_PREGLOBAL: SubaruCarInfo("Subaru Legacy 2015-18"), CAR.OUTBACK_PREGLOBAL: SubaruCarInfo("Subaru Outback 2015-17"), diff --git a/selfdrive/car/tests/test_docs.py b/selfdrive/car/tests/test_docs.py index fcc90b9584..fed6989d40 100755 --- a/selfdrive/car/tests/test_docs.py +++ b/selfdrive/car/tests/test_docs.py @@ -18,7 +18,7 @@ class TestCarDocs(unittest.TestCase): "Run selfdrive/car/docs.py to generate new supported cars documentation") def test_missing_car_info(self): - all_car_info_platforms = [p for i in get_interface_attr("CAR_INFO").values() for p in i] + all_car_info_platforms = get_interface_attr("CAR_INFO", combine_brands=True).keys() for platform in sorted(interfaces.keys()): if platform not in all_car_info_platforms: self.fail("Platform: {} doesn't exist in CarInfo".format(platform)) diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py index ae209b2910..ed7e420e1a 100755 --- a/selfdrive/car/tests/test_fw_fingerprint.py +++ b/selfdrive/car/tests/test_fw_fingerprint.py @@ -4,9 +4,9 @@ import unittest from parameterized import parameterized from cereal import car -from selfdrive.car.car_helpers import interfaces +from selfdrive.car.car_helpers import get_interface_attr, interfaces from selfdrive.car.fingerprints import FW_VERSIONS -from selfdrive.car.fw_versions import match_fw_to_car +from selfdrive.car.fw_versions import REQUESTS, match_fw_to_car CarFw = car.CarParams.CarFw Ecu = car.CarParams.Ecu @@ -57,6 +57,23 @@ class TestFwFingerprint(unittest.TestCase): self.assertTrue(passed, "Blacklisted FW versions found") + def test_fw_request_ecu_whitelist(self): + passed = True + brands = set(r.brand for r in REQUESTS) + versions = get_interface_attr('FW_VERSIONS') + for brand in brands: + whitelisted_ecus = [ecu for r in REQUESTS for ecu in r.whitelist_ecus if r.brand == brand] + brand_ecus = set([fw[0] for car_fw in versions[brand].values() for fw in car_fw]) + + # each ecu in brand's fw versions needs to be whitelisted at least once + ecus_not_whitelisted = set(brand_ecus) - set(whitelisted_ecus) + if len(whitelisted_ecus) and len(ecus_not_whitelisted): + ecu_strings = ", ".join([f'Ecu.{ECU_NAME[ecu]}' for ecu in ecus_not_whitelisted]) + print(f'{brand.title()}: FW query whitelist missing ecus: {ecu_strings}') + passed = False + + self.assertTrue(passed, "Not all ecus in FW versions found in query whitelists") + if __name__ == "__main__": unittest.main() diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 1b577cc6b7..a1ce8c4980 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -38,7 +38,8 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.74 # unknown end-to-end spec tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.7, FRICTION=0.06) + ret.maxLateralAccel = 1.7 + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.06) elif candidate == CAR.PRIUS_V: stop_and_go = True @@ -46,7 +47,8 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 17.4 tire_stiffness_factor = 0.5533 ret.mass = 3340. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06) + ret.maxLateralAccel = 1.8 + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.06) elif candidate in (CAR.RAV4, CAR.RAV4H): stop_and_go = True if (candidate in CAR.RAV4H) else False @@ -54,14 +56,16 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 16.88 # 14.5 is spec end-to-end tire_stiffness_factor = 0.5533 ret.mass = 3650. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=1.8, FRICTION=0.06) + ret.maxLateralAccel = 1.8 + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.06) elif candidate == CAR.COROLLA: ret.wheelbase = 2.70 ret.steerRatio = 18.27 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.8, FRICTION=0.024) + ret.maxLateralAccel = 2.8 + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.024) elif candidate in (CAR.LEXUS_RX, CAR.LEXUS_RXH, CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2): stop_and_go = True @@ -70,6 +74,7 @@ class CarInterface(CarInterfaceBase): ret.wheelSpeedFactor = 1.035 tire_stiffness_factor = 0.5533 ret.mass = 4481. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max + ret.maxLateralAccel = 1.4 set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate in (CAR.CHR, CAR.CHRH): @@ -78,6 +83,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.6 tire_stiffness_factor = 0.7933 ret.mass = 3300. * CV.LB_TO_KG + STD_CARGO_KG + ret.maxLateralAccel = 1.3 set_lat_tune(ret.lateralTuning, LatTunes.PID_F) elif candidate in (CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): @@ -87,8 +93,10 @@ class CarInterface(CarInterfaceBase): tire_stiffness_factor = 0.7933 ret.mass = 3400. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid if candidate in (CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2): - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.4, FRICTION=0.05) + ret.maxLateralAccel = 2.4 + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.05) else: + ret.maxLateralAccel = 2.0 set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate in (CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2): @@ -97,6 +105,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 16.0 tire_stiffness_factor = 0.8 ret.mass = 4700. * CV.LB_TO_KG + STD_CARGO_KG # 4260 + 4-5 people + ret.maxLateralAccel = 2.0 set_lat_tune(ret.lateralTuning, LatTunes.PID_G) elif candidate in (CAR.HIGHLANDER, CAR.HIGHLANDERH): @@ -105,6 +114,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 16.0 tire_stiffness_factor = 0.8 ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid limited + ret.maxLateralAccel = 1.8 set_lat_tune(ret.lateralTuning, LatTunes.PID_G) elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2, CAR.AVALONH_TSS2): @@ -115,6 +125,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.8 # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download tire_stiffness_factor = 0.7983 ret.mass = 3505. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid + ret.maxLateralAccel = 1.6 set_lat_tune(ret.lateralTuning, LatTunes.PID_H) elif candidate in (CAR.RAV4_TSS2, CAR.RAV4_TSS2_2022, CAR.RAV4H_TSS2, CAR.RAV4H_TSS2_2022): @@ -123,6 +134,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.3 tire_stiffness_factor = 0.7933 ret.mass = 3585. * CV.LB_TO_KG + STD_CARGO_KG # Average between ICE and Hybrid + ret.maxLateralAccel = 2.5 set_lat_tune(ret.lateralTuning, LatTunes.PID_D) # 2019+ RAV4 TSS2 uses two different steering racks and specific tuning seems to be necessary. @@ -138,7 +150,8 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.9 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=2.0, FRICTION=0.07) + ret.maxLateralAccel = 2.0 + set_lat_tune(ret.lateralTuning, LatTunes.TORQUE, MAX_LAT_ACCEL=ret.maxLateralAccel, FRICTION=0.07) elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH): stop_and_go = True @@ -146,6 +159,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 16.0 # not optimized tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3677. * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max + ret.maxLateralAccel = 2.2 set_lat_tune(ret.lateralTuning, LatTunes.PID_D) elif candidate == CAR.SIENNA: @@ -154,6 +168,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 15.5 tire_stiffness_factor = 0.444 ret.mass = 4590. * CV.LB_TO_KG + STD_CARGO_KG + ret.maxLateralAccel = 1.6 set_lat_tune(ret.lateralTuning, LatTunes.PID_J) elif candidate in (CAR.LEXUS_IS, CAR.LEXUS_RC): @@ -177,6 +192,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.7 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 4070 * CV.LB_TO_KG + STD_CARGO_KG + ret.maxLateralAccel = 2.0 set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate == CAR.PRIUS_TSS2: @@ -185,6 +201,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.4 # True steerRatio from older prius tire_stiffness_factor = 0.6371 # hand-tune ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG + ret.maxLateralAccel = 2.0 set_lat_tune(ret.lateralTuning, LatTunes.PID_N) elif candidate == CAR.MIRAI: @@ -193,6 +210,7 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 14.8 tire_stiffness_factor = 0.8 ret.mass = 4300. * CV.LB_TO_KG + STD_CARGO_KG + ret.maxLateralAccel = 2.4 set_lat_tune(ret.lateralTuning, LatTunes.PID_C) elif candidate in (CAR.ALPHARD_TSS2, CAR.ALPHARDH_TSS2): diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py index 811b3a6e1b..e97ed6b056 100644 --- a/selfdrive/car/toyota/tunes.py +++ b/selfdrive/car/toyota/tunes.py @@ -50,7 +50,7 @@ def set_long_tune(tune, name): ###### LAT ###### -def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=.1, use_steering_angle=True): +def set_lat_tune(tune, name, MAX_LAT_ACCEL=2.5, FRICTION=0.01, use_steering_angle=True): if name == LatTunes.TORQUE: set_torque_tune(tune, MAX_LAT_ACCEL, FRICTION) elif 'PID' in str(name): diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 0b6948cf44..3204d36b5a 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -100,7 +100,6 @@ class Footnote(Enum): @dataclass class ToyotaCarInfo(CarInfo): package: str = "All" - good_torque: bool = True harness: Enum = Harness.toyota diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 3a0d7c8ce8..90bbd6d889 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -64,14 +64,17 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.ATLAS_MK1: ret.mass = 2011 + STD_CARGO_KG ret.wheelbase = 2.98 + ret.maxLateralAccel = 1.4 elif candidate == CAR.GOLF_MK7: ret.mass = 1397 + STD_CARGO_KG ret.wheelbase = 2.62 + ret.maxLateralAccel = 1.5 elif candidate == CAR.JETTA_MK7: ret.mass = 1328 + STD_CARGO_KG ret.wheelbase = 2.71 + ret.maxLateralAccel = 1.2 elif candidate == CAR.PASSAT_MK8: ret.mass = 1551 + STD_CARGO_KG @@ -92,6 +95,7 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.TIGUAN_MK2: ret.mass = 1715 + STD_CARGO_KG ret.wheelbase = 2.74 + ret.maxLateralAccel = 1.1 elif candidate == CAR.TOURAN_MK2: ret.mass = 1516 + STD_CARGO_KG @@ -109,6 +113,7 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.AUDI_A3_MK3: ret.mass = 1335 + STD_CARGO_KG ret.wheelbase = 2.61 + ret.maxLateralAccel = 1.7 elif candidate == CAR.AUDI_Q2_MK1: ret.mass = 1205 + STD_CARGO_KG @@ -117,6 +122,7 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.AUDI_Q3_MK2: ret.mass = 1623 + STD_CARGO_KG ret.wheelbase = 2.68 + ret.maxLateralAccel = 1.6 elif candidate == CAR.SEAT_ATECA_MK1: ret.mass = 1900 + STD_CARGO_KG diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 820862af9e..8b41dbf402 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -22,7 +22,7 @@ LOW_SPEED_FACTOR = 200 JERK_THRESHOLD = 0.2 -def set_torque_tune(tune, MAX_LAT_ACCEL=2.5, FRICTION=.1): +def set_torque_tune(tune, MAX_LAT_ACCEL=2.5, FRICTION=0.01): tune.init('torque') tune.torque.useSteeringAngle = True tune.torque.kp = 1.0 / MAX_LAT_ACCEL diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 1fc79decef..083aeb79cc 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -16,7 +16,7 @@ def plannerd_thread(sm=None, pm=None): CP = car.CarParams.from_bytes(params.get("CarParams", block=True)) cloudlog.info("plannerd got CarParams: %s", CP.carName) - use_lanelines = not params.get_bool('EndToEndToggle') + use_lanelines = False wide_camera = params.get_bool('WideCameraOnly') cloudlog.event("e2e mode", on=use_lanelines) diff --git a/selfdrive/hardware/tici/agnos.json b/selfdrive/hardware/tici/agnos.json index 855020ffc0..004a0f9dfb 100644 --- a/selfdrive/hardware/tici/agnos.json +++ b/selfdrive/hardware/tici/agnos.json @@ -41,9 +41,9 @@ }, { "name": "system", - "url": "https://commadist.azureedge.net/agnosupdate/system-8aa796767e3af57fd24e4d54280944187b8fcc04c8748f5c20fef2700be10161.img.xz", - "hash": "b51b8247f2b926edc3a2b8242bbab52a484ae72559a3b4a55e961bf6b8d47223", - "hash_raw": "8aa796767e3af57fd24e4d54280944187b8fcc04c8748f5c20fef2700be10161", + "url": "https://commadist.azureedge.net/agnosupdate/system-11fdbc9e8a9cd27f98346d7e1039bc5b3032d0e892ff95fa1258673ff1809bca.img.xz", + "hash": "45b4719a9e580617cf840036b24fb0dcd32491edd9654d8d74c28d91ff362d36", + "hash_raw": "11fdbc9e8a9cd27f98346d7e1039bc5b3032d0e892ff95fa1258673ff1809bca", "size": 10737418240, "sparse": true, "full_check": false, diff --git a/selfdrive/hardware/tici/test_power_draw.py b/selfdrive/hardware/tici/test_power_draw.py index 934d72e3a1..ab2d691a09 100755 --- a/selfdrive/hardware/tici/test_power_draw.py +++ b/selfdrive/hardware/tici/test_power_draw.py @@ -16,11 +16,11 @@ class Proc: power: float rtol: float = 0.05 atol: float = 0.1 - warmup: float = 3. + warmup: float = 6. PROCS = [ - Proc('camerad', 2.25), - Proc('modeld', 0.95), + Proc('camerad', 2.15), + Proc('modeld', 1.0), Proc('dmonitoringmodeld', 0.25), Proc('encoderd', 0.23), ] diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index dae359b905..86f214976c 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -44,19 +44,19 @@ class Laikad: ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist() ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist() - pos_std = float(np.linalg.norm(self.gnss_kf.P[GStates.ECEF_POS])) - vel_std = float(np.linalg.norm(self.gnss_kf.P[GStates.ECEF_VELOCITY])) + pos_std = self.gnss_kf.P[GStates.ECEF_POS].flatten().tolist() + vel_std = self.gnss_kf.P[GStates.ECEF_VELOCITY].flatten().tolist() bearing_deg, bearing_std = get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std) meas_msgs = [create_measurement_msg(m) for m in corrected_measurements] dat = messaging.new_message("gnssMeasurements") - measurement_msg = log.GnssMeasurements.Measurement.new_message + measurement_msg = log.LiveLocationKalman.Measurement.new_message dat.gnssMeasurements = { "positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=localizer_valid), "velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=localizer_valid), - "bearingDeg": measurement_msg(value=[bearing_deg], std=bearing_std, valid=localizer_valid), + "bearingDeg": measurement_msg(value=[bearing_deg], std=[bearing_std], valid=localizer_valid), "ubloxMonoTime": ublox_mono_time, "correctedMeasurements": meas_msgs } @@ -77,7 +77,7 @@ class Laikad: filter_time = self.gnss_kf.filter.filter_time if filter_time is None: cloudlog.info("Init gnss kalman filter") - elif (t - filter_time) > MAX_TIME_GAP: + elif abs(t - filter_time) > MAX_TIME_GAP: cloudlog.error("Time gap of over 10s detected, gnss kalman reset") else: cloudlog.error("Gnss kalman filter state is nan") @@ -136,7 +136,7 @@ def get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std): ned_vel = np.einsum('ij,j ->i', converter.ned_from_ecef_matrix, ecef_vel) bearing = np.arctan2(ned_vel[1], ned_vel[0]) - bearing_std = np.arctan2(vel_std, np.linalg.norm(ned_vel)) + bearing_std = np.arctan2(np.linalg.norm(vel_std), np.linalg.norm(ned_vel)) return float(np.rad2deg(bearing)), float(bearing_std) diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py index bcc62a2932..8421605a55 100755 --- a/selfdrive/manager/build.py +++ b/selfdrive/manager/build.py @@ -17,7 +17,7 @@ from selfdrive.version import is_dirty MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9 CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache") -TOTAL_SCONS_NODES = 2405 +TOTAL_SCONS_NODES = 2035 MAX_BUILD_PROGRESS = 100 PREBUILT = os.path.exists(os.path.join(BASEDIR, 'prebuilt')) diff --git a/selfdrive/navd/helpers.py b/selfdrive/navd/helpers.py index 89fd5e7e4e..eda813154a 100644 --- a/selfdrive/navd/helpers.py +++ b/selfdrive/navd/helpers.py @@ -2,18 +2,24 @@ from __future__ import annotations import json import math -from typing import Any, Dict, List, Optional, Tuple +from typing import Any, Dict, List, Optional, Tuple, Union, cast +from common.conversions import Conversions from common.numpy_fast import clip from common.params import Params EARTH_MEAN_RADIUS = 6371007.2 +SPEED_CONVERSIONS = { + 'km/h': Conversions.KPH_TO_MS, + 'mph': Conversions.MPH_TO_MS, + } class Coordinate: def __init__(self, latitude: float, longitude: float) -> None: self.latitude = latitude self.longitude = longitude + self.annotations: Dict[str, float] = {} @classmethod def from_mapbox_tuple(cls, t: Tuple[float, float]) -> Coordinate: @@ -116,6 +122,12 @@ def string_to_direction(direction: str) -> str: return 'none' +def maxspeed_to_ms(maxspeed: Dict[str, Union[str, float]]) -> float: + unit = cast(str, maxspeed['unit']) + speed = cast(float, maxspeed['speed']) + return SPEED_CONVERSIONS[unit] * speed + + def parse_banner_instructions(instruction: Any, banners: Any, distance_to_maneuver: float = 0.0) -> None: if not len(banners): return diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 0a8eab7e06..52db9b1d08 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -10,11 +10,11 @@ from cereal import log from common.api import Api from common.params import Params from common.realtime import Ratekeeper -from selfdrive.swaglog import cloudlog from selfdrive.navd.helpers import (Coordinate, coordinate_from_param, - distance_along_geometry, + distance_along_geometry, maxspeed_to_ms, minimum_distance, parse_banner_instructions) +from selfdrive.swaglog import cloudlog REROUTE_DISTANCE = 25 MANEUVER_TRANSITION_THRESHOLD = 10 @@ -110,7 +110,7 @@ class RouteEngine: params = { 'access_token': self.mapbox_token, - # 'annotations': 'maxspeed', + 'annotations': 'maxspeed', 'geometries': 'geojson', 'overview': 'full', 'steps': 'true', @@ -131,9 +131,25 @@ class RouteEngine: self.route = r['routes'][0]['legs'][0]['steps'] self.route_geometry = [] + maxspeed_idx = 0 + maxspeeds = r['routes'][0]['legs'][0]['annotation']['maxspeed'] + # Convert coordinates for step in self.route: - self.route_geometry.append([Coordinate.from_mapbox_tuple(c) for c in step['geometry']['coordinates']]) + coords = [] + + for c in step['geometry']['coordinates']: + coord = Coordinate.from_mapbox_tuple(c) + + # Last step does not have maxspeed + if (maxspeed_idx < len(maxspeeds)) and ('unknown' not in maxspeeds[maxspeed_idx]): + coord.annotations['maxspeed'] = maxspeed_to_ms(maxspeeds[maxspeed_idx]) + + coords.append(coord) + maxspeed_idx += 1 + + self.route_geometry.append(coords) + maxspeed_idx -= 1 # Every segment ends with the same coordinate as the start of the next self.step_idx = 0 else: @@ -179,6 +195,23 @@ class RouteEngine: msg.navInstruction.timeRemaining = total_time msg.navInstruction.timeRemainingTypical = total_time_typical + # Speed limit + closest_idx, closest = min(enumerate(geometry), key=lambda p: p[1].distance_to(self.last_position)) + if closest_idx > 0: + # If we are not past the closest point, show previous + if along_geometry < distance_along_geometry(geometry, geometry[closest_idx]): + closest = geometry[closest_idx - 1] + + if 'maxspeed' in closest.annotations: + msg.navInstruction.speedLimit = closest.annotations['maxspeed'] + + # Speed limit sign type + if 'speedLimitSign' in step: + if step['speedLimitSign'] == 'mutcd': + msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.mutcd + elif step['speedLimitSign'] == 'vienna': + msg.navInstruction.speedLimitSign = log.NavInstruction.SpeedLimitSign.vienna + self.pm.send('navInstruction', msg) # Transition to next route segment diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py index 34f35e0ed7..b83629c76a 100755 --- a/selfdrive/test/process_replay/model_replay.py +++ b/selfdrive/test/process_replay/model_replay.py @@ -10,9 +10,8 @@ import cereal.messaging as messaging from cereal.visionipc import VisionIpcServer, VisionStreamType from common.spinner import Spinner from common.timeout import Timeout -from common.transformations.camera import get_view_frame_from_road_frame, eon_f_frame_size, tici_f_frame_size, \ - eon_d_frame_size, tici_d_frame_size -from selfdrive.hardware import PC, TICI +from common.transformations.camera import get_view_frame_from_road_frame, tici_f_frame_size, tici_d_frame_size +from selfdrive.hardware import PC from selfdrive.manager.process_config import managed_processes from selfdrive.test.openpilotci import BASE_URL, get_url from selfdrive.test.process_replay.compare_logs import compare_logs, save_log @@ -23,14 +22,14 @@ from tools.lib.logreader import LogReader TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36" SEGMENT = 0 +MAX_FRAMES = 20 if PC else 1300 SEND_EXTRA_INPUTS = bool(os.getenv("SEND_EXTRA_INPUTS", "0")) VIPC_STREAM = {"roadCameraState": VisionStreamType.VISION_STREAM_ROAD, "driverCameraState": VisionStreamType.VISION_STREAM_DRIVER, "wideRoadCameraState": VisionStreamType.VISION_STREAM_WIDE_ROAD} - -def get_log_fn(ref_commit, test_route, tici=True): - return f"{test_route}_{'model_tici' if tici else 'model'}_{ref_commit}.bz2" +def get_log_fn(ref_commit, test_route): + return f"{test_route}_model_tici_{ref_commit}.bz2" def replace_calib(msg, calib): @@ -41,12 +40,15 @@ def replace_calib(msg, calib): def model_replay(lr, frs): - spinner = Spinner() - spinner.update("starting model replay") + if not PC: + spinner = Spinner() + spinner.update("starting model replay") + else: + spinner = None vipc_server = VisionIpcServer("camerad") - vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *(tici_f_frame_size if TICI else eon_f_frame_size)) - vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *(tici_d_frame_size if TICI else eon_d_frame_size)) + vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *(tici_f_frame_size)) + vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *(tici_d_frame_size)) vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *(tici_f_frame_size)) vipc_server.start_listener() @@ -76,7 +78,7 @@ def model_replay(lr, frs): for cam_msgs in zip_longest(msgs['roadCameraState'], msgs['wideRoadCameraState'], msgs['driverCameraState']): # need a pair of road/wide msgs - if TICI and None in (cam_msgs[0], cam_msgs[1]): + if None in (cam_msgs[0], cam_msgs[1]): break for msg in cam_msgs: @@ -107,7 +109,7 @@ def model_replay(lr, frs): recv = None if msg.which() in ('roadCameraState', 'wideRoadCameraState'): - if not TICI or min(frame_idxs['roadCameraState'], frame_idxs['wideRoadCameraState']) > recv_cnt['modelV2']: + if min(frame_idxs['roadCameraState'], frame_idxs['wideRoadCameraState']) > recv_cnt['modelV2']: recv = "modelV2" elif msg.which() == 'driverCameraState': recv = "driverState" @@ -118,14 +120,19 @@ def model_replay(lr, frs): recv_cnt[recv] += 1 log_msgs.append(messaging.recv_one(sm.sock[recv])) - spinner.update("replaying models: road %d/%d, driver %d/%d" % (frame_idxs['roadCameraState'], - frs['roadCameraState'].frame_count, frame_idxs['driverCameraState'], frs['driverCameraState'].frame_count)) + if spinner: + spinner.update("replaying models: road %d/%d, driver %d/%d" % (frame_idxs['roadCameraState'], + frs['roadCameraState'].frame_count, frame_idxs['driverCameraState'], frs['driverCameraState'].frame_count)) + - if any(frame_idxs[c] >= frs[c].frame_count for c in frame_idxs.keys()): + if any(frame_idxs[c] >= frs[c].frame_count for c in frame_idxs.keys()) or frame_idxs['roadCameraState'] == MAX_FRAMES: break + else: + print(f'Received {frame_idxs["roadCameraState"]} frames') finally: - spinner.close() + if spinner: + spinner.close() managed_processes['modeld'].stop() managed_processes['dmonitoringmodeld'].stop() @@ -144,9 +151,8 @@ if __name__ == "__main__": frs = { 'roadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="fcamera")), 'driverCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="dcamera")), + 'wideRoadCameraState': FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="ecamera")) } - if TICI: - frs['wideRoadCameraState'] = FrameReader(get_url(TEST_ROUTE, SEGMENT, log_type="ecamera")) # run replay log_msgs = model_replay(lr, frs) @@ -156,9 +162,9 @@ if __name__ == "__main__": if not update: with open(ref_commit_fn) as f: ref_commit = f.read().strip() - log_fn = get_log_fn(ref_commit, TEST_ROUTE, tici=TICI) + log_fn = get_log_fn(ref_commit, TEST_ROUTE) try: - cmp_log = LogReader(BASE_URL + log_fn) + cmp_log = list(LogReader(BASE_URL + log_fn))[:2*MAX_FRAMES] ignore = [ 'logMonoTime', @@ -167,7 +173,8 @@ if __name__ == "__main__": 'driverState.modelExecutionTime', 'driverState.dspExecutionTime' ] - tolerance = None if not PC else 1e-3 + # TODO this tolerence is absurdly large + tolerance = 5e-1 if PC else None results: Any = {TEST_ROUTE: {}} results[TEST_ROUTE]["models"] = compare_logs(cmp_log, log_msgs, tolerance=tolerance, ignore_fields=ignore) diff1, diff2, failed = format_diff(results, ref_commit) @@ -182,13 +189,13 @@ if __name__ == "__main__": failed = True # upload new refs - if update or failed: + if (update or failed) and not PC: from selfdrive.test.openpilotci import upload_file print("Uploading new refs") new_commit = get_commit() - log_fn = get_log_fn(new_commit, TEST_ROUTE, tici=TICI) + log_fn = get_log_fn(new_commit, TEST_ROUTE) save_log(log_fn, log_msgs) try: upload_file(log_fn, os.path.basename(log_fn)) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 502fa15f2a..3a9b1d9479 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -49e231ccf06ef35994a3ec907af959c28e3c0870 \ No newline at end of file +0e18bb3317437f2cad6d0a5782a9222eda655d58 diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 4aff797714..ac6f1f1ba0 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -57,12 +57,6 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { "Upload data from the driver facing camera and help improve the driver monitoring algorithm.", "../assets/offroad/icon_monitoring.png", }, - { - "EndToEndToggle", - "\U0001f96c Disable use of lanelines (Alpha) \U0001f96c", - "In this mode openpilot will ignore lanelines and just drive how it thinks a human would.", - "../assets/offroad/icon_road.png", - }, { "DisengageOnAccelerator", "Disengage On Accelerator Pedal", @@ -95,9 +89,6 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { auto toggle = new ParamControl(param, title, desc, icon, this); bool locked = params.getBool((param + "Lock").toStdString()); toggle->setEnabled(!locked); - if (!locked) { - connect(uiState(), &UIState::offroadTransition, toggle, &ParamControl::setEnabled); - } addItem(toggle); } } diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 31d94c2c65..ce61c094bd 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -311,24 +311,19 @@ void NvgWindow::drawLaneLines(QPainter &painter, const UIState *s) { // paint path QLinearGradient bg(0, height(), 0, height() / 4); - if (scene.end_to_end) { - const auto &orientation = (*s->sm)["modelV2"].getModelV2().getOrientation(); - float orientation_future = 0; - if (orientation.getZ().size() > 16) { - orientation_future = std::abs(orientation.getZ()[16]); // 2.5 seconds - } - // straight: 112, in turns: 70 - float curve_hue = fmax(70, 112 - (orientation_future * 420)); - // FIXME: painter.drawPolygon can be slow if hue is not rounded - curve_hue = int(curve_hue * 100 + 0.5) / 100; - - bg.setColorAt(0.0, QColor::fromHslF(148 / 360., 0.94, 0.51, 0.4)); - bg.setColorAt(0.75 / 1.5, QColor::fromHslF(curve_hue / 360., 1.0, 0.68, 0.35)); - bg.setColorAt(1.0, QColor::fromHslF(curve_hue / 360., 1.0, 0.68, 0.0)); - } else { - bg.setColorAt(0, whiteColor()); - bg.setColorAt(1, whiteColor(0)); + const auto &orientation = (*s->sm)["modelV2"].getModelV2().getOrientation(); + float orientation_future = 0; + if (orientation.getZ().size() > 16) { + orientation_future = std::abs(orientation.getZ()[16]); // 2.5 seconds } + // straight: 112, in turns: 70 + float curve_hue = fmax(70, 112 - (orientation_future * 420)); + // FIXME: painter.drawPolygon can be slow if hue is not rounded + curve_hue = int(curve_hue * 100 + 0.5) / 100; + + bg.setColorAt(0.0, QColor::fromHslF(148 / 360., 0.94, 0.51, 0.4)); + bg.setColorAt(0.75 / 1.5, QColor::fromHslF(curve_hue / 360., 1.0, 0.68, 0.35)); + bg.setColorAt(1.0, QColor::fromHslF(curve_hue / 360., 1.0, 0.68, 0.0)); painter.setBrush(bg); painter.drawPolygon(scene.track_vertices.v, scene.track_vertices.cnt); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 811438832b..c1af4ed6f4 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -114,7 +114,7 @@ static void update_model(UIState *s, const cereal::ModelDataV2::Reader &model) { max_distance = std::clamp((float)(lead_d - fmin(lead_d * 0.35, 10.)), 0.0f, max_distance); } max_idx = get_path_length_idx(model_position, max_distance); - update_line_data(s, model_position, scene.end_to_end ? 0.9 : 0.5, 1.22, &scene.track_vertices, max_idx, false); + update_line_data(s, model_position, 0.9, 1.22, &scene.track_vertices, max_idx, false); } static void update_sockets(UIState *s) { @@ -221,7 +221,6 @@ void UIState::updateStatus() { if (scene.started) { status = STATUS_DISENGAGED; scene.started_frame = sm->frame; - scene.end_to_end = Params().getBool("EndToEndToggle"); wide_camera = Params().getBool("WideCameraOnly"); } started_prev = scene.started; diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 51a1f6e21f..192a353ad1 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -107,7 +107,7 @@ typedef struct UIScene { QPointF lead_vertices[2]; float light_sensor, accel_sensor, gyro_sensor; - bool started, ignition, is_metric, longitudinal_control, end_to_end; + bool started, ignition, is_metric, longitudinal_control; uint64_t started_frame; } UIScene; diff --git a/tools/sim/Dockerfile.sim b/tools/sim/Dockerfile.sim index 2ba996d7f8..d869db90a6 100644 --- a/tools/sim/Dockerfile.sim +++ b/tools/sim/Dockerfile.sim @@ -1,25 +1,10 @@ FROM ghcr.io/commaai/openpilot-base-cl:latest RUN apt-get update && apt-get install -y --no-install-recommends\ - apt-utils \ - unzip \ - tar \ - curl \ - xz-utils \ - dbus \ - gcc-arm-none-eabi \ tmux \ vim \ - lsb-core \ - libx11-6 \ && rm -rf /var/lib/apt/lists/* -ENV NVIDIA_VISIBLE_DEVICES all -ENV NVIDIA_DRIVER_CAPABILITIES graphics,utility,compute -ENV QTWEBENGINE_DISABLE_SANDBOX 1 - -RUN dbus-uuidgen > /etc/machine-id - # get same tmux config used on NEOS for debugging RUN cd $HOME && \ curl -O https://raw.githubusercontent.com/commaai/eon-neos-builder/master/devices/eon/home/.tmux.conf