diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index b4cccc2d..dc42a314 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -14,15 +14,15 @@ Most open source development activity is coordinated through our [Discord](https ### Local Testing -You can test your changes on your machine by running `run_docker_tests.sh`. This will run some automated tests in docker against your code. +You can test your changes on your machine by running `run_docker_tests.sh`. This will run some automated tests in docker against your code. ### Automated Testing -All PRs are automatically checked by travis. Check out `.travis.yml` for what travis runs. Any new tests sould be added to travis. +All PRs are automatically checked by Github Actions. Check out `.github/workflows/` for what Github Actions runs. Any new tests sould be added to Github Actions. ### Code Style and Linting -Code is automatically check for style by travis as part of the automated tests. You can also run these yourself by running `check_code_quality.sh`. +Code is automatically checked for style by Github Actions as part of the automated tests. You can also run these tests yourself by running `pylint_openpilot.sh` and `flake8_openpilot.sh`. ## Car Ports (openpilot) @@ -32,9 +32,19 @@ If you port openpilot to a substantially new car brand, see this more generic [B ## Pull Requests -Pull requests should be against the master branch. Before running master on in-car hardware, you'll need to run +Pull requests should be against the master branch. Before running master on in-car hardware, you'll need to clone the submodules too. That can be done by recursively cloning the repository: +``` +git clone https://github.com/commaai/openpilot.git --recursive +``` +Or alternatively, when on the master branch: ``` git submodule init git submodule update ``` -in order to pull down the submodules, such as `panda` and `opendbc`. +The reasons for having submodules on a dedicated repository and our new development philosophy can be found in our [post about externalization](https://medium.com/@comma_ai/a-2020-theme-externalization-13b33326d8b3). +Modules that are in seperate repositories include: +* apks +* cereal +* laika +* opendbc +* panda diff --git a/README.md b/README.md index 42770fe8..1ed8ca60 100644 --- a/README.md +++ b/README.md @@ -62,90 +62,92 @@ At the moment, openpilot supports the [EON DevKit](https://comma.ai/shop/product Supported Cars ------ -| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | -| ----------| ------------------------------| ------------------| -----------------| -------------------| -------------| -| Acura | ILX 2016-18 | AcuraWatch Plus | openpilot | 25mph5 | 25mph | -| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph5 | 12mph | -| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | -| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | -| Chrysler | Pacifica Hybrid 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph | -| Honda | Accord 2018-19 | All | Stock | 0mph | 3mph | -| Honda | Accord Hybrid 2018-19 | All | Stock | 0mph | 3mph | -| Honda | Civic Hatchback 2017-19 | Honda Sensing | Stock | 0mph | 12mph | -| Honda | Civic Sedan/Coupe 2016-18 | Honda Sensing | openpilot | 0mph | 12mph | -| Honda | Civic Sedan/Coupe 2019 | Honda Sensing | Stock | 0mph | 2mph | -| Honda | CR-V 2015-16 | Touring | openpilot | 25mph5 | 12mph | -| Honda | CR-V 2017-19 | Honda Sensing | Stock | 0mph | 12mph | -| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Stock | 0mph | 12mph | -| Honda | Fit 2018-19 | Honda Sensing | openpilot | 25mph5 | 12mph | -| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph5 | 0mph | -| Honda | Passport 2019 | All | openpilot | 25mph5 | 12mph | -| Honda | Pilot 2016-18 | Honda Sensing | openpilot | 25mph5 | 12mph | -| Honda | Pilot 2019 | All | openpilot | 25mph5 | 12mph | -| Honda | Ridgeline 2017-19 | Honda Sensing | openpilot | 25mph5 | 12mph | -| Hyundai | Elantra 2017-191 | SCC + LKAS | Stock | 19mph | 34mph | -| Hyundai | Genesis 20181 | All | Stock | 19mph | 34mph | -| Hyundai | Santa Fe 20191 | All | Stock | 0mph | 0mph | -| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph | -| Jeep | Grand Cherokee 2019 | Adaptive Cruise | Stock | 0mph | 39mph | -| Kia | Optima 20191 | SCC + LKAS | Stock | 0mph | 0mph | -| Kia | Sorento 20181 | All | Stock | 0mph | 0mph | -| Kia | Stinger 20181 | SCC + LKAS | Stock | 0mph | 0mph | -| Lexus | CT Hybrid 2017-18 | All | Stock4| 0mph | 0mph | -| Lexus | ES 2019 | All | openpilot | 0mph | 0mph | -| Lexus | ES Hybrid 2019 | All | openpilot | 0mph | 0mph | -| Lexus | IS 2017-2019 | All | Stock | 22mph | 0mph | -| Lexus | IS Hybrid 2017 | All | Stock | 0mph | 0mph | -| Lexus | NX Hybrid 2018 | All | Stock4| 0mph | 0mph | -| Lexus | RX 2016-17 | All | Stock4| 0mph | 0mph | -| Lexus | RX 2020 | All | openpilot | 0mph | 0mph | -| Lexus | RX Hybrid 2016-19 | All | Stock4| 0mph | 0mph | -| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph | -| Subaru | Impreza 2019-20 | EyeSight | Stock | 0mph | 0mph | -| Toyota | Avalon 2016 | TSS-P | Stock4| 20mph5 | 0mph | -| Toyota | Avalon 2017-18 | All | Stock4| 20mph5 | 0mph | -| Toyota | Camry 2018-19 | All | Stock | 0mph2 | 0mph | -| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph2 | 0mph | -| Toyota | C-HR 2017-19 | All | Stock | 0mph | 0mph | -| Toyota | C-HR Hybrid 2017-19 | All | Stock | 0mph | 0mph | -| Toyota | Corolla 2017-19 | All | Stock4| 20mph5 | 0mph | -| Toyota | Corolla 2020 | All | openpilot | 0mph | 0mph | -| Toyota | Corolla Hatchback 2019-20 | All | openpilot | 0mph | 0mph | -| Toyota | Corolla Hybrid 2020 | All | openpilot | 0mph | 0mph | -| Toyota | Highlander 2017-19 | All | Stock4| 0mph | 0mph | -| Toyota | Highlander Hybrid 2017-19 | All | Stock4| 0mph | 0mph | -| Toyota | Highlander 2020 | All | openpilot | 0mph | 0mph | -| Toyota | Prius 2016 | TSS-P | Stock4| 0mph | 0mph | -| Toyota | Prius 2017-19 | All | Stock4| 0mph | 0mph | -| Toyota | Prius Prime 2017-20 | All | Stock4| 0mph | 0mph | -| Toyota | Rav4 2016 | TSS-P | Stock4| 20mph5 | 0mph | -| Toyota | Rav4 2017-18 | All | Stock4| 20mph5 | 0mph | -| Toyota | Rav4 2019 | All | openpilot | 0mph | 0mph | -| Toyota | Rav4 Hybrid 2016 | TSS-P | Stock4| 0mph | 0mph | -| Toyota | Rav4 Hybrid 2017-18 | All | Stock4| 0mph | 0mph | -| Toyota | Rav4 Hybrid 2019-20 | All | openpilot | 0mph | 0mph | -| Toyota | Sienna 2018 | All | Stock4| 0mph | 0mph | -| Volkswagen| Golf 2016-193 | Driver Assistance | Stock | 0mph | 0mph | +| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | +| ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------| +| Acura | ILX 2016-18 | AcuraWatch Plus | openpilot | 25mph6 | 25mph | +| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph6 | 12mph | +| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | +| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Stock | 0mph | 9mph | +| Chrysler | Pacifica Hybrid 2019-20 | Adaptive Cruise | Stock | 0mph | 39mph | +| Honda | Accord 2018-19 | All | Stock | 0mph | 3mph | +| Honda | Accord Hybrid 2018-19 | All | Stock | 0mph | 3mph | +| Honda | Civic Hatchback 2017-19 | Honda Sensing | Stock | 0mph | 12mph | +| Honda | Civic Sedan/Coupe 2016-18 | Honda Sensing | openpilot | 0mph | 12mph | +| Honda | Civic Sedan/Coupe 2019 | Honda Sensing | Stock | 0mph | 2mph4 | +| Honda | CR-V 2015-16 | Touring | openpilot | 25mph6 | 12mph | +| Honda | CR-V 2017-19 | Honda Sensing | Stock | 0mph | 12mph | +| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Stock | 0mph | 12mph | +| Honda | Fit 2018-19 | Honda Sensing | openpilot | 25mph6 | 12mph | +| Honda | Insight 2019 | Honda Sensing | Stock | 0mph | 3mph | +| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph6 | 0mph | +| Honda | Passport 2019 | All | openpilot | 25mph6 | 12mph | +| Honda | Pilot 2016-18 | Honda Sensing | openpilot | 25mph6 | 12mph | +| Honda | Pilot 2019 | All | openpilot | 25mph6 | 12mph | +| Honda | Ridgeline 2017-19 | Honda Sensing | openpilot | 25mph6 | 12mph | +| Hyundai | Elantra 2017-191 | SCC + LKAS | Stock | 19mph | 34mph | +| Hyundai | Genesis 20181 | All | Stock | 19mph | 34mph | +| Hyundai | Santa Fe 20191 | All | Stock | 0mph | 0mph | +| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Stock | 0mph | 9mph | +| Jeep | Grand Cherokee 2019 | Adaptive Cruise | Stock | 0mph | 39mph | +| Kia | Optima 20191 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Sorento 20181 | All | Stock | 0mph | 0mph | +| Kia | Stinger 20181 | SCC + LKAS | Stock | 0mph | 0mph | +| Lexus | CT Hybrid 2017-18 | All | Stock5| 0mph | 0mph | +| Lexus | ES 2019 | All | openpilot | 0mph | 0mph | +| Lexus | ES Hybrid 2019 | All | openpilot | 0mph | 0mph | +| Lexus | IS 2017-2019 | All | Stock | 22mph | 0mph | +| Lexus | IS Hybrid 2017 | All | Stock | 0mph | 0mph | +| Lexus | NX Hybrid 2018 | All | Stock5| 0mph | 0mph | +| Lexus | RX 2016-17 | All | Stock5| 0mph | 0mph | +| Lexus | RX 2020 | All | openpilot | 0mph | 0mph | +| Lexus | RX Hybrid 2016-19 | All | Stock5| 0mph | 0mph | +| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph | +| Subaru | Impreza 2019-20 | EyeSight | Stock | 0mph | 0mph | +| Toyota | Avalon 2016 | TSS-P | Stock5| 20mph6 | 0mph | +| Toyota | Avalon 2017-18 | All | Stock5| 20mph6 | 0mph | +| Toyota | Camry 2018-19 | All | Stock | 0mph2 | 0mph | +| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph2 | 0mph | +| Toyota | C-HR 2017-19 | All | Stock | 0mph | 0mph | +| Toyota | C-HR Hybrid 2017-19 | All | Stock | 0mph | 0mph | +| Toyota | Corolla 2017-19 | All | Stock5| 20mph6 | 0mph | +| Toyota | Corolla 2020 | All | openpilot | 0mph | 0mph | +| Toyota | Corolla Hatchback 2019-20 | All | openpilot | 0mph | 0mph | +| Toyota | Corolla Hybrid 2020 | All | openpilot | 0mph | 0mph | +| Toyota | Highlander 2017-19 | All | Stock5| 0mph | 0mph | +| Toyota | Highlander Hybrid 2017-19 | All | Stock5| 0mph | 0mph | +| Toyota | Highlander 2020 | All | openpilot | 0mph | 0mph | +| Toyota | Prius 2016 | TSS-P | Stock5| 0mph | 0mph | +| Toyota | Prius 2017-19 | All | Stock5| 0mph | 0mph | +| Toyota | Prius Prime 2017-20 | All | Stock5| 0mph | 0mph | +| Toyota | Rav4 2016 | TSS-P | Stock5| 20mph6 | 0mph | +| Toyota | Rav4 2017-18 | All | Stock5| 20mph6 | 0mph | +| Toyota | Rav4 2019 | All | openpilot | 0mph | 0mph | +| Toyota | Rav4 Hybrid 2016 | TSS-P | Stock5| 0mph | 0mph | +| Toyota | Rav4 Hybrid 2017-18 | All | Stock5| 0mph | 0mph | +| Toyota | Rav4 Hybrid 2019-20 | All | openpilot | 0mph | 0mph | +| Toyota | Sienna 2018 | All | Stock5| 0mph | 0mph | +| Volkswagen| Golf 2016-193 | Driver Assistance | Stock | 0mph | 0mph | 1Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and open sourced [Hyundai giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai), designed for the 2019 Sante Fe; pinout may differ for other Hyundai and Kia models.
228mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
3Requires a [custom connector](https://community.comma.ai/wiki/index.php/Volkswagen#Integration_at_R242_Camera) for the [car harness](https://comma.ai/shop/products/car-harness)
+42019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.
Community Maintained Cars and Features ------ | Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | | ----------| ------------------------------| ------------------| -----------------| -------------------| -------------| -| Buick | Regal 20186 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Cadillac | ATS 20186 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Chevrolet | Malibu 20176 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Chevrolet | Volt 2017-186 | Adaptive Cruise | openpilot | 0mph | 7mph | -| GMC | Acadia Denali 20186| Adaptive Cruise | openpilot | 0mph | 7mph | -| Holden | Astra 20176 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Buick | Regal 20187 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Cadillac | ATS 20187 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Chevrolet | Malibu 20177 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Chevrolet | Volt 2017-187 | Adaptive Cruise | openpilot | 0mph | 7mph | +| GMC | Acadia Denali 20187| Adaptive Cruise | openpilot | 0mph | 7mph | +| Holden | Astra 20177 | Adaptive Cruise | openpilot | 0mph | 7mph | -4When disconnecting the Driver Support Unit (DSU), openpilot ACC will replace stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota). ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).***
-5[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma](https://comma.ai).***
-6Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and [community built giraffe](https://zoneos.com/volt/). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).***
+5When disconnecting the Driver Support Unit (DSU), openpilot ACC will replace stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota). ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).***
+6[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma](https://comma.ai).***
+7Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and [community built giraffe](https://zoneos.com/volt/). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).***
Community Maintained Cars and Features are not verified by comma to meet our [safety model](SAFETY.md). Be extra cautious using them. They are only available after enabling the toggle in `Settings->Developer->Enable Community Features`. diff --git a/RELEASES.md b/RELEASES.md index afce0cd5..542c7a27 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,14 @@ +Version 0.7.4 (2020-03-20) +======================== + * New driving model: improved lane changes and lead car detection + * Improved driver monitoring model: improve eye detection + * Improved calibration stability + * Improved lateral control on some 2019 and 2020 Toyota Prius + * Improved lateral control on VW Golf: 20% more steering torque + * Fixed bug where some 2017 and 2018 Toyota C-HR would use the wrong steering angle sensor + * Support for Honda Insight thanks to theantihero! + * Code cleanup in car abstraction layers and ui + Version 0.7.3 (2020-02-21) ======================== * Support for 2020 Highlander thanks to che220! diff --git a/apk/ai.comma.plus.black.apk b/apk/ai.comma.plus.black.apk deleted file mode 100644 index 94b3a6a1..00000000 Binary files a/apk/ai.comma.plus.black.apk and /dev/null differ diff --git a/apk/ai.comma.plus.frame.apk b/apk/ai.comma.plus.frame.apk deleted file mode 100644 index f52f8086..00000000 Binary files a/apk/ai.comma.plus.frame.apk and /dev/null differ diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index f829c3e3..573a6e69 100644 Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ diff --git a/common/android.py b/common/android.py index e0933e82..5e26924f 100644 --- a/common/android.py +++ b/common/android.py @@ -8,6 +8,7 @@ import random from cereal import log NetworkType = log.ThermalData.NetworkType +NetworkStrength = log.ThermalData.NetworkStrength ANDROID = os.path.isfile('/EON') @@ -128,3 +129,156 @@ def get_network_type(): 19: NetworkType.cell4G } return cell_networks.get(cell_check, NetworkType.none) + +def get_network_strength(network_type): + network_strength = NetworkStrength.unknown + # from SignalStrength.java + def get_lte_level(rsrp, rssnr): + INT_MAX = 2147483647 + lvl_rsrp = NetworkStrength.unknown + lvl_rssnr = NetworkStrength.unknown + if rsrp == INT_MAX: + lvl_rsrp = NetworkStrength.unknown + elif rsrp >= -95: + lvl_rsrp = NetworkStrength.great + elif rsrp >= -105: + lvl_rsrp = NetworkStrength.good + elif rsrp >= -115: + lvl_rsrp = NetworkStrength.moderate + else: + lvl_rsrp = NetworkStrength.poor + if rssnr == INT_MAX: + lvl_rssnr = NetworkStrength.unknown + elif rssnr >= 45: + lvl_rssnr = NetworkStrength.great + elif rssnr >= 10: + lvl_rssnr = NetworkStrength.good + elif rssnr >= -30: + lvl_rssnr = NetworkStrength.moderate + else: + lvl_rssnr = NetworkStrength.poor + return max(lvl_rsrp, lvl_rssnr) + + def get_tdscdma_level(tdscmadbm): + lvl = NetworkStrength.unknown + if tdscmadbm > -25: + lvl = NetworkStrength.unknown + elif tdscmadbm >= -49: + lvl = NetworkStrength.great + elif tdscmadbm >= -73: + lvl = NetworkStrength.good + elif tdscmadbm >= -97: + lvl = NetworkStrength.moderate + elif tdscmadbm >= -110: + lvl = NetworkStrength.poor + return lvl + + def get_gsm_level(asu): + lvl = NetworkStrength.unknown + if asu <= 2 or asu == 99: + lvl = NetworkStrength.unknown + elif asu >= 12: + lvl = NetworkStrength.great + elif asu >= 8: + lvl = NetworkStrength.good + elif asu >= 5: + lvl = NetworkStrength.moderate + else: + lvl = NetworkStrength.poor + return lvl + + def get_evdo_level(evdodbm, evdosnr): + lvl_evdodbm = NetworkStrength.unknown + lvl_evdosnr = NetworkStrength.unknown + if evdodbm >= -65: + lvl_evdodbm = NetworkStrength.great + elif evdodbm >= -75: + lvl_evdodbm = NetworkStrength.good + elif evdodbm >= -90: + lvl_evdodbm = NetworkStrength.moderate + elif evdodbm >= -105: + lvl_evdodbm = NetworkStrength.poor + if evdosnr >= 7: + lvl_evdosnr = NetworkStrength.great + elif evdosnr >= 5: + lvl_evdosnr = NetworkStrength.good + elif evdosnr >= 3: + lvl_evdosnr = NetworkStrength.moderate + elif evdosnr >= 1: + lvl_evdosnr = NetworkStrength.poor + return max(lvl_evdodbm, lvl_evdosnr) + + def get_cdma_level(cdmadbm, cdmaecio): + lvl_cdmadbm = NetworkStrength.unknown + lvl_cdmaecio = NetworkStrength.unknown + if cdmadbm >= -75: + lvl_cdmadbm = NetworkStrength.great + elif cdmadbm >= -85: + lvl_cdmadbm = NetworkStrength.good + elif cdmadbm >= -95: + lvl_cdmadbm = NetworkStrength.moderate + elif cdmadbm >= -100: + lvl_cdmadbm = NetworkStrength.poor + if cdmaecio >= -90: + lvl_cdmaecio = NetworkStrength.great + elif cdmaecio >= -110: + lvl_cdmaecio = NetworkStrength.good + elif cdmaecio >= -130: + lvl_cdmaecio = NetworkStrength.moderate + elif cdmaecio >= -150: + lvl_cdmaecio = NetworkStrength.poor + return max(lvl_cdmadbm, lvl_cdmaecio) + + if network_type == NetworkType.none: + return network_strength + if network_type == NetworkType.wifi: + out = subprocess.check_output('dumpsys connectivity', shell=True).decode('ascii') + network_strength = NetworkStrength.unknown + for line in out.split('\n'): + signal_str = "SignalStrength: " + if signal_str in line: + lvl_idx_start = line.find(signal_str) + len(signal_str) + lvl_idx_end = line.find(']', lvl_idx_start) + lvl = int(line[lvl_idx_start : lvl_idx_end]) + if lvl >= -50: + network_strength = NetworkStrength.great + elif lvl >= -60: + network_strength = NetworkStrength.good + elif lvl >= -70: + network_strength = NetworkStrength.moderate + else: + network_strength = NetworkStrength.poor + return network_strength + else: + # check cell strength + out = subprocess.check_output('dumpsys telephony.registry', shell=True).decode('ascii') + for line in out.split('\n'): + if "mSignalStrength" in line: + arr = line.split(' ') + ns = 0 + if ("gsm" in arr[14]): + rsrp = int(arr[9]) + rssnr = int(arr[11]) + ns = get_lte_level(rsrp, rssnr) + if ns == NetworkStrength.unknown: + tdscmadbm = int(arr[13]) + ns = get_tdscdma_level(tdscmadbm) + if ns == NetworkStrength.unknown: + asu = int(arr[1]) + ns = get_gsm_level(asu) + else: + cdmadbm = int(arr[3]) + cdmaecio = int(arr[4]) + evdodbm = int(arr[5]) + evdosnr = int(arr[7]) + lvl_cdma = get_cdma_level(cdmadbm, cdmaecio) + lvl_edmo = get_evdo_level(evdodbm, evdosnr) + if lvl_edmo == NetworkStrength.unknown: + ns = lvl_cdma + elif lvl_cdma == NetworkStrength.unknown: + ns = lvl_edmo + else: + ns = min(lvl_cdma, lvl_edmo) + network_strength = max(network_strength, ns) + + return network_strength diff --git a/common/apk.py b/common/apk.py index d77b83f1..5716ed45 100644 --- a/common/apk.py +++ b/common/apk.py @@ -6,7 +6,7 @@ import shutil from common.basedir import BASEDIR from selfdrive.swaglog import cloudlog -android_packages = ("ai.comma.plus.offroad", "ai.comma.plus.frame") +android_packages = ("ai.comma.plus.offroad",) def get_installed_apks(): dat = subprocess.check_output(["pm", "list", "packages", "-f"], encoding='utf8').strip().split("\n") @@ -26,9 +26,9 @@ def install_apk(path): os.remove(install_path) return ret == 0 -def start_frame(): +def start_offroad(): set_package_permissions() - system("am start -n ai.comma.plus.frame/.MainActivity") + system("am start -n ai.comma.plus.offroad/.MainActivity") def set_package_permissions(): pm_grant("ai.comma.plus.offroad", "android.permission.ACCESS_FINE_LOCATION") @@ -95,4 +95,3 @@ def pm_apply_packages(cmd): if __name__ == "__main__": update_apks() - diff --git a/common/manager_helpers.py b/common/manager_helpers.py new file mode 100644 index 00000000..a8cfb3df --- /dev/null +++ b/common/manager_helpers.py @@ -0,0 +1,50 @@ +def cputime_total(ct): + return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem + + +def print_cpu_usage(first_proc, last_proc): + r = 0 + procs = [ + ("selfdrive.controls.controlsd", 59.46), + ("./_modeld", 48.94), + ("./loggerd", 28.49), + ("selfdrive.controls.plannerd", 19.77), + ("selfdrive.controls.radard", 9.54), + ("./_ui", 9.54), + ("./camerad", 7.07), + ("selfdrive.locationd.locationd", 7.13), + ("./_sensord", 6.17), + ("selfdrive.controls.dmonitoringd", 5.48), + ("./boardd", 3.63), + ("./_dmonitoringmodeld", 2.67), + ("selfdrive.logmessaged", 2.71), + ("selfdrive.thermald", 2.41), + ("./paramsd", 2.18), + ("selfdrive.locationd.calibrationd", 1.76), + ("./proclogd", 1.54), + ("./_gpsd", 0.09), + ("./clocksd", 0.02), + ("./ubloxd", 0.02), + ("selfdrive.tombstoned", 0), + ("./logcatd", 0), + ("selfdrive.updated", 0), + ] + + dt = (last_proc.logMonoTime - first_proc.logMonoTime) / 1e9 + print("------------------------------------------------") + for proc_name, normal_cpu_usage in procs: + try: + first = [p for p in first_proc.procLog.procs if proc_name in p.cmdline][0] + last = [p for p in last_proc.procLog.procs if proc_name in p.cmdline][0] + cpu_time = cputime_total(last) - cputime_total(first) + cpu_usage = cpu_time / dt * 100. + if cpu_usage > max(normal_cpu_usage * 1.1, normal_cpu_usage + 5.0): + print(f"Warning {proc_name} using more CPU than normal") + r = 1 + + print(f"{proc_name.ljust(35)} {cpu_usage:.2f}%") + except IndexError: + print(f"{proc_name.ljust(35)} NO METRICS FOUND") + print("------------------------------------------------") + + return r diff --git a/common/params.py b/common/params.py index c623e3dc..22ee0aa5 100755 --- a/common/params.py +++ b/common/params.py @@ -22,6 +22,8 @@ file in place without messing with /d. """ import time import os +import string +import binascii import errno import sys import shutil @@ -59,6 +61,7 @@ keys = { "CommunityFeaturesToggle": [TxType.PERSISTENT], "CompletedTrainingVersion": [TxType.PERSISTENT], "ControlsParams": [TxType.PERSISTENT], + "DisablePowerDown": [TxType.PERSISTENT], "DoUninstall": [TxType.CLEAR_ON_MANAGER_START], "DongleId": [TxType.PERSISTENT], "GitBranch": [TxType.PERSISTENT], @@ -407,10 +410,10 @@ if __name__ == "__main__": pp = params.get(k) if pp is None: print("%s is None" % k) - elif all(ord(c) < 128 and ord(c) >= 32 for c in pp): + elif all(chr(c) in string.printable for c in pp): print("%s = %s" % (k, pp)) else: - print("%s = %s" % (k, pp.encode("hex"))) + print("%s = %s" % (k, binascii.hexlify(pp))) # Test multiprocess: # seq 0 100000 | xargs -P20 -I{} python common/params.py DongleId {} && sleep 0.05 diff --git a/models/dmonitoring_model_q.dlc b/models/dmonitoring_model_q.dlc index 59104e18..9dbe0927 100644 Binary files a/models/dmonitoring_model_q.dlc and b/models/dmonitoring_model_q.dlc differ diff --git a/models/supercombo.dlc b/models/supercombo.dlc index d7942d6d..ea6d0a06 100644 Binary files a/models/supercombo.dlc and b/models/supercombo.dlc differ diff --git a/selfdrive/assets/images/battery.png b/selfdrive/assets/images/battery.png new file mode 100644 index 00000000..cac3b909 Binary files /dev/null and b/selfdrive/assets/images/battery.png differ diff --git a/selfdrive/assets/images/battery_charging.png b/selfdrive/assets/images/battery_charging.png new file mode 100644 index 00000000..4c25566d Binary files /dev/null and b/selfdrive/assets/images/battery_charging.png differ diff --git a/selfdrive/assets/images/button_home.png b/selfdrive/assets/images/button_home.png new file mode 100644 index 00000000..9f52faf9 Binary files /dev/null and b/selfdrive/assets/images/button_home.png differ diff --git a/selfdrive/assets/images/button_settings.png b/selfdrive/assets/images/button_settings.png new file mode 100644 index 00000000..e04262b8 Binary files /dev/null and b/selfdrive/assets/images/button_settings.png differ diff --git a/selfdrive/assets/images/network_0.png b/selfdrive/assets/images/network_0.png new file mode 100644 index 00000000..2ce959ca Binary files /dev/null and b/selfdrive/assets/images/network_0.png differ diff --git a/selfdrive/assets/images/network_1.png b/selfdrive/assets/images/network_1.png new file mode 100644 index 00000000..d7ae713f Binary files /dev/null and b/selfdrive/assets/images/network_1.png differ diff --git a/selfdrive/assets/images/network_2.png b/selfdrive/assets/images/network_2.png new file mode 100644 index 00000000..17ecd977 Binary files /dev/null and b/selfdrive/assets/images/network_2.png differ diff --git a/selfdrive/assets/images/network_3.png b/selfdrive/assets/images/network_3.png new file mode 100644 index 00000000..1e854e67 Binary files /dev/null and b/selfdrive/assets/images/network_3.png differ diff --git a/selfdrive/assets/images/network_4.png b/selfdrive/assets/images/network_4.png new file mode 100644 index 00000000..08c9ab91 Binary files /dev/null and b/selfdrive/assets/images/network_4.png differ diff --git a/selfdrive/assets/images/network_5.png b/selfdrive/assets/images/network_5.png new file mode 100644 index 00000000..fba67a95 Binary files /dev/null and b/selfdrive/assets/images/network_5.png differ diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index e3a1bf2f..90043bfe 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -294,7 +294,7 @@ def ws_send(ws, end_event): def backoff(retries): return random.randrange(0, min(128, int(2 ** retries))) -def main(gctx=None): +def main(): params = Params() dongle_id = params.get("DongleId").decode('utf-8') ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id diff --git a/selfdrive/athena/test.py b/selfdrive/athena/test.py index 0bedfdeb..b3e1d692 100755 --- a/selfdrive/athena/test.py +++ b/selfdrive/athena/test.py @@ -41,8 +41,7 @@ class TestAthenadMethods(unittest.TestCase): start = time.time() while time.time() - start < 1: - msg = messaging.new_message() - msg.init('thermal') + msg = messaging.new_message('thermal') pub_sock.send(msg.to_bytes()) time.sleep(0.01) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index ef5faeb3..0955be10 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -392,12 +392,18 @@ void can_health(PubSocket *publisher) { bool cdp_mode = health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP); bool no_ignition_exp = no_ignition_cnt > NO_IGNITION_CNT_MAX; if ((no_ignition_exp || (voltage_f < VBATT_PAUSE_CHARGING)) && cdp_mode && !ignition) { - printf("TURN OFF CHARGING!\n"); - pthread_mutex_lock(&usb_lock); - libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT); - pthread_mutex_unlock(&usb_lock); - printf("POWER DOWN DEVICE\n"); - system("service call power 17 i32 0 i32 1"); + char *disable_power_down = NULL; + size_t disable_power_down_sz = 0; + const int result = read_db_value(NULL, "DisablePowerDown", &disable_power_down, &disable_power_down_sz); + if (disable_power_down_sz != 1 || disable_power_down[0] != '1') { + printf("TURN OFF CHARGING!\n"); + pthread_mutex_lock(&usb_lock); + libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT); + pthread_mutex_unlock(&usb_lock); + printf("POWER DOWN DEVICE\n"); + system("service call power 17 i32 0 i32 1"); + } + if (disable_power_down) free(disable_power_down); } if (!no_ignition_exp && (voltage_f > VBATT_START_CHARGING) && !cdp_mode) { printf("TURN ON CHARGING!\n"); diff --git a/selfdrive/boardd/tests/boardd_old.py b/selfdrive/boardd/tests/boardd_old.py index 48c7a945..f6ca7eb8 100755 --- a/selfdrive/boardd/tests/boardd_old.py +++ b/selfdrive/boardd/tests/boardd_old.py @@ -27,8 +27,7 @@ except Exception: # *** serialization functions *** def can_list_to_can_capnp(can_msgs, msgtype='can'): - dat = messaging.new_message() - dat.init(msgtype, len(can_msgs)) + dat = messaging.new_message(msgtype, len(can_msgs)) for i, can_msg in enumerate(can_msgs): if msgtype == 'sendcan': cc = dat.sendcan[i] @@ -168,8 +167,7 @@ def boardd_loop(rate=100): # health packet @ 2hz if (rk.frame % (rate // 2)) == 0: health = can_health() - msg = messaging.new_message() - msg.init('health') + msg = messaging.new_message('health') # store the health to be logged msg.health.voltage = health['voltage'] @@ -232,7 +230,7 @@ def boardd_proxy_loop(rate=100, address="192.168.2.251"): rk.keep_time() -def main(gctx=None): +def main(): if os.getenv("MOCK") is not None: boardd_mock_loop() elif os.getenv("PROXY") is not None: diff --git a/selfdrive/camerad/cameras/camera_qcom.c b/selfdrive/camerad/cameras/camera_qcom.c index a483a6ba..6cef28f2 100644 --- a/selfdrive/camerad/cameras/camera_qcom.c +++ b/selfdrive/camerad/cameras/camera_qcom.c @@ -2117,7 +2117,8 @@ void cameras_run(DualCameraState *s) { int ret = poll(fds, ARRAYSIZE(fds), 1000); if (ret <= 0) { - LOGE("poll failed (%d)", ret); + if (errno == EINTR) continue; + LOGE("poll failed (%d - %d)", ret, errno); break; } diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc index 7c65f755..304990ca 100644 --- a/selfdrive/camerad/main.cc +++ b/selfdrive/camerad/main.cc @@ -437,6 +437,12 @@ void* processing_thread(void *arg) { framed.setLensErr(frame_data.lens_err); framed.setLensTruePos(frame_data.lens_true_pos); framed.setGainFrac(frame_data.gain_frac); +#ifdef QCOM + kj::ArrayPtr focus_vals(&s->cameras.rear.focus[0], NUM_FOCUS); + kj::ArrayPtr focus_confs(&s->cameras.rear.confidence[0], NUM_FOCUS); + framed.setFocusVal(focus_vals); + framed.setFocusConf(focus_confs); +#endif #ifndef QCOM framed.setImage(kj::arrayPtr((const uint8_t*)s->yuv_ion[yuv_idx].addr, s->yuv_buf_size)); @@ -598,8 +604,11 @@ void* visionserver_client_thread(void* arg) { } int ret = zmq_poll(polls, num_polls, -1); if (ret < 0) { - if (errno == EINTR) continue; - LOGE("poll failed (%d)", ret); + if (errno == EINTR){ + LOGW("poll EINTR"); + continue; + } + LOGE("poll failed (%d - %d)", ret, errno); break; } if (polls[0].revents) { @@ -790,7 +799,8 @@ void* visionserver_thread(void* arg) { int ret = zmq_poll(polls, ARRAYSIZE(polls), -1); if (ret < 0) { - LOGE("poll failed (%d)", ret); + if (errno == EINTR) continue; + LOGE("poll failed (%d - %d)", ret, errno); break; } if (polls[0].revents) { diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index f1766075..e99d61e4 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -29,10 +29,12 @@ def load_interfaces(brand_names): CarInterface = __import__(path + '.interface', fromlist=['CarInterface']).CarInterface if os.path.exists(BASEDIR + '/' + path.replace('.', '/') + '/carcontroller.py'): CarController = __import__(path + '.carcontroller', fromlist=['CarController']).CarController + CarState = __import__(path + '.carstate', fromlist=['CarState']).CarState else: CarController = None + CarState = None for model_name in brand_names[brand_name]: - ret[model_name] = (CarInterface, CarController) + ret[model_name] = (CarInterface, CarController, CarState) return ret @@ -54,7 +56,8 @@ def _get_interface_names(): # imports from directory selfdrive/car// -interfaces = load_interfaces(_get_interface_names()) +interface_names = _get_interface_names() +interfaces = load_interfaces(interface_names) def only_toyota_left(candidate_cars): @@ -69,11 +72,16 @@ def fingerprint(logcan, sendcan, has_relay): cached_params = Params().get("CarParamsCache") if cached_params is not None: + cached_params = car.CarParams.from_bytes(cached_params) + if cached_params.carName == "mock": + cached_params = None + + if cached_params is not None and len(cached_params.carFw) > 0 and cached_params.carVin is not VIN_UNKNOWN: cloudlog.warning("Using cached CarParams") - CP = car.CarParams.from_bytes(cached_params) - vin = CP.carVin - car_fw = list(CP.carFw) + vin = cached_params.carVin + car_fw = list(cached_params.carFw) else: + cloudlog.warning("Getting VIN & FW versions") _, vin = get_vin(logcan, sendcan, bus) car_fw = get_fw_versions(logcan, sendcan, bus) @@ -149,10 +157,10 @@ def get_car(logcan, sendcan, has_relay=False): cloudlog.warning("car doesn't match any fingerprints: %r", fingerprints) candidate = "mock" - CarInterface, CarController = interfaces[candidate] + CarInterface, CarController, CarState = interfaces[candidate] car_params = CarInterface.get_params(candidate, fingerprints, has_relay, car_fw) car_params.carVin = vin car_params.carFw = car_fw car_params.fingerprintSource = source - return CarInterface(car_params, CarController), car_params + return CarInterface(car_params, CarController, CarState), car_params diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index f6b55b9e..8c58d3db 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -1,27 +1,21 @@ from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \ create_wheel_buttons -from selfdrive.car.chrysler.values import Ecu, CAR, SteerLimitParams +from selfdrive.car.chrysler.values import CAR, SteerLimitParams from opendbc.can.packer import CANPacker class CarController(): - def __init__(self, dbc_name, car_fingerprint, enable_camera): + def __init__(self, dbc_name, CP, VM): self.braking = False - # redundant safety check with the board - self.controls_allowed = True self.apply_steer_last = 0 self.ccframe = 0 self.prev_frame = -1 self.hud_count = 0 - self.car_fingerprint = car_fingerprint + self.car_fingerprint = CP.carFingerprint self.alert_active = False self.gone_fast_yet = False self.steer_rate_limited = False - self.fake_ecus = set() - if enable_camera: - self.fake_ecus.add(Ecu.fwdCamera) - self.packer = CANPacker(dbc_name) @@ -41,7 +35,7 @@ class CarController(): moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit self.gone_fast_yet = True - elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020_HYBRID, CAR.JEEP_CHEROKEE_2019): + elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019): if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0): self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5 lkas_active = moving_fast and enabled diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index bab58b96..99e09f00 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -5,63 +5,6 @@ from selfdrive.config import Conversions as CV from selfdrive.car.interfaces import CarStateBase from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD -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), - ("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), - ("COUNTER", "WHEEL_BUTTONS", -1), # incrementing counter for 23b - ] - - # It's considered invalid if it is not received for 10x the expected period (1/f). - checks = [ - # sig_address, frequency - ("BRAKE_2", 50), - ("EPS_STATUS", 100), - ("SPEED_1", 100), - ("WHEEL_SPEEDS", 50), - ("STEERING", 100), - ("ACC_2", 50), - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) - -def get_camera_parser(CP): - signals = [ - # sig_name, sig_address, default - # TODO read in all the other values - ("COUNTER", "LKAS_COMMAND", -1), - ("CAR_MODEL", "LKAS_HUD", -1), - ("LKAS_STATUS_OK", "LKAS_HEARTBIT", -1) - ] - checks = [] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) - class CarState(CarStateBase): def __init__(self, CP): @@ -73,7 +16,6 @@ class CarState(CarStateBase): ret = car.CarState.new_message() - self.frame_23b = int(cp.vl["WHEEL_BUTTONS"]['COUNTER']) self.frame = int(cp.vl["EPS_STATUS"]['COUNTER']) ret.doorOpen = any([cp.vl["DOORS"]['DOOR_OPEN_FL'], @@ -88,7 +30,7 @@ class CarState(CarStateBase): ret.gas = cp.vl["ACCEL_GAS_134"]['ACCEL_134'] ret.gasPressed = ret.gas > 1e-5 - self.esp_disabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1) + ret.espDisabled = (cp.vl["TRACTION_BUTTON"]['TRACTION_OFF'] == 1) ret.wheelSpeeds.fl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FL'] ret.wheelSpeeds.rr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RR'] @@ -121,3 +63,58 @@ class CarState(CarStateBase): self.lkas_status_ok = cp_cam.vl["LKAS_HEARTBIT"]['LKAS_STATUS_OK'] return ret + + @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), + ("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), + ] + + checks = [ + # sig_address, frequency + ("BRAKE_2", 50), + ("EPS_STATUS", 100), + ("SPEED_1", 100), + ("WHEEL_SPEEDS", 50), + ("STEERING", 100), + ("ACC_2", 50), + ] + + 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) + ] + checks = [] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py index 3c5a913f..c06c27e9 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/selfdrive/car/chrysler/chryslercan.py @@ -10,19 +10,10 @@ def calc_checksum(data): jeep chrysler canbus checksum from http://illmatics.com/Remote%20Car%20Hacking.pdf """ - end_index = len(data) - index = 0 checksum = 0xFF - temp_chk = 0 - bit_sum = 0 - if(end_index <= index): - return False - for index in range(0, end_index): + for curr in data[:-1]: shift = 0x80 - curr = data[index] - iterate = 8 - while(iterate > 0): - iterate -= 1 + for i in range(0, 8): bit_sum = curr & shift temp_chk = checksum & 0x80 if (bit_sum != 0): @@ -84,7 +75,6 @@ def create_lkas_command(packer, apply_steer, moving_fast, frame): } dat = packer.make_can_msg("LKAS_COMMAND", 0, values)[2] - dat = dat[:-1] checksum = calc_checksum(dat) values["CHECKSUM"] = checksum @@ -95,6 +85,6 @@ def create_wheel_buttons(frame): # WHEEL_BUTTONS (571) Message sent to cancel ACC. start = b"\x01" # acc cancel set counter = (frame % 10) << 4 - dat = start + counter.to_bytes(1, 'little') - dat = dat + calc_checksum(dat).to_bytes(1, 'little') + dat = start + counter.to_bytes(1, 'little') + b"\x00" + dat = dat[:-1] + calc_checksum(dat).to_bytes(1, 'little') return make_can_msg(0x23b, dat, 0) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 93220fd1..428b7f21 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -2,35 +2,11 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event -from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.chrysler.carstate import CarState, get_can_parser, get_camera_parser from selfdrive.car.chrysler.values import Ecu, ECU_FINGERPRINT, CAR, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase -GearShifter = car.CarState.GearShifter -ButtonType = car.CarState.ButtonEvent.Type - class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP - self.VM = VehicleModel(CP) - - self.gas_pressed_prev = False - self.brake_pressed_prev = False - self.cruise_enabled_prev = False - self.low_speed_alert = False - self.left_blinker_prev = False - self.right_blinker_prev = False - - # *** init the major players *** - self.CS = CarState(CP) - self.cp = get_can_parser(CP) - self.cp_cam = get_camera_parser(CP) - - self.CC = None - if CarController is not None: - self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera) @staticmethod def compute_gb(accel, speed): @@ -38,18 +14,10 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - - ret = car.CarParams.new_message() - + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "chrysler" - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay - ret.safetyModel = car.CarParams.SafetyModel.chrysler - # pedal - ret.enableCruise = True - # Speed conversion: 20, 45 mph ret.wheelbase = 3.089 # in meters for Pacifica Hybrid 2017 ret.steerRatio = 16.2 # Pacifica Hybrid 2017 @@ -69,43 +37,19 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.44 ret.minSteerSpeed = 3.8 # m/s - ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this - if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020_HYBRID, CAR.JEEP_CHEROKEE_2019): - ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. + if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019): # TODO allow 2019 cars to steer down to 13 m/s if already engaged. + ret.minSteerSpeed = 17.5 # m/s 17 on the way up, 13 on the way down once engaged. - # TODO: get actual value, for now starting with reasonable value for - # civic and scaling by mass and wheelbase + # starting with reasonable value for civic and scaling by mass and wheelbase ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) # TODO: start from empirically derived lateral slip stiffness for the civic and scale by # mass and CG position, so all cars will have approximately similar dyn behaviors ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront) - # no rear steering, at least on the listed cars above - ret.steerRatioRear = 0. - - # steer, gas, brake limitations VS speed - ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph - ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph - ret.gasMaxBP = [0.] - ret.gasMaxV = [0.5] - ret.brakeMaxBP = [5., 20.] - ret.brakeMaxV = [1., 0.8] - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay print("ECU Camera Simulated: {0}".format(ret.enableCamera)) - ret.openpilotLongitudinalControl = False - - ret.stoppingControl = False - ret.startAccel = 0.0 - - ret.longitudinalTuning.deadzoneBP = [0., 9.] - ret.longitudinalTuning.deadzoneV = [0., .15] - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] - ret.longitudinalTuning.kiBP = [0., 35.] - ret.longitudinalTuning.kiV = [0.54, 0.36] return ret @@ -123,53 +67,17 @@ class CarInterface(CarInterfaceBase): ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - # TODO: button presses - buttonEvents = [] - - if ret.leftBlinker != self.left_blinker_prev: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.leftBlinker - be.pressed = ret.leftBlinker != 0 - buttonEvents.append(be) - - if ret.rightBlinker != self.right_blinker_prev: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.rightBlinker - be.pressed = ret.rightBlinker != 0 - buttonEvents.append(be) - - ret.buttonEvents = buttonEvents - - self.low_speed_alert = (ret.vEgo < self.CP.minSteerSpeed) + ret.buttonEvents = [] # events - events = [] - if not (ret.gearShifter in (GearShifter.drive, GearShifter.low)): - events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.doorOpen: - events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.seatbeltUnlatched: - events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.esp_disabled: - events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if not ret.cruiseState.available: - events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gearShifter == GearShifter.reverse: - events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if self.CS.steer_error: - events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) + events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.low], gas_resume_speed = 2.) if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - # disable on gas pedal and speed isn't zero. Gas pedal is used to resume ACC - # from a 3+ second stop. - if (ret.gasPressed and (not self.gas_pressed_prev) and ret.vEgo > 2.0): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - - if self.low_speed_alert: + if ret.vEgo < self.CP.minSteerSpeed: events.append(create_event('belowSteerSpeed', [ET.WARNING])) ret.events = events @@ -177,8 +85,6 @@ class CarInterface(CarInterfaceBase): self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled - self.left_blinker_prev = ret.leftBlinker - self.right_blinker_prev = ret.rightBlinker # copy back carState packet to CS self.CS.out = ret.as_reader() diff --git a/selfdrive/car/chrysler/test_chryslercan.py b/selfdrive/car/chrysler/test_chryslercan.py index 637cdc9f..d4d41057 100644 --- a/selfdrive/car/chrysler/test_chryslercan.py +++ b/selfdrive/car/chrysler/test_chryslercan.py @@ -12,8 +12,8 @@ GearShifter = car.CarState.GearShifter class TestChryslerCan(unittest.TestCase): def test_checksum(self): - self.assertEqual(0x75, chryslercan.calc_checksum(b"\x01\x20")) - self.assertEqual(0xcc, chryslercan.calc_checksum(b"\x14\x00\x00\x00\x20")) + self.assertEqual(0x75, chryslercan.calc_checksum(b"\x01\x20\x00")) + self.assertEqual(0xcc, chryslercan.calc_checksum(b"\x14\x00\x00\x00\x20\x00")) def test_hud(self): packer = CANPacker('chrysler_pacifica_2017_hybrid') diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py index 1e9cdcc5..efbcc67b 100644 --- a/selfdrive/car/chrysler/values.py +++ b/selfdrive/car/chrysler/values.py @@ -15,7 +15,6 @@ class CAR: PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017" PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018" PACIFICA_2019_HYBRID = "CHRYSLER PACIFICA HYBRID 2019" - PACIFICA_2020_HYBRID = "CHRYSLER PACIFICA HYBRID 2020" PACIFICA_2018 = "CHRYSLER PACIFICA 2018" # Also covers Pacifica 2017. JEEP_CHEROKEE = "JEEP GRAND CHEROKEE V6 2018" # Also covers Tailhawk 2017. JEEP_CHEROKEE_2019 = "JEEP GRAND CHEROKEE 2019" @@ -54,12 +53,13 @@ FINGERPRINTS = { { 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1562: 8, 1570: 8 }, - # Based on "d26bf42deb1910e7|2020-02-13--16-22-31" - {168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1262: 8, 1284: 8, 1568: 8, 1902: 8, 2015: 8, 2016: 8, 2018: 8, 2023: 8, 2024: 8, 2026: 8, 2031: 8 + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 2015: 8, 2016: 8, 2024: 8 }, - ], - CAR.PACIFICA_2020_HYBRID: [ - {168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 524: 8, 526: 6, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 653: 8, 654: 8, 655: 8, 656: 4, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 2015: 8, 2016: 8, 2024: 8}, + # Based on "8190c7275a24557b|2020-02-24--09-57-23" + { + 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 650: 8, 656: 4, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 683: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 738: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 808: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1216: 8, 1218: 8, 1220: 8, 1225: 8, 1235: 8, 1242: 8, 1246: 8, 1250: 8, 1251: 8, 1252: 8, 1258: 8, 1259: 8, 1260: 8, 1262: 8, 1284: 8, 1568: 8, 1570: 8, 1856: 8, 1858: 8, 1860: 8, 1865: 8, 1875: 8, 1882: 8, 1886: 8, 1890: 8, 1891: 8, 1892: 8, 1898: 8, 1899: 8, 1900: 8, 1902: 8, 2015: 8, 2016: 8, 2018: 8, 2019: 8, 2020: 8, 2023: 8, 2024: 8, 2026: 8, 2027: 8, 2028: 8, 2031: 8 + } ], CAR.JEEP_CHEROKEE: [ # JEEP GRAND CHEROKEE V6 2018 @@ -88,9 +88,6 @@ DBC = { CAR.PACIFICA_2019_HYBRID: dbc_dict( # Same DBC file works. 'chrysler_pacifica_2017_hybrid', # 'pt' 'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar' - CAR.PACIFICA_2020_HYBRID: dbc_dict( # Same DBC file works. - 'chrysler_pacifica_2017_hybrid', # 'pt' - 'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar' CAR.JEEP_CHEROKEE: dbc_dict( # Same DBC file works. 'chrysler_pacifica_2017_hybrid', # 'pt' 'chrysler_pacifica_2017_hybrid_private_fusion'), # 'radar' diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index d708c135..4c0a37ab 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -8,12 +8,12 @@ MAX_STEER_DELTA = 1 TOGGLE_DEBUG = False class CarController(): - def __init__(self, dbc_name, enable_camera, vehicle_model): + def __init__(self, dbc_name, CP, VM): self.packer = CANPacker(dbc_name) - self.enable_camera = enable_camera + self.enable_camera = CP.enableCamera self.enabled_last = False self.main_on_last = False - self.vehicle_model = vehicle_model + self.vehicle_model = VM self.generic_toggle_last = 0 self.steer_alert_last = False self.lkas_action = 0 diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index ec461ab1..20341ac9 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -7,32 +7,6 @@ from selfdrive.car.ford.values import DBC WHEEL_RADIUS = 0.33 -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.), - ("Brake_Lights", "BCM_to_HS_Body", 0.), - ] - - checks = [ - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) - - class CarState(CarStateBase): def update(self, cp): ret = car.CarState.new_message() @@ -58,3 +32,25 @@ class CarState(CarStateBase): self.steer_error = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl'] return ret + + @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.), + ("Brake_Lights", "BCM_to_HS_Body", 0.), + ] + checks = [] + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index 6b1f9340..6a599240 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -3,31 +3,12 @@ from cereal import car from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event -from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.ford.carstate import CarState, get_can_parser from selfdrive.car.ford.values import MAX_ANGLE, Ecu, ECU_FINGERPRINT, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP - self.VM = VehicleModel(CP) - - self.frame = 0 - self.gas_pressed_prev = False - self.brake_pressed_prev = False - self.cruise_enabled_prev = False - - # *** init the major players *** - self.CS = CarState(CP) - - self.cp = get_can_parser(CP) - - self.CC = None - if CarController is not None: - self.CC = CarController(self.cp.dbc_name, CP.enableCamera, self.VM) @staticmethod def compute_gb(accel, speed): @@ -35,19 +16,11 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - - ret = car.CarParams.new_message() - + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "ford" - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay - ret.safetyModel = car.CarParams.SafetyModel.ford ret.dashcamOnly = True - # pedal - ret.enableCruise = True - ret.wheelbase = 2.85 ret.steerRatio = 14.8 ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG @@ -60,10 +33,6 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.44 tire_stiffness_factor = 0.5328 - # 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. - # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) @@ -73,32 +42,11 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - # no rear steering, at least on the listed cars above - ret.steerRatioRear = 0. ret.steerControlType = car.CarParams.SteerControlType.angle - # steer, gas, brake limitations VS speed - ret.steerMaxBP = [0.] # breakpoints at 1 and 40 kph - ret.steerMaxV = [1.0] # 2/3rd torque allowed above 45 kph - ret.gasMaxBP = [0.] - ret.gasMaxV = [0.5] - ret.brakeMaxBP = [5., 20.] - ret.brakeMaxV = [1., 0.8] - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay - ret.openpilotLongitudinalControl = False cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) - ret.stoppingControl = False - ret.startAccel = 0.0 - - ret.longitudinalTuning.deadzoneBP = [0., 9.] - ret.longitudinalTuning.deadzoneV = [0., .15] - ret.longitudinalTuning.kpBP = [0., 5., 35.] - ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] - ret.longitudinalTuning.kiBP = [0., 35.] - ret.longitudinalTuning.kiV = [0.54, 0.36] - return ret # returns a car.CarState @@ -111,10 +59,7 @@ class CarInterface(CarInterfaceBase): ret.canValid = self.cp.can_valid # events - events = [] - - if self.CS.steer_error: - events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) + events = self.create_common_events(ret) # enable request in prius is simple, as we activate when Toyota is active (rising edge) if ret.cruiseState.enabled and not self.cruise_enabled_prev: @@ -122,14 +67,6 @@ class CarInterface(CarInterfaceBase): elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - # disable on pedals rising edge or when brake is pressed and speed isn't zero - if (ret.gasPressed and not self.gas_pressed_prev) or \ - (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) - if self.CS.lkas_state not in [2, 3] and ret.vEgo > 13.* CV.MPH_TO_MS and ret.cruiseState.enabled: events.append(create_event('steerTempUnavailableMute', [ET.WARNING])) diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 96b7756c..95b970f1 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -85,16 +85,16 @@ def match_fw_to_car(fw_versions): ecu_type = ecu[0] addr = ecu[1:] found_version = fw_versions_dict.get(addr, None) - + ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa, Ecu.electricBrakeBooster] if ecu_type == Ecu.esp and candidate in [TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER] and found_version is None: continue # TODO: COROLLA_TSS2 engine can show on two different addresses - if ecu_type == Ecu.engine and candidate == TOYOTA.COROLLA_TSS2 and found_version is None: + if ecu_type == Ecu.engine and candidate in [TOYOTA.COROLLA_TSS2, TOYOTA.CHR] and found_version is None: continue - # Allow DSU not being present - if ecu_type in [Ecu.unknown, Ecu.dsu] and found_version is None: + # ignore non essential ecus + if ecu_type not in ESSENTIAL_ECUS and found_version is None: continue if found_version not in expected_versions: diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 53eccf1f..4fe0e5a1 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -4,7 +4,7 @@ from common.numpy_fast import interp from selfdrive.config import Conversions as CV from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.gm import gmcan -from selfdrive.car.gm.values import DBC, SUPERCRUISE_CARS +from selfdrive.car.gm.values import DBC, SUPERCRUISE_CARS, CanBus from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -69,22 +69,18 @@ def process_hud_alert(hud_alert): return steer class CarController(): - def __init__(self, canbus, car_fingerprint): + def __init__(self, dbc_name, CP, VM): self.pedal_steady = 0. self.start_time = 0. - self.steer_idx = 0 self.apply_steer_last = 0 - self.car_fingerprint = car_fingerprint + self.car_fingerprint = CP.carFingerprint self.lka_icon_status_last = (False, False) self.steer_rate_limited = False - # Setup detection helper. Routes commands to - # an appropriate CAN bus number. - self.canbus = canbus - self.params = CarControllerParams(car_fingerprint) + self.params = CarControllerParams(CP.carFingerprint) - self.packer_pt = CANPacker(DBC[car_fingerprint]['pt']) - self.packer_ch = CANPacker(DBC[car_fingerprint]['chassis']) + self.packer_pt = CANPacker(DBC[CP.carFingerprint]['pt']) + self.packer_ch = CANPacker(DBC[CP.carFingerprint]['chassis']) def update(self, enabled, CS, frame, actuators, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): @@ -93,7 +89,6 @@ class CarController(): # Send CAN commands. can_sends = [] - canbus = self.canbus alert_out = process_hud_alert(hud_alert) steer = alert_out @@ -101,10 +96,10 @@ class CarController(): ### STEER ### if (frame % P.STEER_STEP) == 0: - lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > P.MIN_STEER_SPEED + lkas_enabled = enabled and not CS.steer_warning and CS.out.vEgo > P.MIN_STEER_SPEED if lkas_enabled: new_steer = actuators.steer * P.STEER_MAX - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P) + 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 else: apply_steer = 0 @@ -114,10 +109,10 @@ class CarController(): if self.car_fingerprint in SUPERCRUISE_CARS: can_sends += gmcan.create_steering_control_ct6(self.packer_pt, - canbus, apply_steer, CS.v_ego, idx, lkas_enabled) + CanBus, apply_steer, CS.out.vEgo, idx, lkas_enabled) else: can_sends.append(gmcan.create_steering_control(self.packer_pt, - canbus.powertrain, apply_steer, idx, lkas_enabled)) + CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) ### GAS/BRAKE ### @@ -142,16 +137,16 @@ class CarController(): if (frame % 4) == 0: idx = (frame // 4) % 4 - at_full_stop = enabled and CS.standstill - near_stop = enabled and (CS.v_ego < P.NEAR_STOP_BRAKE_PHASE) - can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, canbus.chassis, apply_brake, idx, near_stop, at_full_stop)) + at_full_stop = enabled and CS.out.standstill + near_stop = enabled and (CS.out.vEgo < P.NEAR_STOP_BRAKE_PHASE) + can_sends.append(gmcan.create_friction_brake_command(self.packer_ch, CanBus.CHASSIS, apply_brake, idx, near_stop, at_full_stop)) - at_full_stop = enabled and CS.standstill - can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, canbus.powertrain, apply_gas, idx, enabled, at_full_stop)) + at_full_stop = enabled and CS.out.standstill + can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, apply_gas, idx, enabled, at_full_stop)) # Send dashboard UI commands (ACC status), 25hz if (frame % 4) == 0: - can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, canbus.powertrain, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car)) + can_sends.append(gmcan.create_acc_dashboard_command(self.packer_pt, CanBus.POWERTRAIN, enabled, hud_v_cruise * CV.MS_TO_KPH, hud_show_car)) # Radar needs to know current speed and yaw rate (50hz), # and that ADAS is alive (10hz) @@ -160,17 +155,17 @@ class CarController(): if frame % time_and_headlights_step == 0: idx = (frame // time_and_headlights_step) % 4 - can_sends.append(gmcan.create_adas_time_status(canbus.obstacle, int((tt - self.start_time) * 60), idx)) - can_sends.append(gmcan.create_adas_headlights_status(canbus.obstacle)) + can_sends.append(gmcan.create_adas_time_status(CanBus.OBSTACLE, int((tt - self.start_time) * 60), idx)) + can_sends.append(gmcan.create_adas_headlights_status(CanBus.OBSTACLE)) speed_and_accelerometer_step = 2 if frame % speed_and_accelerometer_step == 0: idx = (frame // speed_and_accelerometer_step) % 4 - can_sends.append(gmcan.create_adas_steering_status(canbus.obstacle, idx)) - can_sends.append(gmcan.create_adas_accelerometer_speed_status(canbus.obstacle, CS.v_ego, idx)) + can_sends.append(gmcan.create_adas_steering_status(CanBus.OBSTACLE, idx)) + can_sends.append(gmcan.create_adas_accelerometer_speed_status(CanBus.OBSTACLE, CS.out.vEgo, idx)) if frame % P.ADAS_KEEPALIVE_STEP == 0: - can_sends += gmcan.create_adas_keepalive(canbus.powertrain) + can_sends += gmcan.create_adas_keepalive(CanBus.POWERTRAIN) # Show green icon when LKA torque is applied, and # alarming orange icon when approaching torque limit. @@ -181,7 +176,7 @@ class CarController(): lka_icon_status = (lka_active, lka_critical) if frame % P.CAMERA_KEEPALIVE_STEP == 0 \ or lka_icon_status != self.lka_icon_status_last: - can_sends.append(gmcan.create_lka_icon_command(canbus.sw_gmlan, lka_active, lka_critical, steer)) + can_sends.append(gmcan.create_lka_icon_command(CanBus.SW_GMLAN, lka_active, lka_critical, steer)) self.lka_icon_status_last = lka_icon_status return can_sends diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index f924c66f..cb849c0d 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -4,52 +4,10 @@ from selfdrive.config import Conversions as CV 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, \ +from selfdrive.car.gm.values import DBC, CAR, AccState, CanBus, \ CruiseButtons, is_eps_status_ok, \ STEER_THRESHOLD, SUPERCRUISE_CARS -def get_powertrain_can_parser(CP, canbus): - # 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), - ("AcceleratorPedal", "AcceleratorPedal", 0), - ("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS), - ("SteeringWheelAngle", "PSCMSteeringAngle", 0), - ("FLWheelSpd", "EBCMWheelSpdFront", 0), - ("FRWheelSpd", "EBCMWheelSpdFront", 0), - ("RLWheelSpd", "EBCMWheelSpdRear", 0), - ("RRWheelSpd", "EBCMWheelSpdRear", 0), - ("PRNDL", "ECMPRDNL", 0), - ("LKADriverAppldTrq", "PSCMStatus", 0), - ("LKATorqueDeliveredStatus", "PSCMStatus", 0), - ] - - if CP.carFingerprint == CAR.VOLT: - signals += [ - ("RegenPaddle", "EBCMRegenPaddle", 0), - ] - if CP.carFingerprint in SUPERCRUISE_CARS: - signals += [ - ("ACCCmdActive", "ASCMActiveCruiseControlStatus", 0) - ] - else: - signals += [ - ("TractionControlOn", "ESPStatus", 0), - ("EPBClosed", "EPBStatus", 0), - ("CruiseMainOn", "ECMEngineStatus", 0), - ("CruiseState", "AcceleratorPedal2", 0), - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], canbus.powertrain) - class CarState(CarStateBase): def __init__(self, CP): @@ -58,73 +16,109 @@ class CarState(CarStateBase): self.shifter_values = can_define.dv["ECMPRDNL"]["PRNDL"] def update(self, pt_cp): + ret = car.CarState.new_message() + self.prev_cruise_buttons = self.cruise_buttons self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]['ACCButtons'] - self.v_wheel_fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS - self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS - self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS - self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS - self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]) - self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) - self.standstill = self.v_ego_raw < 0.01 + ret.wheelSpeeds.fl = pt_cp.vl["EBCMWheelSpdFront"]['FLWheelSpd'] * CV.KPH_TO_MS + ret.wheelSpeeds.fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS + ret.wheelSpeeds.rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS + ret.wheelSpeeds.rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS + ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgoRaw < 0.01 - self.angle_steers = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle'] - self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None)) - self.user_brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] + ret.steeringAngle = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle'] + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None)) + 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. - self.pedal_gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] - self.user_gas_pressed = self.pedal_gas > 0 + ret.gas = pt_cp.vl["AcceleratorPedal"]['AcceleratorPedal'] / 254. + ret.gasPressed = ret.gas > 1e-5 - self.steer_torque_driver = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq'] - self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD - - # 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed - self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus'] - self.steer_not_allowed = not is_eps_status_ok(self.lkas_status, self.car_fingerprint) + ret.steeringTorque = pt_cp.vl["PSCMStatus"]['LKADriverAppldTrq'] + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD # 1 - open, 0 - closed - self.door_all_closed = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 0 and - pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 0 and - pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 0 and - pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 0) + ret.doorOpen = (pt_cp.vl["BCMDoorBeltStatus"]['FrontLeftDoor'] == 1 or + pt_cp.vl["BCMDoorBeltStatus"]['FrontRightDoor'] == 1 or + pt_cp.vl["BCMDoorBeltStatus"]['RearLeftDoor'] == 1 or + pt_cp.vl["BCMDoorBeltStatus"]['RearRightDoor'] == 1) # 1 - latched - self.seatbelt = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 1 - - self.steer_error = False - - self.brake_error = False - - self.prev_left_blinker_on = self.left_blinker_on - self.prev_right_blinker_on = self.right_blinker_on - self.left_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1 - self.right_blinker_on = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2 + ret.seatbeltUnlatched = pt_cp.vl["BCMDoorBeltStatus"]['LeftSeatBelt'] == 0 + ret.leftBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 1 + ret.rightBlinker = pt_cp.vl["BCMTurnSignals"]['TurnSignals'] == 2 if self.car_fingerprint in SUPERCRUISE_CARS: self.park_brake = False - self.main_on = False - self.acc_active = pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive'] - self.esp_disabled = False - self.regen_pressed = False - self.pcm_acc_status = int(self.acc_active) + ret.cruiseState.available = False + ret.espDisabled = False + regen_pressed = False + self.pcm_acc_status = int(pt_cp.vl["ASCMActiveCruiseControlStatus"]['ACCCmdActive']) else: self.park_brake = pt_cp.vl["EPBStatus"]['EPBClosed'] - self.main_on = pt_cp.vl["ECMEngineStatus"]['CruiseMainOn'] - self.acc_active = False - self.esp_disabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1 + ret.cruiseState.available = bool(pt_cp.vl["ECMEngineStatus"]['CruiseMainOn']) + ret.espDisabled = pt_cp.vl["ESPStatus"]['TractionControlOn'] != 1 self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]['CruiseState'] if self.car_fingerprint == CAR.VOLT: - self.regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle']) + regen_pressed = bool(pt_cp.vl["EBCMRegenPaddle"]['RegenPaddle']) else: - self.regen_pressed = False - - # Brake pedal's potentiometer returns near-zero reading - # even when pedal is not pressed. - if self.user_brake < 10: - self.user_brake = 0 + regen_pressed = False # Regen braking is braking - self.brake_pressed = self.user_brake > 10 or self.regen_pressed + ret.brakePressed = ret.brake > 1e-5 or regen_pressed + ret.cruiseState.enabled = self.pcm_acc_status != AccState.OFF + ret.cruiseState.standstill = self.pcm_acc_status == AccState.STANDSTILL - self.gear_shifter_valid = self.gear_shifter == car.CarState.GearShifter.drive + # 0 - inactive, 1 - active, 2 - temporary limited, 3 - failed + self.lkas_status = pt_cp.vl["PSCMStatus"]['LKATorqueDeliveredStatus'] + self.steer_warning = not is_eps_status_ok(self.lkas_status, self.car_fingerprint) + + return ret + + @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), + ("AcceleratorPedal", "AcceleratorPedal", 0), + ("ACCButtons", "ASCMSteeringButton", CruiseButtons.UNPRESS), + ("SteeringWheelAngle", "PSCMSteeringAngle", 0), + ("FLWheelSpd", "EBCMWheelSpdFront", 0), + ("FRWheelSpd", "EBCMWheelSpdFront", 0), + ("RLWheelSpd", "EBCMWheelSpdRear", 0), + ("RRWheelSpd", "EBCMWheelSpdRear", 0), + ("PRNDL", "ECMPRDNL", 0), + ("LKADriverAppldTrq", "PSCMStatus", 0), + ("LKATorqueDeliveredStatus", "PSCMStatus", 0), + ] + + if CP.carFingerprint == CAR.VOLT: + signals += [ + ("RegenPaddle", "EBCMRegenPaddle", 0), + ] + if CP.carFingerprint in SUPERCRUISE_CARS: + signals += [ + ("ACCCmdActive", "ASCMActiveCruiseControlStatus", 0) + ] + else: + signals += [ + ("TractionControlOn", "ESPStatus", 0), + ("EPBClosed", "EPBStatus", 0), + ("CruiseMainOn", "ECMEngineStatus", 0), + ("CruiseState", "AcceleratorPedal2", 0), + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, [], CanBus.POWERTRAIN) diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index 3db0f5b0..d1e47935 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -11,7 +11,7 @@ def create_steering_control(packer, bus, apply_steer, idx, lkas_active): return packer.make_can_msg("ASCMLKASteeringCmd", bus, values) -def create_steering_control_ct6(packer, canbus, apply_steer, v_ego, idx, enabled): +def create_steering_control_ct6(packer, CanBus, apply_steer, v_ego, idx, enabled): values = { "LKASteeringCmdActive": 1 if enabled else 0, @@ -31,10 +31,10 @@ def create_steering_control_ct6(packer, canbus, apply_steer, v_ego, idx, enabled # pack again with checksum dat = packer.make_can_msg("ASCMLKASteeringCmd", 0, values)[2] - return [0x152, 0, dat, canbus.powertrain], \ - [0x154, 0, dat, canbus.powertrain], \ - [0x151, 0, dat, canbus.chassis], \ - [0x153, 0, dat, canbus.chassis] + return [0x152, 0, dat, CanBus.POWERTRAIN], \ + [0x154, 0, dat, CanBus.POWERTRAIN], \ + [0x151, 0, dat, CanBus.CHASSIS], \ + [0x153, 0, dat, CanBus.CHASSIS] def create_adas_keepalive(bus): diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 450fce85..48b744a9 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -2,41 +2,14 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET -from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.gm.values import DBC, CAR, Ecu, ECU_FINGERPRINT, \ +from selfdrive.car.gm.values import CAR, Ecu, ECU_FINGERPRINT, CruiseButtons, \ SUPERCRUISE_CARS, AccState, FINGERPRINTS -from selfdrive.car.gm.carstate import CarState, CruiseButtons, get_powertrain_can_parser from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase ButtonType = car.CarState.ButtonEvent.Type -class CanBus(CarInterfaceBase): - def __init__(self): - self.powertrain = 0 - self.obstacle = 1 - self.chassis = 2 - self.sw_gmlan = 3 - class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP - - self.frame = 0 - self.gas_pressed_prev = False - self.brake_pressed_prev = False - self.acc_active_prev = 0 - - # *** init the major players *** - canbus = CanBus() - self.CS = CarState(CP) - self.VM = VehicleModel(CP) - self.pt_cp = get_powertrain_can_parser(CP, canbus) - self.ch_cp_dbc_name = DBC[CP.carFingerprint]['chassis'] - - self.CC = None - if CarController is not None: - self.CC = CarController(canbus, CP.carFingerprint) @staticmethod def compute_gb(accel, speed): @@ -44,13 +17,11 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - ret = car.CarParams.new_message() - + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "gm" - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay + ret.safetyModel = car.CarParams.SafetyModel.gm # default to gm + ret.enableCruise = False # stock cruise control is kept off - ret.enableCruise = False # GM port is considered a community feature, since it disables AEB; # TODO: make a port that uses a car harness and it only intercepts the camera ret.communityFeature = True @@ -75,7 +46,6 @@ class CarInterface(CarInterfaceBase): # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1607. + STD_CARGO_KG - ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.69 ret.steerRatio = 15.7 ret.steerRatioRear = 0. @@ -85,7 +55,6 @@ class CarInterface(CarInterfaceBase): # supports stop and go, but initial engage must be above 18mph (which include conservatism) ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1496. + STD_CARGO_KG - ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.83 ret.steerRatio = 15.8 ret.steerRatioRear = 0. @@ -97,14 +66,12 @@ class CarInterface(CarInterfaceBase): # Remaining parameters copied from Volt for now ret.centerToFront = ret.wheelbase * 0.4 ret.minEnableSpeed = 18 * CV.MPH_TO_MS - ret.safetyModel = car.CarParams.SafetyModel.gm ret.steerRatio = 15.7 ret.steerRatioRear = 0. elif candidate == CAR.ACADIA: ret.minEnableSpeed = -1. # engage speed is decided by pcm ret.mass = 4353. * CV.LB_TO_KG + STD_CARGO_KG - ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.86 ret.steerRatio = 14.4 #end to end is 13.46 ret.steerRatioRear = 0. @@ -113,7 +80,6 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.BUICK_REGAL: ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 3779. * CV.LB_TO_KG + STD_CARGO_KG # (3849+3708)/2 - ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.83 #111.4 inches in meters ret.steerRatio = 14.4 # guess for tourx ret.steerRatioRear = 0. @@ -122,7 +88,6 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.CADILLAC_ATS: ret.minEnableSpeed = 18 * CV.MPH_TO_MS ret.mass = 1601. + STD_CARGO_KG - ret.safetyModel = car.CarParams.SafetyModel.gm ret.wheelbase = 2.78 ret.steerRatio = 15.3 ret.steerRatioRear = 0. @@ -138,7 +103,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatioRear = 0. # TODO: there is RAS on this car! ret.centerToFront = ret.wheelbase * 0.465 - # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) @@ -148,95 +112,31 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - ret.steerMaxBP = [0.] # m/s - ret.steerMaxV = [1.] - ret.gasMaxBP = [0.] - ret.gasMaxV = [.5] - ret.brakeMaxBP = [0.] - ret.brakeMaxV = [1.] - ret.longitudinalTuning.kpBP = [5., 35.] ret.longitudinalTuning.kpV = [2.4, 1.5] ret.longitudinalTuning.kiBP = [0.] ret.longitudinalTuning.kiV = [0.36] - ret.longitudinalTuning.deadzoneBP = [0.] - ret.longitudinalTuning.deadzoneV = [0.] ret.stoppingControl = True ret.startAccel = 0.8 ret.steerLimitTimer = 0.4 ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz - ret.steerControlType = car.CarParams.SteerControlType.torque return ret # returns a car.CarState def update(self, c, can_strings): - self.pt_cp.update_strings(can_strings) + self.cp.update_strings(can_strings) - self.CS.update(self.pt_cp) + ret = self.CS.update(self.cp) - # create message - ret = car.CarState.new_message() - - ret.canValid = self.pt_cp.can_valid - - # speeds - ret.vEgo = self.CS.v_ego - ret.aEgo = self.CS.a_ego - ret.vEgoRaw = self.CS.v_ego_raw - ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) - ret.standstill = self.CS.standstill - ret.wheelSpeeds.fl = self.CS.v_wheel_fl - ret.wheelSpeeds.fr = self.CS.v_wheel_fr - ret.wheelSpeeds.rl = self.CS.v_wheel_rl - ret.wheelSpeeds.rr = self.CS.v_wheel_rr - - # gas pedal information. - ret.gas = self.CS.pedal_gas / 254.0 - ret.gasPressed = self.CS.user_gas_pressed - - # brake pedal - ret.brake = self.CS.user_brake / 0xd0 - ret.brakePressed = self.CS.brake_pressed - - # steering wheel - ret.steeringAngle = self.CS.angle_steers - - # torque and user override. Driver awareness - # timer resets when the user uses the steering wheel. - ret.steeringPressed = self.CS.steer_override - ret.steeringTorque = self.CS.steer_torque_driver + ret.canValid = self.cp.can_valid + ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - # cruise state - ret.cruiseState.available = bool(self.CS.main_on) - cruiseEnabled = self.CS.pcm_acc_status != AccState.OFF - ret.cruiseState.enabled = cruiseEnabled - ret.cruiseState.standstill = self.CS.pcm_acc_status == 4 - - ret.leftBlinker = self.CS.left_blinker_on - ret.rightBlinker = self.CS.right_blinker_on - ret.doorOpen = not self.CS.door_all_closed - ret.seatbeltUnlatched = not self.CS.seatbelt - ret.gearShifter = self.CS.gear_shifter - buttonEvents = [] - # blinkers - if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.leftBlinker - be.pressed = self.CS.left_blinker_on - buttonEvents.append(be) - - if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.rightBlinker - be.pressed = self.CS.right_blinker_on - buttonEvents.append(be) - if self.CS.cruise_buttons != self.CS.prev_cruise_buttons and self.CS.prev_cruise_buttons != CruiseButtons.INIT: be = car.CarState.ButtonEvent.new_message() be.type = ButtonType.unknown @@ -247,7 +147,7 @@ class CarInterface(CarInterfaceBase): be.pressed = False but = self.CS.prev_cruise_buttons if but == CruiseButtons.RES_ACCEL: - if not (cruiseEnabled and self.CS.standstill): + if not (ret.cruiseState.enabled and ret.standstill): be.type = ButtonType.accelCruise # Suppress resume button if we're resuming from stop so we don't adjust speed. elif but == CruiseButtons.DECEL_SET: be.type = ButtonType.decelCruise @@ -259,43 +159,20 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = buttonEvents - events = [] - if self.CS.steer_error: - events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) - if self.CS.steer_not_allowed: - events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) - if ret.doorOpen: - events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.seatbeltUnlatched: - events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + events = self.create_common_events(ret) if self.CS.car_fingerprint in SUPERCRUISE_CARS: - if self.CS.acc_active and not self.acc_active_prev: + if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) - if not self.CS.acc_active: + if not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) else: - if self.CS.brake_error: - events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) - if not self.CS.gear_shifter_valid: - events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.esp_disabled: - events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if not self.CS.main_on: - events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) - if self.CS.gear_shifter == 3: - events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + # TODO: why is this only not supercruise? ignore supercruise? if ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) if self.CS.park_brake: events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) - # disable on pedals rising edge or when brake is pressed and speed isn't zero - if (ret.gasPressed and not self.gas_pressed_prev) or \ - (ret.brakePressed): # and (not self.brake_pressed_prev or ret.vEgo > 0.001)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) if ret.cruiseState.standstill: events.append(create_event('resumeRequired', [ET.WARNING])) if self.CS.pcm_acc_status == AccState.FAULTED: @@ -313,15 +190,15 @@ class CarInterface(CarInterfaceBase): ret.events = events # update previous brake/gas pressed - self.acc_active_prev = self.CS.acc_active + self.cruise_enabled_prev = ret.cruiseState.enabled self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed - # cast to reader so it can't be modified - return ret.as_reader() + # copy back carState packet to CS + self.CS.out = ret.as_reader() + + return self.CS.out - # pass in a car.CarControl - # to be called @ 100hz def apply(self, c): hud_v_cruise = c.hudControl.setSpeed if hud_v_cruise > 70: @@ -329,7 +206,7 @@ class CarInterface(CarInterfaceBase): # For Openpilot, "enabled" includes pre-enable. # In GM, PCM faults out if ACC command overlaps user gas. - enabled = c.enabled and not self.CS.user_gas_pressed + enabled = c.enabled and not self.CS.out.gasPressed can_sends = self.CC.update(enabled, self.CS, self.frame, \ c.actuators, diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index d36db942..b20706a0 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -4,8 +4,7 @@ import math import time from cereal import car from opendbc.can.parser import CANParser -from selfdrive.car.gm.interface import CanBus -from selfdrive.car.gm.values import DBC, CAR +from selfdrive.car.gm.values import DBC, CAR, CanBus from selfdrive.config import Conversions as CV from selfdrive.car.interfaces import RadarInterfaceBase @@ -17,7 +16,7 @@ NUM_SLOTS = 20 # messages that are present in DBC LAST_RADAR_MSG = RADAR_HEADER_MSG + NUM_SLOTS -def create_radar_can_parser(canbus, car_fingerprint): +def create_radar_can_parser(car_fingerprint): dbc_f = DBC[car_fingerprint]['radar'] if car_fingerprint in (CAR.VOLT, CAR.MALIBU, CAR.HOLDEN_ASTRA, CAR.ACADIA, CAR.CADILLAC_ATS): @@ -38,7 +37,7 @@ def create_radar_can_parser(canbus, car_fingerprint): checks = [] - return CANParser(dbc_f, signals, checks, canbus.obstacle) + return CANParser(dbc_f, signals, checks, CanBus.OBSTACLE) else: return None @@ -49,9 +48,7 @@ class RadarInterface(RadarInterfaceBase): self.delay = 0 # Delay of radar - canbus = CanBus() - print("Using %d as obstacle CAN bus ID" % canbus.obstacle) - self.rcp = create_radar_can_parser(canbus, CP.carFingerprint) + self.rcp = create_radar_can_parser(CP.carFingerprint) self.trigger_msg = LAST_RADAR_MSG self.updated_messages = set() diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py index ccde62ea..c48ed4e1 100644 --- a/selfdrive/car/gm/values.py +++ b/selfdrive/car/gm/values.py @@ -27,6 +27,12 @@ class AccState: FAULTED = 3 STANDSTILL = 4 +class CanBus: + POWERTRAIN = 0 + OBSTACLE = 1 + CHASSIS = 2 + SW_GMLAN = 3 + def is_eps_status_ok(eps_status, car_fingerprint): valid_eps_status = [] if car_fingerprint in SUPERCRUISE_CARS: diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 9b0e0706..3da8a7b3 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -2,7 +2,7 @@ from collections import namedtuple from cereal import car from common.realtime import DT_CTRL from selfdrive.controls.lib.drive_helpers import rate_limit -from common.numpy_fast import clip +from common.numpy_fast import clip, interp from selfdrive.car import create_gas_command from selfdrive.car.honda import hondacan from selfdrive.car.honda.values import CruiseButtons, CAR, VISUAL_HUD @@ -30,7 +30,7 @@ def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): brake_steady = brake + brake_hyst_gap brake = brake_steady - if (car_fingerprint in (CAR.ACURA_ILX, CAR.CRV)) and brake > 0.0: + if (car_fingerprint in (CAR.ACURA_ILX, CAR.CRV, CAR.CRV_EU)) and brake > 0.0: brake += 0.15 return brake, braking, brake_steady @@ -74,9 +74,18 @@ HUDData = namedtuple("HUDData", ["pcm_accel", "v_cruise", "car", "lanes", "fcw", "acc_alert", "steer_required"]) +class CarControllerParams(): + def __init__(self, CP): + self.BRAKE_MAX = 1024//4 + self.STEER_MAX = CP.lateralParams.torqueBP[-1] + # mirror of list (assuming first item is zero) for interp of signed request values + assert(CP.lateralParams.torqueBP[0] == 0) + assert(CP.lateralParams.torqueBP[0] == 0) + self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP) + self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV) class CarController(): - def __init__(self, dbc_name, CP): + def __init__(self, dbc_name, CP, VM): self.braking = False self.brake_steady = 0. self.brake_last = 0. @@ -84,21 +93,20 @@ class CarController(): self.last_pump_ts = 0. self.packer = CANPacker(dbc_name) self.new_radar_config = False - self.eps_modified = False - for fw in CP.carFw: - if fw.ecu == "eps" and b"," in fw.fwVersion: - print("EPS FW MODIFIED!") - self.eps_modified = True + + self.params = CarControllerParams(CP) def update(self, enabled, CS, frame, actuators, \ pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel, \ hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert): + P = self.params + # *** apply brake hysteresis *** - brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.v_ego, CS.CP.carFingerprint) + brake, self.braking, self.brake_steady = actuator_hystereses(actuators.brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint) # *** no output if not enabled *** - if not enabled and CS.pcm_acc_status: + if not enabled and CS.out.cruiseState.enabled: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = True @@ -126,29 +134,10 @@ class CarController(): # **** process the car messages **** - # *** compute control surfaces *** - BRAKE_MAX = 1024//4 - if CS.CP.carFingerprint in (CAR.ACURA_ILX): - STEER_MAX = 0xF00 - elif CS.CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX): - STEER_MAX = 0x3e8 # CR-V only uses 12-bits and requires a lower value - elif CS.CP.carFingerprint in (CAR.ODYSSEY_CHN): - STEER_MAX = 0x7FFF - elif CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified: - STEER_MAX = 0x1400 - else: - STEER_MAX = 0x1000 - # steer torque is converted back to CAN reference (positive when steering right) apply_gas = clip(actuators.gas, 0., 1.) - apply_brake = int(clip(self.brake_last * BRAKE_MAX, 0, BRAKE_MAX - 1)) - apply_steer = int(clip(-actuators.steer * STEER_MAX, -STEER_MAX, STEER_MAX)) - - if CS.CP.carFingerprint in (CAR.CIVIC) and self.eps_modified: - if apply_steer > 0xA00: - apply_steer = (apply_steer - 0xA00) / 2 + 0xA00 - elif apply_steer < -0xA00: - apply_steer = (apply_steer + 0xA00) / 2 - 0xA00 + apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1)) + 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 @@ -169,7 +158,7 @@ class CarController(): # If using stock ACC, spam cancel command to kill gas when OP disengages. if pcm_cancel_cmd: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) - elif CS.stopped: + elif CS.out.cruiseState.standstill: can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) else: diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index f7b8d577..08e44c02 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -1,3 +1,4 @@ +from cereal import car from collections import defaultdict from common.numpy_fast import interp from opendbc.can.can_define import CANDefine @@ -69,7 +70,7 @@ def get_can_signals(CP): ("SCM_BUTTONS", 25), ] - if CP.carFingerprint == CAR.CRV_HYBRID: + if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL): checks += [ ("GEARBOX", 50), ] @@ -80,7 +81,7 @@ def get_can_signals(CP): if CP.radarOffCan: # Civic is only bosch to use the same brake message as other hondas. - if CP.carFingerprint not in (CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): + if CP.carFingerprint not in (CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT): signals += [("BRAKE_PRESSED", "BRAKE_MODULE", 0)] checks += [("BRAKE_MODULE", 50)] signals += [("CAR_GAS", "GAS_PEDAL_2", 0), @@ -102,7 +103,7 @@ def get_can_signals(CP): else: checks += [("CRUISE_PARAMS", 50)] - if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): + if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT): signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)] elif CP.carFingerprint == CAR.ODYSSEY_CHN: signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1)] @@ -122,7 +123,7 @@ def get_can_signals(CP): elif CP.carFingerprint == CAR.ACURA_ILX: signals += [("CAR_GAS", "GAS_PEDAL_2", 0), ("MAIN_ON", "SCM_BUTTONS", 0)] - elif CP.carFingerprint in (CAR.CRV, CAR.ACURA_RDX, CAR.PILOT_2019, CAR.RIDGELINE): + elif CP.carFingerprint in (CAR.CRV, CAR.CRV_EU, CAR.ACURA_RDX, CAR.PILOT_2019, CAR.RIDGELINE): signals += [("MAIN_ON", "SCM_BUTTONS", 0)] elif CP.carFingerprint == CAR.FIT: signals += [("CAR_GAS", "GAS_PEDAL_2", 0), @@ -149,37 +150,6 @@ def get_can_signals(CP): return signals, checks -def get_can_parser(CP): - signals, checks = get_can_signals(CP) - bus_pt = 1 if CP.isPandaBlack and CP.carFingerprint in HONDA_BOSCH else 0 - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_pt) - - -def get_cam_can_parser(CP): - signals = [] - - if CP.carFingerprint in HONDA_BOSCH: - signals += [("ACCEL_COMMAND", "ACC_CONTROL", 0), - ("AEB_STATUS", "ACC_CONTROL", 0)] - else: - 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)] - - - # all hondas except CRV, RDX and 2019 Odyssey@China use 0xe4 for steering - checks = [(0xe4, 100)] - if CP.carFingerprint in [CAR.CRV, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]: - checks = [(0x194, 100)] - - bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2 - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam) - class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) @@ -193,9 +163,9 @@ class CarState(CarStateBase): self.cruise_setting = 0 self.v_cruise_pcm_prev = 0 self.cruise_mode = 0 - self.stopped = 0 def update(self, cp, cp_cam): + ret = car.CarState.new_message() # car params v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping @@ -204,21 +174,19 @@ class CarState(CarStateBase): # update prevs, update must run once per loop self.prev_cruise_buttons = self.cruise_buttons self.prev_cruise_setting = self.cruise_setting - self.prev_left_blinker_on = self.left_blinker_on - self.prev_right_blinker_on = self.right_blinker_on # ******************* parse out can ******************* - if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): # TODO: find wheels moving bit in dbc - self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 - self.door_all_closed = not cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN'] + if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT): # TODO: find wheels moving bit in dbc + ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 + ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]['DRIVERS_DOOR_OPEN']) elif self.CP.carFingerprint == CAR.ODYSSEY_CHN: - self.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 - self.door_all_closed = not cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN'] + ret.standstill = cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] < 0.1 + ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]['DRIVERS_DOOR_OPEN']) else: - self.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING'] - self.door_all_closed = not any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'], - cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']]) - self.seatbelt = not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] and cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED'] + ret.standstill = not cp.vl["STANDSTILL"]['WHEELS_MOVING'] + ret.doorOpen = any([cp.vl["DOORS_STATUS"]['DOOR_OPEN_FL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_FR'], + cp.vl["DOORS_STATUS"]['DOOR_OPEN_RL'], cp.vl["DOORS_STATUS"]['DOOR_OPEN_RR']]) + ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LAMP'] or not cp.vl["SEATBELT_STATUS"]['SEATBELT_DRIVER_LATCHED']) steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]['STEER_STATUS']] self.steer_error = steer_status not in ['NORMAL', 'NO_TORQUE_ALERT_1', 'NO_TORQUE_ALERT_2', 'LOW_SPEED_LOCKOUT', 'TMP_FAULT'] @@ -231,114 +199,146 @@ class CarState(CarStateBase): self.brake_error = 0 else: self.brake_error = cp.vl["STANDSTILL"]['BRAKE_ERROR_1'] or cp.vl["STANDSTILL"]['BRAKE_ERROR_2'] - self.esp_disabled = cp.vl["VSA_STATUS"]['ESP_DISABLED'] + ret.espDisabled = cp.vl["VSA_STATUS"]['ESP_DISABLED'] != 0 speed_factor = SPEED_FACTOR[self.CP.carFingerprint] - self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor - self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor - self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor - self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor - v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr)/4. + ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS * speed_factor + ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor + ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor + ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor + v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr)/4. # blend in transmission speed at low speed, since it has more low speed accuracy - self.v_weight = interp(v_wheel, v_weight_bp, v_weight_v) - self.v_ego_raw = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \ - self.v_weight * v_wheel + v_weight = interp(v_wheel, v_weight_bp, v_weight_v) + ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) + ret.steeringAngle = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] + ret.steeringRate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] + + self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING'] + self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS'] + + ret.leftBlinker = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] != 0 + ret.rightBlinker = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] != 0 + self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE'] + + if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT): + self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 + main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON'] + elif self.CP.carFingerprint == CAR.ODYSSEY_CHN: + self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 + main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] + else: + self.park_brake = 0 # TODO + main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] + + gear = int(cp.vl["GEARBOX"]['GEAR_SHIFTER']) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None)) + + self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS'] + # crv doesn't include cruise control + if self.CP.carFingerprint in (CAR.CRV, CAR.CRV_EU, CAR.ODYSSEY, CAR.ACURA_RDX, CAR.RIDGELINE, CAR.PILOT_2019, CAR.ODYSSEY_CHN): + ret.gas = self.pedal_gas / 256. + else: + ret.gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] / 256. # 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: self.user_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2. - self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change - - self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR'] - self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] - self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] - - self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING'] - self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS'] - - self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] - self.left_blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] - self.right_blinker_on = cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] - self.brake_hold = cp.vl["VSA_STATUS"]['BRAKE_HOLD_ACTIVE'] - - if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_HYBRID): - self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 - self.main_on = cp.vl["SCM_FEEDBACK"]['MAIN_ON'] - elif self.CP.carFingerprint == CAR.ODYSSEY_CHN: - self.park_brake = cp.vl["EPB_STATUS"]['EPB_STATE'] != 0 - self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] + self.user_gas_pressed = self.user_gas > 1e-5 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change + ret.gasPressed = self.user_gas_pressed else: - self.park_brake = 0 # TODO - self.main_on = cp.vl["SCM_BUTTONS"]['MAIN_ON'] + ret.gasPressed = self.pedal_gas > 1e-5 - can_gear_shifter = int(cp.vl["GEARBOX"]['GEAR_SHIFTER']) - self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None)) + ret.steeringTorque = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR'] + ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]['MOTOR_TORQUE'] + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.CP.carFingerprint] - self.pedal_gas = cp.vl["POWERTRAIN_DATA"]['PEDAL_GAS'] - # crv doesn't include cruise control - if self.CP.carFingerprint in (CAR.CRV, CAR.ODYSSEY, CAR.ACURA_RDX, CAR.RIDGELINE, CAR.PILOT_2019, CAR.ODYSSEY_CHN): - self.car_gas = self.pedal_gas - else: - self.car_gas = cp.vl["GAS_PEDAL_2"]['CAR_GAS'] - - self.steer_torque_driver = cp.vl["STEER_STATUS"]['STEER_TORQUE_SENSOR'] - self.steer_torque_motor = cp.vl["STEER_MOTOR_TORQUE"]['MOTOR_TORQUE'] - self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.CP.carFingerprint] - - self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] + self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != 0 if self.CP.radarOffCan: self.cruise_mode = cp.vl["ACC_HUD"]['CRUISE_CONTROL_LABEL'] - self.stopped = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252. - self.cruise_speed_offset = calc_cruise_offset(0, self.v_ego) - if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.ACCORDH, CAR.CRV_HYBRID): - self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] - self.brake_pressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] or \ + ret.cruiseState.standstill = cp.vl["ACC_HUD"]['CRUISE_SPEED'] == 252. + ret.cruiseState.speedOffset = calc_cruise_offset(0, ret.vEgo) + if self.CP.carFingerprint in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.ACCORDH, CAR.CRV_HYBRID, CAR.INSIGHT): + ret.brakePressed = cp.vl["POWERTRAIN_DATA"]['BRAKE_PRESSED'] != 0 or \ (self.brake_switch and self.brake_switch_prev and \ cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] != self.brake_switch_ts) self.brake_switch_prev = self.brake_switch self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] else: - self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] + ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0 # On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this. - self.v_cruise_pcm = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED'] - self.v_cruise_pcm_prev = self.v_cruise_pcm + ret.cruiseState.speed = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]['CRUISE_SPEED'] > 160.0 else cp.vl["ACC_HUD"]['CRUISE_SPEED'] * CV.KPH_TO_MS + self.v_cruise_pcm_prev = ret.cruiseState.speed else: - self.brake_switch = cp.vl["POWERTRAIN_DATA"]['BRAKE_SWITCH'] - self.cruise_speed_offset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], self.v_ego) - self.v_cruise_pcm = cp.vl["CRUISE"]['CRUISE_SPEED_PCM'] + ret.cruiseState.speedOffset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]['CRUISE_SPEED_OFFSET'], ret.vEgo) + ret.cruiseState.speed = cp.vl["CRUISE"]['CRUISE_SPEED_PCM'] * CV.KPH_TO_MS # brake switch has shown some single time step noise, so only considered when # switch is on for at least 2 consecutive CAN samples - self.brake_pressed = 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_ts) + 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_ts)) self.brake_switch_prev = self.brake_switch self.brake_switch_ts = cp.ts["POWERTRAIN_DATA"]['BRAKE_SWITCH'] - self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE'] - self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS'] + ret.brake = cp.vl["VSA_STATUS"]['USER_BRAKE'] + ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS'] != 0 + ret.cruiseState.available = bool(main_on) and self.cruise_mode == 0 # 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.RIDGELINE): - if self.user_brake > 0.05: - self.brake_pressed = 1 + if ret.brake > 0.05: + ret.brakePressed = True # TODO: discover the CAN msg that has the imperial unit bit for all other cars self.is_metric = not cp.vl["HUD_SETTING"]['IMPERIAL_UNIT'] if self.CP.carFingerprint in (CAR.CIVIC) else False if self.CP.carFingerprint in HONDA_BOSCH: - self.stock_aeb = bool(cp_cam.vl["ACC_CONTROL"]["AEB_STATUS"] and cp_cam.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5) + ret.stockAeb = bool(cp_cam.vl["ACC_CONTROL"]["AEB_STATUS"] and cp_cam.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5) else: - self.stock_aeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5) + ret.stockAeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5) if self.CP.carFingerprint in HONDA_BOSCH: self.stock_hud = False - self.stock_fcw = False + ret.stockFcw = False else: - self.stock_fcw = bool(cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0) + ret.stockFcw = cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0 self.stock_hud = cp_cam.vl["ACC_HUD"] self.stock_brake = cp_cam.vl["BRAKE_COMMAND"] + + return ret + + @staticmethod + def get_can_parser(CP): + signals, checks = get_can_signals(CP) + bus_pt = 1 if CP.isPandaBlack and CP.carFingerprint in HONDA_BOSCH else 0 + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_pt) + + @staticmethod + def get_cam_can_parser(CP): + signals = [] + + if CP.carFingerprint in HONDA_BOSCH: + signals += [("ACCEL_COMMAND", "ACC_CONTROL", 0), + ("AEB_STATUS", "ACC_CONTROL", 0)] + else: + 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)] + + + # all hondas except CRV, RDX and 2019 Odyssey@China use 0xe4 for steering + checks = [(0xe4, 100)] + if CP.carFingerprint in [CAR.CRV, CAR.CRV_EU, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]: + checks = [(0x194, 100)] + + bus_cam = 1 if CP.carFingerprint in HONDA_BOSCH and not CP.isPandaBlack else 2 + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, bus_cam) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index b055bc55..de43c4ca 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -6,8 +6,6 @@ from common.realtime import DT_CTRL from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET, get_events -from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.honda.carstate import CarState, get_can_parser, get_cam_can_parser from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, Ecu, ECU_FINGERPRINT, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.controls.lib.planner import _A_CRUISE_MAX_V_FOLLOWING @@ -16,7 +14,6 @@ from selfdrive.car.interfaces import CarInterfaceBase A_ACC_MAX = max(_A_CRUISE_MAX_V_FOLLOWING) ButtonType = car.CarState.ButtonEvent.Type -GearShifter = car.CarState.GearShifter def compute_gb_honda(accel, speed): creep_brake = 0.0 @@ -73,25 +70,11 @@ def get_compute_gb_acura(): class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP + def __init__(self, CP, CarController, CarState): + super().__init__(CP, CarController, CarState) - self.frame = 0 self.last_enable_pressed = 0 self.last_enable_sent = 0 - self.gas_pressed_prev = False - self.brake_pressed_prev = False - - self.cp = get_can_parser(CP) - self.cp_cam = get_cam_can_parser(CP) - - # *** init the major players *** - self.CS = CarState(CP) - self.VM = VehicleModel(CP) - - self.CC = None - if CarController is not None: - self.CC = CarController(self.cp.dbc_name, CP) if self.CS.CP.carFingerprint == CAR.ACURA_ILX: self.compute_gb = get_compute_gb_acura() @@ -133,10 +116,8 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - ret = car.CarParams.new_message() + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "honda" - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay if candidate in HONDA_BOSCH: ret.safetyModel = car.CarParams.SafetyModel.hondaBoschHarness if has_relay else car.CarParams.SafetyModel.hondaBoschGiraffe @@ -160,7 +141,7 @@ class CarInterface(CarInterfaceBase): # which improves controls quality as it removes the steering column torsion from feedback. # Tire stiffness factor fictitiously lower if it includes the steering column torsion effect. # For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani" - + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0], [0]] ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward @@ -169,16 +150,40 @@ class CarInterface(CarInterfaceBase): if fw.ecu == "eps" and b"," in fw.fwVersion: eps_modified = True - if candidate in [CAR.CIVIC, CAR.CIVIC_BOSCH]: + if candidate == CAR.CIVIC: stop_and_go = True ret.mass = CivicParams.MASS ret.wheelbase = CivicParams.WHEELBASE ret.centerToFront = CivicParams.CENTER_TO_FRONT ret.steerRatio = 15.38 # 10.93 is end-to-end spec + if eps_modified: + # stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE + # stock request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x1680, 0x1680 + # modified request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x2880, 0x3180 + # stock filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108 + # modified filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0400, 0x0480 + # note: max request allowed is 4096, but request is capped at 3840 in firmware, so modifications result in 2x max + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]] + else: + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]] tire_stiffness_factor = 1. - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.4], [0.12]] if eps_modified else [[0.8], [0.24]] - ret.lateralTuning.pid.kf = 0.00006 + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.54, 0.36] + + elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL): + stop_and_go = True + ret.mass = CivicParams.MASS + ret.wheelbase = CivicParams.WHEELBASE + ret.centerToFront = CivicParams.CENTER_TO_FRONT + 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.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] ret.longitudinalTuning.kiBP = [0., 35.] @@ -192,6 +197,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.83 ret.centerToFront = ret.wheelbase * 0.39 ret.steerRatio = 16.33 # 11.82 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.8467 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -205,6 +211,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.67 ret.centerToFront = ret.wheelbase * 0.37 ret.steerRatio = 18.61 # 15.3 is spec end-to-end + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.72 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -212,12 +219,13 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] - elif candidate == CAR.CRV: + elif candidate in (CAR.CRV, CAR.CRV_EU): stop_and_go = False ret.mass = 3572. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.62 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.89 # 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.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -232,8 +240,16 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 is spec end-to-end + if eps_modified: + # stock request input values: 0x0000, 0x00DB, 0x01BB, 0x0296, 0x0377, 0x0454, 0x0532, 0x0610, 0x067F + # stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400 + # modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]] + ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]] + else: + 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 - ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] ret.longitudinalTuning.kiBP = [0., 35.] @@ -246,6 +262,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 # 12.3 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.677 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -259,6 +276,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.53 ret.centerToFront = ret.wheelbase * 0.39 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.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.06]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -272,6 +290,7 @@ 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.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -285,6 +304,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 3.00 ret.centerToFront = ret.wheelbase * 0.41 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.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -298,6 +318,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.90 ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY ret.steerRatio = 14.35 + ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -311,6 +332,7 @@ class CarInterface(CarInterfaceBase): 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]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -324,6 +346,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 3.18 ret.centerToFront = ret.wheelbase * 0.41 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.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -331,6 +354,21 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.kiBP = [0., 35.] ret.longitudinalTuning.kiV = [0.18, 0.12] + elif candidate == CAR.INSIGHT: + stop_and_go = True + ret.mass = 2987. * CV.LB_TO_KG + STD_CARGO_KG + ret.wheelbase = 2.7 + ret.centerToFront = ret.wheelbase * 0.39 + 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.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] + ret.longitudinalTuning.kpBP = [0., 5., 35.] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiBP = [0., 35.] + ret.longitudinalTuning.kiV = [0.18, 0.12] + + else: raise ValueError("unsupported car %s" % candidate) @@ -350,21 +388,11 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - # no rear steering, at least on the listed cars above - ret.steerRatioRear = 0. - - # no max steer limit VS speed - ret.steerMaxBP = [0.] # m/s - ret.steerMaxV = [1.] # max steer allowed - ret.gasMaxBP = [0.] # m/s ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed ret.brakeMaxBP = [5., 20.] # m/s ret.brakeMaxV = [1., 0.8] # max brake allowed - ret.longitudinalTuning.deadzoneBP = [0.] - ret.longitudinalTuning.deadzoneV = [0.] - ret.stoppingControl = True ret.startAccel = 0.5 @@ -380,79 +408,16 @@ class CarInterface(CarInterfaceBase): self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) - self.CS.update(self.cp, self.cp_cam) - - # create message - ret = car.CarState.new_message() + ret = self.CS.update(self.cp, self.cp_cam) ret.canValid = self.cp.can_valid and self.cp_cam.can_valid - - # speeds - ret.vEgo = self.CS.v_ego - ret.aEgo = self.CS.a_ego - ret.vEgoRaw = self.CS.v_ego_raw - ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) - ret.standstill = self.CS.standstill - ret.wheelSpeeds.fl = self.CS.v_wheel_fl - ret.wheelSpeeds.fr = self.CS.v_wheel_fr - ret.wheelSpeeds.rl = self.CS.v_wheel_rl - ret.wheelSpeeds.rr = self.CS.v_wheel_rr - - # gas pedal - ret.gas = self.CS.car_gas / 256.0 - if not self.CP.enableGasInterceptor: - ret.gasPressed = self.CS.pedal_gas > 0 - else: - ret.gasPressed = self.CS.user_gas_pressed - - # brake pedal - ret.brake = self.CS.user_brake - ret.brakePressed = self.CS.brake_pressed != 0 + ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) # FIXME: read sendcan for brakelights brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1 ret.brakeLights = bool(self.CS.brake_switch or c.actuators.brake > brakelights_threshold) - # steering wheel - ret.steeringAngle = self.CS.angle_steers - ret.steeringRate = self.CS.angle_steers_rate - - # gear shifter lever - ret.gearShifter = self.CS.gear_shifter - - ret.steeringTorque = self.CS.steer_torque_driver - ret.steeringTorqueEps = self.CS.steer_torque_motor - ret.steeringPressed = self.CS.steer_override - - # cruise state - ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 - ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS - ret.cruiseState.available = bool(self.CS.main_on) and not bool(self.CS.cruise_mode) - ret.cruiseState.speedOffset = self.CS.cruise_speed_offset - ret.cruiseState.standstill = False - - # TODO: button presses buttonEvents = [] - ret.leftBlinker = bool(self.CS.left_blinker_on) - ret.rightBlinker = bool(self.CS.right_blinker_on) - - ret.doorOpen = not self.CS.door_all_closed - ret.seatbeltUnlatched = not self.CS.seatbelt - - ret.stockAeb = self.CS.stock_aeb - ret.stockFcw = self.CS.stock_fcw - - if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.leftBlinker - be.pressed = self.CS.left_blinker_on != 0 - buttonEvents.append(be) - - if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.rightBlinker - be.pressed = self.CS.right_blinker_on != 0 - buttonEvents.append(be) if self.CS.cruise_buttons != self.CS.prev_cruise_buttons: be = car.CarState.ButtonEvent.new_message() @@ -489,25 +454,9 @@ class CarInterface(CarInterfaceBase): ret.buttonEvents = buttonEvents # events - events = [] - if self.CS.steer_error: - events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) - elif self.CS.steer_warning: - events.append(create_event('steerTempUnavailable', [ET.WARNING])) + events = self.create_common_events(ret) if self.CS.brake_error: events.append(create_event('brakeUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) - if not ret.gearShifter == GearShifter.drive: - events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.doorOpen: - events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.seatbeltUnlatched: - events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.esp_disabled: - events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if not self.CS.main_on or self.CS.cruise_mode: - events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gearShifter == GearShifter.reverse: - events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.brake_hold and self.CS.CP.carFingerprint not in HONDA_BOSCH: events.append(create_event('brakeHold', [ET.NO_ENTRY, ET.USER_DISABLE])) if self.CS.park_brake: @@ -516,14 +465,6 @@ class CarInterface(CarInterfaceBase): if self.CP.enableCruise and ret.vEgo < self.CP.minEnableSpeed: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) - # disable on pedals rising edge or when brake is pressed and speed isn't zero - if (ret.gasPressed and not self.gas_pressed_prev) or \ - (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) - # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low if self.CP.enableCruise and not ret.cruiseState.enabled and (c.actuators.brake <= 0. or not self.CP.openpilotLongitudinalControl): @@ -569,8 +510,8 @@ class CarInterface(CarInterfaceBase): self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed - # cast to reader so it can't be modified - return ret.as_reader() + self.CS.out = ret.as_reader() + return self.CS.out # pass in a car.CarControl # to be called @ 100hz diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 7e273e84..9cf63a69 100644 --- a/selfdrive/car/honda/values.py +++ b/selfdrive/car/honda/values.py @@ -27,9 +27,11 @@ class CAR: ACCORDH = "HONDA ACCORD 2018 HYBRID TOURING" CIVIC = "HONDA CIVIC 2016 TOURING" CIVIC_BOSCH = "HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019" + CIVIC_BOSCH_DIESEL = "HONDA CIVIC SEDAN 1.6 DIESEL" ACURA_ILX = "ACURA ILX 2016 ACURAWATCH PLUS" CRV = "HONDA CR-V 2016 TOURING" CRV_5G = "HONDA CR-V 2017 EX" + CRV_EU = "HONDA CR-V 2016 EXECUTIVE" CRV_HYBRID = "HONDA CR-V 2019 HYBRID" FIT = "HONDA FIT 2018 EX" ODYSSEY = "HONDA ODYSSEY 2018 EX-L" @@ -38,6 +40,7 @@ class CAR: PILOT = "HONDA PILOT 2017 TOURING" PILOT_2019 = "HONDA PILOT 2019 ELITE" RIDGELINE = "HONDA RIDGELINE 2017 BLACK EDITION" + INSIGHT = "HONDA INSIGHT 2019 TOURING" # diag message that in some Nidec cars only appear with 1s freq if VIN query is performed DIAG_MSGS = {1600: 5, 1601: 8} @@ -69,10 +72,10 @@ FINGERPRINTS = { # 2017 Civic Hatchback LX { 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 815: 8, 825: 4, 829: 5, 846: 8, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 892: 8, 918: 7, 927: 8, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1092: 1, 1108: 8, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1633: 8 - }, - # 2019 Civic Hatchback EX - { - 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1633: 8 + }], + CAR.CIVIC_BOSCH_DIESEL: [{ + # 2019 Civic Sedan 1.6 i-dtec Diesel European + 57: 3, 148: 8, 228: 5, 308: 5, 316: 8, 330: 8, 344: 8, 380: 8, 399: 7, 419: 8, 420: 8, 426: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 507: 1, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 801: 3, 804: 8, 806: 8, 808: 8, 815: 8, 824: 8, 825: 4, 829: 5, 837: 5, 862: 8, 881: 8, 882: 4, 884: 8, 887: 8, 888: 8, 891: 8, 902: 8, 918: 7, 927: 8, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1092: 1, 1108: 8, 1115: 2, 1125: 8, 1296: 8, 1302: 8, 1322: 5, 1337: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8 }], CAR.CRV: [{ 57: 3, 145: 8, 316: 8, 340: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 6, 401: 8, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 493: 3, 506: 8, 507: 1, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 661: 4, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 829: 5, 882: 2, 884: 7, 888: 8, 891: 8, 892: 8, 923: 2, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 1424: 5, 1600: 5, 1601: 8, @@ -80,6 +83,10 @@ FINGERPRINTS = { CAR.CRV_5G: [{ 57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1115: 2, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1633: 8, 1670: 5 }], + # 1057: 5 1024: 5 are also on the OBD2 bus. their lengths differ from the camera's f-can bus. re-fingerprint after obd2 connection is split in panda firmware from bus 1. + CAR.CRV_EU: [{ + 57: 3, 145: 8, 308: 5, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 6, 404: 4, 419: 8, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 493: 3, 506: 8, 507: 1, 510: 3, 538: 3, 542: 7, 545: 4, 597: 8, 660: 8, 661: 4, 768: 8, 769: 8, 773: 7, 777: 8, 780: 8, 800: 8, 801: 3, 803: 8, 804: 8, 808: 8, 824: 8, 829: 5, 837: 5, 862: 8, 882: 2, 884: 7, 888: 8, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 930: 8, 931: 8, 983: 8, 1024: 8, 1027: 5, 1029: 8, 1033: 5, 1036: 8, 1039: 8, 1040: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1045: 8, 1046: 8, 1047: 8, 1056: 8, 1057: 8, 1058: 8, 1059: 8, 1060: 8, 1064: 7, 1072: 8, 1073: 8, 1074: 8, 1075: 8, 1076: 8, 1077: 8, 1078: 8, 1079: 8, 1080: 8, 1081: 8, 1088: 8, 1089: 8, 1090: 8, 1091: 8, 1092: 8, 1093: 8, 1108: 8, 1125: 8, 1279: 8, 1280: 8, 1296: 8, 1297: 8, 1365: 5, 1424: 5, 1600: 5, 1601: 8, + }], CAR.CRV_HYBRID: [{ 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 408: 6, 415: 6, 419: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 490: 8, 495: 8, 525: 8, 531: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 814: 4, 829: 5, 833: 6, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 930: 8, 931: 8, 1302: 8, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1626: 5, 1627: 5 }], @@ -116,9 +123,16 @@ FINGERPRINTS = { # 2019 Ridgeline { 57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 419: 8, 420: 8, 422:8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 795: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 871: 8, 882: 2, 884: 7, 892: 8, 923: 2, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1365: 5, 424: 5, 1613: 5, 1616: 5, 1618: 5, 1623: 5, 1668: 5 + }], + # 2019 Insight + CAR.INSIGHT: [{ + 57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 476: 8, 477: 8, 479: 8, 490: 8, 495: 8, 507: 1, 525: 8, 531: 8, 545: 6, 547: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 829: 5, 832: 3, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 954: 2, 985: 3, 1029: 8, 1093: 4, 1115: 2, 1302: 8, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1652: 8, 2015: 3 }] } +# Don't use theses fingerprints for fingerprinting, they are still needed for ECU detection +IGNORED_FINGERPRINTS = [CAR.INSIGHT, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_EU] + # add DIAG_MSGS to fingerprints for c in FINGERPRINTS: for f, _ in enumerate(FINGERPRINTS[c]): @@ -128,74 +142,105 @@ for c in FINGERPRINTS: # TODO: Figure out what is relevant FW_VERSIONS = { CAR.ACCORD: { - (Ecu.unknown, 0x18da10f1, None): [ + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-6A0-A640\x00\x00', b'37805-6B2-A550\x00\x00', b'37805-6B2-A650\x00\x00', b'37805-6B2-A660\x00\x00', b'37805-6B2-M520\x00\x00', ], - (Ecu.unknown, 0x18da0bf1, None): [b'54008-TVC-A910\x00\x00'], - (Ecu.unknown, 0x18da1ef1, None): [b'28102-6B8-A560\x00\x00', b'28102-6B8-M520\x00\x00'], - (Ecu.unknown, 0x18da2bf1, None): [b'46114-TVA-A060\x00\x00', b'46114-TVA-A080\x00\x00'], - (Ecu.unknown, 0x18da28f1, None): [b'57114-TVA-C050\x00\x00'], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TVC-A910\x00\x00', + ], + (Ecu.transmission, 0x18da1ef1, None): [ + b'28102-6B8-A560\x00\x00', + b'28102-6B8-M520\x00\x00', + ], + (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ + b'46114-TVA-A060\x00\x00', + b'46114-TVA-A080\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TVA-C050\x00\x00', + ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TVA-A150\x00\x00', b'39990-TVA-A160\x00\x00', b'39990-TVA-X030\x00\x00', ], - (Ecu.unknown, 0x18da3af1, None): [b'39390-TVA-A020\x00\x00'], - (Ecu.unknown, 0x18da53f1, None): [b'77959-TVA-A460\x00\x00', b'77959-TVA-X330\x00\x00'], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.unknown, 0x18da3af1, None): [ + b'39390-TVA-A020\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TVA-A460\x00\x00', + b'77959-TVA-X330\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TVA-A210\x00\x00', b'78109-TVC-A010\x00\x00', b'78109-TVC-A110\x00\x00', b'78109-TVC-A210\x00\x00', b'78109-TVC-M510\x00\x00', ], - (Ecu.unknown, 0x18da61f1, None): [b'78209-TVA-A010\x00\x00'], - (Ecu.unknown, 0x18dab0f1, None): [ + (Ecu.hud, 0x18da61f1, None): [ + b'78209-TVA-A010\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TVA-A160\x00\x00', b'36802-TVA-A160\x00\x00', b'36802-TVA-A170\x00\x00', b'36802-TWA-A070\x00\x00', ], - (Ecu.unknown, 0x18dab5f1, None): [b'36161-TVA-A060\x00\x00', b'36161-TWA-A070\x00\x00'], - (Ecu.unknown, 0x18daeff1, None): [b'38897-TVA-A010\x00\x00'], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TVA-A060\x00\x00', + b'36161-TWA-A070\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TVA-A010\x00\x00', + ], }, CAR.ACCORD_15: { - (Ecu.unknown, 0x18da10f1, None): [ + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-6A0-9620\x00\x00', b'37805-6A0-A640\x00\x00', b'37805-6A0-A740\x00\x00', b'37805-6A0-A840\x00\x00', b'37805-6A0-A850\x00\x00', ], - (Ecu.unknown, 0x18da1ef1, None): [ + (Ecu.transmission, 0x18da1ef1, None): [ b'28101-6A7-A220\x00\x00', b'28101-6A7-A320\x00\x00', b'28101-6A7-A510\x00\x00', ], - (Ecu.unknown, 0x18daeff1, None): [b'38897-TVA-A230\x00\x00'], - (Ecu.unknown, 0x18da2bf1, None): [ + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TVA-A230\x00\x00', + ], + (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ b'46114-TVA-A050\x00\x00', b'46114-TVA-A060\x00\x00', b'46114-TVA-A120\x00\x00', ], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TVA-A010\x00\x00', b'78109-TVA-A210\x00\x00', b'78109-TVA-A220\x00\x00', b'78109-TVA-A310\x00\x00', b'78109-TWA-A210\x00\x00', ], - (Ecu.unknown, 0x18da61f1, None): [b'78209-TVA-A010\x00\x00'], - (Ecu.unknown, 0x18dab5f1, None): [b'36161-TVA-A060\x00\x00'], - (Ecu.unknown, 0x18da53f1, None): [b'77959-TVA-A460\x00\x00'], - (Ecu.unknown, 0x18da28f1, None): [ + (Ecu.hud, 0x18da61f1, None): [ + b'78209-TVA-A010\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TVA-A060\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TVA-A460\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ b'57114-TVA-B050\x00\x00', b'57114-TVA-B040\x00\x00', ], - (Ecu.unknown, 0x18dab0f1, None): [ + (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TVA-A150\x00\x00', b'36802-TVA-A160\x00\x00', b'36802-TVA-A170\x00\x00', @@ -207,23 +252,41 @@ FW_VERSIONS = { ], }, CAR.ACCORDH: { - (Ecu.unknown, 0x18daeff1, None): [b'38897-TWA-A120\x00\x00'], - (Ecu.unknown, 0x18da28f1, None): [b'57114-TWA-A040\x00\x00'], - (Ecu.unknown, 0x18da53f1, None): [b'77959-TWA-A440\x00\x00'], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TWA-A120\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TWA-A040\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TWA-A440\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TWA-A010\x00\x00', b'78109-TWA-A120\x00\x00', b'78109-TWA-A210\x00\x00', b'78109-TWA-A110\x00\x00', ], - (Ecu.unknown, 0x18da0bf1, None): [b'54008-TWA-A910\x00\x00'], - (Ecu.unknown, 0x18da61f1, None): [b'78209-TVA-A010\x00\x00'], - (Ecu.unknown, 0x18dab5f1, None): [b'36161-TWA-A070\x00\x00'], - (Ecu.unknown, 0x18dab0f1, None): [b'36802-TWA-A080\x00\x00', b'36802-TWA-A070\x00\x00'], - (Ecu.eps, 0x18da30f1, None): [b'39990-TVA-A160\x00\x00', b'39990-TVA-A150\x00\x00'], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TWA-A910\x00\x00', + ], + (Ecu.hud, 0x18da61f1, None): [ + b'78209-TVA-A010\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TWA-A070\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TWA-A080\x00\x00', + b'36802-TWA-A070\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TVA-A160\x00\x00', + b'39990-TVA-A150\x00\x00', + ], }, CAR.CIVIC: { - (Ecu.unknown, 0x18da10f1, None): [ + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-5AA-A640\x00\x00', b'37805-5AA-A650\x00\x00', b'37805-5AA-A670\x00\x00', @@ -236,7 +299,7 @@ FW_VERSIONS = { b'37805-5BA-L940\x00\x00', b'37805-5BA-L960\x00\x00', ], - (Ecu.unknown, 0x18da1ef1, None): [ + (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5CG-A040\x00\x00', b'28101-5CG-A050\x00\x00', b'28101-5CG-A070\x00\x00', @@ -247,10 +310,10 @@ FW_VERSIONS = { b'28101-5DJ-A060\x00\x00', b'28101-5DJ-A510\x00\x00', ], - (Ecu.unknown, 0x18da28f1, None): [ + (Ecu.vsa, 0x18da28f1, None): [ b'57114-TBA-A550\x00\x00', b'57114-TBA-A560\x00\x00', - b'57114-TBA-A570\x00\x00' + b'57114-TBA-A570\x00\x00', ], (Ecu.eps, 0x18da30f1, None): [ b'39990-TBA,A030\x00\x00', @@ -258,12 +321,12 @@ FW_VERSIONS = { b'39990-TBG-A030\x00\x00', b'39990-TEG-A010\x00\x00', ], - (Ecu.unknown, 0x18da53f1, None): [ + (Ecu.srs, 0x18da53f1, None): [ b'77959-TBA-A030\x00\x00', b'77959-TBA-A040\x00\x00', b'77959-TBG-A030\x00\x00', ], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TBC-A310\x00\x00', b'78109-TBC-A320\x00\x00', b'78109-TBC-A510\x00\x00', @@ -273,31 +336,33 @@ FW_VERSIONS = { b'78109-TBH-A530\x00\x00', b'78109-TEG-A310\x00\x00', ], - (Ecu.unknown, 0x18dab0f1, None): [ + (Ecu.fwdCamera, 0x18dab0f1, None): [ b'36161-TBA-A030\x00\x00', b'36161-TBC-A020\x00\x00', b'36161-TBC-A030\x00\x00', b'36161-TEG-A010\x00\x00', ], - (Ecu.unknown, 0x18daeff1, None): [ + (Ecu.gateway, 0x18daeff1, None): [ b'38897-TBA-A010\x00\x00', b'38897-TBA-A020\x00\x00', ], }, CAR.CIVIC_BOSCH: { - (Ecu.unknown, 0x18da10f1, None): [ + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-5AA-A950\x00\x00', b'37805-5AA-L950\x00\x00', b'37805-5AN-A750\x00\x00', b'37805-5AN-A830\x00\x00', b'37805-5AN-A930\x00\x00', + b'37805-5AN-A950\x00\x00', b'37805-5AN-L940\x00\x00', b'37805-5AN-LH20\x00\x00', b'37805-5AN-LJ20\x00\x00', b'37805-5AZ-E850\x00\x00', b'37805-5BB-L640\x00\x00', + b'37805-5AN-AH20\x00\x00', ], - (Ecu.unknown, 0x18da1ef1, None): [ + (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5CG-A920\x00\x00', b'28101-5CG-C220\x00\x00', b'28101-5CG-C320\x00\x00', @@ -308,7 +373,7 @@ FW_VERSIONS = { b'28101-5DJ-A710\x00\x00', b'28101-5DV-E330\x00\x00', ], - (Ecu.unknown, 0x18da28f1, None): [ + (Ecu.vsa, 0x18da28f1, None): [ b'57114-TBG-A340\x00\x00', b'57114-TGG-A340\x00\x00', b'57114-TGL-G330\x00\x00', @@ -322,13 +387,13 @@ FW_VERSIONS = { b'39990-TGL-E130\x00\x00', b'39990-TGG-A020\x00\x00', ], - (Ecu.unknown, 0x18da53f1, None): [ + (Ecu.srs, 0x18da53f1, None): [ b'77959-TBA-A060\x00\x00', b'77959-TGG-A020\x00\x00', b'77959-TGG-G010\x00\x00', b'77959-TGG-A020\x00\x00', ], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TBA-A910\x00\x00', b'78109-TBC-A740\x00\x00', b'78109-TGG-A210\x00\x00', @@ -338,13 +403,13 @@ FW_VERSIONS = { b'78109-TGG-A820\x00\x00', b'78109-TGL-G120\x00\x00', ], - (Ecu.unknown, 0x18dab0f1, None): [ + (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TBA-A150\x00\x00', b'36802-TGG-A050\x00\x00', b'36802-TGL-G040\x00\x00', b'36802-TGG-A060\x00\x00', ], - (Ecu.unknown, 0x18dab5f1, None): [ + (Ecu.fwdCamera, 0x18dab5f1, None): [ b'36161-TBA-A130\x00\x00', b'36161-TGG-A060\x00\x00', b'36161-TGL-G050\x00\x00', @@ -355,9 +420,23 @@ FW_VERSIONS = { b'38897-TBA-A020\x00\x00', b'38897-TBA-A020\x00\x00', ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TBA-A110\x00\x00', + b'38897-TBA-A020\x00\x00', + ], + }, + CAR.CIVIC_BOSCH_DIESEL: { + (Ecu.programmedFuelInjection, 0x18da10f1, None): [b'37805-59N-G830\x00\x00'], + (Ecu.transmission, 0x18da1ef1, None): [b'28101-59Y-G620\x00\x00'], + (Ecu.vsa, 0x18da28f1, None): [b'57114-TGN-E320\x00\x00'], + (Ecu.eps, 0x18da30f1, None): [b'39990-TFK-G020\x00\x00'], + (Ecu.srs, 0x18da53f1, None): [b'77959-TFK-G210\x00\x00'], + (Ecu.combinationMeter, 0x18da60f1, None): [b'78109-TFK-G020\x00\x00'], + (Ecu.fwdRadar, 0x18dab0f1, None): [b'36802-TFK-G130\x00\x00'], + (Ecu.shiftByWire, 0x18da0bf1, None): [b'54008-TGN-E010\x00\x00'], }, CAR.CRV_5G: { - (Ecu.unknown, 0x18da10f1, None): [ + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-5PA-3080\x00\x00', b'37805-5PA-4050\x00\x00', b'37805-5PA-6530\x00\x00', @@ -369,7 +448,7 @@ FW_VERSIONS = { b'37805-5PA-A880\x00\x00', b'37805-5PA-A890\x00\x00', ], - (Ecu.unknown, 0x18da1ef1, None): [ + (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5RG-A020\x00\x00', b'28101-5RG-A030\x00\x00', b'28101-5RG-A040\x00\x00', @@ -378,78 +457,127 @@ FW_VERSIONS = { b'28101-5RH-A040\x00\x00', b'28101-5RH-A120\x00\x00', ], - (Ecu.unknown, 0x18da28f1, None): [ + (Ecu.vsa, 0x18da28f1, None): [ b'57114-TLA-A040\x00\x00', b'57114-TLA-A050\x00\x00', b'57114-TLA-A060\x00\x00', ], - (Ecu.eps, 0x18da30f1, None): [b'39990-TLA-A040\x00\x00', b'39990-TLA,A040\x00\x00'], - (Ecu.unknown, 0x18da2bf1, None): [b'46114-TLA-A040\x00\x00', b'46114-TLA-A050\x00\x00'], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TLA-A040\x00\x00', + b'39990-TLA,A040\x00\x00', + ], + (Ecu.electricBrakeBooster, 0x18da2bf1, None): [ + b'46114-TLA-A040\x00\x00', + b'46114-TLA-A050\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TLA-A110\x00\x00', b'78109-TLA-A210\x00\x00', b'78109-TLA-C210\x00\x00', b'78109-TLB-A110\x00\x00', b'78109-TLB-A210\x00\x00', ], - (Ecu.unknown, 0x18daeff1, None): [b'38897-TLA-A010\x00\x00', b'38897-TNY-G010\x00\x00'], - (Ecu.unknown, 0x18dab0f1, None): [ + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TLA-A010\x00\x00', + b'38897-TNY-G010\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ b'36802-TLA-A040\x00\x00', b'36802-TLA-A050\x00\x00', b'36802-TLA-A060\x00\x00', ], - (Ecu.unknown, 0x18dab5f1, None): [ + (Ecu.fwdCamera, 0x18dab5f1, None): [ b'36161-TLA-A060\x00\x00', b'36161-TLA-A070\x00\x00', b'36161-TLA-A080\x00\x00', ], - (Ecu.unknown, 0x18da53f1, None): [ + (Ecu.srs, 0x18da53f1, None): [ b'77959-TLA-A240\x00\x00', b'77959-TLA-A250\x00\x00', b'77959-TLA-A320\x00\x00', ], }, + CAR.CRV_EU: { + (Ecu.programmedFuelInjection, 0x18da10f1, None): [b'37805-R5Z-G740\x00\x00'], + (Ecu.vsa, 0x18da28f1, None): [b'57114-T1V-G920\x00\x00'], + (Ecu.fwdRadar, 0x18dab0f1, None): [b'36161-T1V-G520\x00\x00'], + (Ecu.shiftByWire, 0x18da0bf1, None): [b'54008-T1V-G010\x00\x00'], + (Ecu.transmission, 0x18da1ef1, None): [b'28101-5LH-E120\x00\x00'], + (Ecu.combinationMeter, 0x18da60f1, None): [b'78109-T1V-G020\x00\x00'], + (Ecu.srs, 0x18da53f1, None): [b'77959-T1G-G940\x00\x00'], + }, CAR.CRV_HYBRID: { - (Ecu.unknown, 0x18da28f1, None): [b'57114-TPA-G020\x00\x00'], - (Ecu.eps, 0x18da30f1, None): [b'39990-TPA-G030\x00\x00'], - (Ecu.unknown, 0x18daeff1, None): [b'38897-TMA-H110\x00\x00'], - (Ecu.unknown, 0x18da0bf1, None): [b'54008-TMB-H510\x00\x00'], - (Ecu.unknown, 0x18dab5f1, None): [b'36161-TPA-E050\x00\x00'], - (Ecu.unknown, 0x18da60f1, None): [b'78109-TPA-G520\x00\x00'], - (Ecu.unknown, 0x18da61f1, None): [b'78209-TLA-X010\x00\x00'], - (Ecu.unknown, 0x18dab0f1, None): [b'36802-TPA-E040\x00\x00'], - (Ecu.unknown, 0x18da53f1, None): [b'77959-TLA-G220\x00\x00'], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-TPA-G020\x00\x00', + ], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TPA-G030\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TMA-H110\x00\x00', + ], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-TMB-H510\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TPA-E050\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-TPA-G520\x00\x00', + ], + (Ecu.hud, 0x18da61f1, None): [ + b'78209-TLA-X010\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TPA-E040\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TLA-G220\x00\x00', + ], }, CAR.ODYSSEY: { - (Ecu.unknown, 0x18daeff1, None): [b'38897-THR-A010\x00\x00', b'38897-THR-A020\x00\x00'], - (Ecu.unknown, 0x18da10f1, None): [ + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-THR-A010\x00\x00', + b'38897-THR-A020\x00\x00', + ], + (Ecu.programmedFuelInjection, 0x18da10f1, None): [ b'37805-5MR-A250\x00\x00', b'37805-5MR-A310\x00\x00', b'37805-5MR-A750\x00\x00', b'37805-5MR-A840\x00\x00', b'37805-5MR-C620\x00\x00', ], - (Ecu.eps, 0x18da30f1, None): [b'39990-THR-A020\x00\x00', b'39990-THR-A030\x00\x00'], - (Ecu.unknown, 0x18da53f1, None): [b'77959-THR-A010\x00\x00', b'77959-THR-A110\x00\x00'], - (Ecu.unknown, 0x18dab0f1, None): [ + (Ecu.eps, 0x18da30f1, None): [ + b'39990-THR-A020\x00\x00', + b'39990-THR-A030\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-THR-A010\x00\x00', + b'77959-THR-A110\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab0f1, None): [ b'36161-THR-A030\x00\x00', b'36161-THR-A110\x00\x00', b'36161-THR-A720\x00\x00', b'36161-THR-A810\x00\x00', b'36161-THR-C010\x00\x00', ], - (Ecu.unknown, 0x18da1ef1, None): [ + (Ecu.transmission, 0x18da1ef1, None): [ b'28101-5NZ-A310\x00\x00', b'28101-5NZ-C310\x00\x00', b'28102-5MX-A001\x00\x00', b'28102-5MX-A610\x00\x00', b'28102-5MX-A710\x00\x00', + b'28102-5MX-A900\x00\x00', b'28102-5MX-A910\x00\x00', b'28102-5MX-C001\x00\x00', b'28103-5NZ-A300\x00\x00', ], - (Ecu.unknown, 0x18da28f1, None): [b'57114-THR-A040\x00\x00', b'57114-THR-A110\x00\x00'], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-THR-A040\x00\x00', + b'57114-THR-A110\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-THR-A230\x00\x00', b'78109-THR-A430\x00\x00', b'78109-THR-A820\x00\x00', @@ -462,20 +590,31 @@ FW_VERSIONS = { b'78109-THR-C330\x00\x00', b'78109-THR-CE20\x00\x00', ], - (Ecu.unknown, 0x18da0bf1, None): [b'54008-THR-A020\x00\x00'], + (Ecu.shiftByWire, 0x18da0bf1, None): [ + b'54008-THR-A020\x00\x00', + ], }, CAR.PILOT_2019: { - (Ecu.eps, 0x18da30f1, None): [b'39990-TG7-A060\x00\x00', b'39990-TGS-A230\x00\x00'], - (Ecu.unknown, 0x18daeff1, None): [b'38897-TG7-A110\x00\x00', b'38897-TG7-A030\x00\x00'], - (Ecu.unknown, 0x18dab0f1, None): [ + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TG7-A060\x00\x00', + b'39990-TGS-A230\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-TG7-A110\x00\x00', + b'38897-TG7-A030\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab0f1, None): [ b'36161-TG7-A630\x00\x00', b'36161-TG7-A930\x00\x00', b'36161-TG8-A630\x00\x00', b'36161-TGS-A130\x00\x00', b'36161-TGT-A030\x00\x00', ], - (Ecu.unknown, 0x18da53f1, None): [b'77959-TG7-A210\x00\x00', b'77959-TGS-A010\x00\x00'], - (Ecu.unknown, 0x18da60f1, None): [ + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TG7-A210\x00\x00', + b'77959-TGS-A010\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ b'78109-TG7-AJ20\x00\x00', b'78109-TG7-AP10\x00\x00', b'78109-TG7-AP20\x00\x00', @@ -484,7 +623,7 @@ FW_VERSIONS = { b'78109-TGS-AP20\x00\x00', b'78109-TGT-AJ20\x00\x00', ], - (Ecu.unknown, 0x18da28f1, None): [ + (Ecu.vsa, 0x18da28f1, None): [ b'57114-TG7-A630\x00\x00', b'57114-TG7-A730\x00\x00', b'57114-TG8-A630\x00\x00', @@ -493,12 +632,38 @@ FW_VERSIONS = { ], }, CAR.RIDGELINE: { - (Ecu.eps, 0x18da30f1, None): [b'39990-T6Z-A020\x00\x00'], - (Ecu.unknown, 0x18dab0f1, None): [b'36161-T6Z-A310\x00\x00'], - (Ecu.unknown, 0x18daeff1, None): [b'38897-T6Z-A010\x00\x00'], - (Ecu.unknown, 0x18da60f1, None): [b'78109-T6Z-A420\x00\x00'], - (Ecu.unknown, 0x18da53f1, None): [b'77959-T6Z-A020\x00\x00'], - (Ecu.unknown, 0x18da28f1, None): [b'57114-T6Z-A130\x00\x00'], + (Ecu.eps, 0x18da30f1, None): [ + b'39990-T6Z-A020\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab0f1, None): [ + b'36161-T6Z-A310\x00\x00', + ], + (Ecu.gateway, 0x18daeff1, None): [ + b'38897-T6Z-A010\x00\x00', + ], + (Ecu.combinationMeter, 0x18da60f1, None): [ + b'78109-T6Z-A420\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-T6Z-A020\x00\x00', + ], + (Ecu.vsa, 0x18da28f1, None): [ + b'57114-T6Z-A130\x00\x00', + ], + }, + CAR.INSIGHT: { + (Ecu.eps, 0x18da30f1, None): [ + b'39990-TXM-A040\x00\x00', + ], + (Ecu.fwdRadar, 0x18dab0f1, None): [ + b'36802-TXM-A070\x00\x00', + ], + (Ecu.fwdCamera, 0x18dab5f1, None): [ + b'36161-TXM-A050\x00\x00', + ], + (Ecu.srs, 0x18da53f1, None): [ + b'77959-TXM-A230\x00\x00', + ], }, } @@ -510,8 +675,10 @@ DBC = { CAR.ACURA_RDX: dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'), CAR.CIVIC: dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'), CAR.CIVIC_BOSCH: dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None), + CAR.CIVIC_BOSCH_DIESEL: dbc_dict('honda_civic_sedan_16_diesel_2019_can_generated', None), CAR.CRV: dbc_dict('honda_crv_touring_2016_can_generated', 'acura_ilx_2016_nidec'), CAR.CRV_5G: dbc_dict('honda_crv_ex_2017_can_generated', None), + CAR.CRV_EU: dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'), CAR.CRV_HYBRID: dbc_dict('honda_crv_hybrid_2019_can_generated', None), CAR.FIT: 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'), @@ -519,6 +686,7 @@ DBC = { 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.RIDGELINE: dbc_dict('honda_ridgeline_black_edition_2017_can_generated', 'acura_ilx_2016_nidec'), + CAR.INSIGHT: dbc_dict('honda_insight_ex_2019_can_generated', None), } STEER_THRESHOLD = { @@ -529,8 +697,10 @@ STEER_THRESHOLD = { CAR.ACURA_RDX: 400, CAR.CIVIC: 1200, CAR.CIVIC_BOSCH: 1200, + CAR.CIVIC_BOSCH_DIESEL: 1200, CAR.CRV: 1200, CAR.CRV_5G: 1200, + CAR.CRV_EU: 400, CAR.CRV_HYBRID: 1200, CAR.FIT: 1200, CAR.ODYSSEY: 1200, @@ -538,6 +708,7 @@ STEER_THRESHOLD = { CAR.PILOT: 1200, CAR.PILOT_2019: 1200, CAR.RIDGELINE: 1200, + CAR.INSIGHT: 1200, } SPEED_FACTOR = { @@ -548,8 +719,10 @@ SPEED_FACTOR = { CAR.ACURA_RDX: 1., CAR.CIVIC: 1., CAR.CIVIC_BOSCH: 1., + CAR.CIVIC_BOSCH_DIESEL: 1., CAR.CRV: 1.025, CAR.CRV_5G: 1.025, + CAR.CRV_EU: 1.025, CAR.CRV_HYBRID: 1.025, CAR.FIT: 1., CAR.ODYSSEY: 1., @@ -557,6 +730,7 @@ SPEED_FACTOR = { CAR.PILOT: 1., CAR.PILOT_2019: 1., CAR.RIDGELINE: 1., + CAR.INSIGHT: 1., } # msgs sent for steering controller by camera module on can 0. @@ -565,4 +739,4 @@ ECU_FINGERPRINT = { Ecu.fwdCamera: [0xE4, 0x194], # steer torque cmd } -HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CRV_5G, CAR.CRV_HYBRID] +HONDA_BOSCH = [CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT] diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index 0a6144ba..efd8796e 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -5,9 +5,9 @@ from opendbc.can.packer import CANPacker class CarController(): - def __init__(self, dbc_name, car_fingerprint): + def __init__(self, dbc_name, CP, VM): self.apply_steer_last = 0 - self.car_fingerprint = car_fingerprint + self.car_fingerprint = CP.carFingerprint self.lkas11_cnt = 0 self.cnt = 0 self.last_resume_cnt = 0 @@ -18,7 +18,7 @@ class CarController(): ### Steering Torque new_steer = actuators.steer * SteerLimitParams.STEER_MAX - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams) + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer if not enabled: @@ -38,7 +38,7 @@ class CarController(): if pcm_cancel_cmd: can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL)) - elif CS.stopped and (self.cnt - self.last_resume_cnt) > 5: + elif CS.out.cruiseState.standstill and (self.cnt - self.last_resume_cnt) > 5: self.last_resume_cnt = self.cnt can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.RES_ACCEL)) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 5305396f..59c825f4 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -1,221 +1,222 @@ from cereal import car -from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD +from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD, FEATURES from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from selfdrive.config import Conversions as CV GearShifter = car.CarState.GearShifter -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_TSigLHSw", "CGW1", 0), - ("CF_Gway_TurnSigLh", "CGW1", 0), - ("CF_Gway_TSigRHSw", "CGW1", 0), - ("CF_Gway_TurnSigRh", "CGW1", 0), - ("CF_Gway_ParkBrakeSw", "CGW1", 0), - - ("BRAKE_ACT", "EMS12", 0), - ("PV_AV_CAN", "EMS12", 0), - ("TPS", "EMS12", 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), - - ("CF_Clu_InhibitD", "CLU15", 0), - ("CF_Clu_InhibitP", "CLU15", 0), - ("CF_Clu_InhibitN", "CLU15", 0), - ("CF_Clu_InhibitR", "CLU15", 0), - - ("CF_Lvr_Gear", "LVR12",0), - ("CUR_GR", "TCU12",0), - - ("ACCEnable", "TCS13", 0), - ("ACC_REQ", "TCS13", 0), - ("DriverBraking", "TCS13", 0), - ("DriverOverride", "TCS13", 0), - - ("ESC_Off_Step", "TCS15", 0), - - ("CF_Lvr_GearInf", "LVR11", 0), #Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev) - - ("CR_Mdps_DrvTq", "MDPS11", 0), - - ("CR_Mdps_StrColTq", "MDPS12", 0), - ("CF_Mdps_ToiActive", "MDPS12", 0), - ("CF_Mdps_ToiUnavail", "MDPS12", 0), - ("CF_Mdps_FailStat", "MDPS12", 0), - ("CR_Mdps_OutTq", "MDPS12", 0), - - ("VSetDis", "SCC11", 0), - ("SCCInfoDisplay", "SCC11", 0), - ("ACCMode", "SCC12", 1), - - ("SAS_Angle", "SAS11", 0), - ("SAS_Speed", "SAS11", 0), - ] - - checks = [ - # address, frequency - ("MDPS12", 50), - ("MDPS11", 100), - ("TCS15", 10), - ("TCS13", 50), - ("CLU11", 50), - ("ESP12", 100), - ("EMS12", 100), - ("CGW1", 10), - ("CGW4", 5), - ("WHL_SPD11", 50), - ("SCC11", 50), - ("SCC12", 50), - ("SAS11", 100) - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) - - -def get_camera_parser(CP): - - signals = [ - # sig_name, sig_address, default - ("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_ToiFlt", "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) - ] - - checks = [] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) - - class CarState(CarStateBase): def update(self, cp, cp_cam): - # update prevs, update must run once per Loop - self.prev_left_blinker_on = self.left_blinker_on - self.prev_right_blinker_on = self.right_blinker_on + ret = car.CarState.new_message() - self.door_all_closed = True - self.seatbelt = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] + ret.doorOpen = False # FIXME + ret.seatbeltUnlatched = cp.vl["CGW1"]['CF_Gway_DrvSeatBeltSw'] == 0 - self.brake_pressed = cp.vl["TCS13"]['DriverBraking'] - self.esp_disabled = cp.vl["TCS15"]['ESC_Off_Step'] + ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS + ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS + ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS + ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS + ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw'] - self.main_on = True - self.acc_active = cp.vl["SCC12"]['ACCMode'] != 0 - self.pcm_acc_status = int(self.acc_active) + ret.standstill = ret.vEgoRaw < 0.1 - self.v_wheel_fl = cp.vl["WHL_SPD11"]['WHL_SPD_FL'] * CV.KPH_TO_MS - self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS - self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS - self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS - self.v_ego_raw = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4. - self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) + ret.steeringAngle = cp.vl["SAS11"]['SAS_Angle'] + ret.steeringRate = cp.vl["SAS11"]['SAS_Speed'] + ret.yawRate = cp.vl["ESP12"]['YAW_RATE'] + ret.leftBlinker = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] != 0 + ret.rightBlinker = cp.vl["CGW1"]['CF_Gway_TSigRHSw'] != 0 + ret.steeringTorque = cp.vl["MDPS11"]['CR_Mdps_DrvTq'] + ret.steeringTorqueEps = cp.vl["MDPS12"]['CR_Mdps_OutTq'] + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD - self.low_speed_lockout = self.v_ego_raw < 1.0 - - is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"]) - speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS - self.cruise_set_speed = cp.vl["SCC11"]['VSetDis'] * speed_conv - self.standstill = not self.v_ego_raw > 0.1 - - self.angle_steers = cp.vl["SAS11"]['SAS_Angle'] - self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed'] - self.yaw_rate = cp.vl["ESP12"]['YAW_RATE'] - self.main_on = True - self.left_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigLHSw'] - self.right_blinker_on = cp.vl["CGW1"]['CF_Gway_TSigRHSw'] - self.steer_override = abs(cp.vl["MDPS11"]['CR_Mdps_DrvTq']) > STEER_THRESHOLD - self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE - self.steer_error = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail'] - self.brake_error = 0 - self.steer_torque_driver = cp.vl["MDPS11"]['CR_Mdps_DrvTq'] - self.steer_torque_motor = cp.vl["MDPS12"]['CR_Mdps_OutTq'] - self.stopped = cp.vl["SCC11"]['SCCInfoDisplay'] == 4. - - self.user_brake = 0 - - self.brake_pressed = cp.vl["TCS13"]['DriverBraking'] - self.brake_lights = bool(self.brake_pressed) - if (cp.vl["TCS13"]["DriverOverride"] == 0 and cp.vl["TCS13"]['ACC_REQ'] == 1): - self.pedal_gas = 0 + # cruise state + ret.cruiseState.enabled = cp.vl["SCC12"]['ACCMode'] != 0 + ret.cruiseState.available = True + ret.cruiseState.standstill = cp.vl["SCC11"]['SCCInfoDisplay'] == 4. + if ret.cruiseState.enabled: + is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"]) + speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS + ret.cruiseState.speed = cp.vl["SCC11"]['VSetDis'] * speed_conv else: - self.pedal_gas = cp.vl["EMS12"]['TPS'] - self.car_gas = cp.vl["EMS12"]['TPS'] + ret.cruiseState.speed = 0 + + ret.brake = 0 # FIXME + ret.brakePressed = cp.vl["TCS13"]['DriverBraking'] != 0 + ret.brakeLights = ret.brakePressed + ret.gas = cp.vl["EMS12"]['PV_AV_CAN'] / 100 + ret.gasPressed = cp.vl["EMS16"]["CF_Ems_AclAct"] != 0 + ret.espDisabled = cp.vl["TCS15"]['ESC_Off_Step'] != 0 # Gear Selecton - This is not compatible with all Kia/Hyundai's, But is the best way for those it is compatible with gear = cp.vl["LVR12"]["CF_Lvr_Gear"] if gear == 5: - self.gear_shifter = GearShifter.drive + gear_shifter = GearShifter.drive elif gear == 6: - self.gear_shifter = GearShifter.neutral + gear_shifter = GearShifter.neutral elif gear == 0: - self.gear_shifter = GearShifter.park + gear_shifter = GearShifter.park elif gear == 7: - self.gear_shifter = GearShifter.reverse + gear_shifter = GearShifter.reverse else: - self.gear_shifter = GearShifter.unknown + gear_shifter = GearShifter.unknown # Gear Selection via Cluster - For those Kia/Hyundai which are not fully discovered, we can use the Cluster Indicator for Gear Selection, as this seems to be standard over all cars, but is not the preferred method. if cp.vl["CLU15"]["CF_Clu_InhibitD"] == 1: - self.gear_shifter_cluster = GearShifter.drive + gear_shifter_cluster = GearShifter.drive elif cp.vl["CLU15"]["CF_Clu_InhibitN"] == 1: - self.gear_shifter_cluster = GearShifter.neutral + gear_shifter_cluster = GearShifter.neutral elif cp.vl["CLU15"]["CF_Clu_InhibitP"] == 1: - self.gear_shifter_cluster = GearShifter.park + gear_shifter_cluster = GearShifter.park elif cp.vl["CLU15"]["CF_Clu_InhibitR"] == 1: - self.gear_shifter_cluster = GearShifter.reverse + gear_shifter_cluster = GearShifter.reverse else: - self.gear_shifter_cluster = GearShifter.unknown + gear_shifter_cluster = GearShifter.unknown # Gear Selecton via TCU12 gear2 = cp.vl["TCU12"]["CUR_GR"] if gear2 == 0: - self.gear_tcu = GearShifter.park + gear_tcu = GearShifter.park elif gear2 == 14: - self.gear_tcu = GearShifter.reverse + gear_tcu = GearShifter.reverse elif gear2 > 0 and gear2 < 9: # unaware of anything over 8 currently - self.gear_tcu = GearShifter.drive + gear_tcu = GearShifter.drive else: - self.gear_tcu = GearShifter.unknown + gear_tcu = GearShifter.unknown + + # gear shifter + if self.CP.carFingerprint in FEATURES["use_cluster_gears"]: + ret.gearShifter = gear_shifter_cluster + elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]: + ret.gearShifter = gear_tcu + else: + ret.gearShifter = gear_shifter # save the entire LKAS11 and CLU11 self.lkas11 = cp_cam.vl["LKAS11"] self.clu11 = cp.vl["CLU11"] + self.park_brake = cp.vl["CGW1"]['CF_Gway_ParkBrakeSw'] + self.steer_state = cp.vl["MDPS12"]['CF_Mdps_ToiActive'] #0 NOT ACTIVE, 1 ACTIVE + self.steer_warning = cp.vl["MDPS12"]['CF_Mdps_ToiUnavail'] + self.brake_error = 0 + + return ret + + @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_TSigLHSw", "CGW1", 0), + ("CF_Gway_TurnSigLh", "CGW1", 0), + ("CF_Gway_TSigRHSw", "CGW1", 0), + ("CF_Gway_TurnSigRh", "CGW1", 0), + ("CF_Gway_ParkBrakeSw", "CGW1", 0), + + ("BRAKE_ACT", "EMS12", 0), + ("PV_AV_CAN", "EMS12", 0), + ("CF_Ems_AclAct", "EMS16", 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), + + ("CF_Clu_InhibitD", "CLU15", 0), + ("CF_Clu_InhibitP", "CLU15", 0), + ("CF_Clu_InhibitN", "CLU15", 0), + ("CF_Clu_InhibitR", "CLU15", 0), + + ("CF_Lvr_Gear", "LVR12",0), + ("CUR_GR", "TCU12",0), + + ("ACCEnable", "TCS13", 0), + ("DriverBraking", "TCS13", 0), + + ("ESC_Off_Step", "TCS15", 0), + + ("CF_Lvr_GearInf", "LVR11", 0), #Transmission Gear (0 = N or P, 1-8 = Fwd, 14 = Rev) + + ("CR_Mdps_DrvTq", "MDPS11", 0), + + ("CR_Mdps_StrColTq", "MDPS12", 0), + ("CF_Mdps_ToiActive", "MDPS12", 0), + ("CF_Mdps_ToiUnavail", "MDPS12", 0), + ("CF_Mdps_FailStat", "MDPS12", 0), + ("CR_Mdps_OutTq", "MDPS12", 0), + + ("VSetDis", "SCC11", 0), + ("SCCInfoDisplay", "SCC11", 0), + ("ACCMode", "SCC12", 1), + + ("SAS_Angle", "SAS11", 0), + ("SAS_Speed", "SAS11", 0), + ] + + checks = [ + # address, frequency + ("MDPS12", 50), + ("MDPS11", 100), + ("TCS15", 10), + ("TCS13", 50), + ("CLU11", 50), + ("ESP12", 100), + ("EMS12", 100), + ("EMS16", 100), + ("CGW1", 10), + ("CGW4", 5), + ("WHL_SPD11", 50), + ("SCC11", 50), + ("SCC12", 50), + ("SAS11", 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_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_ToiFlt", "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) + ] + + checks = [] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 2db3cd45..2ec2d57a 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -2,36 +2,11 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event -from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.hyundai.carstate import CarState, get_can_parser, get_camera_parser -from selfdrive.car.hyundai.values import Ecu, ECU_FINGERPRINT, CAR, get_hud_alerts, FEATURES, FINGERPRINTS +from selfdrive.car.hyundai.values import Ecu, ECU_FINGERPRINT, CAR, get_hud_alerts, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase -GearShifter = car.CarState.GearShifter -ButtonType = car.CarState.ButtonEvent.Type - class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP - self.VM = VehicleModel(CP) - self.idx = 0 - self.lanes = 0 - self.lkas_request = 0 - - self.gas_pressed_prev = False - self.brake_pressed_prev = False - self.cruise_enabled_prev = False - self.low_speed_alert = False - - # *** init the major players *** - self.CS = CarState(CP) - self.cp = get_can_parser(CP) - self.cp_cam = get_camera_parser(CP) - - self.CC = None - if CarController is not None: - self.CC = CarController(self.cp.dbc_name, CP.carFingerprint) @staticmethod def compute_gb(accel, speed): @@ -39,15 +14,11 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - - ret = car.CarParams.new_message() + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "hyundai" - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay - ret.radarOffCan = True ret.safetyModel = car.CarParams.SafetyModel.hyundai - ret.enableCruise = True # stock acc + ret.radarOffCan = True ret.steerActuatorDelay = 0.1 # Default delay ret.steerRateCost = 0.5 @@ -108,14 +79,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]] ret.minSteerSpeed = 0. - ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this - ret.longitudinalTuning.kpBP = [0.] - ret.longitudinalTuning.kpV = [0.] - ret.longitudinalTuning.kiBP = [0.] - ret.longitudinalTuning.kiV = [0.] - ret.longitudinalTuning.deadzoneBP = [0.] - ret.longitudinalTuning.deadzoneV = [0.] - ret.centerToFront = ret.wheelbase * 0.4 # TODO: get actual value, for now starting with reasonable value for @@ -127,140 +90,32 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - - # no rear steering, at least on the listed cars above - ret.steerRatioRear = 0. - ret.steerControlType = car.CarParams.SteerControlType.torque - - # steer, gas, brake limitations VS speed - ret.steerMaxBP = [0.] - ret.steerMaxV = [1.0] - ret.gasMaxBP = [0.] - ret.gasMaxV = [1.] - ret.brakeMaxBP = [0.] - ret.brakeMaxV = [1.] - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay - ret.openpilotLongitudinalControl = False - - ret.stoppingControl = False - ret.startAccel = 0.0 return ret - # returns a car.CarState def update(self, c, can_strings): - # ******************* do can recv ******************* self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) - self.CS.update(self.cp, self.cp_cam) - # create message - ret = car.CarState.new_message() - + ret = self.CS.update(self.cp, self.cp_cam) ret.canValid = self.cp.can_valid and self.cp_cam.can_valid - # speeds - ret.vEgo = self.CS.v_ego - ret.vEgoRaw = self.CS.v_ego_raw - ret.aEgo = self.CS.a_ego - ret.yawRate = self.CS.yaw_rate - ret.standstill = self.CS.standstill - ret.wheelSpeeds.fl = self.CS.v_wheel_fl - ret.wheelSpeeds.fr = self.CS.v_wheel_fr - ret.wheelSpeeds.rl = self.CS.v_wheel_rl - ret.wheelSpeeds.rr = self.CS.v_wheel_rr - - # gear shifter - if self.CP.carFingerprint in FEATURES["use_cluster_gears"]: - ret.gearShifter = self.CS.gear_shifter_cluster - elif self.CP.carFingerprint in FEATURES["use_tcu_gears"]: - ret.gearShifter = self.CS.gear_tcu - else: - ret.gearShifter = self.CS.gear_shifter - - # gas pedal - ret.gas = self.CS.car_gas - ret.gasPressed = self.CS.pedal_gas > 1e-3 # tolerance to avoid false press reading - - # brake pedal - ret.brake = self.CS.user_brake - ret.brakePressed = self.CS.brake_pressed != 0 - ret.brakeLights = self.CS.brake_lights - - # steering wheel - ret.steeringAngle = self.CS.angle_steers - ret.steeringRate = self.CS.angle_steers_rate # it's unsigned - - ret.steeringTorque = self.CS.steer_torque_driver - ret.steeringPressed = self.CS.steer_override - - # cruise state - ret.cruiseState.enabled = self.CS.pcm_acc_status != 0 - if self.CS.pcm_acc_status != 0: - ret.cruiseState.speed = self.CS.cruise_set_speed - else: - ret.cruiseState.speed = 0 - ret.cruiseState.available = bool(self.CS.main_on) - ret.cruiseState.standstill = False - # TODO: button presses - buttonEvents = [] + ret.buttonEvents = [] - if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.leftBlinker - be.pressed = self.CS.left_blinker_on != 0 - buttonEvents.append(be) - - if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.rightBlinker - be.pressed = self.CS.right_blinker_on != 0 - buttonEvents.append(be) - - ret.buttonEvents = buttonEvents - ret.leftBlinker = bool(self.CS.left_blinker_on) - ret.rightBlinker = bool(self.CS.right_blinker_on) - - ret.doorOpen = not self.CS.door_all_closed - ret.seatbeltUnlatched = not self.CS.seatbelt - - # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) - if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: - self.low_speed_alert = True - if ret.vEgo > (self.CP.minSteerSpeed + 4.): - self.low_speed_alert = False - - events = [] - if not ret.gearShifter == GearShifter.drive: - events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.doorOpen: - events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.seatbeltUnlatched: - events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.esp_disabled: - events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if not self.CS.main_on: - events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gearShifter == GearShifter.reverse: - events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if self.CS.steer_error: - events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) + events = self.create_common_events(ret) if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - # disable on pedals rising edge or when brake is pressed and speed isn't zero - if (ret.gasPressed and not self.gas_pressed_prev) or \ - (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgoRaw > 0.1)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) - + # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s) + if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.: + self.low_speed_alert = True + if ret.vEgo > (self.CP.minSteerSpeed + 4.): + self.low_speed_alert = False if self.low_speed_alert: events.append(create_event('belowSteerSpeed', [ET.WARNING])) @@ -270,7 +125,8 @@ class CarInterface(CarInterfaceBase): self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled - return ret.as_reader() + self.CS.out = ret.as_reader() + return self.CS.out def apply(self, c): diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py index d29aba44..7fe689c6 100644 --- a/selfdrive/car/hyundai/values.py +++ b/selfdrive/car/hyundai/values.py @@ -44,9 +44,14 @@ FINGERPRINTS = { { 67: 8, 68: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 7, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 5, 897: 8, 902: 8, 903: 6, 916: 8, 1024: 2, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1184: 8, 1265: 4, 1280: 1, 1281: 3, 1287: 4, 1292: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1334: 8, 1335: 8, 1345: 8, 1363: 8, 1369: 8, 1370: 8, 1378: 4, 1379: 8, 1384: 5, 1407: 8, 1419: 8, 1427: 6, 1434: 2, 1456: 4 }], - CAR.KIA_OPTIMA: [{ - 64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 1952: 8, 1960: 8, 1988: 8, 1996: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 - }], + CAR.KIA_OPTIMA: [ + { + 64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 273: 8, 274: 8, 275: 8, 339: 8, 356: 4, 399: 8, 447: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 884: 8, 897: 8, 899: 8, 902: 8, 903: 6, 909: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1186: 2, 1191: 2, 1253: 8, 1254: 8, 1255: 8, 1265: 4, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1414: 3, 1415: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1486: 8, 1487: 8, 1491: 8, 1530: 8, 1532: 5, 1952: 8, 1960: 8, 1988: 8, 1996: 8, 2001: 8, 2004: 8, 2008: 8, 2009: 8, 2012: 8, 2016: 8, 2017: 8, 2024: 8, 2025: 8 + }, + { + 64: 8, 66: 8, 67: 8, 68: 8, 127: 8, 128: 8, 129: 8, 273: 8, 274: 8, 275: 8, 339: 8, 354: 3, 356: 4, 399: 8, 512: 6, 544: 8, 593: 8, 608: 8, 688: 5, 790: 8, 809: 8, 832: 8, 897: 8, 899: 8, 902: 8, 903: 6, 912: 7, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1151: 6, 1168: 7, 1170: 8, 1265: 4, 1268: 8, 1280: 1, 1282: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1349: 8, 1351: 8, 1353: 8, 1356: 8, 1363: 8, 1365: 8, 1366: 8, 1367: 8, 1369: 8, 1407: 8, 1419: 8, 1425: 2, 1427: 6, 1440: 8, 1456: 4, 1470: 8, 1472: 8, 1491: 8, 1492: 8 + }, + ], CAR.KIA_SORENTO: [{ 67: 8, 68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 356: 4, 544: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1064: 8, 1078: 4, 1107: 5, 1136: 8, 1151: 6, 1168: 7, 1170: 8, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1370: 8, 1371: 8, 1384: 8, 1407: 8, 1411: 8, 1419: 8, 1425: 2, 1427: 6, 1444: 8, 1456: 4, 1470: 8, 1489: 1 }], diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index e98b2681..a25df91f 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -4,14 +4,31 @@ from cereal import car from common.kalman.simple_kalman import KF1D from common.realtime import DT_CTRL from selfdrive.car import gen_empty_fingerprint +from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event +from selfdrive.controls.lib.vehicle_model import VehicleModel GearShifter = car.CarState.GearShifter # generic car and radar interfaces class CarInterfaceBase(): - def __init__(self, CP, CarController): - pass + def __init__(self, CP, CarController, CarState): + self.CP = CP + self.VM = VehicleModel(CP) + + self.frame = 0 + self.gas_pressed_prev = False + self.brake_pressed_prev = False + self.cruise_enabled_prev = False + self.low_speed_alert = False + + self.CS = CarState(CP) + self.cp = self.CS.get_can_parser(CP) + self.cp_cam = self.CS.get_cam_can_parser(CP) + + self.CC = None + if CarController is not None: + self.CC = CarController(self.cp.dbc_name, CP, self.VM) @staticmethod def calc_accel_override(a_ego, a_target, v_ego, v_target): @@ -25,6 +42,37 @@ class CarInterfaceBase(): def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): raise NotImplementedError + # returns a set of default params to avoid repetition in car specific params + @staticmethod + def get_std_params(candidate, fingerprint, has_relay): + ret = car.CarParams.new_message() + ret.carFingerprint = candidate + ret.isPandaBlack = has_relay + + # standard ALC params + ret.steerControlType = car.CarParams.SteerControlType.torque + ret.steerMaxBP = [0.] + ret.steerMaxV = [1.] + + # stock ACC by default + ret.enableCruise = True + ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this + ret.steerRatioRear = 0. # no rear steering, at least on the listed cars aboveA + ret.gasMaxBP = [0.] + ret.gasMaxV = [.5] # half max brake + ret.brakeMaxBP = [0.] + ret.brakeMaxV = [1.] + ret.openpilotLongitudinalControl = False + ret.startAccel = 0.0 + ret.stoppingControl = False + ret.longitudinalTuning.deadzoneBP = [0.] + ret.longitudinalTuning.deadzoneV = [0.] + ret.longitudinalTuning.kpBP = [0.] + ret.longitudinalTuning.kpV = [1.] + ret.longitudinalTuning.kiBP = [0.] + ret.longitudinalTuning.kiV = [1.] + return ret + # returns a car.CarState, pass in car.CarControl def update(self, c, can_strings): raise NotImplementedError @@ -33,6 +81,39 @@ class CarInterfaceBase(): def apply(self, c): raise NotImplementedError + def create_common_events(self, cs_out, extra_gears=[], gas_resume_speed=-1): + events = [] + + if cs_out.doorOpen: + events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if cs_out.seatbeltUnlatched: + events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if cs_out.gearShifter != GearShifter.drive and cs_out.gearShifter not in extra_gears: + events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if cs_out.gearShifter == GearShifter.reverse: + events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) + if not cs_out.cruiseState.available: + events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) + if cs_out.espDisabled: + events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + if cs_out.gasPressed: + events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) + + # TODO: move this stuff to the capnp strut + if getattr(self.CS, "steer_error", False): + events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) + elif getattr(self.CS, "steer_warning", False): + events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) + + # Disable on rising edge of gas or brake. Also disable on brake when speed > 0. + # Optionally allow to press gas at zero speed to resume. + # e.g. Chrysler does not spam the resume button yet, so resuming with gas is handy. FIXME! + if (cs_out.gasPressed and (not self.gas_pressed_prev) and cs_out.vEgo > gas_resume_speed) or \ + (cs_out.brakePressed and (not self.brake_pressed_prev or not cs_out.standstill)): + events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) + + return events + class RadarInterfaceBase(): def __init__(self, CP): self.pts = {} @@ -51,8 +132,6 @@ class CarStateBase: def __init__(self, CP): self.CP = CP self.car_fingerprint = CP.carFingerprint - self.left_blinker_on = 0 - self.right_blinker_on = 0 self.cruise_buttons = 0 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) @@ -74,3 +153,7 @@ class CarStateBase: return {'P': GearShifter.park, 'R': GearShifter.reverse, 'N': GearShifter.neutral, 'E': GearShifter.eco, 'T': GearShifter.manumatic, 'D': GearShifter.drive, 'S': GearShifter.sport, 'L': GearShifter.low, 'B': GearShifter.brake}.get(gear, GearShifter.unknown) + + @staticmethod + def get_cam_can_parser(CP): + return None diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index 09c5b5c6..dc972440 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -14,7 +14,7 @@ LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS) class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): + def __init__(self, CP, CarController, CarState): self.CP = CP self.CC = CarController @@ -35,17 +35,9 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - - ret = car.CarParams.new_message() - + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "mock" - ret.carFingerprint = candidate - ret.safetyModel = car.CarParams.SafetyModel.noOutput - ret.openpilotLongitudinalControl = False - - # FIXME: hardcoding honda civic 2016 touring params so they can be used to - # scale unknown params for other cars ret.mass = 1700. ret.rotationalInertia = 2500. ret.wheelbase = 2.70 @@ -53,22 +45,6 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13. # reasonable ret.tireStiffnessFront = 1e6 # very stiff to neglect slip ret.tireStiffnessRear = 1e6 # very stiff to neglect slip - ret.steerRatioRear = 0. - - ret.steerMaxBP = [0.] - ret.steerMaxV = [0.] # 2/3rd torque allowed above 45 kph - ret.gasMaxBP = [0.] - ret.gasMaxV = [0.] - ret.brakeMaxBP = [0.] - ret.brakeMaxV = [0.] - - ret.longitudinalTuning.kpBP = [0.] - ret.longitudinalTuning.kpV = [0.] - ret.longitudinalTuning.kiBP = [0.] - ret.longitudinalTuning.kiV = [0.] - ret.longitudinalTuning.deadzoneBP = [0.] - ret.longitudinalTuning.deadzoneV = [0.] - ret.steerActuatorDelay = 0. return ret diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index dce4df81..335e1893 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -18,19 +18,17 @@ class CarControllerParams(): class CarController(): - def __init__(self, car_fingerprint): + def __init__(self, dbc_name, CP, VM): self.lkas_active = False - self.steer_idx = 0 self.apply_steer_last = 0 - self.car_fingerprint = car_fingerprint self.es_distance_cnt = -1 self.es_lkas_cnt = -1 self.steer_rate_limited = False # Setup detection helper. Routes commands to # an appropriate CAN bus number. - self.params = CarControllerParams(car_fingerprint) - self.packer = CANPacker(DBC[car_fingerprint]['pt']) + self.params = CarControllerParams(CP.carFingerprint) + self.packer = CANPacker(DBC[CP.carFingerprint]['pt']) def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line): """ Controls thread """ @@ -50,7 +48,7 @@ class CarController(): # limits due to driver torque new_steer = int(round(apply_steer)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P) + 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 lkas_enabled = enabled diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 10500ce6..a6cd6e9f 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -1,138 +1,136 @@ import copy +from cereal import car from selfdrive.config import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD -def get_powertrain_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), - ("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), - ("Units", "Dash_State", 1), - ] - - checks = [ - # sig_address, frequency - ("Dashlights", 10), - ("CruiseControl", 20), - ("Wheel_Speeds", 50), - ("Steering_Torque", 50), - ("BodyInfo", 10), - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) - - -def get_camera_can_parser(CP): - signals = [ - ("Cruise_Set_Speed", "ES_DashStatus", 0), - - ("Counter", "ES_Distance", 0), - ("Signal1", "ES_Distance", 0), - ("Signal2", "ES_Distance", 0), - ("Main", "ES_Distance", 0), - ("Signal3", "ES_Distance", 0), - - ("Checksum", "ES_LKAS_State", 0), - ("Counter", "ES_LKAS_State", 0), - ("Keep_Hands_On_Wheel", "ES_LKAS_State", 0), - ("Empty_Box", "ES_LKAS_State", 0), - ("Signal1", "ES_LKAS_State", 0), - ("LKAS_ACTIVE", "ES_LKAS_State", 0), - ("Signal2", "ES_LKAS_State", 0), - ("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0), - ("LKAS_ENABLE_3", "ES_LKAS_State", 0), - ("Signal3", "ES_LKAS_State", 0), - ("LKAS_ENABLE_2", "ES_LKAS_State", 0), - ("Signal4", "ES_LKAS_State", 0), - ("LKAS_Left_Line_Visible", "ES_LKAS_State", 0), - ("Signal6", "ES_LKAS_State", 0), - ("LKAS_Right_Line_Visible", "ES_LKAS_State", 0), - ("Signal7", "ES_LKAS_State", 0), - ("FCW_Cont_Beep", "ES_LKAS_State", 0), - ("FCW_Repeated_Beep", "ES_LKAS_State", 0), - ("Throttle_Management_Activated", "ES_LKAS_State", 0), - ("Traffic_light_Ahead", "ES_LKAS_State", 0), - ("Right_Depart", "ES_LKAS_State", 0), - ("Signal5", "ES_LKAS_State", 0), - - ] - - checks = [ - ("ES_DashStatus", 10), - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) - class CarState(CarStateBase): def __init__(self, CP): super().__init__(CP) - # initialize can parser self.left_blinker_cnt = 0 self.right_blinker_cnt = 0 def update(self, cp, cp_cam): + ret = car.CarState.new_message() - self.pedal_gas = cp.vl["Throttle"]['Throttle_Pedal'] - self.brake_pressure = cp.vl["Brake_Pedal"]['Brake_Pedal'] - self.user_gas_pressed = self.pedal_gas > 0 - self.brake_pressed = self.brake_pressure > 0 - self.brake_lights = bool(self.brake_pressed) + ret.gas = cp.vl["Throttle"]['Throttle_Pedal'] / 255. + ret.gasPressed = ret.gas > 1e-5 + ret.brakePressed = cp.vl["Brake_Pedal"]['Brake_Pedal'] > 1e-5 + ret.brakeLights = ret.brakePressed - self.v_wheel_fl = cp.vl["Wheel_Speeds"]['FL'] * CV.KPH_TO_MS - self.v_wheel_fr = cp.vl["Wheel_Speeds"]['FR'] * CV.KPH_TO_MS - self.v_wheel_rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS - self.v_wheel_rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS - - self.v_cruise_pcm = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed'] - # 1 = imperial, 6 = metric - if cp.vl["Dash_State"]['Units'] == 1: - self.v_cruise_pcm *= CV.MPH_TO_KPH - - self.v_ego_raw = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4. + ret.wheelSpeeds.fl = cp.vl["Wheel_Speeds"]['FL'] * CV.KPH_TO_MS + ret.wheelSpeeds.fr = cp.vl["Wheel_Speeds"]['FR'] * CV.KPH_TO_MS + ret.wheelSpeeds.rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS + ret.wheelSpeeds.rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS + ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4. # Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default - self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) - - self.standstill = self.v_ego_raw < 0.01 - - self.prev_left_blinker_on = self.left_blinker_on - self.prev_right_blinker_on = self.right_blinker_on + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) + ret.standstill = ret.vEgoRaw < 0.01 # continuous blinker signals for assisted lane change self.left_blinker_cnt = 50 if cp.vl["Dashlights"]['LEFT_BLINKER'] else max(self.left_blinker_cnt - 1, 0) - self.left_blinker_on = self.left_blinker_cnt > 0 - + ret.leftBlinker = self.left_blinker_cnt > 0 self.right_blinker_cnt = 50 if cp.vl["Dashlights"]['RIGHT_BLINKER'] else max(self.right_blinker_cnt - 1, 0) - self.right_blinker_on = self.right_blinker_cnt > 0 + ret.rightBlinker = self.right_blinker_cnt > 0 - self.seatbelt_unlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1 - self.steer_torque_driver = cp.vl["Steering_Torque"]['Steer_Torque_Sensor'] - self.acc_active = cp.vl["CruiseControl"]['Cruise_Activated'] - self.main_on = cp.vl["CruiseControl"]['Cruise_On'] - self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.car_fingerprint] - self.angle_steers = cp.vl["Steering_Torque"]['Steering_Angle'] - self.door_open = any([cp.vl["BodyInfo"]['DOOR_OPEN_RR'], + ret.steeringAngle = cp.vl["Steering_Torque"]['Steering_Angle'] + ret.steeringTorque = cp.vl["Steering_Torque"]['Steer_Torque_Sensor'] + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.car_fingerprint] + + ret.cruiseState.enabled = cp.vl["CruiseControl"]['Cruise_Activated'] != 0 + ret.cruiseState.available = cp.vl["CruiseControl"]['Cruise_On'] != 0 + ret.cruiseState.speed = cp_cam.vl["ES_DashStatus"]['Cruise_Set_Speed'] * CV.KPH_TO_MS + # 1 = imperial, 6 = metric + if cp.vl["Dash_State"]['Units'] == 1: + ret.cruiseState.speed *= CV.MPH_TO_KPH + + ret.seatbeltUnlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1 + ret.doorOpen = any([cp.vl["BodyInfo"]['DOOR_OPEN_RR'], cp.vl["BodyInfo"]['DOOR_OPEN_RL'], cp.vl["BodyInfo"]['DOOR_OPEN_FR'], cp.vl["BodyInfo"]['DOOR_OPEN_FL']]) self.es_distance_msg = copy.copy(cp_cam.vl["ES_Distance"]) self.es_lkas_msg = copy.copy(cp_cam.vl["ES_LKAS_State"]) + + return ret + + @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), + ("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), + ("Units", "Dash_State", 1), + ] + + checks = [ + # sig_address, frequency + ("Dashlights", 10), + ("CruiseControl", 20), + ("Wheel_Speeds", 50), + ("Steering_Torque", 50), + ("BodyInfo", 10), + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) + + @staticmethod + def get_cam_can_parser(CP): + signals = [ + ("Cruise_Set_Speed", "ES_DashStatus", 0), + + ("Counter", "ES_Distance", 0), + ("Signal1", "ES_Distance", 0), + ("Signal2", "ES_Distance", 0), + ("Main", "ES_Distance", 0), + ("Signal3", "ES_Distance", 0), + + ("Checksum", "ES_LKAS_State", 0), + ("Counter", "ES_LKAS_State", 0), + ("Keep_Hands_On_Wheel", "ES_LKAS_State", 0), + ("Empty_Box", "ES_LKAS_State", 0), + ("Signal1", "ES_LKAS_State", 0), + ("LKAS_ACTIVE", "ES_LKAS_State", 0), + ("Signal2", "ES_LKAS_State", 0), + ("Backward_Speed_Limit_Menu", "ES_LKAS_State", 0), + ("LKAS_ENABLE_3", "ES_LKAS_State", 0), + ("Signal3", "ES_LKAS_State", 0), + ("LKAS_ENABLE_2", "ES_LKAS_State", 0), + ("Signal4", "ES_LKAS_State", 0), + ("LKAS_Left_Line_Visible", "ES_LKAS_State", 0), + ("Signal6", "ES_LKAS_State", 0), + ("LKAS_Right_Line_Visible", "ES_LKAS_State", 0), + ("Signal7", "ES_LKAS_State", 0), + ("FCW_Cont_Beep", "ES_LKAS_State", 0), + ("FCW_Repeated_Beep", "ES_LKAS_State", 0), + ("Throttle_Management_Activated", "ES_LKAS_State", 0), + ("Traffic_light_Ahead", "ES_LKAS_State", 0), + ("Right_Depart", "ES_LKAS_State", 0), + ("Signal5", "ES_LKAS_State", 0), + + ] + + checks = [ + ("ES_DashStatus", 10), + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index d4388727..0f2f4176 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -2,33 +2,11 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET -from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.subaru.values import CAR -from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser, get_camera_can_parser from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase -ButtonType = car.CarState.ButtonEvent.Type - class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP - - self.frame = 0 - self.acc_active_prev = 0 - self.gas_pressed_prev = False - - # *** init the major players *** - self.CS = CarState(CP) - self.VM = VehicleModel(CP) - self.pt_cp = get_powertrain_can_parser(CP) - self.cam_cp = get_camera_can_parser(CP) - - self.gas_pressed_prev = False - - self.CC = None - if CarController is not None: - self.CC = CarController(CP.carFingerprint) @staticmethod def compute_gb(accel, speed): @@ -36,16 +14,12 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - ret = car.CarParams.new_message() + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "subaru" ret.radarOffCan = True - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay ret.safetyModel = car.CarParams.SafetyModel.subaru - ret.enableCruise = True - # force openpilot to fake the stock camera, since car harness is not supported yet and old style giraffe (with switches) # was never released ret.enableCamera = True @@ -62,26 +36,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 0.00005 ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0., 20.], [0., 20.]] ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2, 0.3], [0.02, 0.03]] - ret.steerMaxBP = [0.] # m/s - ret.steerMaxV = [1.] - - ret.steerControlType = car.CarParams.SteerControlType.torque - ret.steerRatioRear = 0. - # testing tuning - - # No long control in subaru - ret.gasMaxBP = [0.] - ret.gasMaxV = [0.] - ret.brakeMaxBP = [0.] - ret.brakeMaxV = [0.] - ret.longitudinalTuning.deadzoneBP = [0.] - ret.longitudinalTuning.deadzoneV = [0.] - ret.longitudinalTuning.kpBP = [0.] - ret.longitudinalTuning.kpV = [0.] - ret.longitudinalTuning.kiBP = [0.] - ret.longitudinalTuning.kiV = [0.] - - # end from gm # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase @@ -95,97 +49,35 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def update(self, c, can_strings): - self.pt_cp.update_strings(can_strings) - self.cam_cp.update_strings(can_strings) + self.cp.update_strings(can_strings) + self.cp_cam.update_strings(can_strings) - self.CS.update(self.pt_cp, self.cam_cp) + ret = self.CS.update(self.cp, self.cp_cam) - # create message - ret = car.CarState.new_message() - - ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid - - # speeds - ret.vEgo = self.CS.v_ego - ret.aEgo = self.CS.a_ego - ret.vEgoRaw = self.CS.v_ego_raw - ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) - ret.standstill = self.CS.standstill - ret.wheelSpeeds.fl = self.CS.v_wheel_fl - ret.wheelSpeeds.fr = self.CS.v_wheel_fr - ret.wheelSpeeds.rl = self.CS.v_wheel_rl - ret.wheelSpeeds.rr = self.CS.v_wheel_rr - - # steering wheel - ret.steeringAngle = self.CS.angle_steers - - # torque and user override. Driver awareness - # timer resets when the user uses the steering wheel. - ret.steeringPressed = self.CS.steer_override - ret.steeringTorque = self.CS.steer_torque_driver + 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 - - ret.gas = self.CS.pedal_gas / 255. - ret.gasPressed = self.CS.user_gas_pressed - - # cruise state - ret.cruiseState.enabled = bool(self.CS.acc_active) - ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS - ret.cruiseState.available = bool(self.CS.main_on) - ret.cruiseState.speedOffset = 0. - - ret.leftBlinker = self.CS.left_blinker_on - ret.rightBlinker = self.CS.right_blinker_on - ret.seatbeltUnlatched = self.CS.seatbelt_unlatched - ret.doorOpen = self.CS.door_open + ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) buttonEvents = [] - - # blinkers - if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.leftBlinker - be.pressed = self.CS.left_blinker_on - buttonEvents.append(be) - - if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.rightBlinker - be.pressed = self.CS.right_blinker_on - buttonEvents.append(be) - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.accelCruise + be.type = car.CarState.ButtonEvent.Type.accelCruise buttonEvents.append(be) + events = self.create_common_events(ret, extra_gears=[car.CarState.GearShifter.unknown]) - events = [] - if ret.seatbeltUnlatched: - events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - - if ret.doorOpen: - events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - - if self.CS.acc_active and not self.acc_active_prev: + if ret.cruiseState.enabled and not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) - if not self.CS.acc_active: + if not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - # disable on gas pedal rising edge - if (ret.gasPressed and not self.gas_pressed_prev): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) - ret.events = events - # update previous brake/gas pressed self.gas_pressed_prev = ret.gasPressed - self.acc_active_prev = self.CS.acc_active + self.brake_pressed_prev = ret.brakePressed + self.cruise_enabled_prev = ret.cruiseState.enabled - # cast to reader so it can't be modified - return ret.as_reader() + self.CS.out = ret.as_reader() + return self.CS.out def apply(self, c): can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 24c3bfed..6f21fec4 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,9 +1,8 @@ from cereal import car -from common.numpy_fast import clip, interp +from common.numpy_fast import clip from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, make_can_msg from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ - create_ipas_steer_command, create_accel_command, \ - create_acc_cancel_command, create_fcw_command + create_accel_command, create_acc_cancel_command, create_fcw_command from selfdrive.car.toyota.values import Ecu, CAR, STATIC_MSGS, SteerLimitParams from opendbc.can.packer import CANPacker @@ -15,14 +14,6 @@ ACCEL_MAX = 1.5 # 1.5 m/s2 ACCEL_MIN = -3.0 # 3 m/s2 ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN) - -# Steer angle limits (tested at the Crows Landing track and considered ok) -ANGLE_MAX_BP = [0., 5.] -ANGLE_MAX_V = [510., 300.] -ANGLE_DELTA_BP = [0., 5., 15.] -ANGLE_DELTA_V = [5., .8, .15] # windup limit -ANGLE_DELTA_VU = [5., 3.5, 0.4] # unwind limit - def accel_hysteresis(accel, accel_steady, enabled): # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command @@ -51,53 +42,22 @@ def process_hud_alert(hud_alert): return steer, fcw -def ipas_state_transition(steer_angle_enabled, enabled, ipas_active, ipas_reset_counter): - - if enabled and not steer_angle_enabled: - #ipas_reset_counter = max(0, ipas_reset_counter - 1) - #if ipas_reset_counter == 0: - # steer_angle_enabled = True - #else: - # steer_angle_enabled = False - #return steer_angle_enabled, ipas_reset_counter - return True, 0 - - elif enabled and steer_angle_enabled: - if steer_angle_enabled and not ipas_active: - ipas_reset_counter += 1 - else: - ipas_reset_counter = 0 - if ipas_reset_counter > 10: # try every 0.1s - steer_angle_enabled = False - return steer_angle_enabled, ipas_reset_counter - - else: - return False, 0 - - class CarController(): - def __init__(self, dbc_name, car_fingerprint, enable_camera, enable_dsu, enable_apg): + def __init__(self, dbc_name, CP, VM): self.braking = False - # redundant safety check with the board - self.controls_allowed = True self.last_steer = 0 - self.last_angle = 0 self.accel_steady = 0. - self.car_fingerprint = car_fingerprint + self.car_fingerprint = CP.carFingerprint self.alert_active = False self.last_standstill = False self.standstill_req = False - self.angle_control = False - self.steer_angle_enabled = False - self.ipas_reset_counter = 0 self.last_fault_frame = -200 self.steer_rate_limited = False self.fake_ecus = set() - if enable_camera: self.fake_ecus.add(Ecu.fwdCamera) - if enable_dsu: self.fake_ecus.add(Ecu.dsu) - if enable_apg: self.fake_ecus.add(Ecu.apgs) + if CP.enableCamera: self.fake_ecus.add(Ecu.fwdCamera) + if CP.enableDsu: self.fake_ecus.add(Ecu.dsu) self.packer = CANPacker(dbc_name) @@ -122,7 +82,7 @@ class CarController(): # steer torque new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) - apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams) + apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, SteerLimitParams) self.steer_rate_limited = new_steer != apply_steer # only cut torque when steer state is a known fault @@ -136,41 +96,20 @@ class CarController(): else: apply_steer_req = 1 - self.steer_angle_enabled, self.ipas_reset_counter = \ - ipas_state_transition(self.steer_angle_enabled, enabled, CS.ipas_active, self.ipas_reset_counter) - #print("{0} {1} {2}".format(self.steer_angle_enabled, self.ipas_reset_counter, CS.ipas_active)) - - # steer angle - if self.steer_angle_enabled and CS.ipas_active: - apply_angle = actuators.steerAngle - angle_lim = interp(CS.v_ego, ANGLE_MAX_BP, ANGLE_MAX_V) - apply_angle = clip(apply_angle, -angle_lim, angle_lim) - - # windup slower - if self.last_angle * apply_angle > 0. and abs(apply_angle) > abs(self.last_angle): - angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_V) - else: - angle_rate_lim = interp(CS.v_ego, ANGLE_DELTA_BP, ANGLE_DELTA_VU) - - apply_angle = clip(apply_angle, self.last_angle - angle_rate_lim, self.last_angle + angle_rate_lim) - else: - apply_angle = CS.angle_steers - if not enabled and CS.pcm_acc_status: # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated pcm_cancel_cmd = 1 # on entering standstill, send standstill request - if CS.standstill and not self.last_standstill: + if CS.out.standstill and not self.last_standstill: self.standstill_req = True if CS.pcm_acc_status != 8: # pcm entered standstill or it's disabled self.standstill_req = False self.last_steer = apply_steer - self.last_angle = apply_angle self.last_accel = apply_accel - self.last_standstill = CS.standstill + self.last_standstill = CS.out.standstill can_sends = [] @@ -181,20 +120,11 @@ class CarController(): # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages if Ecu.fwdCamera in self.fake_ecus: - if self.angle_control: - can_sends.append(create_steer_command(self.packer, 0., 0, frame)) - else: - can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) - - if self.angle_control: - can_sends.append(create_ipas_steer_command(self.packer, apply_angle, self.steer_angle_enabled, - Ecu.apgs in self.fake_ecus)) - elif Ecu.apgs in self.fake_ecus: - can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) + can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) # we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus): - lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged + lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged # Lexus IS uses a different cancellation message if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index cb172b59..e5d58f0d 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,85 +1,10 @@ +from cereal import car from common.numpy_fast import mean 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, TSS2_CAR, NO_DSU_CAR - - -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", "SEATS_DOORS", 1), - ("DOOR_OPEN_FR", "SEATS_DOORS", 1), - ("DOOR_OPEN_RL", "SEATS_DOORS", 1), - ("DOOR_OPEN_RR", "SEATS_DOORS", 1), - ("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1), - ("TC_DISABLED", "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), - ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), - ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0), - ("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers - ("LKA_STATE", "EPS_STATUS", 0), - ("IPAS_STATE", "EPS_STATUS", 1), - ("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0), - ("AUTO_HIGH_BEAM", "LIGHT_STALK", 0), - ] - - checks = [ - ("BRAKE_MODULE", 40), - ("GAS_PEDAL", 33), - ("WHEEL_SPEEDS", 80), - ("STEER_ANGLE_SENSOR", 80), - ("PCM_CRUISE", 33), - ("STEER_TORQUE_SENSOR", 50), - ("EPS_STATUS", 25), - ] - - if CP.carFingerprint == CAR.LEXUS_IS: - signals.append(("MAIN_ON", "DSU_CRUISE", 0)) - signals.append(("SET_SPEED", "DSU_CRUISE", 0)) - 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)) - checks.append(("PCM_CRUISE_2", 33)) - - if CP.carFingerprint in NO_DSU_CAR: - signals += [("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0)] - - if CP.carFingerprint == CAR.PRIUS: - signals += [("STATE", "AUTOPARK_STATUS", 0)] - - # 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)) - checks.append(("GAS_SENSOR", 50)) - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0) - - -def get_cam_can_parser(CP): - - signals = [("FORCE", "PRE_COLLISION", 0), ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0)] - - # use steering message to check if panda is connected to frc - checks = [("STEERING_LKA", 42)] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) +from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_STOP_TIMER_CAR class CarState(CarStateBase): @@ -87,79 +12,167 @@ class CarState(CarStateBase): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = can_define.dv["GEAR_PACKET"]['GEAR'] + + # All TSS2 car have the accurate sensor + self.accurate_steer_angle_seen = CP.carFingerprint in TSS2_CAR + + # On NO_DSU cars but not TSS2 cars the cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] + # is zeroed to where the steering angle is at start. + # Need to apply an offset as soon as the steering angle measurements are both received + self.needs_angle_offset = CP.carFingerprint not in TSS2_CAR self.angle_offset = 0. - self.init_angle_offset = False def update(self, cp, cp_cam): - # update prevs, update must run once per loop - self.prev_left_blinker_on = self.left_blinker_on - self.prev_right_blinker_on = self.right_blinker_on + ret = car.CarState.new_message() - self.door_all_closed = not any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'], - cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']]) - self.seatbelt = not cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] + ret.doorOpen = any([cp.vl["SEATS_DOORS"]['DOOR_OPEN_FL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_FR'], + cp.vl["SEATS_DOORS"]['DOOR_OPEN_RL'], cp.vl["SEATS_DOORS"]['DOOR_OPEN_RR']]) + ret.seatbeltUnlatched = cp.vl["SEATS_DOORS"]['SEATBELT_DRIVER_UNLATCHED'] != 0 - self.brake_pressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] + ret.brakePressed = cp.vl["BRAKE_MODULE"]['BRAKE_PRESSED'] != 0 + ret.brakeLights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or ret.brakePressed) if self.CP.enableGasInterceptor: - self.pedal_gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2. + ret.gas = (cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS'] + cp.vl["GAS_SENSOR"]['INTERCEPTOR_GAS2']) / 2. + ret.gasPressed = ret.gas > 15 else: - self.pedal_gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] - self.esp_disabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] + ret.gas = cp.vl["GAS_PEDAL"]['GAS_PEDAL'] + ret.gasPressed = ret.gas > 1e-5 - self.v_wheel_fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS - self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS - self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS - self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS - self.v_ego_raw = mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]) - self.v_ego, self.a_ego = self.update_speed_kf(self.v_ego_raw) + ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FL'] * CV.KPH_TO_MS + ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS + ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS + ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS + ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - self.standstill = not self.v_ego_raw > 0.001 + ret.standstill = ret.vEgoRaw < 0.001 - if self.CP.carFingerprint in TSS2_CAR: - self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - elif self.CP.carFingerprint in NO_DSU_CAR: - # cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] is zeroed to where the steering angle is at start. - # need to apply an offset as soon as the steering angle measurements are both received - self.angle_steers = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset - angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] - if abs(angle_wheel) > 1e-3 and abs(self.angle_steers) > 1e-3 and not self.init_angle_offset: - self.init_angle_offset = True - self.angle_offset = self.angle_steers - angle_wheel + # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero + if abs(cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE']) > 1e-3: + self.accurate_steer_angle_seen = True + + if self.accurate_steer_angle_seen: + ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset + + if self.needs_angle_offset: + angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] + if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3: + self.needs_angle_offset = False + self.angle_offset = ret.steeringAngle - angle_wheel else: - self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] - self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE'] + ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION'] + + ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE'] can_gear = int(cp.vl["GEAR_PACKET"]['GEAR']) - self.gear_shifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) - if self.CP.carFingerprint == CAR.LEXUS_IS: - self.main_on = cp.vl["DSU_CRUISE"]['MAIN_ON'] - else: - self.main_on = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] - self.left_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1 - self.right_blinker_on = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2 + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None)) + ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1 + ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2 - # 2 is standby, 10 is active. TODO: check that everything else is really a faulty state - self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE'] - self.steer_error = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5] - self.ipas_active = cp.vl['EPS_STATUS']['IPAS_STATE'] == 3 - self.brake_error = 0 - self.steer_torque_driver = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER'] - self.steer_torque_motor = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS'] + ret.steeringTorque = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_DRIVER'] + ret.steeringTorqueEps = cp.vl["STEER_TORQUE_SENSOR"]['STEER_TORQUE_EPS'] # we could use the override bit from dbc, but it's triggered at too high torque values - self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD + ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD - self.user_brake = 0 if self.CP.carFingerprint == CAR.LEXUS_IS: - self.v_cruise_pcm = cp.vl["DSU_CRUISE"]['SET_SPEED'] + ret.cruiseState.available = cp.vl["DSU_CRUISE"]['MAIN_ON'] != 0 + ret.cruiseState.speed = cp.vl["DSU_CRUISE"]['SET_SPEED'] * CV.KPH_TO_MS self.low_speed_lockout = False else: - self.v_cruise_pcm = cp.vl["PCM_CRUISE_2"]['SET_SPEED'] + ret.cruiseState.available = cp.vl["PCM_CRUISE_2"]['MAIN_ON'] != 0 + ret.cruiseState.speed = cp.vl["PCM_CRUISE_2"]['SET_SPEED'] * CV.KPH_TO_MS self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]['LOW_SPEED_LOCKOUT'] == 2 self.pcm_acc_status = cp.vl["PCM_CRUISE"]['CRUISE_STATE'] - self.pcm_acc_active = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']) - self.brake_lights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or self.brake_pressed) - if self.CP.carFingerprint == CAR.PRIUS: - self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0 + if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor: + # ignore standstill in hybrid vehicles, since pcm allows to restart without + # receiving any special command. Also if interceptor is detected + ret.cruiseState.standstill = False else: - self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM']) + ret.cruiseState.standstill = self.pcm_acc_status == 7 + ret.cruiseState.enabled = bool(cp.vl["PCM_CRUISE"]['CRUISE_ACTIVE']) - self.stock_aeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5) + if self.CP.carFingerprint == CAR.PRIUS: + ret.genericToggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0 + else: + ret.genericToggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM']) + ret.stockAeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5) + + ret.espDisabled = cp.vl["ESP_CONTROL"]['TC_DISABLED'] != 0 + # 2 is standby, 10 is active. TODO: check that everything else is really a faulty state + self.steer_state = cp.vl["EPS_STATUS"]['LKA_STATE'] + self.steer_warning = cp.vl["EPS_STATUS"]['LKA_STATE'] not in [1, 5] + + return ret + + @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", "SEATS_DOORS", 1), + ("DOOR_OPEN_FR", "SEATS_DOORS", 1), + ("DOOR_OPEN_RL", "SEATS_DOORS", 1), + ("DOOR_OPEN_RR", "SEATS_DOORS", 1), + ("SEATBELT_DRIVER_UNLATCHED", "SEATS_DOORS", 1), + ("TC_DISABLED", "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), + ("STEER_TORQUE_DRIVER", "STEER_TORQUE_SENSOR", 0), + ("STEER_TORQUE_EPS", "STEER_TORQUE_SENSOR", 0), + ("STEER_ANGLE", "STEER_TORQUE_SENSOR", 0), + ("TURN_SIGNALS", "STEERING_LEVERS", 3), # 3 is no blinkers + ("LKA_STATE", "EPS_STATUS", 0), + ("BRAKE_LIGHTS_ACC", "ESP_CONTROL", 0), + ("AUTO_HIGH_BEAM", "LIGHT_STALK", 0), + ] + + checks = [ + ("BRAKE_MODULE", 40), + ("GAS_PEDAL", 33), + ("WHEEL_SPEEDS", 80), + ("STEER_ANGLE_SENSOR", 80), + ("PCM_CRUISE", 33), + ("STEER_TORQUE_SENSOR", 50), + ("EPS_STATUS", 25), + ] + + if CP.carFingerprint == CAR.LEXUS_IS: + signals.append(("MAIN_ON", "DSU_CRUISE", 0)) + signals.append(("SET_SPEED", "DSU_CRUISE", 0)) + 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)) + checks.append(("PCM_CRUISE_2", 33)) + + + if CP.carFingerprint == CAR.PRIUS: + signals += [("STATE", "AUTOPARK_STATUS", 0)] + + # 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)) + checks.append(("GAS_SENSOR", 50)) + + 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)] + + # use steering message to check if panda is connected to frc + checks = [("STEERING_LKA", 42)] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 2) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index ff397e44..c8e6da55 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -2,35 +2,12 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event -from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser -from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, NO_STOP_TIMER_CAR, TSS2_CAR, FINGERPRINTS +from selfdrive.car.toyota.values import Ecu, ECU_FINGERPRINT, CAR, TSS2_CAR, FINGERPRINTS from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.swaglog import cloudlog from selfdrive.car.interfaces import CarInterfaceBase -ButtonType = car.CarState.ButtonEvent.Type -GearShifter = car.CarState.GearShifter - class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP - self.VM = VehicleModel(CP) - - self.frame = 0 - self.gas_pressed_prev = False - self.brake_pressed_prev = False - self.cruise_enabled_prev = False - - # *** init the major players *** - self.CS = CarState(CP) - - self.cp = get_can_parser(CP) - self.cp_cam = get_cam_can_parser(CP) - - self.CC = None - if CarController is not None: - self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs) @staticmethod def compute_gb(accel, speed): @@ -38,17 +15,11 @@ class CarInterface(CarInterfaceBase): @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - - ret = car.CarParams.new_message() + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) ret.carName = "toyota" - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay - ret.safetyModel = car.CarParams.SafetyModel.toyota - ret.enableCruise = True - ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 @@ -69,19 +40,6 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.indi.outerLoopGain = 3.0 ret.lateralTuning.indi.timeConstant = 1.0 ret.lateralTuning.indi.actuatorEffectiveness = 1.0 - - # TODO: Determine if this is better than INDI - # ret.lateralTuning.init('lqr') - # ret.lateralTuning.lqr.scale = 1500.0 - # ret.lateralTuning.lqr.ki = 0.01 - - # ret.lateralTuning.lqr.a = [0., 1., -0.22619643, 1.21822268] - # ret.lateralTuning.lqr.b = [-1.92006585e-04, 3.95603032e-05] - # ret.lateralTuning.lqr.c = [1., 0.] - # ret.lateralTuning.lqr.k = [-110.73572306, 451.22718255] - # ret.lateralTuning.lqr.l = [0.03233671, 0.03185757] - # ret.lateralTuning.lqr.dcGain = 0.002237852961363602 - ret.steerActuatorDelay = 0.5 elif candidate in [CAR.RAV4, CAR.RAV4H]: @@ -285,28 +243,16 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - # no rear steering, at least on the listed cars above - ret.steerRatioRear = 0. - ret.steerControlType = car.CarParams.SteerControlType.torque - - # steer, gas, brake limitations VS speed - ret.steerMaxBP = [16. * CV.KPH_TO_MS, 45. * CV.KPH_TO_MS] # breakpoints at 1 and 40 kph - ret.steerMaxV = [1., 1.] # 2/3rd torque allowed above 45 kph - ret.brakeMaxBP = [0.] - ret.brakeMaxV = [1.] - ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.fwdCamera) or has_relay # Detect smartDSU, which intercepts ACC_CMD from the DSU allowing openpilot to send it smartDsu = 0x2FF in fingerprint[0] # In TSS2 cars the camera does long control ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.dsu) and candidate not in TSS2_CAR - ret.enableApgs = False # is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, Ecu.apgs) ret.enableGasInterceptor = 0x201 in fingerprint[0] # 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 = ret.enableCamera and (smartDsu or ret.enableDsu or candidate in TSS2_CAR) cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu) - cloudlog.warning("ECU APGS Simulated: %r", ret.enableApgs) cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor) # min speed to enable ACC. if car can do stop and go, then set enabling speed @@ -321,8 +267,6 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.deadzoneV = [0., .15] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kiBP = [0., 35.] - ret.stoppingControl = False - ret.startAccel = 0.0 if ret.enableGasInterceptor: ret.gasMaxBP = [0., 9., 35] @@ -343,105 +287,18 @@ class CarInterface(CarInterfaceBase): self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) - self.CS.update(self.cp, self.cp_cam) - - # create message - ret = car.CarState.new_message() + ret = self.CS.update(self.cp, self.cp_cam) ret.canValid = self.cp.can_valid and self.cp_cam.can_valid - - # speeds - ret.vEgo = self.CS.v_ego - ret.vEgoRaw = self.CS.v_ego_raw - ret.aEgo = self.CS.a_ego - ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego) - ret.standstill = self.CS.standstill - ret.wheelSpeeds.fl = self.CS.v_wheel_fl - ret.wheelSpeeds.fr = self.CS.v_wheel_fr - ret.wheelSpeeds.rl = self.CS.v_wheel_rl - ret.wheelSpeeds.rr = self.CS.v_wheel_rr - - # gear shifter - ret.gearShifter = self.CS.gear_shifter - - # gas pedal - ret.gas = self.CS.pedal_gas - if self.CP.enableGasInterceptor: - # use interceptor values to disengage on pedal press - ret.gasPressed = self.CS.pedal_gas > 15 - else: - ret.gasPressed = self.CS.pedal_gas > 0 - - # brake pedal - ret.brake = self.CS.user_brake - ret.brakePressed = self.CS.brake_pressed != 0 - ret.brakeLights = self.CS.brake_lights - - # steering wheel - ret.steeringAngle = self.CS.angle_steers - ret.steeringRate = self.CS.angle_steers_rate - - ret.steeringTorque = self.CS.steer_torque_driver - ret.steeringTorqueEps = self.CS.steer_torque_motor - ret.steeringPressed = self.CS.steer_override + ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo) ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False - - # cruise state - ret.cruiseState.enabled = self.CS.pcm_acc_active - ret.cruiseState.speed = self.CS.v_cruise_pcm * CV.KPH_TO_MS - ret.cruiseState.available = bool(self.CS.main_on) - ret.cruiseState.speedOffset = 0. - - if self.CP.carFingerprint in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor: - # ignore standstill in hybrid vehicles, since pcm allows to restart without - # receiving any special command - # also if interceptor is detected - ret.cruiseState.standstill = False - else: - ret.cruiseState.standstill = self.CS.pcm_acc_status == 7 - - buttonEvents = [] - if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.leftBlinker - be.pressed = self.CS.left_blinker_on != 0 - buttonEvents.append(be) - - if self.CS.right_blinker_on != self.CS.prev_right_blinker_on: - be = car.CarState.ButtonEvent.new_message() - be.type = ButtonType.rightBlinker - be.pressed = self.CS.right_blinker_on != 0 - buttonEvents.append(be) - - ret.buttonEvents = buttonEvents - ret.leftBlinker = bool(self.CS.left_blinker_on) - ret.rightBlinker = bool(self.CS.right_blinker_on) - - ret.doorOpen = not self.CS.door_all_closed - ret.seatbeltUnlatched = not self.CS.seatbelt - - ret.genericToggle = self.CS.generic_toggle - ret.stockAeb = self.CS.stock_aeb + ret.buttonEvents = [] # events - events = [] + events = self.create_common_events(ret) if self.cp_cam.can_invalid_cnt >= 200 and self.CP.enableCamera: events.append(create_event('invalidGiraffeToyota', [ET.PERMANENT])) - if not ret.gearShifter == GearShifter.drive and self.CP.openpilotLongitudinalControl: - events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.doorOpen: - events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.seatbeltUnlatched: - events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.esp_disabled and self.CP.openpilotLongitudinalControl: - events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if not self.CS.main_on and self.CP.openpilotLongitudinalControl: - events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gearShifter == GearShifter.reverse and self.CP.openpilotLongitudinalControl: - events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if self.CS.steer_error: - events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) if self.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl: events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT])) if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl: @@ -459,21 +316,14 @@ class CarInterface(CarInterfaceBase): elif not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) - # disable on pedals rising edge or when brake is pressed and speed isn't zero - if (ret.gasPressed and not self.gas_pressed_prev) or \ - (ret.brakePressed and (not self.brake_pressed_prev or ret.vEgo > 0.001)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) - ret.events = events self.gas_pressed_prev = ret.gasPressed self.brake_pressed_prev = ret.brakePressed self.cruise_enabled_prev = ret.cruiseState.enabled - return ret.as_reader() + self.CS.out = ret.as_reader() + return self.CS.out # pass in a car.CarControl # to be called @ 100hz diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 48d6229c..a5d14972 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -1,27 +1,3 @@ -def create_ipas_steer_command(packer, steer, enabled, apgs_enabled): - """Creates a CAN message for the Toyota Steer Command.""" - if steer < 0: - direction = 3 - elif steer > 0: - direction = 1 - else: - direction = 2 - - mode = 3 if enabled else 1 - - values = { - "STATE": mode, - "DIRECTION_CMD": direction, - "ANGLE": steer, - "SET_ME_X10": 0x10, - "SET_ME_X40": 0x40 - } - if apgs_enabled: - return packer.make_can_msg("STEERING_IPAS", 0, values) - else: - return packer.make_can_msg("STEERING_IPAS_COMMA", 0, values) - - def create_steer_command(packer, steer, steer_req, raw_cnt): """Creates a CAN message for the Toyota Steer Command.""" diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py index 6b09b41a..98065aa2 100644 --- a/selfdrive/car/toyota/values.py +++ b/selfdrive/car/toyota/values.py @@ -57,20 +57,11 @@ STATIC_MSGS = [ (0x470, Ecu.dsu, (CAR.PRIUS, CAR.LEXUS_RXH), 1, 100, b'\x00\x00\x02\x7a'), (0x470, Ecu.dsu, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.RAV4H, CAR.SIENNA, CAR.LEXUS_CTH), 1, 100, b'\x00\x00\x01\x79'), (0x4CB, Ecu.dsu, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.LEXUS_NXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDERH, CAR.HIGHLANDER, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH, CAR.LEXUS_RX), 0, 100, b'\x0c\x00\x00\x00\x00\x00\x00\x00'), - - (0x292, Ecu.apgs, (CAR.PRIUS), 0, 3, b'\x00\x00\x00\x00\x00\x00\x00\x9e'), - (0x32E, Ecu.apgs, (CAR.PRIUS), 0, 20, b'\x00\x00\x00\x00\x00\x00\x00\x00'), - (0x396, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\xBD\x00\x00\x00\x60\x0F\x02\x00'), - (0x43A, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\x84\x00\x00\x00\x00\x00\x00\x00'), - (0x43B, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00\x00'), - (0x497, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00\x00'), - (0x4CC, Ecu.apgs, (CAR.PRIUS), 0, 100, b'\x0D\x00\x00\x00\x00\x00\x00\x00'), ] ECU_FINGERPRINT = { Ecu.fwdCamera: [0x2e4], # steer torque cmd Ecu.dsu: [0x283], # accel cmd - Ecu.apgs: [0x835], # angle cmd } @@ -79,14 +70,13 @@ FINGERPRINTS = { 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8 }], CAR.RAV4H: [{ - 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 296: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 550: 8, 552: 4, 560: 7, 562: 4, 581: 5, 608: 8, 610: 5, 643: 7, 705: 8, 713: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 + 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 296: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 550: 8, 552: 4, 560: 7, 562: 4, 581: 5, 608: 8, 610: 5, 643: 7, 705: 8, 713: 8, 725: 2, 740: 5, 767:4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }, # Chinese RAV4 { 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 830: 7, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1207: 8, 1227: 8, 1235: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8 }], CAR.PRIUS: [{ - # with ipas 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767:4, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2,898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8 }, #2019 LE @@ -140,7 +130,7 @@ FINGERPRINTS = { CAR.LEXUS_RX_TSS2: [ # 2020 Lexus RX 350 { - 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 740: 5, 742: 8, 743: 8, 764: 8, 765: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8,1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594:8, 1595: 8, 1600: 8, 1649: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8 + 36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 401: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 740: 5, 742: 8, 743: 8, 764: 8, 765: 8, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 987: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1076: 8, 1077: 8,1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1592: 8, 1594:8, 1595: 8, 1600: 8, 1649: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8 }], CAR.CHR: [{ 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 705: 8, 740: 5, 767:4, 800: 8, 810: 2, 812: 8, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 869: 7, 870: 7, 871: 2, 898: 8, 913: 8, 918: 8, 921: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 1014: 8, 1017: 8, 1020: 8, 1021: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1082: 8, 1083: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8 @@ -286,6 +276,7 @@ FW_VERSIONS = { b'\x018966333P4400\x00\x00\x00\x00', b'\x018966333P4500\x00\x00\x00\x00', b'\x018966333P4700\x00\x00\x00\x00', + b'\x018966333Q6200\x00\x00\x00\x00', ], (Ecu.dsu, 0x791, None): [ b'8821F0607200 ', @@ -298,7 +289,10 @@ FW_VERSIONS = { b'F152606290\x00\x00\x00\x00\x00\x00', b'F152633540\x00\x00\x00\x00\x00\x00', ], - (Ecu.eps, 0x7a1, None): [b'8965B33540\x00\x00\x00\x00\x00\x00'], + (Ecu.eps, 0x7a1, None): [ + b'8965B33540\x00\x00\x00\x00\x00\x00', + b'8965B33542\x00\x00\x00\x00\x00\x00', + ], (Ecu.fwdRadar, 0x750, 0xf): [ # Same as 0x791 b'8821F0601300 ', b'8821F0603300 ', @@ -308,6 +302,7 @@ FW_VERSIONS = { b'8646F0601200 ', b'8646F0601300 ', b'8646F0603400 ', + b'8646F0605000 ', ], }, CAR.CAMRYH: { @@ -340,12 +335,40 @@ FW_VERSIONS = { ], }, CAR.CHR: { - (Ecu.dsu, 0x791, None): [b'8821FF404100 '], - (Ecu.esp, 0x7b0, None): [b'F1526F4122\x00\x00\x00\x00\x00\x00'], - (Ecu.eps, 0x7a1, None): [b'8965B10040\x00\x00\x00\x00\x00\x00'], - (Ecu.engine, 0x7e0, None): [b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00'], - (Ecu.fwdRadar, 0x750, 0xf): [b'8821FF404100 '], - (Ecu.fwdCamera, 0x750, 0x6d): [b'8646FF404000 '], + (Ecu.engine, 0x700, None): [ + b'\x0189663F413100\x00\x00\x00\x00', + ], + (Ecu.dsu, 0x791, None): [ + b'8821FF401600 ', + b'8821FF404100 ', + ], + (Ecu.esp, 0x7b0, None): [ + b'F1526F4073\x00\x00\x00\x00\x00\x00', + b'F1526F4122\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B10011\x00\x00\x00\x00\x00\x00', + b'8965B10040\x00\x00\x00\x00\x00\x00', + ], + (Ecu.engine, 0x7e0, None): [ + b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'8821FF401600 ', + b'8821FF404100 ', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'8646FF401800 ', + b'8646FF404000 ', + ], + }, + CAR.CHRH: { + (Ecu.engine, 0x700, None): [b'\x0289663F431000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00'], + (Ecu.esp, 0x7b0, None): [b'F152610040\x00\x00\x00\x00\x00\x00'], + (Ecu.dsu, 0x791, None): [b'8821FF407100 '], + (Ecu.eps, 0x7a1, None): [b'8965B10050\x00\x00\x00\x00\x00\x00'], + (Ecu.fwdRadar, 0x750, 0xf): [b'8821FF407100 '], + (Ecu.fwdCamera, 0x750, 0x6d): [b'8646FF407000 '], }, CAR.COROLLA: { (Ecu.engine, 0x7e0, None): [ @@ -449,9 +472,11 @@ FW_VERSIONS = { (Ecu.esp, 0x7b0, None): [ b'F152612590\x00\x00\x00\x00\x00\x00', b'F152612691\x00\x00\x00\x00\x00\x00', + b'F152612692\x00\x00\x00\x00\x00\x00', b'F152612700\x00\x00\x00\x00\x00\x00', b'F152612800\x00\x00\x00\x00\x00\x00', b'F152612840\x00\x00\x00\x00\x00\x00', + b'F152612A10\x00\x00\x00\x00\x00\x00', b'F152642540\x00\x00\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ @@ -531,6 +556,7 @@ FW_VERSIONS = { b'\x03896634786000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710001\x00\x00\x00\x00', b'\x03896634789000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4703002\x00\x00\x00\x00', b'\x038966347A3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4707001\x00\x00\x00\x00', + b'\x038966347B7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710001\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ b'8965B47021\x00\x00\x00\x00\x00\x00', @@ -638,7 +664,6 @@ FW_VERSIONS = { }, CAR.RAV4_TSS2: { (Ecu.engine, 0x700, None): [ - b'\x018966333Q6200\x00\x00\x00\x00', b'\x018966342E2000\x00\x00\x00\x00', b'\x018966342M8000\x00\x00\x00\x00', b'\x018966342T1000\x00\x00\x00\x00', @@ -653,7 +678,6 @@ FW_VERSIONS = { b'\x02896634A18000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ - b'F152606230\x00\x00\x00\x00\x00\x00', b'F152642520\x00\x00\x00\x00\x00\x00', b'\x01F15260R210\x00\x00\x00\x00\x00\x00', b'\x01F15260R220\x00\x00\x00\x00\x00\x00', @@ -662,20 +686,17 @@ FW_VERSIONS = { b'\x01F152642710\x00\x00\x00\x00\x00\x00', ], (Ecu.eps, 0x7a1, None): [ - b'8965B33540\x00\x00\x00\x00\x00\x00', b'8965B42170\x00\x00\x00\x00\x00\x00', b'8965B42171\x00\x00\x00\x00\x00\x00', b'\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ - b'8821F0607200 ', b'\x018821F3301100\x00\x00\x00\x00', b'\x018821F3301200\x00\x00\x00\x00', b'\x018821F3301300\x00\x00\x00\x00', b'\x018821F3301400\x00\x00\x00\x00', ], (Ecu.fwdCamera, 0x750, 0x6d): [ - b'8646F0605000 ', b'\x028646F4203200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F4203300\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', b'\x028646F4203400\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', @@ -684,12 +705,14 @@ FW_VERSIONS = { }, CAR.RAV4H_TSS2: { (Ecu.engine, 0x700, None): [ + b'\x018966342M5000\x00\x00\x00\x00', b'\x018966342X6000\x00\x00\x00\x00', b'\x028966342W4001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', b'\x02896634A23001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00', ], (Ecu.esp, 0x7b0, None): [ b'F152642291\x00\x00\x00\x00\x00\x00', + b'F152642330\x00\x00\x00\x00\x00\x00', b'F152642531\x00\x00\x00\x00\x00\x00', b'F152642532\x00\x00\x00\x00\x00\x00', ], @@ -697,6 +720,7 @@ FW_VERSIONS = { b'8965B42170\x00\x00\x00\x00\x00\x00', b'8965B42171\x00\x00\x00\x00\x00\x00', b'8965B42181\x00\x00\x00\x00\x00\x00', + b'\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00', ], (Ecu.fwdRadar, 0x750, 0xf): [ b'\x018821F3301200\x00\x00\x00\x00', @@ -737,6 +761,28 @@ FW_VERSIONS = { ], (Ecu.fwdCamera, 0x750, 0x6d): [b'8646F0801100\x00\x00\x00\x00'], }, + CAR.LEXUS_ESH_TSS2: { + (Ecu.engine, 0x700, None): [ + b'\x028966333S8000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00', + b'\x028966333V4000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00', + ], + (Ecu.esp, 0x7b0, None): [ + b'F152633423\x00\x00\x00\x00\x00\x00', + b'F152633680\x00\x00\x00\x00\x00\x00', + ], + (Ecu.eps, 0x7a1, None): [ + b'8965B33252\x00\x00\x00\x00\x00\x00', + b'8965B33590\x00\x00\x00\x00\x00\x00', + ], + (Ecu.fwdRadar, 0x750, 0xf): [ + b'\x018821F3301100\x00\x00\x00\x00', + b'\x018821F3301300\x00\x00\x00\x00', + ], + (Ecu.fwdCamera, 0x750, 0x6d): [ + b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00', + b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00', + ], + }, CAR.LEXUS_RXH: { (Ecu.engine, 0x7e0, None): [ b'\x02348Q4000\x00\x00\x00\x00\x00\x00\x00\x00A4802000\x00\x00\x00\x00\x00\x00\x00\x00', diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index 610e1d5a..6c90d69e 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -1,20 +1,17 @@ from cereal import car from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.volkswagen import volkswagencan -from selfdrive.car.volkswagen.values import DBC, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams +from selfdrive.car.volkswagen.values import DBC, CANBUS, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert class CarController(): - def __init__(self, canbus, car_fingerprint): + def __init__(self, dbc_name, CP, VM): self.apply_steer_last = 0 - self.car_fingerprint = car_fingerprint - # Setup detection helper. Routes commands to an appropriate CAN bus number. - self.canbus = canbus - self.packer_pt = CANPacker(DBC[car_fingerprint]['pt']) + self.packer_pt = CANPacker(DBC[CP.carFingerprint]['pt']) self.hcaSameTorqueCount = 0 self.hcaEnabledFrameCount = 0 @@ -32,7 +29,6 @@ class CarController(): # Send CAN commands. can_sends = [] - canbus = self.canbus #-------------------------------------------------------------------------- # # @@ -49,14 +45,14 @@ class CarController(): # FAULT AVOIDANCE: HCA must not be enabled at standstill. Also stop # commanding HCA if there's a fault, so the steering rack recovers. - if enabled and not (CS.standstill or CS.steeringFault): + if enabled and not (CS.out.standstill or CS.steeringFault): # FAULT AVOIDANCE: Requested HCA torque must not exceed 3.0 Nm. This # is inherently handled by scaling to STEER_MAX. The rack doesn't seem # to care about up/down rate, but we have some evidence it may do its # own rate limiting, and matching OP helps for accurate tuning. new_steer = int(round(actuators.steer * P.STEER_MAX)) - apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steeringTorque, P) + 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 # FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending @@ -102,7 +98,7 @@ class CarController(): self.apply_steer_last = apply_steer idx = (frame / P.HCA_STEP) % 16 - can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_pt, canbus.pt, apply_steer, + can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_pt, CANBUS.pt, apply_steer, idx, hcaEnabled)) #-------------------------------------------------------------------------- @@ -116,15 +112,15 @@ class CarController(): # filters LDW_02 from the factory camera and OP emits LDW_02 at 10Hz. if frame % P.LDW_STEP == 0: - hcaEnabled = True if enabled and not CS.standstill else False + hcaEnabled = True if enabled and not CS.out.standstill else False if visual_alert == VisualAlert.steerRequired: hud_alert = MQB_LDW_MESSAGES["laneAssistTakeOverSilent"] else: hud_alert = MQB_LDW_MESSAGES["none"] - can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, canbus.pt, hcaEnabled, - CS.steeringPressed, hud_alert, leftLaneVisible, + can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, hcaEnabled, + CS.out.steeringPressed, hud_alert, leftLaneVisible, rightLaneVisible)) #-------------------------------------------------------------------------- @@ -140,11 +136,11 @@ class CarController(): # stock ACC with OP disengagement, or to auto-resume from stop. if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP: - if not enabled and CS.accEnabled: + if not enabled and CS.out.cruiseState.enabled: # Cancel ACC if it's engaged with OP disengaged. self.graButtonStatesToSend = BUTTON_STATES.copy() self.graButtonStatesToSend["cancel"] = True - elif enabled and CS.standstill: + elif enabled and CS.out.standstill: # Blip the Resume button if we're engaged at standstill. # FIXME: This is a naive implementation, improve with visiond or radar input. # A subset of MQBs like to "creep" too aggressively with this implementation. @@ -182,7 +178,7 @@ class CarController(): if self.graMsgSentCount == 0: self.graMsgStartFramePrev = frame idx = (CS.graMsgBusCounter + 1) % 16 - can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, canbus.pt, self.graButtonStatesToSend, CS, idx)) + can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, CANBUS.pt, self.graButtonStatesToSend, CS, idx)) self.graMsgSentCount += 1 if self.graMsgSentCount >= P.GRA_VBP_COUNT: self.graButtonStatesToSend = None diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index 79cd0631..91b24dc4 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -1,149 +1,59 @@ import numpy as np +from cereal import car from selfdrive.config import Conversions as CV from selfdrive.car.interfaces import CarStateBase from opendbc.can.parser import CANParser from opendbc.can.can_define import CANDefine -from selfdrive.car.volkswagen.values import DBC, BUTTON_STATES, CarControllerParams - -def get_mqb_pt_can_parser(CP, canbus): - # this function generates lists for signal, messages and initial values - signals = [ - # sig_name, sig_address, default - ("LWI_Lenkradwinkel", "LWI_01", 0), # Absolute steering angle - ("LWI_VZ_Lenkradwinkel", "LWI_01", 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 - ("BH_Blinker_li", "Gateway_72", 0), # Left turn signal on - ("BH_Blinker_re", "Gateway_72", 0), # Right turn signal on - ("GE_Fahrstufe", "Getriebe_11", 0), # Auto trans gear selector position - ("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_Status_Bremsdruck", "ESP_05", 0), # Brakes applied - ("ESP_Bremsdruck", "ESP_05", 0), # Brake pressure applied - ("MO_Fahrpedalrohwert_01", "Motor_20", 0), # Accelerator pedal value - ("MO_Kuppl_schalter", "Motor_14", 0), # Clutch switch - ("Driver_Strain", "EPS_01", 0), # Absolute driver torque input - ("Driver_Strain_VZ", "EPS_01", 0), # Driver torque input sign - ("HCA_Ready", "EPS_01", 0), # Steering rack HCA support configured - ("ESP_Tastung_passiv", "ESP_21", 0), # Stability control disabled - ("KBI_MFA_v_Einheit_02", "Einheiten_01", 0), # MPH vs KMH speed display - ("KBI_Handbremse", "Kombi_01", 0), # Manual handbrake applied - ("TSK_Fahrzeugmasse_02", "Motor_16", 0), # Estimated vehicle mass from drivetrain coordinator - ("ACC_Status_ACC", "ACC_06", 0), # ACC engagement status - ("ACC_Typ", "ACC_06", 0), # ACC type (follow to stop, stop&go) - ("SetSpeed", "ACC_02", 0), # ACC set speed - ("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 - ] - - checks = [ - # sig_address, frequency - ("LWI_01", 100), # From J500 Steering Assist with integrated sensors - ("EPS_01", 100), # From J500 Steering Assist with integrated sensors - ("ESP_19", 100), # From J104 ABS/ESP controller - ("ESP_05", 50), # From J104 ABS/ESP controller - ("ESP_21", 50), # From J104 ABS/ESP controller - ("ACC_06", 50), # From J428 ACC radar control module - ("Motor_20", 50), # From J623 Engine control module - ("GRA_ACC_01", 33), # From J??? steering wheel control buttons - ("ACC_02", 17), # From J428 ACC radar control module - ("Getriebe_11", 20), # From J743 Auto transmission control module - ("Gateway_72", 10), # From J533 CAN gateway (aggregated data) - ("Motor_14", 10), # From J623 Engine control module - ("Airbag_02", 5), # From J234 Airbag control module - ("Kombi_01", 2), # From J285 Instrument cluster - ("Motor_16", 2), # From J623 Engine control module - ("Einheiten_01", 1), # From J??? not known if gateway, cluster, or BCM - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.pt) - -# A single signal is monitored from the camera CAN bus, and then ignored, -# so the presence of CAN traffic can be verified with cam_cp.valid. - -def get_mqb_cam_can_parser(CP, canbus): - - signals = [ - # sig_name, sig_address, default - ("Kombi_Lamp_Green", "LDW_02", 0), # Lane Assist status LED - ] - - checks = [ - # sig_address, frequency - ("LDW_02", 10) # From R242 Driver assistance camera - ] - - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.cam) - +from selfdrive.car.volkswagen.values import DBC, CANBUS, BUTTON_STATES, CarControllerParams class CarState(CarStateBase): - def __init__(self, CP, canbus): + def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = can_define.dv["Getriebe_11"]['GE_Fahrstufe'] self.buttonStates = BUTTON_STATES.copy() def update(self, pt_cp): + ret = car.CarState.new_message() # Update vehicle speed and acceleration from ABS wheel speeds. - self.wheelSpeedFL = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS - self.wheelSpeedFR = pt_cp.vl["ESP_19"]['ESP_VR_Radgeschw_02'] * CV.KPH_TO_MS - self.wheelSpeedRL = pt_cp.vl["ESP_19"]['ESP_HL_Radgeschw_02'] * CV.KPH_TO_MS - self.wheelSpeedRR = pt_cp.vl["ESP_19"]['ESP_HR_Radgeschw_02'] * CV.KPH_TO_MS + ret.wheelSpeeds.fl = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS + ret.wheelSpeeds.fr = pt_cp.vl["ESP_19"]['ESP_VR_Radgeschw_02'] * CV.KPH_TO_MS + ret.wheelSpeeds.rl = pt_cp.vl["ESP_19"]['ESP_HL_Radgeschw_02'] * CV.KPH_TO_MS + ret.wheelSpeeds.rr = pt_cp.vl["ESP_19"]['ESP_HR_Radgeschw_02'] * CV.KPH_TO_MS - self.vEgoRaw = float(np.mean([self.wheelSpeedFL, self.wheelSpeedFR, self.wheelSpeedRL, self.wheelSpeedRR])) - self.vEgo, self.aEgo = self.update_speed_kf(self.vEgoRaw) + ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])) + ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) - self.standstill = self.vEgoRaw < 0.1 + ret.standstill = ret.vEgoRaw < 0.1 # Update steering angle, rate, yaw rate, and driver input torque. VW send # the sign/direction in a separate signal so they must be recombined. - self.steeringAngle = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] - self.steeringRate = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] - self.steeringTorque = pt_cp.vl["EPS_01"]['Driver_Strain'] * (1,-1)[int(pt_cp.vl["EPS_01"]['Driver_Strain_VZ'])] - self.steeringPressed = abs(self.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE - self.yawRate = pt_cp.vl["ESP_02"]['ESP_Gierrate'] * (1,-1)[int(pt_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD + ret.steeringAngle = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] + ret.steeringRate = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] + ret.steeringTorque = pt_cp.vl["EPS_01"]['Driver_Strain'] * (1,-1)[int(pt_cp.vl["EPS_01"]['Driver_Strain_VZ'])] + ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE + ret.yawRate = pt_cp.vl["ESP_02"]['ESP_Gierrate'] * (1,-1)[int(pt_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD # Update gas, brakes, and gearshift. - self.gas = pt_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0 - self.gasPressed = self.gas > 0 - self.brake = pt_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects - self.brakePressed = bool(pt_cp.vl["ESP_05"]['ESP_Fahrer_bremst']) - self.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck']) + ret.gas = pt_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0 + ret.gasPressed = ret.gas > 0 + ret.brake = pt_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects + ret.brakePressed = bool(pt_cp.vl["ESP_05"]['ESP_Fahrer_bremst']) + ret.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck']) # Update gear and/or clutch position data. can_gear_shifter = int(pt_cp.vl["Getriebe_11"]['GE_Fahrstufe']) - self.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None)) + ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear_shifter, None)) # Update door and trunk/hatch lid open status. - self.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_offen'], - pt_cp.vl["Gateway_72"]['ZV_BT_offen'], - pt_cp.vl["Gateway_72"]['ZV_HFS_offen'], - pt_cp.vl["Gateway_72"]['ZV_HBFS_offen'], - pt_cp.vl["Gateway_72"]['ZV_HD_offen']]) + ret.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_offen'], + pt_cp.vl["Gateway_72"]['ZV_BT_offen'], + pt_cp.vl["Gateway_72"]['ZV_HFS_offen'], + pt_cp.vl["Gateway_72"]['ZV_HBFS_offen'], + pt_cp.vl["Gateway_72"]['ZV_HD_offen']]) # Update seatbelt fastened status. - self.seatbeltUnlatched = False if pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] == 3 else True + ret.seatbeltUnlatched = False if pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] == 3 else True # Update driver preference for metric. VW stores many different unit # preferences, including separate units for for distance vs. speed. @@ -151,42 +61,35 @@ class CarState(CarStateBase): self.displayMetricUnits = not pt_cp.vl["Einheiten_01"]["KBI_MFA_v_Einheit_02"] # Update ACC radar status. - accStatus = pt_cp.vl["ACC_06"]['ACC_Status_ACC'] - if accStatus == 1: - # ACC okay but disabled - self.accFault = False - self.accAvailable = False - self.accEnabled = False - elif accStatus == 2: + accStatus = pt_cp.vl["TSK_06"]['TSK_Status'] + if accStatus == 2: # ACC okay and enabled, but not currently engaged - self.accFault = False - self.accAvailable = True - self.accEnabled = False + ret.cruiseState.available = True + ret.cruiseState.enabled = False elif accStatus in [3, 4, 5]: # ACC okay and enabled, currently engaged and regulating speed (3) or engaged with driver accelerating (4) or overrun (5) - self.accFault = False - self.accAvailable = True - self.accEnabled = True + ret.cruiseState.available = True + ret.cruiseState.enabled = True else: - # ACC fault of some sort. Seen statuses 6 or 7 for CAN comms disruptions, visibility issues, etc. - self.accFault = True - self.accAvailable = False - self.accEnabled = False + # ACC okay but disabled (1), or a radar visibility or other fault/disruption (6 or 7) + ret.cruiseState.available = False + ret.cruiseState.enabled = False # Update ACC setpoint. When the setpoint is zero or there's an error, the # radar sends a set-speed of ~90.69 m/s / 203mph. - self.accSetSpeed = pt_cp.vl["ACC_02"]['SetSpeed'] - if self.accSetSpeed > 90: self.accSetSpeed = 0 + ret.cruiseState.speed = pt_cp.vl["ACC_02"]['SetSpeed'] + if ret.cruiseState.speed > 90: + ret.cruiseState.speed = 0 # Update control button states for turn signals and ACC controls. - self.buttonStates["leftBlinker"] = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_li']) - self.buttonStates["rightBlinker"] = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_re']) self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Hoch']) self.buttonStates["decelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Runter']) self.buttonStates["cancel"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Abbrechen']) self.buttonStates["setCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Setzen']) self.buttonStates["resumeCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Wiederaufnahme']) self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Verstellung_Zeitluecke']) + ret.leftBlinker = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_li']) + ret.rightBlinker = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_re']) # Read ACC hardware button type configuration info that has to pass thru # to the radar. Ends up being different for steering wheel buttons vs @@ -205,5 +108,101 @@ class CarState(CarStateBase): # Additional safety checks performed in CarInterface. self.parkingBrakeSet = bool(pt_cp.vl["Kombi_01"]['KBI_Handbremse']) # FIXME: need to include an EPB check as well - self.stabilityControlDisabled = pt_cp.vl["ESP_21"]['ESP_Tastung_passiv'] + ret.espDisabled = pt_cp.vl["ESP_21"]['ESP_Tastung_passiv'] != 0 + return ret + + @staticmethod + def get_can_parser(CP): + # this function generates lists for signal, messages and initial values + signals = [ + # sig_name, sig_address, default + ("LWI_Lenkradwinkel", "LWI_01", 0), # Absolute steering angle + ("LWI_VZ_Lenkradwinkel", "LWI_01", 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 + ("BH_Blinker_li", "Gateway_72", 0), # Left turn signal on + ("BH_Blinker_re", "Gateway_72", 0), # Right turn signal on + ("GE_Fahrstufe", "Getriebe_11", 0), # Auto trans gear selector position + ("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_Status_Bremsdruck", "ESP_05", 0), # Brakes applied + ("ESP_Bremsdruck", "ESP_05", 0), # Brake pressure applied + ("MO_Fahrpedalrohwert_01", "Motor_20", 0), # Accelerator pedal value + ("MO_Kuppl_schalter", "Motor_14", 0), # Clutch switch + ("Driver_Strain", "EPS_01", 0), # Absolute driver torque input + ("Driver_Strain_VZ", "EPS_01", 0), # Driver torque input sign + ("HCA_Ready", "EPS_01", 0), # Steering rack HCA support configured + ("ESP_Tastung_passiv", "ESP_21", 0), # Stability control disabled + ("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 + ("TSK_Fahrzeugmasse_02", "Motor_16", 0), # Estimated vehicle mass from drivetrain coordinator + ("ACC_Status_ACC", "ACC_06", 0), # ACC engagement status + ("ACC_Typ", "ACC_06", 0), # ACC type (follow to stop, stop&go) + ("SetSpeed", "ACC_02", 0), # ACC set speed + ("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 + ] + + checks = [ + # sig_address, frequency + ("LWI_01", 100), # From J500 Steering Assist with integrated sensors + ("EPS_01", 100), # From J500 Steering Assist with integrated sensors + ("ESP_19", 100), # From J104 ABS/ESP controller + ("ESP_05", 50), # From J104 ABS/ESP controller + ("ESP_21", 50), # From J104 ABS/ESP controller + ("ACC_06", 50), # From J428 ACC radar control module + ("Motor_20", 50), # From J623 Engine control module + ("TSK_06", 50), # From J623 Engine control module + ("GRA_ACC_01", 33), # From J??? steering wheel control buttons + ("ACC_02", 17), # From J428 ACC radar control module + ("Getriebe_11", 20), # From J743 Auto transmission control module + ("Gateway_72", 10), # From J533 CAN gateway (aggregated data) + ("Motor_14", 10), # From J623 Engine control module + ("Airbag_02", 5), # From J234 Airbag control module + ("Kombi_01", 2), # From J285 Instrument cluster + ("Motor_16", 2), # From J623 Engine control module + ("Einheiten_01", 1), # From J??? not known if gateway, cluster, or BCM + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, CANBUS.pt) + + # A single signal is monitored from the camera CAN bus, and then ignored, + # so the presence of CAN traffic can be verified with cam_cp.valid. + + @staticmethod + def get_cam_can_parser(CP): + + signals = [ + # sig_name, sig_address, default + ("Kombi_Lamp_Green", "LDW_02", 0), # Lane Assist status LED + ] + + checks = [ + # sig_address, frequency + ("LDW_02", 10) # From R242 Driver assistance camera + ] + + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, CANBUS.cam) diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 0e8b5206..4a2f7cb7 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -1,67 +1,38 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET -from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES -from selfdrive.car.volkswagen.carstate import CarState, get_mqb_pt_can_parser, get_mqb_cam_can_parser from common.params import Params from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase GEAR = car.CarState.GearShifter -class CANBUS: - pt = 0 - cam = 2 - class CarInterface(CarInterfaceBase): - def __init__(self, CP, CarController): - self.CP = CP - self.CC = None + def __init__(self, CP, CarController, CarState): + super().__init__(CP, CarController, CarState) - self.frame = 0 - - self.gasPressedPrev = False - self.brakePressedPrev = False - self.cruiseStateEnabledPrev = False self.displayMetricUnitsPrev = None self.buttonStatesPrev = BUTTON_STATES.copy() - # *** init the major players *** - self.CS = CarState(CP, CANBUS) - self.VM = VehicleModel(CP) - self.pt_cp = get_mqb_pt_can_parser(CP, CANBUS) - self.cam_cp = get_mqb_cam_can_parser(CP, CANBUS) - - # sending if read only is False - if CarController is not None: - self.CC = CarController(CANBUS, CP.carFingerprint) - @staticmethod def compute_gb(accel, speed): return float(accel) / 4.0 @staticmethod def get_params(candidate, fingerprint=gen_empty_fingerprint(), has_relay=False, car_fw=[]): - ret = car.CarParams.new_message() - - ret.carFingerprint = candidate - ret.isPandaBlack = has_relay + ret = CarInterfaceBase.get_std_params(candidate, fingerprint, has_relay) if candidate == CAR.GOLF: # Set common MQB parameters that will apply globally ret.carName = "volkswagen" + ret.radarOffCan = True ret.safetyModel = car.CarParams.SafetyModel.volkswagen - ret.enableCruise = True # Stock ACC still controls acceleration and braking - ret.openpilotLongitudinalControl = False - ret.steerControlType = car.CarParams.SteerControlType.torque # Additional common MQB parameters that may be overridden per-vehicle ret.steerRateCost = 0.5 ret.steerActuatorDelay = 0.05 # Hopefully all MQB racks are similar here ret.steerLimitTimer = 0.4 - ret.steerMaxBP = [0.] # m/s - ret.steerMaxV = [1.] # As a starting point for speed-adjusted lateral tuning, use the example # map speed breakpoints from a VW Tiguan (SSP 399 page 9). It's unclear @@ -89,19 +60,6 @@ class CarInterface(CarInterfaceBase): ret.enableCamera = True # Stock camera detection doesn't apply to VW ret.transmissionType = car.CarParams.TransmissionType.automatic - ret.steerRatioRear = 0. - - # No support for OP longitudinal control on Volkswagen at this time. - ret.gasMaxBP = [0.] - ret.gasMaxV = [0.] - ret.brakeMaxBP = [0.] - ret.brakeMaxV = [0.] - ret.longitudinalTuning.deadzoneBP = [0.] - ret.longitudinalTuning.deadzoneV = [0.] - ret.longitudinalTuning.kpBP = [0.] - ret.longitudinalTuning.kpV = [0.] - ret.longitudinalTuning.kiBP = [0.] - ret.longitudinalTuning.kiV = [0.] # TODO: get actual value, for now starting with reasonable value for # civic and scaling by mass and wheelbase @@ -117,63 +75,24 @@ class CarInterface(CarInterfaceBase): # returns a car.CarState def update(self, c, can_strings): canMonoTimes = [] - events = [] buttonEvents = [] params = Params() - ret = car.CarState.new_message() # Process the most recent CAN message traffic, and check for validity # The camera CAN has no signals we use at this time, but we process it # anyway so we can test connectivity with can_valid - self.pt_cp.update_strings(can_strings) - self.cam_cp.update_strings(can_strings) - self.CS.update(self.pt_cp) - ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid + self.cp.update_strings(can_strings) + self.cp_cam.update_strings(can_strings) - # Wheel and vehicle speed, yaw rate - ret.wheelSpeeds.fl = self.CS.wheelSpeedFL - ret.wheelSpeeds.fr = self.CS.wheelSpeedFR - ret.wheelSpeeds.rl = self.CS.wheelSpeedRL - ret.wheelSpeeds.rr = self.CS.wheelSpeedRR - ret.vEgoRaw = self.CS.vEgoRaw - ret.vEgo = self.CS.vEgo - ret.aEgo = self.CS.aEgo - ret.standstill = self.CS.standstill - - # Steering wheel position, movement, yaw rate, and driver input - ret.steeringAngle = self.CS.steeringAngle - ret.steeringRate = self.CS.steeringRate - ret.steeringTorque = self.CS.steeringTorque - ret.steeringPressed = self.CS.steeringPressed + ret = self.CS.update(self.cp) + 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 - ret.yawRate = self.CS.yawRate - - # Gas, brakes and shifting - ret.gas = self.CS.gas - ret.gasPressed = self.CS.gasPressed - ret.brake = self.CS.brake - ret.brakePressed = self.CS.brakePressed - ret.brakeLights = self.CS.brakeLights - ret.gearShifter = self.CS.gearShifter - - # Doors open, seatbelt unfastened - ret.doorOpen = self.CS.doorOpen - ret.seatbeltUnlatched = self.CS.seatbeltUnlatched # Update the EON metric configuration to match the car at first startup, # or if there's been a change. if self.CS.displayMetricUnits != self.displayMetricUnitsPrev: params.put("IsMetric", "1" if self.CS.displayMetricUnits else "0") - # Blinker switch updates - ret.leftBlinker = self.CS.buttonStates["leftBlinker"] - ret.rightBlinker = self.CS.buttonStates["rightBlinker"] - - # ACC cruise state - ret.cruiseState.available = self.CS.accAvailable - ret.cruiseState.enabled = self.CS.accEnabled - ret.cruiseState.speed = self.CS.accSetSpeed - # 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: @@ -183,40 +102,20 @@ class CarInterface(CarInterfaceBase): be.pressed = self.CS.buttonStates[button] buttonEvents.append(be) - # Vehicle operation safety checks and events - if ret.doorOpen: - events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.seatbeltUnlatched: - events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.gearShifter == GEAR.reverse: - events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if not ret.gearShifter in [GEAR.drive, GEAR.eco, GEAR.sport]: - events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.stabilityControlDisabled: - events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + events = self.create_common_events(ret, extra_gears=[GEAR.eco, GEAR.sport]) + + # Vehicle health and operation safety checks if self.CS.parkingBrakeSet: events.append(create_event('parkBrake', [ET.NO_ENTRY, ET.USER_DISABLE])) - - # Vehicle health safety checks and events - if self.CS.accFault: - events.append(create_event('radarFault', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.steeringFault: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) - # Per the Comma safety model, disable on pedals rising edge or when brake - # is pressed and speed isn't zero. - if (ret.gasPressed and not self.gasPressedPrev) or \ - (ret.brakePressed and (not self.brakePressedPrev or not ret.standstill)): - events.append(create_event('pedalPressed', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gasPressed: - events.append(create_event('pedalPressed', [ET.PRE_ENABLE])) - # Engagement and longitudinal control using stock ACC. Make sure OP is # disengaged if stock ACC is disengaged. if not ret.cruiseState.enabled: events.append(create_event('pcmDisable', [ET.USER_DISABLE])) # Attempt OP engagement only on rising edge of stock ACC engagement. - elif not self.cruiseStateEnabledPrev: + elif not self.cruise_enabled_prev: events.append(create_event('pcmEnable', [ET.ENABLE])) ret.events = events @@ -224,14 +123,14 @@ class CarInterface(CarInterfaceBase): ret.canMonoTimes = canMonoTimes # update previous car states - self.gasPressedPrev = ret.gasPressed - self.brakePressedPrev = ret.brakePressed - self.cruiseStateEnabledPrev = ret.cruiseState.enabled + self.gas_pressed_prev = ret.gasPressed + self.brake_pressed_prev = ret.brakePressed + self.cruise_enabled_prev = ret.cruiseState.enabled self.displayMetricUnitsPrev = self.CS.displayMetricUnits self.buttonStatesPrev = self.CS.buttonStates.copy() - # cast to reader so it can't be modified - return ret.as_reader() + self.CS.out = ret.as_reader() + return self.CS.out def apply(self, c): can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py index 045e03ae..30c32b60 100644 --- a/selfdrive/car/volkswagen/values.py +++ b/selfdrive/car/volkswagen/values.py @@ -9,18 +9,20 @@ class CarControllerParams: GRA_VBP_COUNT = 16 # Send VBP messages for ~0.5s (GRA_ACC_STEP * 16) # Observed documented MQB limits: 3.00 Nm max, rate of change 5.00 Nm/sec. - # Limiting both torque and rate-of-change based on real-world testing and - # Comma's safety requirements for minimum time to lane departure. - STEER_MAX = 250 # Max heading control assist torque 2.50 Nm - STEER_DELTA_UP = 4 # Max HCA reached in 1.25s (STEER_MAX / (50Hz * 1.25)) + # Limiting rate-of-change based on real-world testing and Comma's safety + # requirements for minimum time to lane departure. + STEER_MAX = 300 # Max heading control assist torque 3.00 Nm + STEER_DELTA_UP = 4 # Max HCA reached in 1.50s (STEER_MAX / (50Hz * 1.50)) STEER_DELTA_DOWN = 10 # Min HCA reached in 0.60s (STEER_MAX / (50Hz * 0.60)) STEER_DRIVER_ALLOWANCE = 80 STEER_DRIVER_MULTIPLIER = 3 # weight driver torque heavily STEER_DRIVER_FACTOR = 1 # from dbc +class CANBUS: + pt = 0 + cam = 2 + BUTTON_STATES = { - "leftBlinker": False, - "rightBlinker": False, "accelCruise": False, "decelCruise": False, "cancel": False, diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 74d3ae1b..714b71d8 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.7.3-release" +#define COMMA_VERSION "0.7.4-release" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 680e511e..8c135782 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -61,6 +61,8 @@ def events_to_bytes(events): for e in events: if isinstance(e, capnp.lib.capnp._DynamicStructReader): e = e.as_builder() + if not e.is_root: + e = e.copy() ret.append(e.to_bytes()) return ret @@ -345,8 +347,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk force_decel = sm['dMonitoringState'].awarenessStatus < 0. # controlsState - dat = messaging.new_message() - dat.init('controlsState') + dat = messaging.new_message('controlsState') dat.valid = CS.canValid dat.controlsState = { "alertText1": AM.alert_text_1, @@ -398,8 +399,7 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk pm.send('controlsState', dat) # carState - cs_send = messaging.new_message() - cs_send.init('carState') + cs_send = messaging.new_message('carState') cs_send.valid = CS.canValid cs_send.carState = CS cs_send.carState.events = events @@ -408,21 +408,18 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk # carEvents - logged every second or on change events_bytes = events_to_bytes(events) if (sm.frame % int(1. / DT_CTRL) == 0) or (events_bytes != events_prev): - ce_send = messaging.new_message() - ce_send.init('carEvents', len(events)) + ce_send = messaging.new_message('carEvents', len(events)) ce_send.carEvents = events pm.send('carEvents', ce_send) # carParams - logged every 50 seconds (> 1 per segment) if (sm.frame % int(50. / DT_CTRL) == 0): - cp_send = messaging.new_message() - cp_send.init('carParams') + cp_send = messaging.new_message('carParams') cp_send.carParams = CP pm.send('carParams', cp_send) # carControl - cc_send = messaging.new_message() - cc_send.init('carControl') + cc_send = messaging.new_message('carControl') cc_send.valid = CS.canValid cc_send.carControl = CC pm.send('carControl', cc_send) diff --git a/selfdrive/controls/dmonitoringd.py b/selfdrive/controls/dmonitoringd.py index 3c888d7b..86afb9dc 100755 --- a/selfdrive/controls/dmonitoringd.py +++ b/selfdrive/controls/dmonitoringd.py @@ -82,8 +82,7 @@ def dmonitoringd_thread(sm=None, pm=None): events = driver_status.update(events, driver_engaged, sm['carState'].cruiseState.enabled, sm['carState'].standstill) # dMonitoringState packet - dat = messaging.new_message() - dat.init('dMonitoringState') + dat = messaging.new_message('dMonitoringState') dat.dMonitoringState = { "events": events, "faceDetected": driver_status.face_detected, diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index e6a64f1e..88fb8fbb 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -3,7 +3,7 @@ from cereal import car, log # Priority class Priority: LOWEST = 0 - LOW_LOWEST = 1 + LOWER = 1 LOW = 2 MID = 3 HIGH = 4 @@ -169,28 +169,28 @@ ALERTS = [ "Be ready to take over at any time", "Always keep hands on wheel and eyes on road", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), Alert( "startupMaster", "WARNING: This branch is not tested", "Always keep hands on wheel and eyes on road", AlertStatus.userPrompt, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), Alert( "startupNoControl", "Dashcam mode", "Always keep hands on wheel and eyes on road", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), Alert( "startupNoCar", "Dashcam mode with unsupported car", "Always keep hands on wheel and eyes on road", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 15.), Alert( "ethicalDilemma", @@ -693,21 +693,21 @@ ALERTS = [ "LKAS Fault: Restart the car to engage", "", AlertStatus.normal, AlertSize.small, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "brakeUnavailablePermanent", "Cruise Fault: Restart the car to engage", "", AlertStatus.normal, AlertSize.small, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "lowSpeedLockoutPermanent", "Cruise Fault: Restart the car to engage", "", AlertStatus.normal, AlertSize.small, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "calibrationIncompletePermanent", @@ -721,14 +721,14 @@ ALERTS = [ "Unsupported Giraffe Configuration", "Visit comma.ai/tg", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "internetConnectivityNeededPermanent", "Please connect to Internet", "An Update Check Is Required to Engage", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "communityFeatureDisallowedPermanent", @@ -742,28 +742,28 @@ ALERTS = [ "No Data from Device Sensors", "Reboot your Device", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "soundsUnavailablePermanent", "Speaker not found", "Reboot your Device", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "lowMemoryPermanent", "RAM Critically Low", "Reboot your Device", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "carUnrecognizedPermanent", "Dashcam Mode", "Car Unrecognized", AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., .2), Alert( "vehicleModelInvalid", diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 0e84ad8b..ff090bcd 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -65,9 +65,9 @@ class LanePlanner(): self.l_prob = md.leftLane.prob # left line prob self.r_prob = md.rightLane.prob # right line prob - if len(md.meta.desirePrediction): - self.l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1] - self.r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1] + if len(md.meta.desireState): + self.l_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeLeft - 1] + self.r_lane_change_prob = md.meta.desireState[log.PathPlan.Desire.laneChangeRight - 1] def update_d_poly(self, v_ego): # only offset left and right lane lines; offsetting p_poly does not make sense diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index f694af37..b8335406 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -28,8 +28,7 @@ class LongitudinalMpc(): def send_mpc_solution(self, pm, qp_iterations, calculation_time): qp_iterations = max(0, qp_iterations) - dat = messaging.new_message() - dat.init('liveLongitudinalMpc') + dat = messaging.new_message('liveLongitudinalMpc') dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego) dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego) dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego) diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 83b1190f..0e8a4eff 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -39,6 +39,7 @@ DESIRES = { }, } + def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): states[0].x = v_ego * delay states[0].psi = v_ego * curvature_factor * math.radians(steer_angle) / steer_ratio * delay @@ -58,6 +59,7 @@ class PathPlanner(): 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.prev_one_blinker = False def setup_mpc(self): @@ -113,6 +115,7 @@ class PathPlanner(): # 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 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: @@ -122,14 +125,20 @@ class PathPlanner(): self.lane_change_state = LaneChangeState.laneChangeStarting # starting - elif self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5: - self.lane_change_state = LaneChangeState.laneChangeFinishing + elif self.lane_change_state == LaneChangeState.laneChangeStarting: + # fade out lanelines over 1s + self.lane_change_ll_prob = max(self.lane_change_ll_prob - 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 # finishing - elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2: - if one_blinker: + 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 one_blinker and self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.preLaneChange - else: + elif self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.off if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: @@ -143,8 +152,8 @@ class PathPlanner(): # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: - self.LP.l_prob = 0. - self.LP.r_prob = 0. + self.LP.l_prob *= self.lane_change_ll_prob + self.LP.r_prob *= self.lane_change_ll_prob self.libmpc.init_weights(MPC_COST_LAT.PATH / 10.0, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) else: self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) @@ -188,8 +197,7 @@ class PathPlanner(): self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 2 - plan_send = messaging.new_message() - plan_send.init('pathPlan') + plan_send = messaging.new_message('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model']) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] @@ -213,8 +221,7 @@ class PathPlanner(): pm.send('pathPlan', plan_send) if LOG_MPC: - dat = messaging.new_message() - dat.init('liveMpc') + dat = messaging.new_message('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 1d6140a8..7e6a126e 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -214,8 +214,7 @@ class Planner(): radar_can_error = car.RadarData.Error.canError in radar_errors # **** send the plan **** - plan_send = messaging.new_message() - plan_send.init('plan') + plan_send = messaging.new_message('plan') plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'radarState']) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index a99a0bdf..0b852331 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -163,8 +163,7 @@ class RadarD(): self.tracks[idens[idx]].reset_a_lead(aLeadK, aLeadTau) # *** publish radarState *** - dat = messaging.new_message() - dat.init('radarState') + dat = messaging.new_message('radarState') dat.valid = sm.all_alive_and_valid(service_list=['controlsState', 'model']) dat.radarState.mdMonoTime = self.last_md_ts dat.radarState.canMonoTimes = list(rr.canMonoTimes) @@ -223,8 +222,7 @@ def radard_thread(sm=None, pm=None, can_sock=None): # *** publish tracks for UI debugging (keep last) *** tracks = RD.tracks - dat = messaging.new_message() - dat.init('liveTracks', len(tracks)) + dat = messaging.new_message('liveTracks', len(tracks)) for cnt, ids in enumerate(sorted(tracks.keys())): dat.liveTracks[cnt] = { diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 63545e6e..11995c6c 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -46,8 +46,7 @@ def run_following_distance_simulation(v_lead, t_end=200.0): dt) # Setup CarState - CS = messaging.new_message() - CS.init('carState') + CS = messaging.new_message('carState') CS.carState.vEgo = v_ego CS.carState.aEgo = a_ego diff --git a/selfdrive/crash.py b/selfdrive/crash.py index 6ceb56b6..74a7094a 100644 --- a/selfdrive/crash.py +++ b/selfdrive/crash.py @@ -6,8 +6,9 @@ import capnp from selfdrive.version import version, dirty from selfdrive.swaglog import cloudlog +from common.android import ANDROID -if os.getenv("NOLOG") or os.getenv("NOCRASH"): +if os.getenv("NOLOG") or os.getenv("NOCRASH") or not ANDROID: def capture_exception(*exc_info): pass def bind_user(**kwargs): diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py index 4f324711..6e102962 100755 --- a/selfdrive/debug/live_cpu_and_temp.py +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -14,9 +14,24 @@ def cputime_busy(ct): return ct.user + ct.nice + ct.system + ct.irq + ct.softirq +def proc_cputime_total(ct): + return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem + + +def proc_name(proc): + name = proc.name + if len(proc.cmdline): + name = proc.cmdline[0] + if len(proc.exe): + name = proc.exe + " - " + name + + return name + + if __name__ == "__main__": parser = argparse.ArgumentParser() parser.add_argument('--mem', action='store_true') + parser.add_argument('--cpu', action='store_true') args = parser.parse_args() sm = SubMaster(['thermal', 'procLog']) @@ -26,6 +41,9 @@ if __name__ == "__main__": total_times = [0., 0., 0., 0.] busy_times = [0., 0., 0.0, 0.] + prev_proclog = None + prev_proclog_t = None + while True: sm.update() @@ -37,7 +55,6 @@ if __name__ == "__main__": if sm.updated['procLog']: m = sm['procLog'] - cores = [0., 0., 0., 0.] total_times_new = [0., 0., 0., 0.] busy_times_new = [0., 0., 0.0, 0.] @@ -57,16 +74,33 @@ if __name__ == "__main__": print("CPU %.2f%% - RAM: %.2f - Temp %.2f" % (100. * np.mean(cores), last_mem, last_temp)) + if args.cpu and prev_proclog is not None: + procs = {} + dt = (sm.logMonoTime['procLog'] - prev_proclog_t) / 1e9 + for proc in m.procs: + try: + name = proc_name(proc) + prev_proc = [p for p in prev_proclog.procs if proc.pid == p.pid][0] + cpu_time = proc_cputime_total(proc) - proc_cputime_total(prev_proc) + cpu_usage = cpu_time / dt * 100. + procs[name] = cpu_usage + except IndexError: + pass + + print("Top CPU usage:") + for k, v in sorted(procs.items(), key=lambda item: item[1], reverse=True)[:10]: + print(f"{k.rjust(70)} {v:.2f} %") + print() + if args.mem: mems = {} for proc in m.procs: - name = proc.name - if len(proc.cmdline): - name = proc.cmdline[0] - if len(proc.exe): - name = proc.exe + " - " + name + name = proc_name(proc) mems[name] = float(proc.memRss) / 1e6 print("Top memory usage:") for k, v in sorted(mems.items(), key=lambda item: item[1], reverse=True)[:10]: print(f"{k.rjust(70)} {v:.2f} MB") print() + + prev_proclog = m + prev_proclog_t = sm.logMonoTime['procLog'] diff --git a/selfdrive/debug/show_matching_cars.py b/selfdrive/debug/show_matching_cars.py index df2b7036..273ae146 100755 --- a/selfdrive/debug/show_matching_cars.py +++ b/selfdrive/debug/show_matching_cars.py @@ -16,8 +16,7 @@ candidate_cars = all_known_cars() for addr, l in fingerprint.items(): - dat = messaging.new_message() - dat.init('can', 1) + dat = messaging.new_message('can', 1) msg = dat.can[0] msg.address = addr diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py index 5bf8c4a7..2e668046 100755 --- a/selfdrive/debug/test_fw_query_on_routes.py +++ b/selfdrive/debug/test_fw_query_on_routes.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 +import argparse import os import traceback -import sys from tqdm import tqdm from tools.lib.logreader import LogReader from selfdrive.car.fw_versions import match_fw_to_car @@ -13,14 +13,15 @@ from selfdrive.car.honda.values import FINGERPRINTS as HONDA_FINGERPRINTS if __name__ == "__main__": - if len(sys.argv) < 2: - print("Usage: ./test_fw_query_on_routes.py /") - sys.exit(1) + parser = argparse.ArgumentParser(description='Run FW fingerprint on Qlog of route or list of routes') + parser.add_argument('route', help='Route or file with list of routes') + parser.add_argument('--car', help='Force comparison fingerprint to known car') + args = parser.parse_args() - if os.path.exists(sys.argv[1]): - routes = list(open(sys.argv[1])) + if os.path.exists(args.route): + routes = list(open(args.route)) else: - routes = [sys.argv[1]] + routes = [args.route] wrong = 0 good = 0 @@ -51,6 +52,9 @@ if __name__ == "__main__": dongles.append(dongle_id) live_fingerprint = msg.carParams.carFingerprint + if args.car is not None: + live_fingerprint = args.car + if live_fingerprint not in list(TOYOTA_FINGERPRINTS.keys()) + list(HONDA_FINGERPRINTS.keys()): continue diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index b2b5c18a..bc0f0a03 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -14,13 +14,13 @@ from common.transformations.camera import view_frame_from_device_frame, get_view MPH_TO_MS = 0.44704 MIN_SPEED_FILTER = 15 * MPH_TO_MS -MAX_SPEED_STD = 1.5 +MAX_VEL_ANGLE_STD = np.radians(0.25) MAX_YAW_RATE_FILTER = np.radians(2) # per second # This is all 20Hz, blocks needed for efficiency BLOCK_SIZE = 100 INPUTS_NEEDED = 5 # allow to update VP every so many frames -INPUTS_WANTED = 20 # We want a little bit more than we need for stability +INPUTS_WANTED = 50 # We want a little bit more than we need for stability WRITE_CYCLES = 10 # write every 1000 cycles VP_INIT = np.array([W/2., H/2.]) @@ -89,9 +89,10 @@ class Calibrator(): self.just_calibrated = True def handle_cam_odom(self, trans, rot, trans_std, rot_std): - if ((trans[0] > MIN_SPEED_FILTER) and - (trans_std[0] < MAX_SPEED_STD) and - (abs(rot[2]) < MAX_YAW_RATE_FILTER)): + straight_and_fast = ((trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) + certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < MAX_VEL_ANGLE_STD) or + (self.valid_blocks < INPUTS_NEEDED)) + if straight_and_fast and certain_if_calib: # intrinsics are not eon intrinsics, since this is calibrated frame intrinsics = intrinsics_from_vp(self.vp) new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans)) @@ -104,7 +105,8 @@ class Calibrator(): self.block_idx += 1 self.valid_blocks = max(self.block_idx, self.valid_blocks) self.block_idx = self.block_idx % INPUTS_WANTED - self.vp = np.mean(self.vps[:max(1, self.valid_blocks)], axis=0) + if self.valid_blocks > 0: + self.vp = np.mean(self.vps[:self.valid_blocks], axis=0) self.update_status() if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated): @@ -119,8 +121,7 @@ class Calibrator(): calib = get_calib_from_vp(self.vp) extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height) - cal_send = messaging.new_message() - cal_send.init('liveCalibration') + cal_send = messaging.new_message('liveCalibration') cal_send.liveCalibration.calStatus = self.cal_status cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100) cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()] diff --git a/selfdrive/locationd/kalman/SConscript b/selfdrive/locationd/kalman/SConscript index 2a67ad64..dcacb97c 100644 --- a/selfdrive/locationd/kalman/SConscript +++ b/selfdrive/locationd/kalman/SConscript @@ -1,19 +1,25 @@ -Import('env') +Import('env', 'arch') templates = Glob('templates/*') sympy_helpers = "helpers/sympy_helpers.py" ekf_sym = "helpers/ekf_sym.py" to_build = { - 'pos_computer_4': 'helpers/lst_sq_computer.py', - 'pos_computer_5': 'helpers/lst_sq_computer.py', - 'feature_handler_5': 'helpers/feature_handler.py', - 'gnss': 'models/gnss_kf.py', - 'loc_4': 'models/loc_kf.py', + 'car': 'models/car_kf.py', 'live': 'models/live_kf.py', - 'lane': '#xx/pipeline/lib/ekf/lane_kf.py', } + +if arch != "aarch64": + to_build.update({ + 'lane': '#xx/pipeline/lib/ekf/lane_kf.py', + 'pos_computer_4': 'helpers/lst_sq_computer.py', + 'pos_computer_5': 'helpers/lst_sq_computer.py', + 'feature_handler_5': 'helpers/feature_handler.py', + 'loc_4': 'models/loc_kf.py', + 'gnss': 'models/gnss_kf.py', + }) + found = {} for target, command in to_build.items(): diff --git a/selfdrive/locationd/kalman/helpers/__init__.py b/selfdrive/locationd/kalman/helpers/__init__.py index 5ac7fb3f..2ec5ba0d 100644 --- a/selfdrive/locationd/kalman/helpers/__init__.py +++ b/selfdrive/locationd/kalman/helpers/__init__.py @@ -56,28 +56,44 @@ class ObservationKind(): PSEUDORANGE = 22 PSEUDORANGE_RATE = 23 - names = ['Unknown', - 'No observation', - 'GPS NED', - 'Odometric speed', - 'Phone gyro', - 'GPS velocity', - 'GPS pseudorange', - 'GPS pseudorange rate', - 'Speed', - 'No rotation', - 'Phone acceleration', - 'ORB point', - 'ECEF pos', - 'camera odometric translation', - 'camera odometric rotation', - 'ORB features', - 'MSCKF test', - 'Feature track test', - 'Lane ecef point', - 'imu frame eulers', - 'GLONASS pseudorange', - 'GLONASS pseudorange rate'] + ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s] + ROAD_FRAME_YAW_RATE = 25 # [rad/s] + STEER_ANGLE = 26 # [rad] + ANGLE_OFFSET_FAST = 27 # [rad] + STIFFNESS = 28 # [-] + STEER_RATIO = 29 # [-] + + names = [ + 'Unknown', + 'No observation', + 'GPS NED', + 'Odometric speed', + 'Phone gyro', + 'GPS velocity', + 'GPS pseudorange', + 'GPS pseudorange rate', + 'Speed', + 'No rotation', + 'Phone acceleration', + 'ORB point', + 'ECEF pos', + 'camera odometric translation', + 'camera odometric rotation', + 'ORB features', + 'MSCKF test', + 'Feature track test', + 'Lane ecef point', + 'imu frame eulers', + 'GLONASS pseudorange', + 'GLONASS pseudorange rate', + + 'Road Frame x,y speed', + 'Road Frame yaw rate', + 'Steer Angle', + 'Fast Angle Offset', + 'Stiffness', + 'Steer Ratio', + ] @classmethod def to_string(cls, kind): diff --git a/selfdrive/locationd/kalman/helpers/ekf_sym.py b/selfdrive/locationd/kalman/helpers/ekf_sym.py index 98d4a87b..c9eb093a 100644 --- a/selfdrive/locationd/kalman/helpers/ekf_sym.py +++ b/selfdrive/locationd/kalman/helpers/ekf_sym.py @@ -27,7 +27,7 @@ def null(H, eps=1e-12): return np.transpose(null_space) -def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[]): +def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[], global_vars=None): # optional state transition matrix, H modifier # and err_function if an error-state kalman filter (ESKF) # is desired. Best described in "Quaternion kinematics @@ -74,8 +74,13 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No # linearize with jacobians F_sym = f_err_sym.jacobian(x_err_sym) - for sym in x_err_sym: - F_sym = F_sym.subs(sym, 0) + + if eskf_params: + for sym in x_err_sym: + F_sym = F_sym.subs(sym, 0) + + assert dt_sym in F_sym.free_symbols + for i in range(len(obs_eqs)): obs_eqs[i].append(obs_eqs[i][0].jacobian(x_sym)) if msckf and obs_eqs[i][1] in feature_track_kinds: @@ -105,7 +110,7 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym])) # Generate and wrap all th c code - header, code = sympy_into_c(sympy_functions) + header, code = sympy_into_c(sympy_functions, global_vars) extra_header = "#define DIM %d\n" % dim_x extra_header += "#define EDIM %d\n" % dim_err extra_header += "#define MEDIM %d\n" % dim_main_err @@ -135,6 +140,17 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No code += '\nextern "C"{\n' + extra_header + "\n}\n" code += "\n" + open(os.path.join(TEMPLATE_DIR, "ekf_c.c")).read() code += '\nextern "C"{\n' + extra_post + "\n}\n" + + if global_vars is not None: + global_code = '\nextern "C"{\n' + for var in global_vars: + global_code += f"\ndouble {var.name};\n" + global_code += f"\nvoid set_{var.name}(double x){{ {var.name} = x;}}\n" + extra_header += f"\nvoid set_{var.name}(double x);\n" + + global_code += '\n}\n' + code = global_code + code + header += "\n" + extra_header write_code(name, code, header) @@ -142,7 +158,7 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No class EKF_sym(): def __init__(self, name, Q, x_initial, P_initial, dim_main, dim_main_err, - N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[]): + N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[], global_vars=None): """Generates process function and all observation functions for the kalman filter.""" self.msckf = N > 0 self.N = N @@ -163,6 +179,8 @@ class EKF_sym(): # tested for outlier rejection self.maha_test_kinds = maha_test_kinds + self.global_vars = global_vars + # process noise self.Q = Q @@ -221,6 +239,11 @@ class EKF_sym(): if self.msckf and kind in self.feature_track_kinds: self.Hes[kind] = wrap_2lists("He_%d" % kind) + if self.global_vars is not None: + for var in self.global_vars: + fun_name = f"set_{var.name}" + setattr(self, fun_name, getattr(lib, fun_name)) + # wrap the C++ predict function def _predict_blas(x, P, dt): lib.predict(ffi.cast("double *", x.ctypes.data), @@ -336,6 +359,17 @@ class EKF_sym(): self.rewind_states = self.rewind_states[-REWIND_TO_KEEP:] self.rewind_obscache = self.rewind_obscache[-REWIND_TO_KEEP:] + def predict(self, t): + # initialize time + if self.filter_time is None: + self.filter_time = t + + # predict + dt = t - self.filter_time + assert dt >= 0 + self.x, self.P = self._predict(self.x, self.P, dt) + self.filter_time = t + def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False): # TODO handle rewinding at this level" diff --git a/selfdrive/locationd/kalman/helpers/sympy_helpers.py b/selfdrive/locationd/kalman/helpers/sympy_helpers.py index f8c4f1c5..dee9fb37 100644 --- a/selfdrive/locationd/kalman/helpers/sympy_helpers.py +++ b/selfdrive/locationd/kalman/helpers/sympy_helpers.py @@ -46,11 +46,11 @@ def quat_matrix_r(p): [p[3], p[2], -p[1], p[0]]]) -def sympy_into_c(sympy_functions): +def sympy_into_c(sympy_functions, global_vars=None): from sympy.utilities import codegen routines = [] for name, expr, args in sympy_functions: - r = codegen.make_routine(name, expr, language="C99") + r = codegen.make_routine(name, expr, language="C99", global_vars=global_vars) # argument ordering input to sympy is broken with function with output arguments nargs = [] diff --git a/selfdrive/locationd/kalman/models/car_kf.py b/selfdrive/locationd/kalman/models/car_kf.py new file mode 100755 index 00000000..aa729fc1 --- /dev/null +++ b/selfdrive/locationd/kalman/models/car_kf.py @@ -0,0 +1,196 @@ +#!/usr/bin/env python3 + +import math +import numpy as np +import sympy as sp + +from selfdrive.locationd.kalman.helpers import ObservationKind +from selfdrive.locationd.kalman.helpers.ekf_sym import EKF_sym, gen_code + +i = 0 + + +def _slice(n): + global i + s = slice(i, i + n) + i += n + + return s + + +class States(): + # Vehicle model params + STIFFNESS = _slice(1) # [-] + STEER_RATIO = _slice(1) # [-] + ANGLE_OFFSET = _slice(1) # [rad] + ANGLE_OFFSET_FAST = _slice(1) # [rad] + + VELOCITY = _slice(2) # (x, y) [m/s] + YAW_RATE = _slice(1) # [rad/s] + STEER_ANGLE = _slice(1) # [rad] + + +class CarKalman(): + name = 'car' + + x_initial = np.array([ + 1.0, + 15.0, + 0.0, + 0.0, + + 10.0, 0.0, + 0.0, + 0.0, + ]) + + # state covariance + P_initial = np.diag([ + .1**2, + .1**2, + math.radians(0.1)**2, + math.radians(0.1)**2, + + 10**2, 10**2, + 1**2, + 1**2, + ]) + + # process noise + Q = np.diag([ + (.05/10)**2, + .0001**2, + math.radians(0.01)**2, + math.radians(0.2)**2, + + .1**2, .1**2, + math.radians(0.1)**2, + math.radians(0.1)**2, + ]) + + obs_noise = { + ObservationKind.ROAD_FRAME_XY_SPEED: np.diag([0.1**2, 0.1**2]), + ObservationKind.ROAD_FRAME_YAW_RATE: np.atleast_2d(math.radians(0.1)**2), + ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.1)**2), + ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(5.0)**2), + ObservationKind.STEER_RATIO: np.atleast_2d(50.0**2), + ObservationKind.STIFFNESS: np.atleast_2d(50.0**2), + } + + maha_test_kinds = [] # [ObservationKind.ROAD_FRAME_YAW_RATE, ObservationKind.ROAD_FRAME_XY_SPEED] + global_vars = [ + sp.Symbol('mass'), + sp.Symbol('rotational_inertia'), + sp.Symbol('center_to_front'), + sp.Symbol('center_to_rear'), + sp.Symbol('stiffness_front'), + sp.Symbol('stiffness_rear'), + ] + + @staticmethod + def generate_code(): + dim_state = CarKalman.x_initial.shape[0] + name = CarKalman.name + maha_test_kinds = CarKalman.maha_test_kinds + + # globals + m, j, aF, aR, cF_orig, cR_orig = CarKalman.global_vars + + # make functions and jacobians with sympy + # state variables + state_sym = sp.MatrixSymbol('state', dim_state, 1) + state = sp.Matrix(state_sym) + + # Vehicle model constants + x = state[States.STIFFNESS, :][0, 0] + + cF, cR = x * cF_orig, x * cR_orig + angle_offset = state[States.ANGLE_OFFSET, :][0, 0] + angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0] + sa = state[States.STEER_ANGLE, :][0, 0] + + sR = state[States.STEER_RATIO, :][0, 0] + u, v = state[States.VELOCITY, :] + r = state[States.YAW_RATE, :][0, 0] + + A = sp.Matrix(np.zeros((2, 2))) + A[0, 0] = -(cF + cR) / (m * u) + A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u + A[1, 0] = -(cF * aF - cR * aR) / (j * u) + A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u) + + B = sp.Matrix(np.zeros((2, 1))) + B[0, 0] = cF / m / sR + B[1, 0] = (cF * aF) / j / sR + + x = sp.Matrix([v, r]) # lateral velocity, yaw rate + x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) + + dt = sp.Symbol('dt') + state_dot = sp.Matrix(np.zeros((dim_state, 1))) + state_dot[States.VELOCITY.start + 1, 0] = x_dot[0] + state_dot[States.YAW_RATE.start, 0] = x_dot[1] + + # Basic descretization, 1st order integrator + # Can be pretty bad if dt is big + f_sym = state + dt * state_dot + + # + # Observation functions + # + obs_eqs = [ + [sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None], + [sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None], + [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], + ] + + gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, maha_test_kinds=maha_test_kinds, global_vars=CarKalman.global_vars) + + def __init__(self): + self.dim_state = self.x_initial.shape[0] + + # init filter + self.filter = EKF_sym(self.name, self.Q, self.x_initial, self.P_initial, self.dim_state, self.dim_state, maha_test_kinds=self.maha_test_kinds, global_vars=self.global_vars) + + @property + def x(self): + return self.filter.state() + + @property + def P(self): + return self.filter.covs() + + def predict(self, t): + return self.filter.predict(t) + + def rts_smooth(self, estimates): + return self.filter.rts_smooth(estimates, norm_quats=False) + + def get_R(self, kind, n): + obs_noise = self.obs_noise[kind] + dim = obs_noise.shape[0] + R = np.zeros((n, dim, dim)) + for i in range(n): + R[i, :, :] = obs_noise + return R + + def init_state(self, state, covs_diag=None, covs=None, filter_time=None): + if covs_diag is not None: + P = np.diag(covs_diag) + elif covs is not None: + P = covs + else: + P = self.filter.covs() + self.filter.init_state(state, P, filter_time) + + def predict_and_observe(self, t, kind, data): + if len(data) > 0: + data = np.atleast_2d(data) + self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data))) + + +if __name__ == "__main__": + CarKalman.generate_code() diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py old mode 100755 new mode 100644 index 3cf9560d..001f6d2e --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -6,56 +6,113 @@ import numpy as np import cereal.messaging as messaging import common.transformations.coordinates as coord from common.transformations.orientation import (ecef_euler_from_ned, - euler2quat, + euler_from_quat, ned_euler_from_ecef, - quat2euler, - rotations_from_quats) + quat_from_euler, + rot_from_quat, rot_from_euler) from selfdrive.locationd.kalman.helpers import ObservationKind, KalmanError from selfdrive.locationd.kalman.models.live_kf import LiveKalman, States from selfdrive.swaglog import cloudlog +#from datetime import datetime +#from laika.gps_time import GPSTime + VISION_DECIMATION = 2 SENSOR_DECIMATION = 10 +def to_float(arr): + return [float(arr[0]), float(arr[1]), float(arr[2])] + + class Localizer(): def __init__(self, disabled_logs=[], dog=None): self.kf = LiveKalman() self.reset_kalman() self.max_age = .2 # seconds self.disabled_logs = disabled_logs + self.calib = np.zeros(3) + self.device_from_calib = np.eye(3) + self.calib_from_device = np.eye(3) + self.calibrated = 0 def liveLocationMsg(self, time): - fix = messaging.log.LiveLocationData.new_message() - predicted_state = self.kf.x + predicted_std = np.diagonal(self.kf.P) fix_ecef = predicted_state[States.ECEF_POS] + fix_ecef_std = predicted_std[States.ECEF_POS_ERR] + vel_ecef = predicted_state[States.ECEF_VELOCITY] + vel_ecef_std = predicted_std[States.ECEF_VELOCITY_ERR] fix_pos_geo = coord.ecef2geodetic(fix_ecef) - fix.lat = float(fix_pos_geo[0]) - fix.lon = float(fix_pos_geo[1]) - fix.alt = float(fix_pos_geo[2]) + fix_pos_geo_std = coord.ecef2geodetic(fix_ecef + fix_ecef_std) - fix_pos_geo + ned_vel = self.converter.ecef2ned(fix_ecef + vel_ecef) - self.converter.ecef2ned(fix_ecef) + ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef) + device_from_ecef = rot_from_quat(predicted_state[States.ECEF_ORIENTATION]).T + vel_device = device_from_ecef.dot(vel_ecef) + vel_device_std = device_from_ecef.dot(vel_ecef_std) + orientation_ecef = euler_from_quat(predicted_state[States.ECEF_ORIENTATION]) + orientation_ecef_std = predicted_std[States.ECEF_ORIENTATION_ERR] + orientation_ned = ned_euler_from_ecef(fix_ecef, orientation_ecef) + orientation_ned_std = ned_euler_from_ecef(fix_ecef, orientation_ecef + orientation_ecef_std) - orientation_ned + vel_calib = self.calib_from_device.dot(vel_device) + vel_calib_std = self.calib_from_device.dot(vel_device_std) + acc_calib = self.calib_from_device.dot(predicted_state[States.ACCELERATION]) + acc_calib_std = self.calib_from_device.dot(predicted_std[States.ACCELERATION_ERR]) + ang_vel_calib = self.calib_from_device.dot(predicted_state[States.ANGULAR_VELOCITY]) + ang_vel_calib_std = self.calib_from_device.dot(predicted_std[States.ANGULAR_VELOCITY_ERR]) - fix.speed = float(np.linalg.norm(predicted_state[States.ECEF_VELOCITY])) - orientation_ned_euler = ned_euler_from_ecef(fix_ecef, quat2euler(predicted_state[States.ECEF_ORIENTATION])) - fix.roll = math.degrees(orientation_ned_euler[0]) - fix.pitch = math.degrees(orientation_ned_euler[1]) - fix.heading = math.degrees(orientation_ned_euler[2]) + fix = messaging.log.LiveLocationKalman.new_message() + fix.positionGeodetic.value = to_float(fix_pos_geo) + fix.positionGeodetic.std = to_float(fix_pos_geo_std) + fix.positionGeodetic.valid = True + fix.positionECEF.value = to_float(fix_ecef) + fix.positionECEF.std = to_float(fix_ecef_std) + fix.positionECEF.valid = True + fix.velocityECEF.value = to_float(vel_ecef) + fix.velocityECEF.std = to_float(vel_ecef_std) + fix.velocityECEF.valid = True + fix.velocityNED.value = to_float(ned_vel) + fix.velocityNED.std = to_float(ned_vel_std) + fix.velocityNED.valid = True + fix.velocityDevice.value = to_float(vel_device) + fix.velocityDevice.std = to_float(vel_device_std) + fix.velocityDevice.valid = True + fix.accelerationDevice.value = to_float(predicted_state[States.ACCELERATION]) + fix.accelerationDevice.std = to_float(predicted_std[States.ACCELERATION_ERR]) + fix.accelerationDevice.valid = True - fix.gyro = [float(predicted_state[10]), float(predicted_state[11]), float(predicted_state[12])] - fix.accel = [float(predicted_state[19]), float(predicted_state[20]), float(predicted_state[21])] + fix.orientationECEF.value = to_float(orientation_ecef) + fix.orientationECEF.std = to_float(orientation_ecef_std) + fix.orientationECEF.valid = True + fix.orientationNED.value = to_float(orientation_ned) + fix.orientationNED.std = to_float(orientation_ned_std) + fix.orientationNED.valid = True + fix.angularVelocityDevice.value = to_float(predicted_state[States.ANGULAR_VELOCITY]) + fix.angularVelocityDevice.std = to_float(predicted_std[States.ANGULAR_VELOCITY_ERR]) + fix.angularVelocityDevice.valid = True - ned_vel = self.converter.ecef2ned(predicted_state[States.ECEF_POS] + predicted_state[States.ECEF_VELOCITY]) - self.converter.ecef2ned(predicted_state[States.ECEF_POS]) - fix.vNED = [float(ned_vel[0]), float(ned_vel[1]), float(ned_vel[2])] - fix.source = 'kalman' + fix.velocityCalibrated.value = to_float(vel_calib) + fix.velocityCalibrated.std = to_float(vel_calib_std) + fix.velocityCalibrated.valid = True + fix.angularVelocityCalibrated.value = to_float(ang_vel_calib) + fix.angularVelocityCalibrated.std = to_float(ang_vel_calib_std) + fix.angularVelocityCalibrated.valid = True + fix.accelerationCalibrated.value = to_float(acc_calib) + fix.accelerationCalibrated.std = to_float(acc_calib_std) + fix.accelerationCalibrated.valid = True - #local_vel = rotations_from_quats(predicted_state[States.ECEF_ORIENTATION]).T.dot(predicted_state[States.ECEF_VELOCITY]) - #fix.pitchCalibration = math.degrees(math.atan2(local_vel[2], local_vel[0])) - #fix.yawCalibration = math.degrees(math.atan2(local_vel[1], local_vel[0])) + #fix.gpsWeek = self.time.week + #fix.gpsTimeOfWeek = self.time.tow + fix.unixTimestampMillis = self.unix_timestamp_millis - imu_frame = predicted_state[States.IMU_OFFSET] - fix.imuFrame = [math.degrees(imu_frame[0]), math.degrees(imu_frame[1]), math.degrees(imu_frame[2])] + if self.filter_ready and self.calibrated: + fix.status = 'valid' + elif self.filter_ready: + fix.status = 'uncalibrated' + else: + fix.status = 'uninitialized' return fix def update_kalman(self, time, kind, meas): @@ -75,23 +132,26 @@ class Localizer(): self.converter = coord.LocalCoord.from_geodetic([log.latitude, log.longitude, log.altitude]) fix_ecef = self.converter.ned2ecef([0, 0, 0]) + #self.time = GPSTime.from_datetime(datetime.utcfromtimestamp(log.timestamp*1e-3)) + self.unix_timestamp_millis = log.timestamp + # TODO initing with bad bearing not allowed, maybe not bad? if not self.filter_ready and log.speed > 5: self.filter_ready = True initial_ecef = fix_ecef gps_bearing = math.radians(log.bearing) initial_pose_ecef = ecef_euler_from_ned(initial_ecef, [0, 0, gps_bearing]) - initial_pose_ecef_quat = euler2quat(initial_pose_ecef) + initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef) gps_speed = log.speed quat_uncertainty = 0.2**2 - initial_pose_ecef_quat = euler2quat(initial_pose_ecef) + initial_pose_ecef_quat = quat_from_euler(initial_pose_ecef) initial_state = LiveKalman.initial_x initial_covs_diag = LiveKalman.initial_P_diag initial_state[States.ECEF_POS] = initial_ecef initial_state[States.ECEF_ORIENTATION] = initial_pose_ecef_quat - initial_state[States.ECEF_VELOCITY] = rotations_from_quats(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0])) + initial_state[States.ECEF_VELOCITY] = rot_from_quat(initial_pose_ecef_quat).dot(np.array([gps_speed, 0, 0])) initial_covs_diag[States.ECEF_POS_ERR] = 10**2 initial_covs_diag[States.ECEF_ORIENTATION_ERR] = quat_uncertainty @@ -119,12 +179,16 @@ class Localizer(): self.cam_counter += 1 if self.cam_counter % VISION_DECIMATION == 0: + rot_device = self.device_from_calib.dot(log.rot) + rot_device_std = self.device_from_calib.dot(log.rotStd) self.update_kalman(current_time, ObservationKind.CAMERA_ODO_ROTATION, - np.concatenate([log.rot, log.rotStd])) + np.concatenate([rot_device, rot_device_std])) + trans_device = self.device_from_calib.dot(log.trans) + trans_device_std = self.device_from_calib.dot(log.transStd) self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, - np.concatenate([log.trans, log.transStd])) + np.concatenate([trans_device, trans_device_std])) def handle_sensors(self, current_time, log): # TODO does not yet account for double sensor readings in the log @@ -147,6 +211,12 @@ class Localizer(): v = sensor_reading.acceleration.v self.update_kalman(current_time, ObservationKind.PHONE_ACCEL, [-v[2], -v[1], -v[0]]) + def handle_live_calib(self, current_time, log): + self.calib = log.rpyCalib + self.device_from_calib = rot_from_euler(self.calib) + self.calib_from_device = self.device_from_calib.T + self.calibrated = log.calStatus == 1 + def reset_kalman(self): self.filter_time = None self.filter_ready = False @@ -160,9 +230,9 @@ class Localizer(): def locationd_thread(sm, pm, disabled_logs=[]): if sm is None: - sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry']) + sm = messaging.SubMaster(['gpsLocationExternal', 'sensorEvents', 'cameraOdometry', 'liveCalibration']) if pm is None: - pm = messaging.PubMaster(['liveLocation']) + pm = messaging.PubMaster(['liveLocationKalman']) localizer = Localizer(disabled_logs=disabled_logs) @@ -180,16 +250,16 @@ def locationd_thread(sm, pm, disabled_logs=[]): localizer.handle_car_state(t, sm[sock]) elif sock == "cameraOdometry": localizer.handle_cam_odo(t, sm[sock]) + elif sock == "liveCalibration": + localizer.handle_live_calib(t, sm[sock]) if localizer.filter_ready and sm.updated['gpsLocationExternal']: t = sm.logMonoTime['gpsLocationExternal'] - msg = messaging.new_message() + msg = messaging.new_message('liveLocationKalman') msg.logMonoTime = t - msg.init('liveLocation') - msg.liveLocation = localizer.liveLocationMsg(t * 1e-9) - - pm.send('liveLocation', msg) + msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9) + pm.send('liveLocationKalman', msg) def main(sm=None, pm=None): diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py new file mode 100755 index 00000000..93523502 --- /dev/null +++ b/selfdrive/locationd/paramsd.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python3 +import math + +import cereal.messaging as messaging +from selfdrive.locationd.kalman.models.car_kf import CarKalman, ObservationKind, States + +CARSTATE_DECIMATION = 5 + + +class ParamsLearner: + def __init__(self, CP): + self.kf = CarKalman() + + self.kf.filter.set_mass(CP.mass) # pylint: disable=no-member + self.kf.filter.set_rotational_inertia(CP.rotationalInertia) # pylint: disable=no-member + self.kf.filter.set_center_to_front(CP.centerToFront) # pylint: disable=no-member + self.kf.filter.set_center_to_rear(CP.wheelbase - CP.centerToFront) # pylint: disable=no-member + self.kf.filter.set_stiffness_front(CP.tireStiffnessFront) # pylint: disable=no-member + self.kf.filter.set_stiffness_rear(CP.tireStiffnessRear) # pylint: disable=no-member + + self.active = False + + self.speed = 0 + self.steering_pressed = False + self.steering_angle = 0 + self.carstate_counter = 0 + + def update_active(self): + self.active = (abs(self.steering_angle) < 45 or not self.steering_pressed) and self.speed > 5 + + def handle_log(self, t, which, msg): + if which == 'liveLocationKalman': + + v_calibrated = msg.velocityCalibrated.value + # v_calibrated_std = msg.velocityCalibrated.std + self.speed = v_calibrated[0] + + yaw_rate = msg.angularVelocityCalibrated.value[2] + # yaw_rate_std = msg.angularVelocityCalibrated.std[2] + + self.update_active() + if self.active: + self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_YAW_RATE, [-yaw_rate]) + self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_XY_SPEED, [[v_calibrated[0], -v_calibrated[1]]]) + + # Clamp values + x = self.kf.x + if not (10 < x[States.STEER_RATIO] < 25): + self.kf.predict_and_observe(t, ObservationKind.STEER_RATIO, [15.0]) + + if not (0.5 < x[States.STIFFNESS] < 3.0): + self.kf.predict_and_observe(t, ObservationKind.STIFFNESS, [1.0]) + + else: + self.kf.filter.filter_time = t - 0.1 + + elif which == 'carState': + self.carstate_counter += 1 + if self.carstate_counter % CARSTATE_DECIMATION == 0: + self.steering_angle = msg.steeringAngle + self.steering_pressed = msg.steeringPressed + + self.update_active() + if self.active: + self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, [math.radians(msg.steeringAngle)]) + self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, [0]) + else: + self.kf.filter.filter_time = t - 0.1 + + +def main(sm=None, pm=None): + if sm is None: + sm = messaging.SubMaster(['liveLocationKalman', 'carState']) + if pm is None: + pm = messaging.PubMaster(['liveParameters']) + + # TODO: Read from car params at runtime + from selfdrive.car.toyota.interface import CarInterface + from selfdrive.car.toyota.values import CAR + + CP = CarInterface.get_params(CAR.COROLLA_TSS2) + learner = ParamsLearner(CP) + + while True: + sm.update() + + for which, updated in sm.updated.items(): + if not updated: + continue + t = sm.logMonoTime[which] * 1e-9 + learner.handle_log(t, which, sm[which]) + + # TODO: set valid to false when locationd stops sending + # TODO: make sure controlsd knows when there is no gyro + # TODO: move posenetValid somewhere else to show the model uncertainty alert + # TODO: Save and resume values from param + # TODO: Change KF to allow mass, etc to be inputs in predict step + + if sm.updated['carState']: + msg = messaging.new_message('liveParameters') + msg.logMonoTime = sm.logMonoTime['carState'] + + msg.liveParameters.valid = True # TODO: Check if learned values are sane + msg.liveParameters.posenetValid = True + msg.liveParameters.sensorValid = True + + x = learner.kf.x + msg.liveParameters.steerRatio = float(x[States.STEER_RATIO]) + msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) + msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET]) + msg.liveParameters.angleOffset = math.degrees(x[States.ANGLE_OFFSET_FAST]) + + # P = learner.kf.P + # print() + # print("sR", float(x[States.STEER_RATIO]), float(P[States.STEER_RATIO, States.STEER_RATIO])**0.5) + # print("x ", float(x[States.STIFFNESS]), float(P[States.STIFFNESS, States.STIFFNESS])**0.5) + # print("ao avg ", math.degrees(x[States.ANGLE_OFFSET]), math.degrees(P[States.ANGLE_OFFSET, States.ANGLE_OFFSET])**0.5) + # print("ao ", math.degrees(x[States.ANGLE_OFFSET_FAST]), math.degrees(P[States.ANGLE_OFFSET_FAST, States.ANGLE_OFFSET_FAST])**0.5) + + pm.send('liveParameters', msg) + + +if __name__ == "__main__": + main() diff --git a/selfdrive/locationd/test/ubloxd.py b/selfdrive/locationd/test/ubloxd.py index e2959581..65fdfce9 100755 --- a/selfdrive/locationd/test/ubloxd.py +++ b/selfdrive/locationd/test/ubloxd.py @@ -276,7 +276,7 @@ def handle_msg(dev, msg, nav_frame_buffer): #if dev is not None and dev.dev is not None: # dev.close() -def main(gctx=None): +def main(): global gpsLocationExternal, ubloxGnss nav_frame_buffer = {} nav_frame_buffer[0] = {} diff --git a/selfdrive/locationd/test/ubloxd_easy.py b/selfdrive/locationd/test/ubloxd_easy.py index a2fa753e..8f226604 100755 --- a/selfdrive/locationd/test/ubloxd_easy.py +++ b/selfdrive/locationd/test/ubloxd_easy.py @@ -10,7 +10,7 @@ import cereal.messaging as messaging unlogger = os.getenv("UNLOGGER") is not None # debug prints -def main(gctx=None): +def main(): poller = zmq.Poller() gpsLocationExternal = messaging.pub_sock('gpsLocationExternal') diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript index dfc32fc8..1feb899c 100644 --- a/selfdrive/loggerd/SConscript +++ b/selfdrive/loggerd/SConscript @@ -1,6 +1,6 @@ Import('env', 'arch', 'messaging', 'common', 'visionipc') -src = ['loggerd.cc', 'logger.c'] +src = ['loggerd.cc', 'logger.cc'] libs = ['zmq', 'czmq', 'capnp', 'kj', 'z', 'avformat', 'avcodec', 'swscale', 'avutil', 'yuv', 'bz2', common, 'json', messaging, visionipc] diff --git a/selfdrive/loggerd/deleter.py b/selfdrive/loggerd/deleter.py index b9ae0e8d..27c40e83 100644 --- a/selfdrive/loggerd/deleter.py +++ b/selfdrive/loggerd/deleter.py @@ -35,7 +35,7 @@ def deleter_thread(exit_event): exit_event.wait(30) -def main(gctx=None): +def main(): deleter_thread(threading.Event()) diff --git a/selfdrive/loggerd/logger.c b/selfdrive/loggerd/logger.cc similarity index 86% rename from selfdrive/loggerd/logger.c rename to selfdrive/loggerd/logger.cc index 7dfb610b..0bb6c1b7 100644 --- a/selfdrive/loggerd/logger.c +++ b/selfdrive/loggerd/logger.cc @@ -17,6 +17,21 @@ #include "logger.h" +#include +#include "cereal/gen/cpp/log.capnp.h" + +static void log_sentinel(LoggerState *s, cereal::Sentinel::SentinelType type) { + capnp::MallocMessageBuilder msg; + auto event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + auto sen = event.initSentinel(); + sen.setType(type); + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + + logger_log(s, bytes.begin(), bytes.size(), true); +} + static int mkpath(char* file_path) { assert(file_path && *file_path); char* p; @@ -125,6 +140,9 @@ fail: int logger_next(LoggerState *s, const char* root_path, char* out_segment_path, size_t out_segment_path_len, int* out_part) { + bool is_start_of_route = !s->cur_handle; + if (!is_start_of_route) log_sentinel(s, cereal::Sentinel::SentinelType::END_OF_SEGMENT); + pthread_mutex_lock(&s->lock); s->part++; @@ -147,6 +165,8 @@ int logger_next(LoggerState *s, const char* root_path, } pthread_mutex_unlock(&s->lock); + + log_sentinel(s, is_start_of_route ? cereal::Sentinel::SentinelType::START_OF_ROUTE : cereal::Sentinel::SentinelType::START_OF_SEGMENT); return 0; } @@ -171,6 +191,8 @@ void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog) { } void logger_close(LoggerState *s) { + log_sentinel(s, cereal::Sentinel::SentinelType::END_OF_ROUTE); + pthread_mutex_lock(&s->lock); free(s->init_data); if (s->cur_handle) { diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 6067f01e..dba2287b 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -72,7 +72,7 @@ def is_on_wifi(): if result is None: return True return 'WIFI' in result - except (AttributeError, subprocess.CalledProcessError): + except Exception: cloudlog.exception("is_on_wifi failed") return False @@ -266,7 +266,7 @@ def uploader_fn(exit_event): backoff = min(backoff*2, 120) cloudlog.info("upload done, success=%r", success) -def main(gctx=None): +def main(): uploader_fn(threading.Event()) if __name__ == "__main__": diff --git a/selfdrive/logmessaged.py b/selfdrive/logmessaged.py index 3362b5ba..693d259a 100755 --- a/selfdrive/logmessaged.py +++ b/selfdrive/logmessaged.py @@ -3,7 +3,7 @@ import zmq from logentries import LogentriesHandler import cereal.messaging as messaging -def main(gctx=None): +def main(): # setup logentries. we forward log messages to it le_token = "e8549616-0798-4d7e-a2ca-2513ae81fa17" le_handler = LogentriesHandler(le_token, use_tls=False, verbose=False) diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 0d8efc8d..4a63241d 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -56,7 +56,10 @@ def unblock_stdout(): except (OSError, IOError, UnicodeDecodeError): pass - os._exit(os.wait()[1]) + # os.wait() returns a tuple with the pid and a 16 bit value + # whose low byte is the signal number and whose high byte is the exit satus + exit_status = os.wait()[1] >> 8 + os._exit(exit_status) if __name__ == "__main__": @@ -126,13 +129,14 @@ from selfdrive.version import version, dirty from selfdrive.loggerd.config import ROOT from selfdrive.launcher import launcher from common import android -from common.apk import update_apks, pm_apply_packages, start_frame +from common.apk import update_apks, pm_apply_packages, start_offroad +from common.manager_helpers import print_cpu_usage ThermalStatus = cereal.log.ThermalData.ThermalStatus # comment out anything you don't want to run managed_processes = { - "thermald": "selfdrive.thermald", + "thermald": "selfdrive.thermald.thermald", "uploader": "selfdrive.loggerd.uploader", "deleter": "selfdrive.loggerd.deleter", "controlsd": "selfdrive.controls.controlsd", @@ -383,6 +387,9 @@ def manager_thread(): # now loop thermal_sock = messaging.sub_sock('thermal') + if os.getenv("GET_CPU_USAGE"): + proc_sock = messaging.sub_sock('procLog', conflate=True) + cloudlog.info("manager start") cloudlog.info({"environ": os.environ}) @@ -399,16 +406,23 @@ def manager_thread(): for p in persistent_processes: start_managed_process(p) - # start frame + # start offroad if ANDROID: pm_apply_packages('enable') - start_frame() + start_offroad() if os.getenv("NOBOARD") is None: start_managed_process("pandad") + if os.getenv("BLOCK") is not None: + for k in os.getenv("BLOCK").split(","): + del managed_processes[k] + logger_dead = False + start_t = time.time() + first_proc = None + while 1: msg = messaging.recv_sock(thermal_sock, wait=True) @@ -444,6 +458,21 @@ def manager_thread(): if params.get("DoUninstall", encoding='utf8') == "1": break + if os.getenv("GET_CPU_USAGE"): + dt = time.time() - start_t + + # Get first sample + if dt > 30 and first_proc is None: + first_proc = messaging.recv_sock(proc_sock) + + # Get last sample and exit + if dt > 90: + first_proc = first_proc + last_proc = messaging.recv_sock(proc_sock, wait=True) + + cleanup_all_processes(None, None) + sys.exit(print_cpu_usage(first_proc, last_proc)) + def manager_prepare(spinner=None): # build all processes os.chdir(os.path.dirname(os.path.abspath(__file__))) @@ -475,40 +504,29 @@ def main(): params = Params() params.manager_start() + default_params = [ + ("CommunityFeaturesToggle", "0"), + ("CompletedTrainingVersion", "0"), + ("IsMetric", "0"), + ("RecordFront", "0"), + ("HasAcceptedTerms", "0"), + ("HasCompletedSetup", "0"), + ("IsUploadRawEnabled", "1"), + ("IsLdwEnabled", "1"), + ("IsGeofenceEnabled", "-1"), + ("SpeedLimitOffset", "0"), + ("LongitudinalControl", "0"), + ("LimitSetSpeed", "0"), + ("LimitSetSpeedNeural", "0"), + ("LastUpdateTime", datetime.datetime.now().isoformat().encode('utf8')), + ("OpenpilotEnabledToggle", "1"), + ("LaneChangeEnabled", "1"), + ] + # set unset params - if params.get("CommunityFeaturesToggle") is None: - params.put("CommunityFeaturesToggle", "0") - if params.get("CompletedTrainingVersion") is None: - params.put("CompletedTrainingVersion", "0") - if params.get("IsMetric") is None: - params.put("IsMetric", "0") - if params.get("RecordFront") is None: - params.put("RecordFront", "0") - if params.get("HasAcceptedTerms") is None: - params.put("HasAcceptedTerms", "0") - if params.get("HasCompletedSetup") is None: - params.put("HasCompletedSetup", "0") - if params.get("IsUploadRawEnabled") is None: - params.put("IsUploadRawEnabled", "1") - if params.get("IsLdwEnabled") is None: - params.put("IsLdwEnabled", "1") - if params.get("IsGeofenceEnabled") is None: - params.put("IsGeofenceEnabled", "-1") - if params.get("SpeedLimitOffset") is None: - params.put("SpeedLimitOffset", "0") - if params.get("LongitudinalControl") is None: - params.put("LongitudinalControl", "0") - if params.get("LimitSetSpeed") is None: - params.put("LimitSetSpeed", "0") - if params.get("LimitSetSpeedNeural") is None: - params.put("LimitSetSpeedNeural", "0") - if params.get("LastUpdateTime") is None: - t = datetime.datetime.now().isoformat() - params.put("LastUpdateTime", t.encode('utf8')) - if params.get("OpenpilotEnabledToggle") is None: - params.put("OpenpilotEnabledToggle", "1") - if params.get("LaneChangeEnabled") is None: - params.put("LaneChangeEnabled", "1") + for k, v in default_params: + if params.get(k) is None: + params.put(k, v) # is this chffrplus? if os.getenv("PASSIVE") is not None: @@ -531,6 +549,8 @@ def main(): try: manager_thread() + except SystemExit: + raise except Exception: traceback.print_exc() crash.capture_exception() diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index b7d1c1ce..b544a3e5 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -10,10 +10,11 @@ #define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2 + 1 #define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 2 #define LEAD_IDX RL_IDX + MODEL_PATH_DISTANCE*2 + 2 -#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION +#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION #define LONG_V_IDX LONG_X_IDX + TIME_DISTANCE*2 #define LONG_A_IDX LONG_V_IDX + TIME_DISTANCE*2 -#define META_IDX LONG_A_IDX + TIME_DISTANCE*2 +#define DESIRE_STATE_IDX LONG_A_IDX + TIME_DISTANCE*2 +#define META_IDX DESIRE_STATE_IDX + DESIRE_LEN #define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE #define OUTPUT_SIZE POSE_IDX + POSE_SIZE #ifdef TEMPORAL @@ -43,7 +44,9 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t #ifdef DESIRE s->desire = (float*)malloc(DESIRE_SIZE * sizeof(float)); for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = 0.0; - s->m->addDesire(s->desire, DESIRE_SIZE); + s->pulse_desire = (float*)malloc(DESIRE_SIZE * sizeof(float)); + for (int i = 0; i < DESIRE_SIZE; i++) s->pulse_desire[i] = 0.0; + s->m->addDesire(s->pulse_desire, DESIRE_SIZE); #endif // Build Vandermonde matrix @@ -61,7 +64,16 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, mat3 transform, void* sock, float *desire_in) { #ifdef DESIRE if (desire_in != NULL) { - for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = desire_in[i]; + for (int i = 0; i < DESIRE_SIZE; i++) { + // Model decides when action is completed + // so desire input is just a pulse triggered on rising edge + if (desire_in[i] - s->desire[i] == 1) { + s->pulse_desire[i] = desire_in[i]; + } else { + s->pulse_desire[i] = 0.0; + } + s->desire[i] = desire_in[i]; + } } #endif @@ -88,7 +100,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, net_outputs.long_x = &s->output[LONG_X_IDX]; net_outputs.long_v = &s->output[LONG_V_IDX]; net_outputs.long_a = &s->output[LONG_A_IDX]; - net_outputs.meta = &s->output[META_IDX]; + net_outputs.meta = &s->output[DESIRE_STATE_IDX]; net_outputs.pose = &s->output[POSE_IDX]; return net_outputs; } @@ -167,11 +179,11 @@ void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bo path.setStd(std); } -void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx) { +void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx, int t_offset) { const double x_scale = 10.0; const double y_scale = 10.0; - lead.setProb(sigmoid(data[LEAD_MDN_N*MDN_GROUP_SIZE])); + lead.setProb(sigmoid(data[LEAD_MDN_N*MDN_GROUP_SIZE + t_offset])); lead.setDist(x_scale * data[mdn_max_idx*MDN_GROUP_SIZE]); lead.setStd(x_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS])); lead.setRelY(y_scale * data[mdn_max_idx*MDN_GROUP_SIZE + 1]); @@ -183,11 +195,13 @@ void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, in } void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) { - meta.setEngagedProb(meta_data[0]); - meta.setGasDisengageProb(meta_data[1]); - meta.setBrakeDisengageProb(meta_data[2]); - meta.setSteerOverrideProb(meta_data[3]); - kj::ArrayPtr desire_pred(&meta_data[OTHER_META_SIZE], DESIRE_PRED_SIZE); + kj::ArrayPtr desire_state(&meta_data[0], DESIRE_LEN); + meta.setDesireState(desire_state); + meta.setEngagedProb(meta_data[DESIRE_LEN]); + meta.setGasDisengageProb(meta_data[DESIRE_LEN + 1]); + meta.setBrakeDisengageProb(meta_data[DESIRE_LEN + 2]); + meta.setSteerOverrideProb(meta_data[DESIRE_LEN + 3]); + kj::ArrayPtr desire_pred(&meta_data[DESIRE_LEN + OTHER_META_SIZE], DESIRE_PRED_SIZE); meta.setDesirePrediction(desire_pred); } @@ -228,22 +242,24 @@ void model_publish(PubSocket *sock, uint32_t frame_id, // Find the distribution that corresponds to the current lead int mdn_max_idx = 0; + int t_offset = 0; for (int i=1; i net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) { + if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) { mdn_max_idx = i; } } auto lead = framed.initLead(); - fill_lead(lead, net_outputs.lead, mdn_max_idx); + fill_lead(lead, net_outputs.lead, mdn_max_idx, t_offset); // Find the distribution that corresponds to the lead in 2s mdn_max_idx = 0; + t_offset = 1; for (int i=1; i net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) { + if (net_outputs.lead[i*MDN_GROUP_SIZE + 8 + t_offset] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8 + t_offset]) { mdn_max_idx = i; } } auto lead_future = framed.initLeadFuture(); - fill_lead(lead_future, net_outputs.lead, mdn_max_idx); + fill_lead(lead_future, net_outputs.lead, mdn_max_idx, t_offset); auto meta = framed.initMeta(); diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index bf46cb9d..742eb6ce 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -34,6 +34,7 @@ #define MODEL_PATH_DISTANCE 192 #define POLYFIT_DEGREE 4 #define SPEED_PERCENTILES 10 +#define DESIRE_LEN 8 #define DESIRE_PRED_SIZE 32 #define OTHER_META_SIZE 4 #define LEAD_MDN_N 5 // probs for 5 groups @@ -51,6 +52,7 @@ struct ModelDataRaw { float *long_x; float *long_v; float *long_a; + float *desire_state; float *meta; float *pose; }; @@ -63,6 +65,7 @@ typedef struct ModelState { RunModel *m; #ifdef DESIRE float *desire; + float *pulse_desire; #endif } ModelState; diff --git a/selfdrive/pandad.py b/selfdrive/pandad.py index 12d6719f..bf747af0 100755 --- a/selfdrive/pandad.py +++ b/selfdrive/pandad.py @@ -86,7 +86,7 @@ def update_panda(): raise AssertionError -def main(gctx=None): +def main(): update_panda() os.chdir("boardd") diff --git a/selfdrive/registration.py b/selfdrive/registration.py index bebe0549..5b01d144 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -15,9 +15,10 @@ def register(): params.put("Version", version) params.put("TermsVersion", terms_version) params.put("TrainingVersion", training_version) - params.put("GitCommit", get_git_commit()) - params.put("GitBranch", get_git_branch()) - params.put("GitRemote", get_git_remote()) + + params.put("GitCommit", get_git_commit(default="")) + params.put("GitBranch", get_git_branch(default="")) + params.put("GitRemote", get_git_remote(default="")) params.put("SubscriberInfo", get_subscriber_info()) # create a key for auth diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 2170cb71..7d6b24d4 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -357,8 +357,7 @@ class Plant(): # Fake sockets that controlsd subscribes to - live_parameters = messaging.new_message() - live_parameters.init('liveParameters') + live_parameters = messaging.new_message('liveParameters') live_parameters.liveParameters.valid = True live_parameters.liveParameters.sensorValid = True live_parameters.liveParameters.posenetValid = True @@ -366,19 +365,16 @@ class Plant(): live_parameters.liveParameters.stiffnessFactor = 1.0 Plant.live_params.send(live_parameters.to_bytes()) - driver_state = messaging.new_message() - driver_state.init('driverState') + driver_state = messaging.new_message('driverState') driver_state.driverState.faceOrientation = [0.] * 3 driver_state.driverState.facePosition = [0.] * 2 Plant.driverState.send(driver_state.to_bytes()) - health = messaging.new_message() - health.init('health') + health = messaging.new_message('health') health.health.controlsAllowed = True Plant.health.send(health.to_bytes()) - thermal = messaging.new_message() - thermal.init('thermal') + thermal = messaging.new_message('thermal') thermal.thermal.freeSpace = 1. thermal.thermal.batteryPercent = 100 Plant.thermal.send(thermal.to_bytes()) @@ -386,10 +382,8 @@ class Plant(): # ******** publish a fake model going straight and fake calibration ******** # note that this is worst case for MPC, since model will delay long mpc by one time step if publish_model and self.frame % 5 == 0: - md = messaging.new_message() - cal = messaging.new_message() - md.init('model') - cal.init('liveCalibration') + md = messaging.new_message('model') + cal = messaging.new_message('liveCalibration') md.model.frameId = 0 for x in [md.model.path, md.model.leftLane, md.model.rightLane]: x.points = [0.0]*50 diff --git a/selfdrive/test/longitudinal_maneuvers/plant_ui.py b/selfdrive/test/longitudinal_maneuvers/plant_ui.py index f4b7715e..3c73f832 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant_ui.py +++ b/selfdrive/test/longitudinal_maneuvers/plant_ui.py @@ -65,8 +65,7 @@ if __name__ == "__main__": else: cruise_buttons = 0 - md = messaging.new_message() - md.init('model') + md = messaging.new_message('model') md.model.frameId = 0 for x in [md.model.path, md.model.leftLane, md.model.rightLane]: x.points = [0.0]*50 diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index 6cd689f1..02e4e63c 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -333,7 +333,6 @@ class LongitudinalControl(unittest.TestCase): params.put("OpenpilotEnabledToggle", "1") params.put("CommunityFeaturesToggle", "1") - manager.gctx = {} manager.prepare_managed_process('radard') manager.prepare_managed_process('controlsd') manager.prepare_managed_process('plannerd') diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index 82e701a7..731e7167 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -2,6 +2,7 @@ import bz2 import os import sys +import numbers import dictdiffer if "CI" in os.environ: @@ -13,7 +14,7 @@ from tools.lib.logreader import LogReader def save_log(dest, log_msgs): dat = b"" - for msg in log_msgs: + for msg in tqdm(log_msgs): dat += msg.as_builder().to_bytes() dat = bz2.compress(dat) @@ -22,7 +23,7 @@ def save_log(dest, log_msgs): def remove_ignored_fields(msg, ignore): msg = msg.as_builder() - for key, val in ignore: + for key in ignore: attr = msg keys = key.split(".") if msg.which() not in key and len(keys) > 1: @@ -34,21 +35,29 @@ def remove_ignored_fields(msg, ignore): except: break else: + v = getattr(attr, keys[-1]) + if isinstance(v, bool): + val = False + elif isinstance(v, numbers.Number): + val = 0 + else: + raise NotImplementedError setattr(attr, keys[-1], val) return msg.as_reader() -def compare_logs(log1, log2, ignore=[]): +def compare_logs(log1, log2, ignore_fields=[], ignore_msgs=[]): + filter_msgs = lambda m: m.which() not in ignore_msgs + log1, log2 = [list(filter(filter_msgs, log)) for log in (log1, log2)] assert len(log1) == len(log2), "logs are not same length: " + str(len(log1)) + " VS " + str(len(log2)) - ignore_fields = [k for k, v in ignore] diff = [] for msg1, msg2 in tqdm(zip(log1, log2)): if msg1.which() != msg2.which(): print(msg1, msg2) - assert False, "msgs not aligned between logs" + raise Exception("msgs not aligned between logs") - msg1_bytes = remove_ignored_fields(msg1, ignore).as_builder().to_bytes() - msg2_bytes = remove_ignored_fields(msg2, ignore).as_builder().to_bytes() + msg1_bytes = remove_ignored_fields(msg1, ignore_fields).as_builder().to_bytes() + msg2_bytes = remove_ignored_fields(msg2, ignore_fields).as_builder().to_bytes() if msg1_bytes != msg2_bytes: msg1_dict = msg1.to_dict(verbose=True) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 9b27308b..b8288987 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -61,8 +61,7 @@ class FakeSocket: class DumbSocket: def __init__(self, s=None): if s is not None: - dat = messaging.new_message() - dat.init(s) + dat = messaging.new_message(s) self.data = dat.to_bytes() def receive(self, non_blocking=False): @@ -108,11 +107,10 @@ class FakePubMaster(messaging.PubMaster): self.sock = {} self.last_updated = None for s in services: - data = messaging.new_message() try: - data.init(s) + data = messaging.new_message(s) except: - data.init(s, 0) + data = messaging.new_message(s, 0) self.data[s] = data.as_reader() self.sock[s] = DumbSocket() self.send_called = threading.Event() @@ -200,7 +198,7 @@ CONFIGS = [ "thermal": [], "health": [], "liveCalibration": [], "dMonitoringState": [], "plan": [], "pathPlan": [], "gpsLocation": [], "model": [], }, - ignore=[("logMonoTime", 0), ("valid", True), ("controlsState.startMonoTime", 0), ("controlsState.cumLagMs", 0)], + ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"], init_callback=fingerprint, should_recv_callback=None, ), @@ -210,7 +208,7 @@ CONFIGS = [ "can": ["radarState", "liveTracks"], "liveParameters": [], "controlsState": [], "model": [], }, - ignore=[("logMonoTime", 0), ("valid", True), ("radarState.cumLagMs", 0)], + ignore=["logMonoTime", "valid", "radarState.cumLagMs"], init_callback=get_car_params, should_recv_callback=radar_rcv_callback, ), @@ -220,7 +218,7 @@ CONFIGS = [ "model": ["pathPlan"], "radarState": ["plan"], "carState": [], "controlsState": [], "liveParameters": [], }, - ignore=[("logMonoTime", 0), ("valid", True), ("plan.processingDelay", 0)], + ignore=["logMonoTime", "valid", "plan.processingDelay"], init_callback=get_car_params, should_recv_callback=None, ), @@ -229,7 +227,7 @@ CONFIGS = [ pub_sub={ "cameraOdometry": ["liveCalibration"] }, - ignore=[("logMonoTime", 0), ("valid", True)], + ignore=["logMonoTime", "valid"], init_callback=get_car_params, should_recv_callback=calibration_rcv_callback, ), @@ -239,7 +237,7 @@ CONFIGS = [ "driverState": ["dMonitoringState"], "liveCalibration": [], "carState": [], "model": [], "gpsLocation": [], }, - ignore=[("logMonoTime", 0), ("valid", True)], + ignore=["logMonoTime", "valid"], init_callback=get_car_params, should_recv_callback=None, ), diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 1414556b..150649e6 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -917e6889be1691fb96e7566a92e0c6bbefc861a4 \ No newline at end of file +b98f4b6858a21fd6602fb41c6a21f69b1ca81921 \ No newline at end of file diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 6832aba3..f57fc83c 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -1,116 +1,179 @@ #!/usr/bin/env python3 +import argparse import os import requests import sys import tempfile -from selfdrive.test.process_replay.compare_logs import compare_logs +from selfdrive.car.car_helpers import interface_names from selfdrive.test.process_replay.process_replay import replay_process, CONFIGS +from selfdrive.test.process_replay.compare_logs import compare_logs from tools.lib.logreader import LogReader + +INJECT_MODEL = 0 + segments = [ - "0375fdf7b1ce594d|2019-06-13--08-32-25--3", # HONDA.ACCORD - "99c94dc769b5d96e|2019-08-03--14-19-59--2", # HONDA.CIVIC - "cce908f7eb8db67d|2019-08-02--15-09-51--3", # TOYOTA.COROLLA_TSS2 - "7ad88f53d406b787|2019-07-09--10-18-56--8", # GM.VOLT - "704b2230eb5190d6|2019-07-06--19-29-10--0", # HYUNDAI.KIA_SORENTO - "b6e1317e1bfbefa6|2019-07-06--04-05-26--5", # CHRYSLER.JEEP_CHEROKEE - "7873afaf022d36e2|2019-07-03--18-46-44--0", # SUBARU.IMPREZA + ("HONDA", "0375fdf7b1ce594d|2019-06-13--08-32-25--3"), # HONDA.ACCORD + ("HONDA", "99c94dc769b5d96e|2019-08-03--14-19-59--2"), # HONDA.CIVIC + ("TOYOTA", "77611a1fac303767|2020-02-29--13-29-33--3"), # TOYOTA.COROLLA_TSS2 + ("GM", "7cc2a8365b4dd8a9|2018-12-02--12-10-44--2"), # GM.ACADIA + ("CHRYSLER", "b6849f5cf2c926b1|2020-02-28--07-29-48--13"), # CHRYSLER.PACIFICA + ("HYUNDAI", "38bfd238edecbcd7|2018-08-29--22-02-15--4"), # HYUNDAI.SANTA_FE + #("CHRYSLER", "b6e1317e1bfbefa6|2020-03-04--13-11-40"), # CHRYSLER.JEEP_CHEROKEE + ("SUBARU", "7873afaf022d36e2|2019-07-03--18-46-44--0"), # SUBARU.IMPREZA + ("VOLKSWAGEN", "76b83eb0245de90e|2020-03-05--19-16-05--3"), # VW.GOLF + + # Enable when port is tested and dascamOnly is no longer set + # ("NISSAN", "fbbfa6af821552b9|2020-03-03--08-09-43--0"), # NISSAN.XTRAIL ] -def get_segment(segment_name): +# ford doesn't need to be tested until a full port is done +excluded_interfaces = ["mock", "ford", "nissan"] + +BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" + +# run the full test (including checks) when no args given +FULL_TEST = len(sys.argv) <= 1 + +def get_segment(segment_name, original=True): route_name, segment_num = segment_name.rsplit("--", 1) - rlog_url = "https://commadataci.blob.core.windows.net/openpilotci/%s/%s/rlog.bz2" \ - % (route_name.replace("|", "/"), segment_num) - r = requests.get(rlog_url) - if r.status_code != 200: - return None + if original: + rlog_url = BASE_URL + "%s/%s/rlog.bz2" % (route_name.replace("|", "/"), segment_num) + else: + process_replay_dir = os.path.dirname(os.path.abspath(__file__)) + model_ref_commit = open(os.path.join(process_replay_dir, "model_ref_commit")).read().strip() + rlog_url = BASE_URL + "%s/%s/rlog_%s.bz2" % (route_name.replace("|", "/"), segment_num, model_ref_commit) + req = requests.get(rlog_url) + assert req.status_code == 200, ("Failed to download log for %s" % segment_name) with tempfile.NamedTemporaryFile(delete=False, suffix=".bz2") as f: - f.write(r.content) + f.write(req.content) return f.name +def test_process(cfg, lr, cmp_log_fn, ignore_fields=[], ignore_msgs=[]): + if not os.path.isfile(cmp_log_fn): + req = requests.get(BASE_URL + os.path.basename(cmp_log_fn)) + assert req.status_code == 200, ("Failed to download %s" % cmp_log_fn) + + with tempfile.NamedTemporaryFile(suffix=".bz2") as f: + f.write(req.content) + f.flush() + f.seek(0) + cmp_log_msgs = list(LogReader(f.name)) + else: + cmp_log_msgs = list(LogReader(cmp_log_fn)) + + 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: + for msg in log_msgs: + if msg.which() == "controlsState": + if msg.controlsState.active: + break + else: + segment = cmp_log_fn.split("/")[-1].split("_")[0] + raise Exception("Route never enabled: %s" % segment) + + return compare_logs(cmp_log_msgs, log_msgs, ignore_fields+cfg.ignore, ignore_msgs) + +def format_diff(results, ref_commit): + diff1, diff2 = "", "" + diff2 += "***** tested against commit %s *****\n" % ref_commit + + failed = False + for segment, result in list(results.items()): + diff1 += "***** results for segment %s *****\n" % segment + diff2 += "***** differences for segment %s *****\n" % segment + + for proc, diff in list(result.items()): + diff1 += "\t%s\n" % proc + diff2 += "*** process: %s ***\n" % proc + + if isinstance(diff, str): + diff1 += "\t\t%s\n" % diff + failed = True + elif len(diff): + cnt = {} + for d in diff: + diff2 += "\t%s\n" % str(d) + + k = str(d[1]) + cnt[k] = 1 if k not in cnt else cnt[k] + 1 + + for k, v in sorted(cnt.items()): + diff1 += "\t\t%s: %s\n" % (k, v) + failed = True + return diff1, diff2, failed + if __name__ == "__main__": - process_replay_dir = os.path.dirname(os.path.abspath(__file__)) - ref_commit_fn = os.path.join(process_replay_dir, "ref_commit") + parser = argparse.ArgumentParser(description="Regression test to identify changes in a process's output") - if not os.path.isfile(ref_commit_fn): + # whitelist has precedence over blacklist in case both are defined + parser.add_argument("--whitelist-procs", type=str, nargs="*", default=[], + help="Whitelist given processes from the test (e.g. controlsd)") + parser.add_argument("--whitelist-cars", type=str, nargs="*", default=[], + help="Whitelist given cars from the test (e.g. HONDA)") + parser.add_argument("--blacklist-procs", type=str, nargs="*", default=[], + help="Blacklist given processes from the test (e.g. controlsd)") + parser.add_argument("--blacklist-cars", type=str, nargs="*", default=[], + help="Blacklist given cars from the test (e.g. HONDA)") + parser.add_argument("--ignore-fields", type=str, nargs="*", default=[], + help="Extra fields or msgs to ignore (e.g. carState.events)") + parser.add_argument("--ignore-msgs", type=str, nargs="*", default=[], + help="Msgs to ignore (e.g. carEvents)") + args = parser.parse_args() + + cars_whitelisted = len(args.whitelist_cars) > 0 + procs_whitelisted = len(args.whitelist_procs) > 0 + + process_replay_dir = os.path.dirname(os.path.abspath(__file__)) + try: + ref_commit = open(os.path.join(process_replay_dir, "ref_commit")).read().strip() + except: print("couldn't find reference commit") sys.exit(1) - ref_commit = open(ref_commit_fn).read().strip() print("***** testing against commit %s *****" % ref_commit) + # check to make sure all car brands are tested + if FULL_TEST: + tested_cars = set(c.lower() for c, _ in segments) + untested = (set(interface_names) - set(excluded_interfaces)) - tested_cars + assert len(untested) == 0, "Cars missing routes: %s" % (str(untested)) + results = {} - for segment in segments: + for car_brand, segment in segments: + if (cars_whitelisted and car_brand.upper() not in args.whitelist_cars) or \ + (not cars_whitelisted and car_brand.upper() in args.blacklist_cars): + continue + print("***** testing route segment %s *****\n" % segment) results[segment] = {} rlog_fn = get_segment(segment) - - if rlog_fn is None: - print("failed to get segment %s" % segment) - sys.exit(1) - lr = LogReader(rlog_fn) for cfg in CONFIGS: - log_msgs = replay_process(cfg, lr) + if (procs_whitelisted and cfg.proc_name not in args.whitelist_procs) or \ + (not procs_whitelisted and cfg.proc_name in args.blacklist_procs): + continue - log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit)) - - if not os.path.isfile(log_fn): - url = "https://commadataci.blob.core.windows.net/openpilotci/" - req = requests.get(url + os.path.basename(log_fn)) - if req.status_code != 200: - results[segment][cfg.proc_name] = "failed to download comparison log" - continue - - with tempfile.NamedTemporaryFile(suffix=".bz2") as f: - f.write(req.content) - f.flush() - f.seek(0) - cmp_log_msgs = list(LogReader(f.name)) - else: - cmp_log_msgs = list(LogReader(log_fn)) - - diff = compare_logs(cmp_log_msgs, log_msgs, cfg.ignore) - results[segment][cfg.proc_name] = diff + cmp_log_fn = os.path.join(process_replay_dir, "%s_%s_%s.bz2" % (segment, cfg.proc_name, ref_commit)) + results[segment][cfg.proc_name] = test_process(cfg, lr, cmp_log_fn, args.ignore_fields, args.ignore_msgs) os.remove(rlog_fn) - failed = False + diff1, diff2, failed = format_diff(results, ref_commit) with open(os.path.join(process_replay_dir, "diff.txt"), "w") as f: - f.write("***** tested against commit %s *****\n" % ref_commit) + f.write(diff2) + print(diff1) - for segment, result in list(results.items()): - f.write("***** differences for segment %s *****\n" % segment) - print("***** results for segment %s *****" % segment) - - for proc, diff in list(result.items()): - f.write("*** process: %s ***\n" % proc) - print("\t%s" % proc) - - if isinstance(diff, str): - print("\t\t%s" % diff) - failed = True - elif len(diff): - cnt = {} - for d in diff: - f.write("\t%s\n" % str(d)) - - k = str(d[1]) - cnt[k] = 1 if k not in cnt else cnt[k] + 1 - - for k, v in sorted(cnt.items()): - print("\t\t%s: %s" % (k, v)) - failed = True - - if failed: - print("TEST FAILED") - else: - print("TEST SUCCEEDED") + print("TEST", "FAILED" if failed else "SUCCEEDED") print("\n\nTo update the reference logs for this test run:") print("./update_refs.py") diff --git a/selfdrive/test/process_replay/update_refs.py b/selfdrive/test/process_replay/update_refs.py index e0da2513..4a769d86 100755 --- a/selfdrive/test/process_replay/update_refs.py +++ b/selfdrive/test/process_replay/update_refs.py @@ -20,7 +20,7 @@ if __name__ == "__main__": with open(ref_commit_fn, "w") as f: f.write(ref_commit) - for segment in segments: + for car_brand, segment in segments: rlog_fn = get_segment(segment) if rlog_fn is None: diff --git a/selfdrive/test/test_car_models.py b/selfdrive/test/test_car_models.py index 1436f404..fd7a8e60 100755 --- a/selfdrive/test/test_car_models.py +++ b/selfdrive/test/test_car_models.py @@ -20,7 +20,7 @@ from selfdrive.car.hyundai.values import CAR as HYUNDAI from selfdrive.car.chrysler.values import CAR as CHRYSLER from selfdrive.car.subaru.values import CAR as SUBARU from selfdrive.car.volkswagen.values import CAR as VOLKSWAGEN -from selfdrive.car.mock.values import CAR as MOCK +from selfdrive.car.nissan.values import CAR as NISSAN os.environ['NOCRASH'] = '1' @@ -58,35 +58,15 @@ def get_route_log(route_name): sys.exit(-1) routes = { - "975b26878285314d|2018-12-25--14-42-13": { - 'carFingerprint': CHRYSLER.PACIFICA_2018_HYBRID, - 'enableCamera': True, - }, - "b0c9d2329ad1606b|2019-01-06--10-11-23": { + "420a8e183f1aed48|2020-03-05--07-15-29": { 'carFingerprint': CHRYSLER.PACIFICA_2017_HYBRID, 'enableCamera': True, }, - "0607d2516fc2148f|2019-02-13--23-03-16": { + "8190c7275a24557b|2020-01-29--08-33-58": { # 2020 model year 'carFingerprint': CHRYSLER.PACIFICA_2019_HYBRID, 'enableCamera': True, }, - "8190c7275a24557b|2020-01-29--08-33-58": { - 'carFingerprint': CHRYSLER.PACIFICA_2020_HYBRID, - 'enableCamera': True, - }, # This pacifica was removed because the fingerprint seemed from a Volt - #"9f7a7e50a51fb9db|2019-01-03--14-05-01": { - # 'carFingerprint': CHRYSLER.PACIFICA_2018, - # 'enableCamera': True, - #}, - "9f7a7e50a51fb9db|2019-01-17--18-34-21": { - 'carFingerprint': CHRYSLER.JEEP_CHEROKEE, - 'enableCamera': True, - }, - "192a598e34926b1e|2019-04-04--13-27-39": { - 'carFingerprint': CHRYSLER.JEEP_CHEROKEE_2019, - 'enableCamera': True, - }, "f1b4c567731f4a1b|2018-04-18--11-29-37": { 'carFingerprint': FORD.FUSION, 'enableCamera': False, @@ -99,31 +79,15 @@ routes = { 'carFingerprint': GM.CADILLAC_CT6, 'enableCamera': True, }, - "265007255e794bce|2018-11-24--22-08-31": { - 'carFingerprint': GM.CADILLAC_ATS, - 'enableCamera': True, - }, "c950e28c26b5b168|2018-05-30--22-03-41": { 'carFingerprint': GM.VOLT, 'enableCamera': True, }, # TODO: use another route that has radar data at start - "aadda448b49c99ad|2018-10-25--17-16-22": { - 'carFingerprint': GM.MALIBU, - 'enableCamera': True, - }, - "49c73650e65ff465|2018-11-19--16-58-04": { - 'carFingerprint': GM.HOLDEN_ASTRA, - 'enableCamera': True, - }, "7cc2a8365b4dd8a9|2018-12-02--12-10-44": { 'carFingerprint': GM.ACADIA, 'enableCamera': True, }, - "aa20e335f61ba898|2018-12-17--21-10-37": { - 'carFingerprint': GM.BUICK_REGAL, - 'enableCamera': False, - }, "aa20e335f61ba898|2019-02-05--16-59-04": { 'carFingerprint': GM.BUICK_REGAL, 'enableCamera': True, @@ -132,13 +96,10 @@ routes = { 'carFingerprint': HONDA.CIVIC, 'enableCamera': True, }, - "c9d60e5e02c04c5c|2018-01-08--16-01-49": { - 'carFingerprint': HONDA.CRV, - 'enableCamera': True, - }, - "1851183c395ef471|2018-05-31--18-07-21": { - 'carFingerprint': HONDA.CRV_5G, + "a859a044a447c2b0|2020-03-03--18-42-45": { + 'carFingerprint': HONDA.CRV_EU, 'enableCamera': True, + 'fingerprintSource': 'fixed', }, "232585b7784c1af4|2019-04-08--14-12-14": { 'carFingerprint': HONDA.CRV_HYBRID, @@ -152,10 +113,6 @@ routes = { 'carFingerprint': HONDA.ACURA_ILX, 'enableCamera': True, }, - "21aa231dee2a68bd|2018-01-30--04-54-41": { - 'carFingerprint': HONDA.ODYSSEY, - 'enableCamera': True, - }, "81722949a62ea724|2019-03-29--15-51-26": { 'carFingerprint': HONDA.ODYSSEY_CHN, 'enableCamera': False, @@ -164,43 +121,48 @@ routes = { 'carFingerprint': HONDA.ODYSSEY_CHN, 'enableCamera': True, }, - "5a2cfe4bb362af9e|2018-02-02--23-41-07": { - 'carFingerprint': HONDA.ACURA_RDX, - 'enableCamera': True, - }, - "3e9592a1c78a3d63|2018-02-08--20-28-24": { - 'carFingerprint': HONDA.PILOT, - 'enableCamera': True, - }, - "34a84d2b765df688|2018-08-28--12-41-00": { - 'carFingerprint': HONDA.PILOT_2019, - 'enableCamera': True, - }, - "900ad17e536c3dc7|2018-04-12--22-02-36": { - 'carFingerprint': HONDA.RIDGELINE, + "08a3deb07573f157|2020-03-06--16-11-19": { + 'carFingerprint': HONDA.ACCORD_15, 'enableCamera': True, }, "f1b4c567731f4a1b|2018-06-06--14-43-46": { 'carFingerprint': HONDA.ACCORD, 'enableCamera': True, }, - "1582e1dc57175194|2018-08-15--07-46-07": { - 'carFingerprint': HONDA.ACCORD_15, + "690c4c9f9f2354c7|2018-09-15--17-36-05": { + 'carFingerprint': HONDA.ACCORDH, 'enableCamera': True, }, - # TODO: This doesnt fingerprint because the fingerprint overlaps with the Insight - # "690c4c9f9f2354c7|2018-09-15--17-36-05": { - # 'carFingerprint': HONDA.ACCORDH, - # 'enableCamera': True, - # }, - "1632088eda5e6c4d|2018-06-07--08-03-18": { + "1ad763dd22ef1a0e|2020-02-29--18-37-03": { + 'carFingerprint': HONDA.CRV_5G, + 'enableCamera': True, + }, + "0a96f86fcfe35964|2020-02-05--07-25-51": { + 'carFingerprint': HONDA.ODYSSEY, + 'enableCamera': True, + }, + "d83f36766f8012a5|2020-02-05--18-42-21": { + 'carFingerprint': HONDA.CIVIC_BOSCH_DIESEL, + 'enableCamera': True, + 'fingerprintSource': 'fixed', + }, + "fb51d190ddfd8a90|2020-02-25--14-43-43": { + 'carFingerprint': HONDA.INSIGHT, + 'enableCamera': True, + 'fingerprintSource': 'fixed', + }, + "07d37d27996096b6|2020-03-04--21-57-27": { + 'carFingerprint': HONDA.PILOT, + 'enableCamera': True, + }, + "22affd6c545d985e|2020-03-08--01-08-09": { + 'carFingerprint': HONDA.PILOT_2019, + 'enableCamera': True, + }, + "0a78dfbacc8504ef|2020-03-04--13-29-55": { 'carFingerprint': HONDA.CIVIC_BOSCH, 'enableCamera': True, }, - #"18971a99f3f2b385|2018-11-14--19-09-31": { - # 'carFingerprint': HONDA.INSIGHT, - # 'enableCamera': True, - #}, "38bfd238edecbcd7|2018-08-22--09-45-44": { 'carFingerprint': HYUNDAI.SANTA_FE, 'enableCamera': False, @@ -209,31 +171,20 @@ routes = { 'carFingerprint': HYUNDAI.SANTA_FE, 'enableCamera': True, }, - "a893a80e5c5f72c8|2019-01-14--20-02-59": { - 'carFingerprint': HYUNDAI.GENESIS, - 'enableCamera': True, - }, - "9d5fb4f0baa1b3e1|2019-01-14--17-45-59": { - 'carFingerprint': HYUNDAI.KIA_SORENTO, - 'enableCamera': True, - }, - "215cd70e9c349266|2018-11-25--22-22-12": { - 'carFingerprint': HYUNDAI.KIA_STINGER, - 'enableCamera': True, - }, - "31390e3eb6f7c29a|2019-01-23--08-56-00": { + "7653b2bce7bcfdaa|2020-03-04--15-34-32": { 'carFingerprint': HYUNDAI.KIA_OPTIMA, 'enableCamera': True, }, - "53ac3251e03f95d7|2019-01-10--13-43-32": { - 'carFingerprint': HYUNDAI.ELANTRA, - 'enableCamera': True, - }, "f7b6be73e3dfd36c|2019-05-12--18-07-16": { 'carFingerprint': TOYOTA.AVALON, 'enableCamera': False, 'enableDsu': False, }, + "6cdecc4728d4af37|2020-02-23--15-44-18": { + 'carFingerprint': TOYOTA.CAMRY, + 'enableCamera': True, + 'enableDsu': False, + }, "f7b6be73e3dfd36c|2019-05-11--22-34-20": { 'carFingerprint': TOYOTA.AVALON, 'enableCamera': True, @@ -281,6 +232,12 @@ routes = { 'enableDsu': True, 'enableGasInterceptor': True, }, + "32a7df20486b0f70|2020-02-06--16-06-50": { + 'carFingerprint': TOYOTA.RAV4H, + 'enableCamera': True, + 'enableDsu': True, + 'enableGasInterceptor': False, + }, "cdf2f7de565d40ae|2019-04-25--03-53-41": { 'carFingerprint': TOYOTA.RAV4_TSS2, 'enableCamera': True, @@ -296,6 +253,11 @@ routes = { 'enableCamera': True, 'enableDsu': False, }, + "25057fa6a5a63dfb|2020-03-04--08-44-23": { + 'carFingerprint': TOYOTA.LEXUS_CTH, + 'enableCamera': True, + 'enableDsu': True, + }, "f49e8041283f2939|2019-05-29--13-48-33": { 'carFingerprint': TOYOTA.LEXUS_ESH_TSS2, 'enableCamera': False, @@ -330,49 +292,7 @@ routes = { 'enableCamera': True, 'enableDsu': False, }, - #FIXME: This works sometimes locally, but never in CI. Timing issue? - #"b0f5a01cf604185c|2018-01-31--20-11-39": { - # 'carFingerprint': TOYOTA.LEXUS_RXH, - # 'enableCamera': False, - # 'enableDsu': False, - #}, - "8ae193ceb56a0efe|2018-06-18--20-07-32": { - 'carFingerprint': TOYOTA.RAV4H, - 'enableCamera': True, - 'enableDsu': True, - }, - "fd10b9a107bb2e49|2018-07-24--16-32-42": { - 'carFingerprint': TOYOTA.CHR, - 'enableCamera': True, - 'enableDsu': False, - }, - "fd10b9a107bb2e49|2018-07-24--20-32-08": { - 'carFingerprint': TOYOTA.CHR, - 'enableCamera': False, - 'enableDsu': False, - }, - "b4c18bf13d5955da|2018-07-29--13-39-46": { - 'carFingerprint': TOYOTA.CHRH, - 'enableCamera': True, - 'enableDsu': False, - }, - "d2d8152227f7cb82|2018-07-25--13-40-56": { - 'carFingerprint': TOYOTA.CAMRY, - 'enableCamera': True, - 'enableDsu': False, - }, - "fbd011384db5e669|2018-07-26--20-51-48": { - 'carFingerprint': TOYOTA.CAMRYH, - 'enableCamera': True, - 'enableDsu': False, - }, - # TODO: This replay has no good model/video - # "c9fa2dd0f76caf23|2018-02-10--13-40-28": { - # 'carFingerprint': TOYOTA.CAMRYH, - # 'enableCamera': False, - # 'enableDsu': False, - # }, - # TODO: missingsome combos for highlander + # TODO: missing some combos for highlander "0a302ffddbb3e3d3|2020-02-08--16-19-08": { 'carFingerprint': TOYOTA.HIGHLANDER_TSS2, 'enableCamera': True, @@ -383,21 +303,11 @@ routes = { 'enableCamera': False, 'enableDsu': False, }, - "362d23d4d5bea2fa|2018-09-02--17-03-55": { - 'carFingerprint': TOYOTA.HIGHLANDERH, - 'enableCamera': True, - 'enableDsu': True, - }, "eb6acd681135480d|2019-06-20--20-00-00": { 'carFingerprint': TOYOTA.SIENNA, 'enableCamera': True, 'enableDsu': False, }, - "362d23d4d5bea2fa|2018-08-10--13-31-40": { - 'carFingerprint': TOYOTA.HIGHLANDERH, - 'enableCamera': False, - 'enableDsu': False, - }, "2e07163a1ba9a780|2019-08-25--13-15-13": { 'carFingerprint': TOYOTA.LEXUS_IS, 'enableCamera': True, @@ -421,66 +331,45 @@ routes = { 'carFingerprint': SUBARU.IMPREZA, 'enableCamera': True, }, - # Tesla route, should result in mock car - "07cb8a788c31f645|2018-06-17--18-50-29": { - 'carFingerprint': MOCK.MOCK, + "fbbfa6af821552b9|2020-03-03--08-09-43": { + 'carFingerprint': NISSAN.XTRAIL, + 'enableCamera': True, }, - ## Route with no can data, should result in mock car. This is not supported anymore - #"bfa17080b080f3ec|2018-06-28--23-27-47": { - # 'carFingerprint': MOCK.MOCK, - #}, } passive_routes = [ - "07cb8a788c31f645|2018-06-17--18-50-29", - #"bfa17080b080f3ec|2018-06-28--23-27-47", ] forced_dashcam_routes = [ # Ford fusion "f1b4c567731f4a1b|2018-04-18--11-29-37", "f1b4c567731f4a1b|2018-04-30--10-15-35", -] -# TODO: replace all these with public routes -non_public_routes = [ - "0607d2516fc2148f|2019-02-13--23-03-16", # CHRYSLER PACIFICA HYBRID 2019 - "3e9592a1c78a3d63|2018-02-08--20-28-24", # HONDA PILOT 2017 TOURING - "aa20e335f61ba898|2019-02-05--16-59-04", # BUICK REGAL ESSENCE 2018 - "1851183c395ef471|2018-05-31--18-07-21", # HONDA CR-V 2017 EX - "9d5fb4f0baa1b3e1|2019-01-14--17-45-59", # KIA SORENTO GT LINE 2018 - "b4c18bf13d5955da|2018-07-29--13-39-46", # TOYOTA C-HR HYBRID 2018 - "5a2cfe4bb362af9e|2018-02-02--23-41-07", # ACURA RDX 2018 ACURAWATCH PLUS - "362d23d4d5bea2fa|2018-08-10--13-31-40", # TOYOTA HIGHLANDER HYBRID 2018 - "aa20e335f61ba898|2018-12-17--21-10-37", # BUICK REGAL ESSENCE 2018 - "215cd70e9c349266|2018-11-25--22-22-12", # KIA STINGER GT2 2018 - "192a598e34926b1e|2019-04-04--13-27-39", # JEEP GRAND CHEROKEE 2019 - "34a84d2b765df688|2018-08-28--12-41-00", # HONDA PILOT 2019 ELITE - "b0c9d2329ad1606b|2019-01-06--10-11-23", # CHRYSLER PACIFICA HYBRID 2017 - "31390e3eb6f7c29a|2019-01-23--08-56-00", # KIA OPTIMA SX 2019 - "fd10b9a107bb2e49|2018-07-24--16-32-42", # TOYOTA C-HR 2018 - "9f7a7e50a51fb9db|2019-01-17--18-34-21", # JEEP GRAND CHEROKEE V6 2018 - "aadda448b49c99ad|2018-10-25--17-16-22", # CHEVROLET MALIBU PREMIER 2017 - "362d23d4d5bea2fa|2018-09-02--17-03-55", # TOYOTA HIGHLANDER HYBRID 2018 - "1582e1dc57175194|2018-08-15--07-46-07", # HONDA ACCORD 2018 LX 1.5T - "fd10b9a107bb2e49|2018-07-24--20-32-08", # TOYOTA C-HR 2018 - "265007255e794bce|2018-11-24--22-08-31", # CADILLAC ATS Premium Performance 2018 - "53ac3251e03f95d7|2019-01-10--13-43-32", # HYUNDAI ELANTRA LIMITED ULTIMATE 2017 - "21aa231dee2a68bd|2018-01-30--04-54-41", # HONDA ODYSSEY 2018 EX-L - "900ad17e536c3dc7|2018-04-12--22-02-36", # HONDA RIDGELINE 2017 BLACK EDITION - "975b26878285314d|2018-12-25--14-42-13", # CHRYSLER PACIFICA HYBRID 2018 - "8ae193ceb56a0efe|2018-06-18--20-07-32", # TOYOTA RAV4 HYBRID 2017 - "a893a80e5c5f72c8|2019-01-14--20-02-59", # HYUNDAI GENESIS 2018 - "49c73650e65ff465|2018-11-19--16-58-04", # HOLDEN ASTRA RS-V BK 2017 - "d2d8152227f7cb82|2018-07-25--13-40-56", # TOYOTA CAMRY 2018 - "07cb8a788c31f645|2018-06-17--18-50-29", # mock - "c9d60e5e02c04c5c|2018-01-08--16-01-49", # HONDA CR-V 2016 TOURING - "1632088eda5e6c4d|2018-06-07--08-03-18", # HONDA CIVIC HATCHBACK 2017 SEDAN/COUPE 2019 - "fbd011384db5e669|2018-07-26--20-51-48", # TOYOTA CAMRY HYBRID 2018 + # Nissan + "fbbfa6af821552b9|2020-03-03--08-09-43", ] # TODO: add routes for these cars -non_tested_cars = [TOYOTA.LEXUS_CTH, CHRYSLER.PACIFICA_2018, HONDA.ACCORDH] +non_tested_cars = [ + CHRYSLER.JEEP_CHEROKEE, + CHRYSLER.JEEP_CHEROKEE_2019, + CHRYSLER.PACIFICA_2018, + CHRYSLER.PACIFICA_2018_HYBRID, + GM.CADILLAC_ATS, + GM.HOLDEN_ASTRA, + GM.MALIBU, + HONDA.ACURA_RDX, + HONDA.CRV, + HONDA.RIDGELINE, + HYUNDAI.ELANTRA, + HYUNDAI.GENESIS, + HYUNDAI.KIA_SORENTO, + HYUNDAI.KIA_STINGER, + TOYOTA.CAMRYH, + TOYOTA.CHR, + TOYOTA.CHRH, + TOYOTA.HIGHLANDERH, +] if __name__ == "__main__": @@ -503,12 +392,9 @@ if __name__ == "__main__": results = {} for route, checks in routes.items(): - if route not in non_public_routes: - print("GETTING ROUTE LOGS") - get_route_log(route) - print("DONE GETTING ROUTE LOGS") - elif "UNLOGGER_PATH" not in os.environ: - continue + print("GETTING ROUTE LOGS") + get_route_log(route) + print("DONE GETTING ROUTE LOGS") params = Params() params.clear_all() @@ -529,10 +415,7 @@ if __name__ == "__main__": # Start unlogger print("Start unlogger") - if route in non_public_routes: - unlogger_cmd = [os.path.join(BASEDIR, os.environ['UNLOGGER_PATH']), route] - else: - unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), route, '/tmp'] + unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), route, '/tmp'] unlogger = subprocess.Popen(unlogger_cmd + ['--disable', 'frame,encodeIdx,plan,pathPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl,carEvents,carParams', '--no-interactive'], preexec_fn=os.setsid) print("Check sockets") diff --git a/selfdrive/test/test_fingerprints.py b/selfdrive/test/test_fingerprints.py index aeb75896..b0c4d451 100755 --- a/selfdrive/test/test_fingerprints.py +++ b/selfdrive/test/test_fingerprints.py @@ -2,6 +2,7 @@ import os import sys from common.basedir import BASEDIR +from selfdrive.car.fingerprints import IGNORED_FINGERPRINTS # messages reserved for CAN based ignition (see can_ignition_hook function in panda/board/drivers/can) # (addr, len) @@ -65,6 +66,9 @@ car_names = [] brand_names = [] for brand in fingerprints: for car in fingerprints[brand]: + if car in IGNORED_FINGERPRINTS: + continue + fingerprints_flat += fingerprints[brand][car] for i in range(len(fingerprints[brand][car])): car_names.append(car) @@ -77,10 +81,6 @@ valid = True for idx1, f1 in enumerate(fingerprints_flat): for idx2, f2 in enumerate(fingerprints_flat): if idx1 < idx2 and not check_fingerprint_consistency(f1, f2): - if car_names[idx1] == car_names[idx2]: - print(f"Warning, overlap in {car_names[idx1]}") - continue - valid = False print("Those two fingerprints are inconsistent {0} {1}".format(car_names[idx1], car_names[idx2])) print("") diff --git a/selfdrive/test/test_openpilot.py b/selfdrive/test/test_openpilot.py index e5ea3273..3e9a3175 100644 --- a/selfdrive/test/test_openpilot.py +++ b/selfdrive/test/test_openpilot.py @@ -2,7 +2,7 @@ import os os.environ['FAKEUPLOAD'] = "1" -from common.apk import update_apks, start_frame, pm_apply_packages, android_packages +from common.apk import update_apks, start_offroad, pm_apply_packages, android_packages from common.params import Params from common.testing import phone_only from selfdrive.manager import manager_init, manager_prepare @@ -57,7 +57,7 @@ def with_apks(): update_apks() pm_apply_packages('enable') - start_frame() + start_offroad() func() diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py new file mode 100644 index 00000000..e03ef698 --- /dev/null +++ b/selfdrive/thermald/power_monitoring.py @@ -0,0 +1,153 @@ +import datetime +import random +import threading +import time +from statistics import mean + +from cereal import log +from selfdrive.swaglog import cloudlog + +PANDA_OUTPUT_VOLTAGE = 5.28 + + +# Parameters +def get_battery_capacity(): + return _read_param("/sys/class/power_supply/battery/capacity", int) + + +def get_battery_status(): + # This does not correspond with actual charging or not. + # If a USB cable is plugged in, it responds with 'Charging', even when charging is disabled + return _read_param("/sys/class/power_supply/battery/status", lambda x: x.strip(), '') + + +def get_battery_current(): + return _read_param("/sys/class/power_supply/battery/current_now", int) + + +def get_battery_voltage(): + return _read_param("/sys/class/power_supply/battery/voltage_now", int) + + +def get_usb_present(): + return _read_param("/sys/class/power_supply/usb/present", lambda x: bool(int(x)), False) + + +def get_battery_charging(): + # This does correspond with actually charging + return _read_param("/sys/class/power_supply/battery/charge_type", lambda x: x.strip() != "N/A", False) + + +def set_battery_charging(on): + with open('/sys/class/power_supply/battery/charging_enabled', 'w') as f: + f.write(f"{1 if on else 0}\n") + + +# Helpers +def _read_param(path, parser, default=0): + try: + with open(path) as f: + return parser(f.read()) + except Exception: + return default + + +def panda_current_to_actual_current(panda_current): + # From white/grey panda schematic + return (3.3 - (panda_current * 3.3 / 4096)) / 8.25 + + +class PowerMonitoring: + def __init__(self): + self.last_measurement_time = None # Used for integration delta + self.power_used_uWh = 0 # Integrated power usage in uWh since going into offroad + self.next_pulsed_measurement_time = None + self.integration_lock = threading.Lock() + + # Calculation tick + def calculate(self, health): + try: + now = time.time() + + # Check that time is valid + if datetime.datetime.fromtimestamp(now).year < 2019: + return + + # Only integrate when there is no ignition + # If health is None, we're probably not in a car, so we don't care + if health is None or (health.health.ignitionLine or health.health.ignitionCan): + self.last_measurement_time = None + self.power_used_uWh = 0 + return + + # First measurement, set integration time + if self.last_measurement_time is None: + self.last_measurement_time = now + return + + # Get current power draw somehow + current_power = 0 + if get_battery_status() == 'Discharging': + # If the battery is discharging, we can use this measurement + # On C2: this is low by about 10-15%, probably mostly due to UNO draw not being factored in + current_power = ((get_battery_voltage() / 1000000) * (get_battery_current() / 1000000)) + elif (health.health.hwType in [log.HealthData.HwType.whitePanda, log.HealthData.HwType.greyPanda]) and (health.health.current > 1): + # If white/grey panda, use the integrated current measurements if the measurement is not 0 + # If the measurement is 0, the current is 400mA or greater, and out of the measurement range of the panda + # This seems to be accurate to about 5% + current_power = (PANDA_OUTPUT_VOLTAGE * panda_current_to_actual_current(health.health.current)) + elif (self.next_pulsed_measurement_time is not None) and (self.next_pulsed_measurement_time <= now): + # TODO: Figure out why this is off by a factor of 3/4??? + FUDGE_FACTOR = 1.33 + + # Turn off charging for about 10 sec in a thread that does not get killed on SIGINT, and perform measurement here to avoid blocking thermal + def perform_pulse_measurement(now): + try: + set_battery_charging(False) + time.sleep(5) + + # Measure for a few sec to get a good average + voltages = [] + currents = [] + for i in range(6): + voltages.append(get_battery_voltage()) + currents.append(get_battery_current()) + time.sleep(1) + current_power = ((mean(voltages) / 1000000) * (mean(currents) / 1000000)) + self._perform_integration(now, current_power * FUDGE_FACTOR) + + # Enable charging again + set_battery_charging(True) + except Exception: + cloudlog.exception("Pulsed power measurement failed") + + # Start pulsed measurement and return + threading.Thread(target=perform_pulse_measurement, args=(now,)).start() + self.next_pulsed_measurement_time = None + return + + elif self.next_pulsed_measurement_time is None: + # On a charging EON with black panda, or drawing more than 400mA out of a white/grey one + # Only way to get the power draw is to turn off charging for a few sec and check what the discharging rate is + # We shouldn't do this very often, so make sure it has been some long-ish random time interval + self.next_pulsed_measurement_time = now + random.randint(120, 180) + return + else: + # Do nothing + return + + # Do the integration + self._perform_integration(now, current_power) + except Exception: + cloudlog.exception("Power monitoring calculation failed:") + + def _perform_integration(self, t, current_power): + self.integration_lock.acquire() + integration_time_h = (t - self.last_measurement_time) / 3600 + self.power_used_uWh += (current_power * 1000000) * integration_time_h + self.last_measurement_time = t + self.integration_lock.release() + + # Get the power usage + def get_power_used(self): + return int(self.power_used_uWh) diff --git a/selfdrive/thermald.py b/selfdrive/thermald/thermald.py similarity index 90% rename from selfdrive/thermald.py rename to selfdrive/thermald/thermald.py index c516acc7..8870ffcc 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -4,10 +4,9 @@ import json import copy import datetime import psutil -import subprocess from smbus2 import SMBus from cereal import log -from common.android import ANDROID, get_network_type +from common.android import ANDROID, get_network_type, get_network_strength from common.basedir import BASEDIR from common.params import Params from common.realtime import sec_since_boot, DT_TRML @@ -18,20 +17,29 @@ from selfdrive.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.loggerd.config import get_available_percent from selfdrive.pandad import get_expected_signature +from selfdrive.thermald.power_monitoring import PowerMonitoring, get_battery_capacity, get_battery_status, get_battery_current, get_battery_voltage, get_usb_present FW_SIGNATURE = get_expected_signature() ThermalStatus = log.ThermalData.ThermalStatus NetworkType = log.ThermalData.NetworkType +NetworkStrength = log.ThermalData.NetworkStrength CURRENT_TAU = 15. # 15s time constant DAYS_NO_CONNECTIVITY_MAX = 7 # do not allow to engage after a week without internet DAYS_NO_CONNECTIVITY_PROMPT = 4 # send an offroad prompt after 4 days with no internet +LEON = False +last_eon_fan_val = None + with open(BASEDIR + "/selfdrive/controls/lib/alerts_offroad.json") as json_file: OFFROAD_ALERTS = json.load(json_file) + def read_tz(x, clip=True): + if not ANDROID: + # we don't monitor thermal on PC + return 0 try: with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f: ret = int(f.read()) @@ -42,9 +50,9 @@ def read_tz(x, clip=True): return ret + def read_thermal(): - dat = messaging.new_message() - dat.init('thermal') + dat = messaging.new_message('thermal') dat.thermal.cpu0 = read_tz(5) dat.thermal.cpu1 = read_tz(7) dat.thermal.cpu2 = read_tz(10) @@ -55,7 +63,7 @@ def read_thermal(): dat.thermal.pa0 = read_tz(25) return dat -LEON = False + def setup_eon_fan(): global LEON @@ -69,11 +77,10 @@ def setup_eon_fan(): bus.write_byte_data(0x21, 0x04, 0x4) # manual override source except IOError: print("LEON detected") - #os.system("echo 1 > /sys/devices/soc/6a00000.ssusb/power_supply/usb/usb_otg") LEON = True bus.close() -last_eon_fan_val = None + def set_eon_fan(val): global LEON, last_eon_fan_val @@ -99,6 +106,7 @@ def set_eon_fan(val): bus.close() last_eon_fan_val = val + # temp thresholds to control fan speed - high hysteresis _TEMP_THRS_H = [50., 65., 80., 10000] # temp thresholds to control fan speed - low hysteresis @@ -124,7 +132,7 @@ def handle_fan_eon(max_cpu_temp, bat_temp, fan_speed, ignition): # no max fan speed unless battery is hot fan_speed = min(fan_speed, _FAN_SPEEDS[-2]) - set_eon_fan(fan_speed//16384) + set_eon_fan(fan_speed // 16384) return fan_speed @@ -137,6 +145,7 @@ def handle_fan_uno(max_cpu_temp, bat_temp, fan_speed, ignition): return new_speed + def thermald_thread(): # prevent LEECO from undervoltage BATT_PERC_OFF = 10 if LEON else 3 @@ -161,6 +170,7 @@ def thermald_thread(): usb_power_prev = True network_type = NetworkType.none + network_strength = NetworkStrength.unknown current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML) health_prev = None @@ -177,6 +187,7 @@ def thermald_thread(): handle_fan = handle_fan_eon params = Params() + pm = PowerMonitoring() while 1: health = messaging.recv_sock(health_sock, wait=True) @@ -196,27 +207,20 @@ def thermald_thread(): if (count % int(10. / DT_TRML)) == 0: try: network_type = get_network_type() - except subprocess.CalledProcessError: - pass + network_strength = get_network_strength(network_type) + except Exception: + cloudlog.exception("Error getting network status") msg.thermal.freeSpace = get_available_percent(default=100.0) / 100.0 msg.thermal.memUsedPercent = int(round(psutil.virtual_memory().percent)) msg.thermal.cpuPerc = int(round(psutil.cpu_percent())) msg.thermal.networkType = network_type - - try: - with open("/sys/class/power_supply/battery/capacity") as f: - msg.thermal.batteryPercent = int(f.read()) - with open("/sys/class/power_supply/battery/status") as f: - msg.thermal.batteryStatus = f.read().strip() - with open("/sys/class/power_supply/battery/current_now") as f: - msg.thermal.batteryCurrent = int(f.read()) - with open("/sys/class/power_supply/battery/voltage_now") as f: - msg.thermal.batteryVoltage = int(f.read()) - with open("/sys/class/power_supply/usb/present") as f: - msg.thermal.usbOnline = bool(int(f.read())) - except FileNotFoundError: - pass + msg.thermal.networkStrength = network_strength + msg.thermal.batteryPercent = get_battery_capacity() + msg.thermal.batteryStatus = get_battery_status() + msg.thermal.batteryCurrent = get_battery_current() + msg.thermal.batteryVoltage = get_battery_voltage() + msg.thermal.usbOnline = get_usb_present() # Fake battery levels on uno for frame if is_uno: @@ -229,7 +233,7 @@ def thermald_thread(): max_cpu_temp = max(msg.thermal.cpu0, msg.thermal.cpu1, msg.thermal.cpu2, msg.thermal.cpu3) / 10.0 max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10., msg.thermal.gpu / 10.) - bat_temp = msg.thermal.bat/1000. + bat_temp = msg.thermal.bat / 1000. fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, ignition) msg.thermal.fanSpeed = fan_speed @@ -238,7 +242,7 @@ def thermald_thread(): if max_cpu_temp > 107. or bat_temp >= 63.: # onroad not allowed thermal_status = ThermalStatus.danger - elif max_comp_temp > 92.5 or bat_temp > 60.: # CPU throttling starts around ~90C + elif max_comp_temp > 92.5 or bat_temp > 60.: # CPU throttling starts around ~90C # hysteresis between onroad not allowed and engage not allowed thermal_status = clip(thermal_status, ThermalStatus.red, ThermalStatus.danger) elif max_cpu_temp > 87.5: @@ -363,6 +367,10 @@ def thermald_thread(): started_seen and (sec_since_boot() - off_ts) > 60: os.system('LD_LIBRARY_PATH="" svc power shutdown') + # Offroad power monitoring + pm.calculate(health) + msg.thermal.offroadPowerUsage = pm.get_power_used() + msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged msg.thermal.started = started_ts is not None msg.thermal.startedTs = int(1e9*(started_ts or 0)) @@ -380,21 +388,20 @@ def thermald_thread(): fw_version_match_prev = fw_version_match should_start_prev = should_start - #print(msg) - # report to server once per minute if (count % int(60. / DT_TRML)) == 0: cloudlog.event("STATUS_PACKET", - count=count, - health=(health.to_dict() if health else None), - location=(location.to_dict() if location else None), - thermal=msg.to_dict()) + count=count, + health=(health.to_dict() if health else None), + location=(location.to_dict() if location else None), + thermal=msg.to_dict()) count += 1 -def main(gctx=None): +def main(): thermald_thread() + if __name__ == "__main__": main() diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py index 292ec040..55a09233 100644 --- a/selfdrive/tombstoned.py +++ b/selfdrive/tombstoned.py @@ -101,7 +101,7 @@ def report_tombstone(fn, client): cloudlog.error({'tombstone': message}) -def main(gctx=None): +def main(): initial_tombstones = set(get_tombstones()) client = Client('https://d3b175702f62402c91ade04d1c547e68:b20d68c813c74f63a7cdf9c4039d8f56@sentry.io/157615', diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript index 5dbf8016..e0c353b4 100644 --- a/selfdrive/ui/SConscript +++ b/selfdrive/ui/SConscript @@ -1,7 +1,7 @@ Import('env', 'arch', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal') -src = ['ui.cc', 'paint.cc', '#phonelibs/nanovg/nanovg.c'] -libs = [common, 'zmq', 'czmq', 'capnp', 'capnp_c', 'm', cereal, 'json', messaging, gpucommon, visionipc] +src = ['ui.cc', 'paint.cc', 'sidebar.cc', '#phonelibs/nanovg/nanovg.c'] +libs = [common, 'zmq', 'czmq', 'capnp', 'capnp_c', 'm', cereal, messaging, gpucommon, visionipc] if arch == "aarch64": src += ['sound.cc', 'slplay.c'] @@ -11,7 +11,7 @@ else: src += ['linux.cc'] libs += ['pthread', 'glfw'] linkflags = [] - + env.Program('_ui', src, LINKFLAGS=linkflags, LIBS=libs) diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc index 6d7b24c3..df3e9d8d 100644 --- a/selfdrive/ui/paint.cc +++ b/selfdrive/ui/paint.cc @@ -108,9 +108,23 @@ static void draw_chevron(UIState *s, float x_in, float y_in, float sz, nvgRestore(s->vg); } -static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) { - const UIScene *scene = &s->scene; +static void draw_lead(UIState *s, float d_rel, float v_rel, float y_rel){ + // Draw lead car indicator + float fillAlpha = 0; + float speedBuff = 10.; + float leadBuff = 40.; + if (d_rel < leadBuff) { + fillAlpha = 255*(1.0-(d_rel/leadBuff)); + if (v_rel < 0) { + fillAlpha += 255*(-1*(v_rel/speedBuff)); + } + fillAlpha = (int)(fmin(fillAlpha, 255)); + } + draw_chevron(s, d_rel, y_rel, 25, + nvgRGBA(201, 34, 49, fillAlpha), nvgRGBA(218, 202, 37, 255)); +} +static void ui_draw_lane_line(UIState *s, const model_path_vertices_data *pvd, NVGcolor color) { nvgSave(s->vg); nvgTranslate(s->vg, 240.0f, 0.0); // rgb-box space nvgTranslate(s->vg, -1440.0f / 2, -1080.0f / 2); // zoom 2x @@ -204,7 +218,7 @@ static void update_all_track_data(UIState *s) { static void ui_draw_track(UIState *s, bool is_mpc, track_vertices_data *pvd) { -const UIScene *scene = &s->scene; + const UIScene *scene = &s->scene; const PathData path = scene->model.path; const float *mpc_x_coords = &scene->mpc_x[0]; const float *mpc_y_coords = &scene->mpc_y[0]; @@ -390,19 +404,10 @@ static void ui_draw_world(UIState *s) { ui_draw_vision_lanes(s); if (scene->lead_status) { - // Draw lead car indicator - float fillAlpha = 0; - float speedBuff = 10.; - float leadBuff = 40.; - if (scene->lead_d_rel < leadBuff) { - fillAlpha = 255*(1.0-(scene->lead_d_rel/leadBuff)); - if (scene->lead_v_rel < 0) { - fillAlpha += 255*(-1*(scene->lead_v_rel/speedBuff)); - } - fillAlpha = (int)(fmin(fillAlpha, 255)); - } - draw_chevron(s, scene->lead_d_rel+2.7, scene->lead_y_rel, 25, - nvgRGBA(201, 34, 49, fillAlpha), nvgRGBA(218, 202, 37, 255)); + draw_lead(s, scene->lead_d_rel, scene->lead_v_rel, scene->lead_y_rel); + } + if ((scene->lead_status2) && (fabs(scene->lead_d_rel - scene->lead_d_rel2) > 3.0)) { + draw_lead(s, scene->lead_d_rel2, scene->lead_v_rel2, scene->lead_y_rel2); } } @@ -873,9 +878,13 @@ static void ui_draw_blank(UIState *s) { void ui_draw(UIState *s) { if (s->vision_connected && s->active_app == cereal_UiLayoutState_App_home && s->status != STATUS_STOPPED) { + ui_draw_sidebar(s); ui_draw_vision(s); } else { ui_draw_blank(s); + if (!s->scene.uilayout_sidebarcollapsed) { + ui_draw_sidebar(s); + } } { @@ -980,6 +989,25 @@ void ui_nvg_init(UIState *s) { assert(s->img_map >= 0); s->img_map = nvgCreateImage(s->vg, "../assets/img_map.png", 1); + assert(s->img_button_settings >= 0); + s->img_button_settings = nvgCreateImage(s->vg, "../assets/images/button_settings.png", 1); + + assert(s->img_button_home >= 0); + s->img_button_home = nvgCreateImage(s->vg, "../assets/images/button_home.png", 1); + + assert(s->img_battery >= 0); + s->img_battery = nvgCreateImage(s->vg, "../assets/images/battery.png", 1); + + assert(s->img_battery_charging >= 0); + s->img_battery_charging = nvgCreateImage(s->vg, "../assets/images/battery_charging.png", 1); + + for(int i=0;i<=5;++i) { + assert(s->img_network[i] >= 0); + char network_asset[32]; + snprintf(network_asset, sizeof(network_asset), "../assets/images/network_%d.png", i); + s->img_network[i] = nvgCreateImage(s->vg, network_asset, 1); + } + // init gl s->frame_program = load_program(frame_vertex_shader, frame_fragment_shader); assert(s->frame_program); diff --git a/selfdrive/ui/sidebar.cc b/selfdrive/ui/sidebar.cc new file mode 100644 index 00000000..717449de --- /dev/null +++ b/selfdrive/ui/sidebar.cc @@ -0,0 +1,234 @@ +#include +#include +#include +#include "ui.hpp" + +static void ui_draw_sidebar_background(UIState *s, bool hasSidebar) { + int sbr_x = hasSidebar ? 0 : -(sbr_w) + bdr_s * 2; + + nvgBeginPath(s->vg); + nvgRect(s->vg, sbr_x, 0, sbr_w, vwp_h); + nvgFillColor(s->vg, COLOR_BLACK_ALPHA); + nvgFill(s->vg); +} + +static void ui_draw_sidebar_settings_button(UIState *s, bool hasSidebar) { + bool settingsActive = s->active_app == cereal_UiLayoutState_App_settings; + const int settings_btn_xr = hasSidebar ? settings_btn_x : -(sbr_w); + + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, settings_btn_xr, settings_btn_y, + settings_btn_w, settings_btn_h, 0, s->img_button_settings, settingsActive ? 1.0f : 0.65f); + nvgRect(s->vg, settings_btn_xr, settings_btn_y, settings_btn_w, settings_btn_h); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); +} + +static void ui_draw_sidebar_home_button(UIState *s, bool hasSidebar) { + bool homeActive = s->active_app == cereal_UiLayoutState_App_home; + const int home_btn_xr = hasSidebar ? home_btn_x : -(sbr_w); + + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, home_btn_xr, home_btn_y, + home_btn_w, home_btn_h, 0, s->img_button_home, homeActive ? 1.0f : 0.65f); + nvgRect(s->vg, home_btn_xr, home_btn_y, home_btn_w, home_btn_h); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); +} + +static void ui_draw_sidebar_network_strength(UIState *s, bool hasSidebar) { + const int network_img_h = 27; + const int network_img_w = 176; + const int network_img_x = hasSidebar ? 58 : -(sbr_w); + const int network_img_y = 196; + const int network_img = s->scene.networkType == cereal_ThermalData_NetworkType_none ? + s->img_network[0] : s->img_network[s->scene.networkStrength + 1]; + + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, network_img_x, network_img_y, + network_img_w, network_img_h, 0, network_img, 1.0f); + nvgRect(s->vg, network_img_x, network_img_y, network_img_w, network_img_h); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); +} + +static void ui_draw_sidebar_battery_icon(UIState *s, bool hasSidebar) { + const int battery_img_h = 36; + const int battery_img_w = 76; + const int battery_img_x = hasSidebar ? 160 : -(sbr_w); + const int battery_img_y = 255; + + int battery_img = strcmp(s->scene.batteryStatus, "Charging") == 0 ? + s->img_battery_charging : s->img_battery; + + nvgBeginPath(s->vg); + nvgRect(s->vg, battery_img_x + 6, battery_img_y + 5, + ((battery_img_w - 19) * (s->scene.batteryPercent * 0.01)), battery_img_h - 11); + nvgFillColor(s->vg, COLOR_WHITE); + nvgFill(s->vg); + + nvgBeginPath(s->vg); + NVGpaint imgPaint = nvgImagePattern(s->vg, battery_img_x, battery_img_y, + battery_img_w, battery_img_h, 0, battery_img, 1.0f); + nvgRect(s->vg, battery_img_x, battery_img_y, battery_img_w, battery_img_h); + nvgFillPaint(s->vg, imgPaint); + nvgFill(s->vg); +} + +static void ui_draw_sidebar_network_type(UIState *s, bool hasSidebar) { + const int network_x = hasSidebar ? 50 : -(sbr_w); + const int network_y = 273; + const int network_w = 100; + const int network_h = 100; + const char *network_types[6] = {"--", "WiFi", "2G", "3G", "4G", "5G"}; + char network_type_str[32]; + + if (s->scene.networkType <= 5) { + snprintf(network_type_str, sizeof(network_type_str), "%s", network_types[s->scene.networkType]); + } + + nvgFillColor(s->vg, COLOR_WHITE); + nvgFontSize(s->vg, 48); + nvgFontFace(s->vg, "sans-regular"); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, network_x, network_y, network_w, network_type_str, NULL); +} + +static void ui_draw_sidebar_metric(UIState *s, const char* label_str, const char* value_str, const int severity, const int y_offset, const char* message_str, bool hasSidebar) { + const int metric_x = hasSidebar ? 30 : -(sbr_w); + const int metric_y = 338 + y_offset; + const int metric_w = 240; + const int metric_h = message_str ? strlen(message_str) > 8 ? 124 : 100 : 148; + NVGcolor status_color; + + if (severity == 0) { + status_color = COLOR_WHITE; + } else if (severity == 1) { + status_color = COLOR_YELLOW; + } else if (severity > 1) { + status_color = COLOR_RED; + } + + nvgBeginPath(s->vg); + nvgRoundedRect(s->vg, metric_x, metric_y, metric_w, metric_h, 20); + nvgStrokeColor(s->vg, severity > 0 ? COLOR_WHITE : COLOR_WHITE_ALPHA); + nvgStrokeWidth(s->vg, 2); + nvgStroke(s->vg); + + nvgBeginPath(s->vg); + nvgRoundedRectVarying(s->vg, metric_x + 6, metric_y + 6, 18, metric_h - 12, 25, 0, 0, 25); + nvgFillColor(s->vg, status_color); + nvgFill(s->vg); + + if (!message_str) { + nvgFillColor(s->vg, COLOR_WHITE); + nvgFontSize(s->vg, 78); + nvgFontFace(s->vg, "sans-bold"); + nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, metric_x + 50, metric_y + 50, metric_w - 60, value_str, NULL); + + nvgFillColor(s->vg, COLOR_WHITE); + nvgFontSize(s->vg, 48); + nvgFontFace(s->vg, "sans-regular"); + nvgTextAlign(s->vg, NVG_ALIGN_LEFT | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, metric_x + 50, metric_y + 50 + 66, metric_w - 60, label_str, NULL); + } else { + nvgFillColor(s->vg, COLOR_WHITE); + nvgFontSize(s->vg, 48); + nvgFontFace(s->vg, "sans-bold"); + nvgTextAlign(s->vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE); + nvgTextBox(s->vg, metric_x + 35, metric_y + (strlen(message_str) > 8 ? 40 : 50), metric_w - 50, message_str, NULL); + } +} + +static void ui_draw_sidebar_storage_metric(UIState *s, bool hasSidebar) { + int storage_severity; + char storage_label_str[32]; + char storage_value_str[32]; + char storage_value_unit[32]; + const int storage_y_offset = 0; + const float storage_pct = ceilf((1.0 - s->scene.freeSpace) * 100); + + if (storage_pct < 75.0) { + storage_severity = 0; + } else if (storage_pct >= 75.0 && storage_pct < 87.0) { + storage_severity = 1; + } else if (storage_pct >= 87.0) { + storage_severity = 2; + } + + snprintf(storage_value_str, sizeof(storage_value_str), "%d", (int)storage_pct); + snprintf(storage_value_unit, sizeof(storage_value_unit), "%s", "%"); + snprintf(storage_label_str, sizeof(storage_label_str), "%s", "STORAGE"); + strcat(storage_value_str, storage_value_unit); + + ui_draw_sidebar_metric(s, storage_label_str, storage_value_str, storage_severity, storage_y_offset, NULL, hasSidebar); +} + +static void ui_draw_sidebar_temp_metric(UIState *s, bool hasSidebar) { + int temp_severity; + char temp_label_str[32]; + char temp_value_str[32]; + char temp_value_unit[32]; + const int temp_y_offset = 148 + 32; + + if (s->scene.thermalStatus == cereal_ThermalData_ThermalStatus_green) { + temp_severity = 0; + } else if (s->scene.thermalStatus == cereal_ThermalData_ThermalStatus_yellow) { + temp_severity = 1; + } else if (s->scene.thermalStatus == cereal_ThermalData_ThermalStatus_red) { + temp_severity = 2; + } else if (s->scene.thermalStatus == cereal_ThermalData_ThermalStatus_danger) { + temp_severity = 3; + } + + snprintf(temp_value_str, sizeof(temp_value_str), "%d", s->scene.paTemp); + snprintf(temp_value_unit, sizeof(temp_value_unit), "%s", "°C"); + snprintf(temp_label_str, sizeof(temp_label_str), "%s", "TEMP"); + strcat(temp_value_str, temp_value_unit); + + ui_draw_sidebar_metric(s, temp_label_str, temp_value_str, temp_severity, temp_y_offset, NULL, hasSidebar); +} + +static void ui_draw_sidebar_panda_metric(UIState *s, bool hasSidebar) { + int panda_severity; + char panda_message_str[32]; + const int panda_y_offset = (148 + 32) * 2; + + if (s->scene.hwType == cereal_HealthData_HwType_unknown) { + panda_severity = 2; + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "NO PANDA"); + } else if (s->scene.hwType == cereal_HealthData_HwType_whitePanda) { + panda_severity = 0; + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA ACTIVE"); + } else if ( + (s->scene.hwType == cereal_HealthData_HwType_greyPanda) || + (s->scene.hwType == cereal_HealthData_HwType_blackPanda) || + (s->scene.hwType == cereal_HealthData_HwType_uno)) { + if (s->scene.satelliteCount == -1) { + panda_severity = 0; + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA ACTIVE"); + } else if (s->scene.satelliteCount < 6) { + panda_severity = 1; + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA\nNO GPS"); + } else if (s->scene.satelliteCount >= 6) { + panda_severity = 0; + snprintf(panda_message_str, sizeof(panda_message_str), "%s", "PANDA GOOD GPS"); + } + } + + ui_draw_sidebar_metric(s, NULL, NULL, panda_severity, panda_y_offset, panda_message_str, hasSidebar); +} + +void ui_draw_sidebar(UIState *s) { + bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; + ui_draw_sidebar_background(s, hasSidebar); + ui_draw_sidebar_settings_button(s, hasSidebar); + ui_draw_sidebar_home_button(s, hasSidebar); + ui_draw_sidebar_network_strength(s, hasSidebar); + ui_draw_sidebar_battery_icon(s, hasSidebar); + ui_draw_sidebar_network_type(s, hasSidebar); + ui_draw_sidebar_storage_metric(s, hasSidebar); + ui_draw_sidebar_temp_metric(s, hasSidebar); + ui_draw_sidebar_panda_metric(s, hasSidebar); +} diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index db28e373..a87ca300 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -6,7 +6,6 @@ #include #include -#include #include #include "common/util.h" @@ -58,6 +57,45 @@ static void set_awake(UIState *s, bool awake) { #endif } +static void navigate_to_settings(UIState *s) { +#ifdef QCOM + system("am broadcast -a 'ai.comma.plus.SidebarSettingsTouchUpInside'"); +#else + // computer UI doesn't have offroad settings +#endif +} + +static void navigate_to_home(UIState *s) { +#ifdef QCOM + system("am broadcast -a 'ai.comma.plus.HomeButtonTouchUpInside'"); +#else + // computer UI doesn't have offroad home +#endif +} + +static void handle_sidebar_touch(UIState *s, int touch_x, int touch_y) { + if (!s->scene.uilayout_sidebarcollapsed && touch_x <= sbr_w) { + if (touch_x >= settings_btn_x && touch_x < (settings_btn_x + settings_btn_w) + && touch_y >= settings_btn_y && touch_y < (settings_btn_y + settings_btn_h)) { + navigate_to_settings(s); + } + if (touch_x >= home_btn_x && touch_x < (home_btn_x + home_btn_w) + && touch_y >= home_btn_y && touch_y < (home_btn_y + home_btn_h)) { + navigate_to_home(s); + if (s->vision_connected) { + s->scene.uilayout_sidebarcollapsed = true; + } + } + } +} + +static void handle_vision_touch(UIState *s, int touch_x, int touch_y) { + if (s->vision_connected && (touch_x >= s->scene.ui_viz_rx - bdr_s) + && (s->active_app != cereal_UiLayoutState_App_settings)) { + s->scene.uilayout_sidebarcollapsed = !s->scene.uilayout_sidebarcollapsed; + } +} + volatile sig_atomic_t do_exit = 0; static void set_do_exit(int sig) { do_exit = 1; @@ -111,19 +149,28 @@ static void ui_init(UIState *s) { s->uilayout_sock = SubSocket::create(s->ctx, "uiLayoutState"); s->livecalibration_sock = SubSocket::create(s->ctx, "liveCalibration"); s->radarstate_sock = SubSocket::create(s->ctx, "radarState"); + s->thermal_sock = SubSocket::create(s->ctx, "thermal"); + s->health_sock = SubSocket::create(s->ctx, "health"); + s->ubloxgnss_sock = SubSocket::create(s->ctx, "ubloxGnss"); assert(s->model_sock != NULL); assert(s->controlsstate_sock != NULL); assert(s->uilayout_sock != NULL); assert(s->livecalibration_sock != NULL); assert(s->radarstate_sock != NULL); + assert(s->thermal_sock != NULL); + assert(s->health_sock != NULL); + assert(s->ubloxgnss_sock != NULL); s->poller = Poller::create({ s->model_sock, s->controlsstate_sock, s->uilayout_sock, s->livecalibration_sock, - s->radarstate_sock + s->radarstate_sock, + s->thermal_sock, + s->health_sock, + s->ubloxgnss_sock }); #ifdef SHOW_SPEEDLIMIT @@ -362,6 +409,11 @@ void handle_message(UIState *s, Message * msg) { s->scene.lead_d_rel = leaddatad.dRel; s->scene.lead_y_rel = leaddatad.yRel; s->scene.lead_v_rel = leaddatad.vRel; + cereal_read_RadarState_LeadData(&leaddatad, datad.leadTwo); + s->scene.lead_status2 = leaddatad.status; + s->scene.lead_d_rel2 = leaddatad.dRel; + s->scene.lead_y_rel2 = leaddatad.yRel; + s->scene.lead_v_rel2 = leaddatad.vRel; s->livempc_or_radarstate_changed = true; } else if (eventd.which == cereal_Event_liveCalibration) { s->scene.world_objects_visible = true; @@ -400,27 +452,57 @@ void handle_message(UIState *s, Message * msg) { cereal_read_UiLayoutState(&datad, eventd.uiLayoutState); s->active_app = datad.activeApp; s->scene.uilayout_sidebarcollapsed = datad.sidebarCollapsed; - s->scene.uilayout_mapenabled = datad.mapEnabled; - - bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; - bool mapEnabled = s->scene.uilayout_mapenabled; - if (mapEnabled) { - s->scene.ui_viz_rx = hasSidebar ? (box_x+nav_w) : (box_x+nav_w-(bdr_s*4)); - s->scene.ui_viz_rw = hasSidebar ? (box_w-nav_w) : (box_w-nav_w+(bdr_s*4)); - s->scene.ui_viz_ro = -(sbr_w + 4*bdr_s); - } else { - s->scene.ui_viz_rx = hasSidebar ? box_x : (box_x-sbr_w+bdr_s*2); - s->scene.ui_viz_rw = hasSidebar ? box_w : (box_w+sbr_w-(bdr_s*2)); - s->scene.ui_viz_ro = hasSidebar ? -(sbr_w - 6*bdr_s) : 0; - } } else if (eventd.which == cereal_Event_liveMapData) { struct cereal_LiveMapData datad; cereal_read_LiveMapData(&datad, eventd.liveMapData); s->scene.map_valid = datad.mapValid; + } else if (eventd.which == cereal_Event_thermal) { + struct cereal_ThermalData datad; + cereal_read_ThermalData(&datad, eventd.thermal); + + s->scene.networkType = datad.networkType; + s->scene.networkStrength = datad.networkStrength; + s->scene.batteryPercent = datad.batteryPercent; + snprintf(s->scene.batteryStatus, sizeof(s->scene.batteryStatus), "%s", datad.batteryStatus.str); + s->scene.freeSpace = datad.freeSpace; + s->scene.thermalStatus = datad.thermalStatus; + s->scene.paTemp = datad.pa0; + } else if (eventd.which == cereal_Event_ubloxGnss) { + struct cereal_UbloxGnss datad; + cereal_read_UbloxGnss(&datad, eventd.ubloxGnss); + if (datad.which == cereal_UbloxGnss_measurementReport) { + struct cereal_UbloxGnss_MeasurementReport reportdatad; + cereal_read_UbloxGnss_MeasurementReport(&reportdatad, datad.measurementReport); + s->scene.satelliteCount = reportdatad.numMeas; + } + } else if (eventd.which == cereal_Event_health) { + struct cereal_HealthData datad; + cereal_read_HealthData(&datad, eventd.health); + + s->scene.hwType = datad.hwType; + s->hardware_timeout = 5*30; // 5 seconds at 30 fps } capn_free(&ctx); } +static void check_messages(UIState *s) { + while(true) { + auto polls = s->poller->poll(0); + + if (polls.size() == 0) + break; + + for (auto sock : polls){ + Message * msg = sock->receive(); + if (msg == NULL) continue; + + handle_message(s, msg); + + delete msg; + } + } +} + static void ui_update(UIState *s) { int err; @@ -490,7 +572,7 @@ static void ui_update(UIState *s) { assert(glGetError() == GL_NO_ERROR); - // Default UI Measurements (Assumes sidebar collapsed) + s->scene.uilayout_sidebarcollapsed = true; s->scene.ui_viz_rx = (box_x-sbr_w+bdr_s*2); s->scene.ui_viz_rw = (box_w+sbr_w-(bdr_s*2)); s->scene.ui_viz_ro = 0; @@ -518,7 +600,7 @@ static void ui_update(UIState *s) { if (ret < 0) { if (errno == EINTR) continue; - LOGW("poll failed (%d)", ret); + LOGE("poll failed (%d - %d)", ret, errno); close(s->ipc_fd); s->ipc_fd = -1; s->vision_connected = false; @@ -571,23 +653,7 @@ static void ui_update(UIState *s) { break; } // peek and consume all events in the zmq queue, then return. - while(true) { - auto polls = s->poller->poll(0); - - if (polls.size() == 0) - return; - - for (auto sock : polls){ - Message * msg = sock->receive(); - if (msg == NULL) continue; - - set_awake(s, true); - - handle_message(s, msg); - - delete msg; - } - } + check_messages(s); } static int vision_subscribe(int fd, VisionPacket *rp, VisionStreamType type) { @@ -728,7 +794,6 @@ fail: return NULL; } - static void* bg_thread(void* args) { UIState *s = (UIState*)args; set_thread_name("bg"); @@ -809,7 +874,6 @@ int main(int argc, char* argv[]) { TouchState touch = {0}; touch_init(&touch); s->touch_fd = touch.fd; - ui_sound_init(); // light sensor scaling params @@ -826,6 +890,9 @@ int main(int argc, char* argv[]) { set_volume(MIN_VOLUME); s->volume_timeout = 5 * UI_FREQ; int draws = 0; + + s->scene.satelliteCount = -1; + while (!do_exit) { bool should_swap = false; if (!s->vision_connected) { @@ -843,34 +910,37 @@ int main(int argc, char* argv[]) { if (smooth_brightness > 255) smooth_brightness = 255; set_brightness(s, (int)smooth_brightness); + // resize vision for collapsing sidebar + const bool hasSidebar = !s->scene.uilayout_sidebarcollapsed; + s->scene.ui_viz_rx = hasSidebar ? box_x : (box_x - sbr_w + (bdr_s * 2)); + s->scene.ui_viz_rw = hasSidebar ? box_w : (box_w + sbr_w - (bdr_s * 2)); + s->scene.ui_viz_ro = hasSidebar ? -(sbr_w - 6 * bdr_s) : 0; + + // poll for touch events + int touch_x = -1, touch_y = -1; + int touched = touch_poll(&touch, &touch_x, &touch_y, 0); + if (touched == 1) { + set_awake(s, true); + handle_sidebar_touch(s, touch_x, touch_y); + handle_vision_touch(s, touch_x, touch_y); + } + if (!s->vision_connected) { - // Car is not started, keep in idle state and awake on touch events - zmq_pollitem_t polls[1] = {{0}}; - polls[0].fd = s->touch_fd; - polls[0].events = ZMQ_POLLIN; - int ret = zmq_poll(polls, 1, 0); - if (ret < 0){ - if (errno == EINTR) continue; - LOGW("poll failed (%d)", ret); - } else if (ret > 0) { - // awake on any touch - int touch_x = -1, touch_y = -1; - int touched = touch_read(&touch, &touch_x, &touch_y); - if (touched == 1) { - set_awake(s, true); - } - } if (s->status != STATUS_STOPPED) { update_status(s, STATUS_STOPPED); } + check_messages(s); } else { + set_awake(s, true); if (s->status == STATUS_STOPPED) { update_status(s, STATUS_DISENGAGED); } // Car started, fetch a new rgb image from ipc and peek for zmq events. ui_update(s); - if(!s->vision_connected) { + if (!s->vision_connected) { // Visiond process is just stopped, force a redraw to make screen blank again. + s->scene.satelliteCount = -1; + s->scene.uilayout_sidebarcollapsed = false; ui_draw(s); glFinish(); should_swap = true; @@ -884,8 +954,15 @@ int main(int argc, char* argv[]) { set_awake(s, false); } - // Don't waste resources on drawing in case screen is off or car is not started. - if (s->awake && s->vision_connected) { + // manage hardware disconnect + if (s->hardware_timeout > 0) { + s->hardware_timeout--; + } else { + s->scene.hwType = cereal_HealthData_HwType_unknown; + } + + // Don't waste resources on drawing in case screen is off + if (s->awake) { ui_draw(s); glFinish(); should_swap = true; diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index 65471c36..ee431a44 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -38,6 +38,12 @@ #define ALERTSIZE_MID 2 #define ALERTSIZE_FULL 3 +#define COLOR_BLACK_ALPHA nvgRGBA(0, 0, 0, 85) +#define COLOR_WHITE nvgRGBA(255, 255, 255, 255) +#define COLOR_WHITE_ALPHA nvgRGBA(255, 255, 255, 85) +#define COLOR_YELLOW nvgRGBA(218, 202, 37, 255) +#define COLOR_RED nvgRGBA(201, 34, 49, 255) + #ifndef QCOM #define UI_60FPS #endif @@ -60,6 +66,14 @@ const int viz_w = vwp_w-(bdr_s*2); const int header_h = 420; const int footer_h = 280; const int footer_y = vwp_h-bdr_s-footer_h; +const int settings_btn_h = 117; +const int settings_btn_w = 200; +const int settings_btn_x = 50; +const int settings_btn_y = 35; +const int home_btn_h = 180; +const int home_btn_w = 180; +const int home_btn_x = 60; +const int home_btn_y = vwp_h - home_btn_h - 40; const int UI_FREQ = 30; // Hz @@ -116,6 +130,9 @@ typedef struct UIScene { int lead_status; float lead_d_rel, lead_y_rel, lead_v_rel; + int lead_status2; + float lead_d_rel2, lead_y_rel2, lead_v_rel2; + int front_box_x, front_box_y, front_box_width, front_box_height; uint64_t alert_ts; @@ -128,6 +145,16 @@ typedef struct UIScene { // Used to show gps planner status bool gps_planner_active; + + uint8_t networkType; + uint8_t networkStrength; + int batteryPercent; + char batteryStatus[64]; + float freeSpace; + uint8_t thermalStatus; + int paTemp; + int hwType; + int satelliteCount; } UIScene; typedef struct { @@ -165,6 +192,11 @@ typedef struct UIState { int img_turn; int img_face; int img_map; + int img_button_settings; + int img_button_home; + int img_battery; + int img_battery_charging; + int img_network[6]; // sockets Context *ctx; @@ -174,7 +206,11 @@ typedef struct UIState { SubSocket *radarstate_sock; SubSocket *map_data_sock; SubSocket *uilayout_sock; + SubSocket *thermal_sock; + SubSocket *health_sock; + SubSocket *ubloxgnss_sock; Poller * poller; + Poller * ublox_poller; int active_app; @@ -218,6 +254,7 @@ typedef struct UIState { int is_metric_timeout; int longitudinal_control_timeout; int limit_set_speed_timeout; + int hardware_timeout; bool controls_seen; @@ -251,8 +288,9 @@ typedef struct UIState { // API void ui_draw_vision_alert(UIState *s, int va_size, int va_color, - const char* va_text1, const char* va_text2); + const char* va_text1, const char* va_text2); void ui_draw(UIState *s); +void ui_draw_sidebar(UIState *s); void ui_nvg_init(UIState *s); #endif diff --git a/selfdrive/updated.py b/selfdrive/updated.py index 29847fdb..acc4603f 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -292,7 +292,7 @@ def attempt_update(): set_update_available_params(new_version=new_version) -def main(gctx=None): +def main(): update_failed_count = 0 overlay_init_done = False wait_helper = WaitTimeHelper() diff --git a/selfdrive/version.py b/selfdrive/version.py index 77dc2666..b45fe384 100644 --- a/selfdrive/version.py +++ b/selfdrive/version.py @@ -4,28 +4,28 @@ import subprocess from selfdrive.swaglog import cloudlog -def get_git_commit(): +def get_git_commit(default=None): try: return subprocess.check_output(["git", "rev-parse", "HEAD"], encoding='utf8').strip() except subprocess.CalledProcessError: - return None + return default -def get_git_branch(): +def get_git_branch(default=None): try: return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"], encoding='utf8').strip() except subprocess.CalledProcessError: - return None + return default -def get_git_full_branchname(): +def get_git_full_branchname(default=None): try: return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], encoding='utf8').strip() except subprocess.CalledProcessError: - return None + return default -def get_git_remote(): +def get_git_remote(default=None): try: local_branch = subprocess.check_output(["git", "name-rev", "--name-only", "HEAD"], encoding='utf8').strip() tracking_remote = subprocess.check_output(["git", "config", "branch." + local_branch + ".remote"], encoding='utf8').strip() @@ -36,7 +36,7 @@ def get_git_remote(): # Not on a branch, fallback return subprocess.check_output(["git", "config", "--get", "remote.origin.url"], encoding='utf8').strip() except subprocess.CalledProcessError: - return None + return default with open(os.path.join(os.path.dirname(os.path.abspath(__file__)), "common", "version.h")) as _versionf: