retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

openpilot v0.7.4 release

Vehicle Researcher 2020-03-16 16:19:36 -07:00
parent 9397e18742
commit d1ad7f3fe1
122 changed files with 3584 additions and 2996 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
CONTRIBUTING.md
View File

@ -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

148
README.md
View File

@ -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 | 25mph<sup>5</sup> | 25mph |
| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph<sup>5</sup> | 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 | 25mph<sup>5</sup> | 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 | 25mph<sup>5</sup> | 12mph |
| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph<sup>5</sup> | 0mph |
| Honda | Passport 2019 | All | openpilot | 25mph<sup>5</sup> | 12mph |
| Honda | Pilot 2016-18 | Honda Sensing | openpilot | 25mph<sup>5</sup> | 12mph |
| Honda | Pilot 2019 | All | openpilot | 25mph<sup>5</sup> | 12mph |
| Honda | Ridgeline 2017-19 | Honda Sensing | openpilot | 25mph<sup>5</sup> | 12mph |
| Hyundai | Elantra 2017-19<sup>1</sup> | SCC + LKAS | Stock | 19mph | 34mph |
| Hyundai | Genesis 2018<sup>1</sup> | All | Stock | 19mph | 34mph |
| Hyundai | Santa Fe 2019<sup>1</sup> | 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 2019<sup>1</sup> | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Sorento 2018<sup>1</sup> | All | Stock | 0mph | 0mph |
| Kia | Stinger 2018<sup>1</sup> | SCC + LKAS | Stock | 0mph | 0mph |
| Lexus | CT Hybrid 2017-18 | All | Stock<sup>4</sup>| 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 | Stock<sup>4</sup>| 0mph | 0mph |
| Lexus | RX 2016-17 | All | Stock<sup>4</sup>| 0mph | 0mph |
| Lexus | RX 2020 | All | openpilot | 0mph | 0mph |
| Lexus | RX Hybrid 2016-19 | All | Stock<sup>4</sup>| 0mph | 0mph |
| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Impreza 2019-20 | EyeSight | Stock | 0mph | 0mph |
| Toyota | Avalon 2016 | TSS-P | Stock<sup>4</sup>| 20mph<sup>5</sup> | 0mph |
| Toyota | Avalon 2017-18 | All | Stock<sup>4</sup>| 20mph<sup>5</sup> | 0mph |
| Toyota | Camry 2018-19 | All | Stock | 0mph<sup>2</sup> | 0mph |
| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph<sup>2</sup> | 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 | Stock<sup>4</sup>| 20mph<sup>5</sup> | 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 | Stock<sup>4</sup>| 0mph | 0mph |
| Toyota | Highlander Hybrid 2017-19 | All | Stock<sup>4</sup>| 0mph | 0mph |
| Toyota | Highlander 2020 | All | openpilot | 0mph | 0mph |
| Toyota | Prius 2016 | TSS-P | Stock<sup>4</sup>| 0mph | 0mph |
| Toyota | Prius 2017-19 | All | Stock<sup>4</sup>| 0mph | 0mph |
| Toyota | Prius Prime 2017-20 | All | Stock<sup>4</sup>| 0mph | 0mph |
| Toyota | Rav4 2016 | TSS-P | Stock<sup>4</sup>| 20mph<sup>5</sup> | 0mph |
| Toyota | Rav4 2017-18 | All | Stock<sup>4</sup>| 20mph<sup>5</sup> | 0mph |
| Toyota | Rav4 2019 | All | openpilot | 0mph | 0mph |
| Toyota | Rav4 Hybrid 2016 | TSS-P | Stock<sup>4</sup>| 0mph | 0mph |
| Toyota | Rav4 Hybrid 2017-18 | All | Stock<sup>4</sup>| 0mph | 0mph |
| Toyota | Rav4 Hybrid 2019-20 | All | openpilot | 0mph | 0mph |
| Toyota | Sienna 2018 | All | Stock<sup>4</sup>| 0mph | 0mph |
| Volkswagen| Golf 2016-19<sup>3</sup> | 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 | 25mph<sup>6</sup> | 25mph |
| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph<sup>6</sup> | 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<sup>4</sup> |
| Honda | CR-V 2015-16 | Touring | openpilot | 25mph<sup>6</sup> | 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 | 25mph<sup>6</sup> | 12mph |
| Honda | Insight 2019 | Honda Sensing | Stock | 0mph | 3mph |
| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph<sup>6</sup> | 0mph |
| Honda | Passport 2019 | All | openpilot | 25mph<sup>6</sup> | 12mph |
| Honda | Pilot 2016-18 | Honda Sensing | openpilot | 25mph<sup>6</sup> | 12mph |
| Honda | Pilot 2019 | All | openpilot | 25mph<sup>6</sup> | 12mph |
| Honda | Ridgeline 2017-19 | Honda Sensing | openpilot | 25mph<sup>6</sup> | 12mph |
| Hyundai | Elantra 2017-19<sup>1</sup> | SCC + LKAS | Stock | 19mph | 34mph |
| Hyundai | Genesis 2018<sup>1</sup> | All | Stock | 19mph | 34mph |
| Hyundai | Santa Fe 2019<sup>1</sup> | 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 2019<sup>1</sup> | SCC + LKAS | Stock | 0mph | 0mph |
| Kia | Sorento 2018<sup>1</sup> | All | Stock | 0mph | 0mph |
| Kia | Stinger 2018<sup>1</sup> | SCC + LKAS | Stock | 0mph | 0mph |
| Lexus | CT Hybrid 2017-18 | All | Stock<sup>5</sup>| 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 | Stock<sup>5</sup>| 0mph | 0mph |
| Lexus | RX 2016-17 | All | Stock<sup>5</sup>| 0mph | 0mph |
| Lexus | RX 2020 | All | openpilot | 0mph | 0mph |
| Lexus | RX Hybrid 2016-19 | All | Stock<sup>5</sup>| 0mph | 0mph |
| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph |
| Subaru | Impreza 2019-20 | EyeSight | Stock | 0mph | 0mph |
| Toyota | Avalon 2016 | TSS-P | Stock<sup>5</sup>| 20mph<sup>6</sup> | 0mph |
| Toyota | Avalon 2017-18 | All | Stock<sup>5</sup>| 20mph<sup>6</sup> | 0mph |
| Toyota | Camry 2018-19 | All | Stock | 0mph<sup>2</sup> | 0mph |
| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph<sup>2</sup> | 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 | Stock<sup>5</sup>| 20mph<sup>6</sup> | 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 | Stock<sup>5</sup>| 0mph | 0mph |
| Toyota | Highlander Hybrid 2017-19 | All | Stock<sup>5</sup>| 0mph | 0mph |
| Toyota | Highlander 2020 | All | openpilot | 0mph | 0mph |
| Toyota | Prius 2016 | TSS-P | Stock<sup>5</sup>| 0mph | 0mph |
| Toyota | Prius 2017-19 | All | Stock<sup>5</sup>| 0mph | 0mph |
| Toyota | Prius Prime 2017-20 | All | Stock<sup>5</sup>| 0mph | 0mph |
| Toyota | Rav4 2016 | TSS-P | Stock<sup>5</sup>| 20mph<sup>6</sup> | 0mph |
| Toyota | Rav4 2017-18 | All | Stock<sup>5</sup>| 20mph<sup>6</sup> | 0mph |
| Toyota | Rav4 2019 | All | openpilot | 0mph | 0mph |
| Toyota | Rav4 Hybrid 2016 | TSS-P | Stock<sup>5</sup>| 0mph | 0mph |
| Toyota | Rav4 Hybrid 2017-18 | All | Stock<sup>5</sup>| 0mph | 0mph |
| Toyota | Rav4 Hybrid 2019-20 | All | openpilot | 0mph | 0mph |
| Toyota | Sienna 2018 | All | Stock<sup>5</sup>| 0mph | 0mph |
| Volkswagen| Golf 2016-19<sup>3</sup> | Driver Assistance | Stock | 0mph | 0mph |
<sup>1</sup>Requires 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. <br />
<sup>2</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. <br />
<sup>3</sup>Requires 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) <br />
<sup>4</sup>2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph. <br />
Community Maintained Cars and Features
------
| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below |
| ----------| ------------------------------| ------------------| -----------------| -------------------| -------------|
| Buick | Regal 2018<sup>6</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Cadillac | ATS 2018<sup>6</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Chevrolet | Malibu 2017<sup>6</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Chevrolet | Volt 2017-18<sup>6</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| GMC | Acadia Denali 2018<sup>6</sup>| Adaptive Cruise | openpilot | 0mph | 7mph |
| Holden | Astra 2017<sup>6</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Buick | Regal 2018<sup>7</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Cadillac | ATS 2018<sup>7</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Chevrolet | Malibu 2017<sup>7</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| Chevrolet | Volt 2017-18<sup>7</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
| GMC | Acadia Denali 2018<sup>7</sup>| Adaptive Cruise | openpilot | 0mph | 7mph |
| Holden | Astra 2017<sup>7</sup> | Adaptive Cruise | openpilot | 0mph | 7mph |
<sup>4</sup>When 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).*** <br />
<sup>5</sup>[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).*** <br />
<sup>6</sup>Requires 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).*** <br />
<sup>5</sup>When 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).*** <br />
<sup>6</sup>[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).*** <br />
<sup>7</sup>Requires 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).*** <br />
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`.

11
RELEASES.md
View File

@ -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!

BIN
apk/ai.comma.plus.black.apk
View File

Binary file not shown.

BIN
apk/ai.comma.plus.frame.apk
View File

Binary file not shown.

BIN
apk/ai.comma.plus.offroad.apk
View File

Binary file not shown.

154
common/android.py
View File

@ -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

7
common/apk.py
View File

@ -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()

50
common/manager_helpers.py 100644
View File

@ -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

7
common/params.py
View File

@ -22,6 +22,8 @@ file in place without messing with <params_dir>/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

BIN
models/dmonitoring_model_q.dlc
View File

Binary file not shown.

BIN
models/supercombo.dlc
View File

Binary file not shown.

BIN
selfdrive/assets/images/battery.png 100644
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.7 KiB

BIN
selfdrive/assets/images/battery_charging.png 100644
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 2.1 KiB

BIN
selfdrive/assets/images/button_home.png 100644
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.5 KiB

BIN
selfdrive/assets/images/button_settings.png 100644
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 957 B

BIN
selfdrive/assets/images/network_0.png 100644
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 416 B

BIN
selfdrive/assets/images/network_1.png 100644
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 462 B

BIN
selfdrive/assets/images/network_2.png 100644
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 593 B

BIN
selfdrive/assets/images/network_3.png 100644
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 524 B

BIN
selfdrive/assets/images/network_4.png 100644
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 541 B

BIN
selfdrive/assets/images/network_5.png 100644
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 503 B

2
selfdrive/athena/athenad.py
View File

@ -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

3
selfdrive/athena/test.py
View File

@ -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)

18
selfdrive/boardd/boardd.cc
View File

@ -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");

8
selfdrive/boardd/tests/boardd_old.py
View File

@ -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:

3
selfdrive/camerad/cameras/camera_qcom.c
View File

@ -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;
}

16
selfdrive/camerad/main.cc
View File

@ -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<const int16_t> focus_vals(&s->cameras.rear.focus[0], NUM_FOCUS);
kj::ArrayPtr<const uint8_t> 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) {

22
selfdrive/car/car_helpers.py
View File

@ -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/<name>/
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

14
selfdrive/car/chrysler/carcontroller.py
View File

@ -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

115
selfdrive/car/chrysler/carstate.py
View File

@ -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)

18
selfdrive/car/chrysler/chryslercan.py
View File

@ -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)

108
selfdrive/car/chrysler/interface.py
View File

@ -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()

4
selfdrive/car/chrysler/test_chryslercan.py
View File

@ -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')

15
selfdrive/car/chrysler/values.py
View File

@ -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'

6
selfdrive/car/ford/carcontroller.py
View File

@ -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

48
selfdrive/car/ford/carstate.py
View File

@ -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)

67
selfdrive/car/ford/interface.py
View File

@ -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]))

8
selfdrive/car/fw_versions.py
View File

@ -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:

49
selfdrive/car/gm/carcontroller.py
View File

@ -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

178
selfdrive/car/gm/carstate.py
View File

@ -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)

10
selfdrive/car/gm/gmcan.py
View File

@ -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):

161
selfdrive/car/gm/interface.py
View File

@ -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,

11
selfdrive/car/gm/radar_interface.py
View File

@ -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()

6
selfdrive/car/gm/values.py
View File

@ -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:

53
selfdrive/car/honda/carcontroller.py
View File

@ -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:

230
selfdrive/car/honda/carstate.py
View File

@ -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)

201
selfdrive/car/honda/interface.py
View File

@ -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

356
selfdrive/car/honda/values.py
View File

@ -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]

8
selfdrive/car/hyundai/carcontroller.py
View File

@ -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))

359
selfdrive/car/hyundai/carstate.py
View File

@ -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)

170
selfdrive/car/hyundai/interface.py
View File

@ -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):

11
selfdrive/car/hyundai/values.py
View File

@ -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
}],

91
selfdrive/car/interfaces.py
View File

@ -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

28
selfdrive/car/mock/interface.py
View File

@ -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

10
selfdrive/car/subaru/carcontroller.py
View File

@ -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

218
selfdrive/car/subaru/carstate.py
View File

@ -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)

136
selfdrive/car/subaru/interface.py
View File

@ -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,

92
selfdrive/car/toyota/carcontroller.py
View File

@ -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:

275
selfdrive/car/toyota/carstate.py
View File

@ -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)

166
selfdrive/car/toyota/interface.py
View File

@ -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

24
selfdrive/car/toyota/toyotacan.py
View File

@ -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."""

94
selfdrive/car/toyota/values.py
View File

@ -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',

28
selfdrive/car/volkswagen/carcontroller.py
View File

@ -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

279
selfdrive/car/volkswagen/carstate.py
View File

@ -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)

135
selfdrive/car/volkswagen/interface.py
View File

@ -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,

14
selfdrive/car/volkswagen/values.py
View File

@ -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,

2
selfdrive/common/version.h
View File

@ -1 +1 @@
#define COMMA_VERSION "0.7.3-release"
#define COMMA_VERSION "0.7.4-release"

17
selfdrive/controls/controlsd.py
View File

@ -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)

3
selfdrive/controls/dmonitoringd.py
View File

@ -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,

28
selfdrive/controls/lib/alerts.py
View File

@ -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",

6
selfdrive/controls/lib/lane_planner.py
View File

@ -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

3
selfdrive/controls/lib/long_mpc.py
View File

@ -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)

29
selfdrive/controls/lib/pathplanner.py
View File

@ -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)

3
selfdrive/controls/lib/planner.py
View File

@ -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'])

6
selfdrive/controls/radard.py
View File

@ -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] = {

3
selfdrive/controls/tests/test_following_distance.py
View File

@ -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

3
selfdrive/crash.py
View File

@ -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):

46
selfdrive/debug/live_cpu_and_temp.py
View File

@ -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']

3
selfdrive/debug/show_matching_cars.py
View File

@ -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

18
selfdrive/debug/test_fw_query_on_routes.py
View File

@ -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 <route_list>/<route>")
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

17
selfdrive/locationd/calibrationd.py
View File

@ -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()]

20
selfdrive/locationd/kalman/SConscript
View File

@ -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():

60
selfdrive/locationd/kalman/helpers/__init__.py
View File

@ -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):

44
selfdrive/locationd/kalman/helpers/ekf_sym.py
View File

@ -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"

4
selfdrive/locationd/kalman/helpers/sympy_helpers.py
View File

@ -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 = []

196
selfdrive/locationd/kalman/models/car_kf.py 100755
View File

@ -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()

140
selfdrive/locationd/locationd.py 100755 → 100644
View File

@ -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):

124
selfdrive/locationd/paramsd.py 100755
View File

@ -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()

2
selfdrive/locationd/test/ubloxd.py
View File

@ -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] = {}

2
selfdrive/locationd/test/ubloxd_easy.py
View File

@ -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')

2
selfdrive/loggerd/SConscript
View File

@ -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]

2
selfdrive/loggerd/deleter.py
View File

@ -35,7 +35,7 @@ def deleter_thread(exit_event):
exit_event.wait(30)
def main(gctx=None):
def main():
deleter_thread(threading.Event())

22
selfdrive/loggerd/logger.c → selfdrive/loggerd/logger.cc
View File

@ -17,6 +17,21 @@
#include "logger.h"
#include <capnp/serialize.h>
#include "cereal/gen/cpp/log.capnp.h"
static void log_sentinel(LoggerState *s, cereal::Sentinel::SentinelType type) {
capnp::MallocMessageBuilder msg;
auto event = msg.initRoot<cereal::Event>();
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) {

4
selfdrive/loggerd/uploader.py
View File

@ -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__":

2
selfdrive/logmessaged.py
View File

@ -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)

96
selfdrive/manager.py
View File

@ -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()

48
selfdrive/modeld/models/driving.cc
View File

@ -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<const float> desire_pred(&meta_data[OTHER_META_SIZE], DESIRE_PRED_SIZE);
kj::ArrayPtr<const float> 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<const float> 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<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 8] > 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<LEAD_MDN_N; i++) {
if (net_outputs.lead[i*MDN_GROUP_SIZE + 9] > 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();

3
selfdrive/modeld/models/driving.h
View File

@ -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;

2
selfdrive/pandad.py
View File

@ -86,7 +86,7 @@ def update_panda():
raise AssertionError
def main(gctx=None):
def main():
update_panda()
os.chdir("boardd")

Some files were not shown because too many files have changed in this diff Show More