openpilot v0.6.6 release
Vehicle Researcher
Willem Melching
parent
b4f4b92ecf
commit
d7f0b402a8
|
|
@ -43,3 +43,6 @@ openpilot
|
|||
notebooks
|
||||
xx
|
||||
|
||||
.coverage*
|
||||
htmlcov
|
||||
|
||||
|
|
|
|||
4
Pipfile
4
Pipfile
|
|
@ -7,6 +7,7 @@ verify_ssl = true
|
|||
opencv-python= "==3.4.2.17"
|
||||
PyQt5 = "*"
|
||||
ipython = "*"
|
||||
networkx = "*"
|
||||
azure-common = "*"
|
||||
azure-nspkg = "*"
|
||||
azure-storage-blob = "*"
|
||||
|
|
@ -99,6 +100,7 @@ gast = "==0.2.2"
|
|||
matplotlib = "*"
|
||||
dictdiffer = "*"
|
||||
aenum = "*"
|
||||
coverage = "*"
|
||||
|
||||
[packages]
|
||||
overpy = {git = "https://github.com/commaai/python-overpy.git",ref = "f86529af402d4642e1faeb146671c40284007323"}
|
||||
|
|
@ -135,7 +137,7 @@ Flask = "*"
|
|||
PyJWT = "*"
|
||||
"Jinja2" = "*"
|
||||
nose = "*"
|
||||
pyflakes = "*"
|
||||
flake8 = "*"
|
||||
pylint = "*"
|
||||
pycryptodome = "*"
|
||||
pillow = "*"
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
169
README.md
169
README.md
|
|
@ -3,7 +3,7 @@
|
|||
Welcome to openpilot
|
||||
======
|
||||
|
||||
[openpilot](http://github.com/commaai/openpilot) is an open source driver assistance system. Currently, it performs the functions of Adaptive Cruise Control (ACC) and Lane Keeping Assist System (LKAS) for selected Honda, Toyota, Acura, Lexus, Chevrolet, Hyundai, Kia. It's about on par with Tesla Autopilot and GM Super Cruise, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan).
|
||||
[openpilot](http://github.com/commaai/openpilot) is an open source driver assistance system. Currently, it performs the functions of Adaptive Cruise Control (ACC) and Automated Lane Centering (ALC) for selected Honda, Toyota, Acura, Lexus, Chevrolet, Hyundai, Kia, Subaru, Volkswagen. It's about on par with Tesla Autopilot and GM Super Cruise, and better than [all other manufacturers](http://www.thedrive.com/tech/5707/the-war-for-autonomous-driving-part-iii-us-vs-germany-vs-japan).
|
||||
|
||||
The openpilot codebase has been written to be concise and to enable rapid prototyping. We look forward to your contributions - improving real vehicle automation has never been easier.
|
||||
|
||||
|
|
@ -29,116 +29,116 @@ Community
|
|||
|
||||
openpilot is developed by [comma.ai](https://comma.ai/) and users like you.
|
||||
|
||||
We have a [Twitter you should follow](https://twitter.com/comma_ai).
|
||||
|
||||
Also, we have a several thousand people community on [Discord](https://discord.comma.ai).
|
||||
[Follow us on Twitter](https://twitter.com/comma_ai) and [join our Discord](https://discord.comma.ai).
|
||||
|
||||
<table>
|
||||
<tr>
|
||||
<td><a href="https://www.youtube.com/watch?v=ICOIin4p70w" title="YouTube" rel="noopener"><img src="https://i.imgur.com/gBTo7yB.png"></a></td>
|
||||
<td><a href="https://www.youtube.com/watch?v=1zCtj3ckGFo" title="YouTube" rel="noopener"><img src="https://i.imgur.com/gNhhcep.png"></a></td>
|
||||
<td><a href="https://www.youtube.com/watch?v=Qd2mjkBIRx0" title="YouTube" rel="noopener"><img src="https://i.imgur.com/tFnSexp.png"></a></td>
|
||||
<td><a href="https://www.youtube.com/watch?v=ju12vlBm59E" title="YouTube" rel="noopener"><img src="https://i.imgur.com/3BKiJVy.png"></a></td>
|
||||
<td><a href="https://www.youtube.com/watch?v=mgAbfr42oI8" title="YouTube" rel="noopener"><img src="https://i.imgur.com/kAtT6Ei.png"></a></td>
|
||||
<td><a href="https://www.youtube.com/watch?v=394rJKeh76k" title="YouTube" rel="noopener"><img src="https://i.imgur.com/lTt8cS2.png"></a></td>
|
||||
<td><a href="https://www.youtube.com/watch?v=1iNOc3cq8cs" title="YouTube" rel="noopener"><img src="https://i.imgur.com/ANnuSpe.png"></a></td>
|
||||
<td><a href="https://www.youtube.com/watch?v=Vr6NgrB-zHw" title="YouTube" rel="noopener"><img src="https://i.imgur.com/Qypanuq.png"></a></td>
|
||||
</tr>
|
||||
<tr>
|
||||
<td><a href="https://www.youtube.com/watch?v=Z5VY5FzgNt4" title="YouTube" rel="noopener"><img src="https://i.imgur.com/3I9XOK2.png"></a></td>
|
||||
<td><a href="https://www.youtube.com/watch?v=blnhZC7OmMg" title="YouTube" rel="noopener"><img src="https://i.imgur.com/f9IgX6s.png"></a></td>
|
||||
<td><a href="https://www.youtube.com/watch?v=iRkz7FuJsA8" title="YouTube" rel="noopener"><img src="https://i.imgur.com/Vo5Zvmn.png"></a></td>
|
||||
<td><a href="https://www.youtube.com/watch?v=IHjEqAKDqjM" title="YouTube" rel="noopener"><img src="https://i.imgur.com/V9Zd81n.png"></a></td>
|
||||
<td><a href="https://www.youtube.com/watch?v=Ug41KIKF0oo" title="YouTube" rel="noopener"><img src="https://i.imgur.com/3caZ7xM.png"></a></td>
|
||||
<td><a href="https://www.youtube.com/watch?v=NVR_CdG1FRg" title="YouTube" rel="noopener"><img src="https://i.imgur.com/bAZOwql.png"></a></td>
|
||||
<td><a href="https://www.youtube.com/watch?v=tkEvIdzdfUE" title="YouTube" rel="noopener"><img src="https://i.imgur.com/EFINEzG.png"></a></td>
|
||||
<td><a href="https://www.youtube.com/watch?v=_P-N1ewNne4" title="YouTube" rel="noopener"><img src="https://i.imgur.com/gAyAq22.png"></a></td>
|
||||
</tr>
|
||||
</table>
|
||||
|
||||
Hardware
|
||||
------
|
||||
|
||||
At the moment openpilot supports the [EON Dashcam DevKit](https://comma.ai/shop/products/eon-dashcam-devkit). A [panda](https://shop.comma.ai/products/panda-obd-ii-dongle) and a [giraffe](https://comma.ai/shop/products/giraffe/) are recommended tools to interface the EON with the car. We'd like to support other platforms as well.
|
||||
At the moment openpilot supports the [EON DevKit](https://comma.ai/shop/products/eon-dashcam-devkit). A [car harness](https://comma.ai/shop/products/car-harness) is recommended to connect the EON to the car. We'd like to support other platforms as well.
|
||||
|
||||
Install openpilot on a neo device by entering ``https://openpilot.comma.ai`` during NEOS setup.
|
||||
|
||||
Supported Cars
|
||||
------
|
||||
|
||||
| Make | Model (US Market Reference)| Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | Giraffe |
|
||||
| ---------------------| ---------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
|
||||
| Acura | ILX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph | Nidec |
|
||||
| Acura | RDX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Buick<sup>3</sup> | Regal 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>6</sup>|
|
||||
| Chevrolet<sup>3</sup>| Malibu 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>6</sup>|
|
||||
| Chevrolet<sup>3</sup>| Volt 2017-18 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>6</sup>|
|
||||
| Cadillac<sup>3</sup> | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>6</sup>|
|
||||
| Chrysler | Pacifica 2017-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA |
|
||||
| Chrysler | Pacifica Hybrid 2017-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA |
|
||||
| Chrysler | Pacifica Hybrid 2019 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | FCA |
|
||||
| GMC<sup>3</sup> | Acadia Denali 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>6</sup>|
|
||||
| Holden<sup>3</sup> | Astra 2017 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | Custom<sup>6</sup>|
|
||||
| Honda | Accord 2018-19 | All | Yes | Stock | 0mph | 3mph | Bosch |
|
||||
| Honda | Accord Hybrid 2018-19 | All | Yes | Stock | 0mph | 3mph | Bosch |
|
||||
| Honda | Civic Sedan/Coupe 2016-18 | Honda Sensing | Yes | Yes | 0mph | 12mph | Nidec |
|
||||
| Honda | Civic Sedan/Coupe 2019 | Honda Sensing | Yes | Stock | 0mph | 2mph | Bosch |
|
||||
| Honda | Civic Hatchback 2017-19 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
|
||||
| Honda | CR-V 2015-16 | Touring | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Honda | CR-V 2017-19 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
|
||||
| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Yes | Stock | 0mph | 12mph | Bosch |
|
||||
| Honda | Fit 2018-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
|
||||
| Honda | Odyssey 2018-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph | Inverted Nidec |
|
||||
| Honda | Passport 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
|
||||
| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph | Inverted Nidec |
|
||||
| Honda | Ridgeline 2017-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph | Nidec |
|
||||
| Hyundai | Santa Fe 2019 | All | Yes | Stock | 0mph | 0mph | Custom<sup>5</sup>|
|
||||
| Hyundai | Elantra 2017-19 | SCC + LKAS | Yes | Stock | 19mph | 34mph | Custom<sup>5</sup>|
|
||||
| Hyundai | Genesis 2018 | All | Yes | Stock | 19mph | 34mph | Custom<sup>5</sup>|
|
||||
| Jeep | Grand Cherokee 2016-18 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | FCA |
|
||||
| Jeep | Grand Cherokee 2019 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | FCA |
|
||||
| Kia | Optima 2019 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>5</sup>|
|
||||
| Kia | Sorento 2018 | All | Yes | Stock | 0mph | 0mph | Custom<sup>5</sup>|
|
||||
| Kia | Stinger 2018 | SCC + LKAS | Yes | Stock | 0mph | 0mph | Custom<sup>5</sup>|
|
||||
| Lexus | CT Hybrid 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Lexus | ES 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
|
||||
| Lexus | ES Hybrid 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
|
||||
| Lexus | RX Hybrid 2016-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Lexus | IS 2017-2019 | All | Yes | Stock | 22mph | 0mph | Toyota |
|
||||
| Lexus | IS Hybrid 2017 | All | Yes | Stock | 0mph | 0mph | Toyota |
|
||||
| Subaru | Crosstrek 2018-19 | EyeSight | Yes | Stock | 0mph | 0mph | Subaru |
|
||||
| Subaru | Impreza 2019-20 | EyeSight | Yes | Stock | 0mph | 0mph | Subaru |
|
||||
| Toyota | Avalon 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
|
||||
| Toyota | Avalon 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
|
||||
| Toyota | Camry 2018-19 | All | Yes | Stock | 0mph<sup>4</sup> | 0mph | Toyota |
|
||||
| Toyota | Camry Hybrid 2018-19 | All | Yes | Stock | 0mph<sup>4</sup> | 0mph | Toyota |
|
||||
| Toyota | C-HR 2017-19 | All | Yes | Stock | 0mph | 0mph | Toyota |
|
||||
| Toyota | C-HR Hybrid 2017-19 | All | Yes | Stock | 0mph | 0mph | Toyota |
|
||||
| Toyota | Corolla 2017-19 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
|
||||
| Toyota | Corolla 2020 | All | Yes | Yes | 0mph | 0mph | Toyota |
|
||||
| Toyota | Corolla Hatchback 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
|
||||
| Toyota | Corolla Hybrid 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
|
||||
| Toyota | Highlander 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Highlander Hybrid 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Prius Prime 2017-20 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
|
||||
| Toyota | Rav4 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph | Toyota |
|
||||
| Toyota | Rav4 2019 | All | Yes | Yes | 0mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 Hybrid 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Toyota | Sienna 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph | Toyota |
|
||||
| Make | Model (US Market Reference) | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below |
|
||||
| ----------------------| -----------------------------------| ---------------------| --------| ---------------| -----------------| ---------------|
|
||||
| Acura | ILX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 25mph |
|
||||
| Acura | RDX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Buick<sup>3</sup> | Regal 2018<sup>6</sup> | Adaptive Cruise | Yes | Yes | 0mph | 7mph |
|
||||
| Chevrolet<sup>3</sup> | Malibu 2017<sup>6</sup> | Adaptive Cruise | Yes | Yes | 0mph | 7mph |
|
||||
| Chevrolet<sup>3</sup> | Volt 2017-18<sup>6</sup> | Adaptive Cruise | Yes | Yes | 0mph | 7mph |
|
||||
| Cadillac<sup>3</sup> | ATS 2018 | Adaptive Cruise | Yes | Yes | 0mph | 7mph |
|
||||
| Chrysler | Pacifica 2017-18<sup>7</sup> | Adaptive Cruise | Yes | Stock | 0mph | 9mph |
|
||||
| Chrysler | Pacifica Hybrid 2017-18<sup>7</sup>| Adaptive Cruise | Yes | Stock | 0mph | 9mph |
|
||||
| Chrysler | Pacifica Hybrid 2019<sup>7</sup> | Adaptive Cruise | Yes | Stock | 0mph | 39mph |
|
||||
| GMC<sup>3</sup> | Acadia Denali 2018<sup>6</sup> | Adaptive Cruise | Yes | Yes | 0mph | 7mph |
|
||||
| Holden<sup>3</sup> | Astra 2017<sup>6</sup> | Adaptive Cruise | Yes | Yes | 0mph | 7mph |
|
||||
| Honda | Accord 2018-19 | All | Yes | Stock | 0mph | 3mph |
|
||||
| Honda | Accord Hybrid 2018-19 | All | Yes | Stock | 0mph | 3mph |
|
||||
| Honda | Civic Sedan/Coupe 2016-18 | Honda Sensing | Yes | Yes | 0mph | 12mph |
|
||||
| Honda | Civic Sedan/Coupe 2019 | Honda Sensing | Yes | Stock | 0mph | 2mph |
|
||||
| Honda | Civic Hatchback 2017-19 | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | CR-V 2015-16 | Touring | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | CR-V 2017-19 | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Yes | Stock | 0mph | 12mph |
|
||||
| Honda | Fit 2018-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Odyssey 2018-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 0mph |
|
||||
| Honda | Passport 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Pilot 2019 | All | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Honda | Ridgeline 2017-19 | Honda Sensing | Yes | Yes | 25mph<sup>1</sup>| 12mph |
|
||||
| Hyundai | Santa Fe 2019<sup>5</sup> | All | Yes | Stock | 0mph | 0mph |
|
||||
| Hyundai | Elantra 2017-19<sup>5</sup> | SCC + LKAS | Yes | Stock | 19mph | 34mph |
|
||||
| Hyundai | Genesis 2018<sup>5</sup> | All | Yes | Stock | 19mph | 34mph |
|
||||
| Jeep | Grand Cherokee 2016-18<sup>7</sup> | Adaptive Cruise | Yes | Stock | 0mph | 9mph |
|
||||
| Jeep | Grand Cherokee 2019<sup>7</sup> | Adaptive Cruise | Yes | Stock | 0mph | 39mph |
|
||||
| Kia | Optima 2019<sup>5</sup> | SCC + LKAS | Yes | Stock | 0mph | 0mph |
|
||||
| Kia | Sorento 2018<sup>5</sup> | All | Yes | Stock | 0mph | 0mph |
|
||||
| Kia | Stinger 2018<sup>5</sup> | SCC + LKAS | Yes | Stock | 0mph | 0mph |
|
||||
| Lexus | CT Hybrid 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Lexus | ES Hybrid 2019 | All | Yes | Yes | 0mph | 0mph |
|
||||
| Lexus | RX Hybrid 2016-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Lexus | IS 2017-2019 | All | Yes | Stock | 22mph | 0mph |
|
||||
| Lexus | IS Hybrid 2017 | All | Yes | Stock | 0mph | 0mph |
|
||||
| Subaru | Crosstrek 2018-19 | EyeSight | Yes | Stock | 0mph | 0mph |
|
||||
| Subaru | Impreza 2019-20 | EyeSight | Yes | Stock | 0mph | 0mph |
|
||||
| Toyota | Avalon 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph |
|
||||
| Toyota | Avalon 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph |
|
||||
| Toyota | Camry 2018-19 | All | Yes | Stock | 0mph<sup>4</sup> | 0mph |
|
||||
| Toyota | Camry Hybrid 2018-19 | All | Yes | Stock | 0mph<sup>4</sup> | 0mph |
|
||||
| Toyota | C-HR 2017-19 | All | Yes | Stock | 0mph | 0mph |
|
||||
| Toyota | C-HR Hybrid 2017-19 | All | Yes | Stock | 0mph | 0mph |
|
||||
| Toyota | Corolla 2017-19 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph |
|
||||
| Toyota | Corolla 2020 | All | Yes | Yes | 0mph | 0mph |
|
||||
| Toyota | Corolla Hatchback 2019 | All | Yes | Yes | 0mph | 0mph |
|
||||
| Toyota | Corolla Hybrid 2020 | All | Yes | Yes | 0mph | 0mph |
|
||||
| Toyota | Highlander 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Highlander Hybrid 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius 2017-19 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Prius Prime 2017-20 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 2016 | TSS-P | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph |
|
||||
| Toyota | Rav4 2017-18 | All | Yes | Yes<sup>2</sup>| 20mph<sup>1</sup>| 0mph |
|
||||
| Toyota | Rav4 2019 | All | Yes | Yes | 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2016 | TSS-P | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Toyota | Sienna 2018 | All | Yes | Yes<sup>2</sup>| 0mph | 0mph |
|
||||
| Volkswagen<sup>8</sup>| Golf 2016-19 | Driver Assistance | Yes | Stock | 0mph | 0mph |
|
||||
|
||||
<sup>1</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.ai](https://comma.ai).*** <br />
|
||||
<sup>2</sup>When disconnecting the Driver Support Unit (DSU), otherwise longitudinal control is 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>3</sup>[GM installation guide](https://zoneos.com/volt/). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).*** <br />
|
||||
<sup>4</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. <br />
|
||||
<sup>5</sup>Open sourced [Hyundai Giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai) is designed for the 2019 Sante Fe; pinout may differ for other Hyundais. <br />
|
||||
<sup>6</sup>Community built Giraffe, find more information [here](https://zoneos.com/shop/). <br />
|
||||
<sup>5</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 Hyundais. <br />
|
||||
<sup>6</sup>Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and community built giraffe, find more information [here](https://zoneos.com/shop/). <br />
|
||||
<sup>7</sup>Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and FCA [giraffe](https://comma.ai/shop/products/giraffe) <br />
|
||||
<sup>8</sup>Requires a [custom connector](https://community.comma.ai/wiki/index.php/Volkswagen#Integration_at_J533_Gateway) for the [car harness](https://comma.ai/shop/products/car-harness) <br />
|
||||
|
||||
Community Maintained Cars
|
||||
------
|
||||
|
||||
| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | Giraffe |
|
||||
| ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
|
||||
| Tesla | Model S 2012-13 | All | Yes | Not yet | Not applicable | 0mph | Custom<sup>7</sup>|
|
||||
| Make | Model (US Market Reference) | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below |
|
||||
| ---------------------| -----------------------------------| ---------------------| --------| ---------------| -----------------| ---------------|
|
||||
| Tesla | Model S 2012-13<sup>9</sup> | All | Yes | NA | NA | 0mph |
|
||||
|
||||
[[Tesla Model S Pull Request]](https://github.com/commaai/openpilot/pull/246) <br />
|
||||
<sup>7</sup>Community built Giraffe, find more information here [Community Tesla Giraffe](https://github.com/jeankalud/neo/tree/tesla_giraffe/giraffe/tesla) <br />
|
||||
<sup>9</sup>Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and community built giraffe, find more information [here](https://github.com/jeankalud/neo/tree/tesla_giraffe/giraffe/tesla). <br />
|
||||
|
||||
Community Maintained Cars are not confirmed by comma.ai to meet our [safety model](https://github.com/commaai/openpilot/blob/devel/SAFETY.md). Be extra cautious using them.
|
||||
|
||||
|
|
@ -149,6 +149,7 @@ In Progress Cars
|
|||
- All Kia, Genesis with SCC and LKAS.
|
||||
- All Chrysler, Jeep, Fiat with Adaptive Cruise Control and LaneSense.
|
||||
- All Subaru with EyeSight.
|
||||
- All Volkswagen, Audi, Škoda and SEAT with Adaptive Cruise Control.
|
||||
|
||||
How can I add support for my car?
|
||||
------
|
||||
|
|
@ -203,7 +204,7 @@ It logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, cr
|
|||
The user facing camera is only logged if you explicitly opt-in in settings.
|
||||
It does not log the microphone.
|
||||
|
||||
By using it, you agree to [our privacy policy](https://community.comma.ai/privacy.html). You understand that use of this software or its related services will generate certain types of user data, which may be logged and stored at the sole discretion of comma.ai. By accepting this agreement, you grant an irrevocable, perpetual, worldwide right to comma.ai for the use of this data.
|
||||
By using it, you agree to [our privacy policy](https://my.comma.ai/privacy). You understand that use of this software or its related services will generate certain types of user data, which may be logged and stored at the sole discretion of comma.ai. By accepting this agreement, you grant an irrevocable, perpetual, worldwide right to comma.ai for the use of this data.
|
||||
|
||||
Testing on PC
|
||||
------
|
||||
|
|
|
|||
17
RELEASES.md
17
RELEASES.md
|
|
@ -1,8 +1,21 @@
|
|||
Version 0.6.6 (2019-11-05)
|
||||
========================
|
||||
* Volkswagen support thanks to jyoung8607!
|
||||
* Toyota Corolla Hybrid with TSS 2.0 support thanks to u8511049!
|
||||
* Lexus ES with TSS 2.0 support thanks to energee!
|
||||
* Fix GM ignition detection and lock safety mode not required anymore
|
||||
* Log panda firmware and dongle ID thanks to martinl!
|
||||
* New driving model: improve path prediction and lead detection
|
||||
* New driver monitoring model, 4x smaller and running on DSP
|
||||
* Display an alert and don't start openpilot if panda has wrong firmware
|
||||
* Fix bug preventing EON from terminating processes after a drive
|
||||
* Remove support for Toyota giraffe without the 120Ohm resistor
|
||||
|
||||
Version 0.6.5 (2019-10-07)
|
||||
========================
|
||||
* NEOS update: upgrade to Python3 and new installer!
|
||||
* comma Harness support!
|
||||
* New driving model: lateral control has lower reliance on lanelines
|
||||
* New driving model: improve path prediction
|
||||
* New driver monitoring model: more accurate face and eye detection
|
||||
* Redesign offroad screen to display updates and alerts
|
||||
* Increase maximum allowed acceleration
|
||||
|
|
@ -62,7 +75,7 @@ Version 0.6 (2019-07-01)
|
|||
* Panda safety code is MISRA compliant and ships with a signed version on release2
|
||||
* New NEOS is 500MB smaller and has a reproducible usr/pipenv
|
||||
* Lexus ES Hybrid support thanks to wocsor!
|
||||
* Improve tuning for supported Toyota with TSS2
|
||||
* Improve tuning for supported Toyota with TSS 2.0
|
||||
* Various other stability improvements
|
||||
|
||||
Version 0.5.13 (2019-05-31)
|
||||
|
|
|
|||
33
SAFETY.md
33
SAFETY.md
|
|
@ -1,8 +1,8 @@
|
|||
openpilot Safety
|
||||
======
|
||||
|
||||
openpilot is an Adaptive Cruise Control (ACC) and Lane Keeping Assist (LKA) system.
|
||||
Like other ACC and LKA systems, openpilot requires the driver to be alert and to
|
||||
openpilot is an Adaptive Cruise Control (ACC) and Automated Lane Centering (ALC) system.
|
||||
Like other ACC and ALC systems, openpilot requires the driver to be alert and to
|
||||
pay attention at all times. We repeat, **driver alertness is necessary, but not
|
||||
sufficient, for openpilot to be used safely**.
|
||||
|
||||
|
|
@ -93,7 +93,7 @@ GM/Chevrolet
|
|||
commands outside these limits. A steering torque rate limit is enforced by the panda firmware and by
|
||||
openpilot, so that the commanded steering torque must rise from 0 to max value no faster than
|
||||
0.75s. Commanded steering torque is gradually limited by the panda firmware and by openpilot if the driver's
|
||||
torque exceeds 12 units in the opposite dicrection to ensure limited applied torque against the
|
||||
torque exceeds 12 units in the opposite direction to ensure limited applied torque against the
|
||||
driver's will.
|
||||
|
||||
- Brake pedal and gas pedal potentiometer signals are contained in the 0xF1 and 0x1A1 CAN messages,
|
||||
|
|
@ -116,7 +116,7 @@ Hyundai/Kia (Lateral only)
|
|||
commands outside the values of -409 and 409. A steering torque rate limit is enforced by the panda firmware and by
|
||||
openpilot, so that the commanded steering torque must rise from 0 to max value no faster than
|
||||
0.85s. Commanded steering torque is gradually limited by the panda firmware and by openpilot if the driver's
|
||||
torque exceeds 50 units in the opposite dicrection to ensure limited applied torque against the
|
||||
torque exceeds 50 units in the opposite direction to ensure limited applied torque against the
|
||||
driver's will.
|
||||
|
||||
Chrysler/Jeep/Fiat (Lateral only)
|
||||
|
|
@ -144,8 +144,31 @@ Subaru (Lateral only)
|
|||
commands outside the values of -2047 and 2047. A steering torque rate limit is enforced by the panda firmware and by
|
||||
openpilot, so that the commanded steering torque must rise from 0 to max value no faster than
|
||||
0.41s. Commanded steering torque is gradually limited by the panda firmware and by openpilot if the driver's
|
||||
torque exceeds 60 units in the opposite dicrection to ensure limited applied torque against the
|
||||
torque exceeds 60 units in the opposite direction to ensure limited applied torque against the
|
||||
driver's will.
|
||||
|
||||
Volkswagen, Audi, SEAT, Škoda (Lateral only)
|
||||
------
|
||||
|
||||
- While the system is engaged, steer commands are subject to the same limits used by the stock system, and
|
||||
additional limits required to meet Comma safety standards.
|
||||
|
||||
- Steering torque is controlled through the CAN message 0x126, also known as HCA_01 for Heading Control Assist.
|
||||
It's limited by openpilot and Panda to a value between -250 and 250, representing 2.5 Nm of torque applied
|
||||
at the steering rack. The vehicle EPS unit will fault for values outside -300 and 300.
|
||||
|
||||
- The vehicle EPS unit will tolerate any rate of increase or decrease, but may limit the effective rate of
|
||||
change to 5.0 Nm/s. In accordance with the Comma AI safety model requirements, a rate limit is enforced by
|
||||
the Panda firmware and by openpilot, so that the commanded steering torque cannot rise from 0 to maximum
|
||||
faster than 1.25s. Commanded steering torque is gradually limited by the Panda firmware and by openpilot
|
||||
if the driver's torque exceeds 0.8 Nm in the opposite direction to ensure limited applied torque against
|
||||
the driver's will.
|
||||
|
||||
- Brake and gas pedal pressed signals are contained in the ESP_05 0x106 and Motor_20 0x121 CAN messages,
|
||||
respectively. A rising edge of either signals triggers a disengagement and is enforced by openpilot.
|
||||
The cancellation due to the rising edge of the gas pressed signal is also enforced by the Panda firmware.
|
||||
Additionally, the cruise control system disengages on the rising edge of the brake pedal pressed signal,
|
||||
and it's enforced by both openpilot and the Panda firmware.
|
||||
|
||||
**Extra note**: comma.ai strongly discourages the use of openpilot forks with safety code either missing or
|
||||
not fully meeting the above requirements.
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -1,6 +1,7 @@
|
|||
#!/bin/bash
|
||||
|
||||
pyflakes $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda")
|
||||
# Only pyflakes checks (--select=F)
|
||||
flake8 --select=F $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda")
|
||||
RESULT=$?
|
||||
if [ $RESULT -eq 0 ]; then
|
||||
pylint $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda")
|
||||
|
|
|
|||
|
|
@ -6,5 +6,4 @@ kalman_dir = os.path.dirname(os.path.abspath(__file__))
|
|||
subprocess.check_call(["make", "simple_kalman_impl.so"], cwd=kalman_dir)
|
||||
|
||||
from .simple_kalman_impl import KF1D as KF1D
|
||||
# Silence pyflakes
|
||||
assert KF1D
|
||||
|
|
|
|||
|
|
@ -77,10 +77,11 @@ keys = {
|
|||
"LiveParameters": [TxType.PERSISTENT],
|
||||
"LongitudinalControl": [TxType.PERSISTENT],
|
||||
"OpenpilotEnabledToggle": [TxType.PERSISTENT],
|
||||
"PandaFirmware": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
||||
"PandaDongleId": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
||||
"Passive": [TxType.PERSISTENT],
|
||||
"RecordFront": [TxType.PERSISTENT],
|
||||
"ReleaseNotes": [TxType.PERSISTENT],
|
||||
"SafetyModelLock": [TxType.PERSISTENT],
|
||||
"ShouldDoUpdate": [TxType.CLEAR_ON_MANAGER_START],
|
||||
"SpeedLimitOffset": [TxType.PERSISTENT],
|
||||
"SubscriberInfo": [TxType.PERSISTENT],
|
||||
|
|
@ -89,9 +90,11 @@ keys = {
|
|||
"UpdateAvailable": [TxType.CLEAR_ON_MANAGER_START],
|
||||
"Version": [TxType.PERSISTENT],
|
||||
"Offroad_ChargeDisabled": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
||||
"Offroad_TemperatureTooHigh": [TxType.CLEAR_ON_MANAGER_START],
|
||||
"Offroad_ConnectivityNeededPrompt": [TxType.CLEAR_ON_MANAGER_START],
|
||||
"Offroad_ConnectivityNeeded": [TxType.CLEAR_ON_MANAGER_START],
|
||||
"Offroad_ConnectivityNeededPrompt": [TxType.CLEAR_ON_MANAGER_START],
|
||||
"Offroad_TemperatureTooHigh": [TxType.CLEAR_ON_MANAGER_START],
|
||||
"Offroad_PandaFirmwareMismatch": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
|
||||
"Offroad_InvalidTime": [TxType.CLEAR_ON_MANAGER_START],
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -1,16 +1,19 @@
|
|||
#!/bin/bash
|
||||
set -e
|
||||
|
||||
SETUP="cd /tmp/openpilot && make -C cereal && "
|
||||
|
||||
docker build -t tmppilot -f Dockerfile.openpilot .
|
||||
|
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py'
|
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && pyflakes $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools")'
|
||||
|
||||
docker run --rm tmppilot /bin/sh -c "$SETUP cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py"
|
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && flake8 --select=F $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools")'
|
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && pylint $(find . -iname "*.py" | grep -vi "^\./pyextra.*" | grep -vi "^\./panda" | grep -vi "^\./tools"); exit $(($? & 3))'
|
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover common'
|
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover selfdrive/can'
|
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover selfdrive/boardd'
|
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && python -m unittest discover selfdrive/controls'
|
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && python -m unittest discover selfdrive/loggerd'
|
||||
docker run --rm -v "$(pwd)"/selfdrive/test/longitudinal_maneuvers/out:/tmp/openpilot/selfdrive/test/longitudinal_maneuvers/out tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/longitudinal_maneuvers && OPTEST=1 ./test_longitudinal.py'
|
||||
docker run --rm tmppilot /bin/sh -c 'cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/process_replay/ && ./test_processes.py'
|
||||
docker run --rm tmppilot /bin/sh -c 'mkdir -p /data/params && cd /tmp/openpilot/ && make -C cereal && cd /tmp/openpilot/selfdrive/test/ && ./test_car_models.py'
|
||||
docker run --rm tmppilot /bin/sh -c "$SETUP python -m unittest discover common"
|
||||
docker run --rm tmppilot /bin/sh -c "$SETUP make -C selfdrive/can -j4 && python -m unittest discover selfdrive/can"
|
||||
docker run --rm tmppilot /bin/sh -c "$SETUP python -m unittest discover selfdrive/boardd"
|
||||
docker run --rm tmppilot /bin/sh -c "$SETUP make -C selfdrive/can -j4 && python -m unittest discover selfdrive/controls"
|
||||
docker run --rm tmppilot /bin/sh -c "$SETUP python -m unittest discover selfdrive/loggerd"
|
||||
docker run --rm -v "$(pwd)"/selfdrive/test/longitudinal_maneuvers/out:/tmp/openpilot/selfdrive/test/longitudinal_maneuvers/out tmppilot /bin/sh -c "$SETUP make -C selfdrive/can -j4 && cd /tmp/openpilot/selfdrive/test/longitudinal_maneuvers && OPTEST=1 ./test_longitudinal.py"
|
||||
docker run --rm tmppilot /bin/sh -c "$SETUP make -C selfdrive/can -j4 && cd /tmp/openpilot/selfdrive/test/process_replay/ && ./test_processes.py"
|
||||
docker run --rm tmppilot /bin/sh -c "$SETUP make -C selfdrive/can -j4 && mkdir -p /data/params && cd /tmp/openpilot/selfdrive/test/ && ./test_car_models.py"
|
||||
|
|
|
|||
|
|
@ -10,22 +10,19 @@ import socket
|
|||
import time
|
||||
import threading
|
||||
import traceback
|
||||
import zmq
|
||||
import base64
|
||||
import requests
|
||||
import six.moves.queue
|
||||
import queue
|
||||
from functools import partial
|
||||
from jsonrpc import JSONRPCResponseManager, dispatcher
|
||||
from websocket import create_connection, WebSocketTimeoutException, ABNF
|
||||
from selfdrive.loggerd.config import ROOT
|
||||
|
||||
import selfdrive.crash as crash
|
||||
import selfdrive.messaging as messaging
|
||||
from common.api import Api
|
||||
from common.params import Params
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.version import version, dirty
|
||||
from functools import reduce
|
||||
|
||||
ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
|
||||
|
|
@ -33,8 +30,8 @@ HANDLER_THREADS = os.getenv('HANDLER_THREADS', 4)
|
|||
LOCAL_PORT_WHITELIST = set([8022])
|
||||
|
||||
dispatcher["echo"] = lambda s: s
|
||||
payload_queue = six.moves.queue.Queue()
|
||||
response_queue = six.moves.queue.Queue()
|
||||
payload_queue = queue.Queue()
|
||||
response_queue = queue.Queue()
|
||||
|
||||
def handle_long_poll(ws):
|
||||
end_event = threading.Event()
|
||||
|
|
@ -66,7 +63,7 @@ def jsonrpc_handler(end_event):
|
|||
data = payload_queue.get(timeout=1)
|
||||
response = JSONRPCResponseManager.handle(data, dispatcher)
|
||||
response_queue.put_nowait(response)
|
||||
except six.moves.queue.Empty:
|
||||
except queue.Empty:
|
||||
pass
|
||||
except Exception as e:
|
||||
cloudlog.exception("athena jsonrpc handler failed")
|
||||
|
|
@ -79,8 +76,8 @@ def jsonrpc_handler(end_event):
|
|||
def getMessage(service=None, timeout=1000):
|
||||
if service is None or service not in service_list:
|
||||
raise Exception("invalid service")
|
||||
socket = messaging.sub_sock(service_list[service].port)
|
||||
socket.setsockopt(zmq.RCVTIMEO, timeout)
|
||||
socket = messaging.sub_sock(service)
|
||||
socket.setTimeout(timeout)
|
||||
ret = messaging.recv_one(socket)
|
||||
return ret.to_dict()
|
||||
|
||||
|
|
@ -99,8 +96,6 @@ def uploadFileToUrl(fn, url, headers):
|
|||
|
||||
def startLocalProxy(global_end_event, remote_ws_uri, local_port):
|
||||
try:
|
||||
cloudlog.event("athena startLocalProxy", remote_ws_uri=remote_ws_uri, local_port=local_port)
|
||||
|
||||
if local_port not in LOCAL_PORT_WHITELIST:
|
||||
raise Exception("Requested local port not whitelisted")
|
||||
|
||||
|
|
@ -169,9 +164,12 @@ def takeSnapshot():
|
|||
ret = snapshot()
|
||||
if ret is not None:
|
||||
def b64jpeg(x):
|
||||
f = io.BytesIO()
|
||||
jpeg_write(f, x)
|
||||
return base64.b64encode(f.getvalue()).decode("utf-8")
|
||||
if x is not None:
|
||||
f = io.BytesIO()
|
||||
jpeg_write(f, x)
|
||||
return base64.b64encode(f.getvalue()).decode("utf-8")
|
||||
else:
|
||||
return None
|
||||
return {'jpegBack': b64jpeg(ret[0]),
|
||||
'jpegFront': b64jpeg(ret[1])}
|
||||
else:
|
||||
|
|
@ -185,6 +183,7 @@ def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event):
|
|||
except WebSocketTimeoutException:
|
||||
pass
|
||||
except Exception:
|
||||
cloudlog.exception("athenad.ws_proxy_recv.exception")
|
||||
traceback.print_exc()
|
||||
break
|
||||
|
||||
|
|
@ -208,8 +207,9 @@ def ws_proxy_send(ws, local_sock, signal_sock, end_event):
|
|||
|
||||
ws.send(data, ABNF.OPCODE_BINARY)
|
||||
except Exception:
|
||||
traceback.print_exc()
|
||||
end_event.set()
|
||||
cloudlog.exception("athenad.ws_proxy_send.exception")
|
||||
traceback.print_exc()
|
||||
end_event.set()
|
||||
|
||||
def ws_recv(ws, end_event):
|
||||
while not end_event.is_set():
|
||||
|
|
@ -219,6 +219,7 @@ def ws_recv(ws, end_event):
|
|||
except WebSocketTimeoutException:
|
||||
pass
|
||||
except Exception:
|
||||
cloudlog.exception("athenad.ws_recv.exception")
|
||||
traceback.print_exc()
|
||||
end_event.set()
|
||||
|
||||
|
|
@ -227,9 +228,10 @@ def ws_send(ws, end_event):
|
|||
try:
|
||||
response = response_queue.get(timeout=1)
|
||||
ws.send(response.json)
|
||||
except six.moves.queue.Empty:
|
||||
except queue.Empty:
|
||||
pass
|
||||
except Exception:
|
||||
cloudlog.exception("athenad.ws_send.exception")
|
||||
traceback.print_exc()
|
||||
end_event.set()
|
||||
|
||||
|
|
@ -241,31 +243,26 @@ def main(gctx=None):
|
|||
dongle_id = params.get("DongleId").decode('utf-8')
|
||||
ws_uri = ATHENA_HOST + "/ws/v2/" + dongle_id
|
||||
|
||||
crash.bind_user(id=dongle_id)
|
||||
crash.bind_extra(version=version, dirty=dirty, is_eon=True)
|
||||
crash.install()
|
||||
|
||||
api = Api(dongle_id)
|
||||
|
||||
conn_retries = 0
|
||||
while 1:
|
||||
try:
|
||||
print("connecting to %s" % ws_uri)
|
||||
ws = create_connection(ws_uri,
|
||||
cookie="jwt=" + api.get_token(),
|
||||
enable_multithread=True)
|
||||
cloudlog.event("athenad.main.connected_ws", ws_uri=ws_uri)
|
||||
ws.settimeout(1)
|
||||
conn_retries = 0
|
||||
handle_long_poll(ws)
|
||||
except (KeyboardInterrupt, SystemExit):
|
||||
break
|
||||
except Exception:
|
||||
cloudlog.exception("athenad.main.exception")
|
||||
conn_retries += 1
|
||||
traceback.print_exc()
|
||||
|
||||
time.sleep(backoff(conn_retries))
|
||||
|
||||
params.delete("AthenadPid")
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
|
|||
|
|
@ -0,0 +1,36 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import time
|
||||
from multiprocessing import Process
|
||||
|
||||
import selfdrive.crash as crash
|
||||
from common.params import Params
|
||||
from selfdrive.manager import launcher
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.version import version, dirty
|
||||
|
||||
ATHENA_MGR_PID_PARAM = "AthenadPid"
|
||||
|
||||
def main():
|
||||
params = Params()
|
||||
dongle_id = params.get("DongleId").decode('utf-8')
|
||||
cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty, is_eon=True)
|
||||
crash.bind_user(id=dongle_id)
|
||||
crash.bind_extra(version=version, dirty=dirty, is_eon=True)
|
||||
crash.install()
|
||||
|
||||
try:
|
||||
while 1:
|
||||
cloudlog.info("starting athena daemon")
|
||||
proc = Process(name='athenad', target=launcher, args=('selfdrive.athena.athenad',))
|
||||
proc.start()
|
||||
proc.join()
|
||||
cloudlog.event("athenad exited", exitcode=proc.exitcode)
|
||||
time.sleep(5)
|
||||
except:
|
||||
cloudlog.exception("manage_athenad.exception")
|
||||
finally:
|
||||
params.delete(ATHENA_MGR_PID_PARAM)
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
@ -16,7 +16,8 @@ WARN_FLAGS = -Werror=implicit-function-declaration \
|
|||
CFLAGS = -std=gnu11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
|
||||
CXXFLAGS = -std=c++11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS)
|
||||
|
||||
ZMQ_LIBS = -l:libczmq.a -l:libzmq.a -lgnustl_shared
|
||||
MESSAGING_FLAGS = -I$(BASEDIR)/selfdrive/messaging
|
||||
MESSAGING_LIBS = $(BASEDIR)/selfdrive/messaging/messaging.a
|
||||
|
||||
JSON_FLAGS = -I$(PHONELIBS)/json/src
|
||||
|
||||
|
|
@ -31,13 +32,12 @@ EXTRA_LIBS = -lusb-1.0
|
|||
ifeq ($(ARCH),aarch64)
|
||||
CFLAGS += -mcpu=cortex-a57
|
||||
CXXFLAGS += -mcpu=cortex-a57
|
||||
EXTRA_LIBS += -lgnustl_shared
|
||||
endif
|
||||
|
||||
|
||||
ifeq ($(ARCH),x86_64)
|
||||
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include
|
||||
ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib/ \
|
||||
-l:libczmq.a -l:libzmq.a
|
||||
EXTRA_LIBS = -lusb-1.0 -lpthread
|
||||
CXXFLAGS += -I/usr/include/libusb-1.0
|
||||
CFLAGS += -I/usr/include/libusb-1.0
|
||||
|
|
@ -58,11 +58,10 @@ OBJS = boardd.o \
|
|||
|
||||
DEPS := $(OBJS:.o=.d)
|
||||
|
||||
boardd: $(OBJS)
|
||||
boardd: $(OBJS) $(MESSAGING_LIBS)
|
||||
@echo "[ LINK ] $@"
|
||||
$(CXX) -fPIC -o '$@' $^ \
|
||||
$(CEREAL_LIBS) \
|
||||
$(ZMQ_LIBS) \
|
||||
$(EXTRA_LIBS)
|
||||
|
||||
boardd.o: boardd.cc
|
||||
|
|
@ -72,6 +71,7 @@ boardd.o: boardd.cc
|
|||
$(CEREAL_CFLAGS) \
|
||||
$(CEREAL_CXXFLAGS) \
|
||||
$(ZMQ_FLAGS) \
|
||||
$(MESSAGING_FLAGS) \
|
||||
-I../ \
|
||||
-I../../ \
|
||||
-c -o '$@' '$<'
|
||||
|
|
@ -100,6 +100,7 @@ libcan_list_to_can_capnp.a: can_list_to_can_capnp.o $(CEREAL_OBJS)
|
|||
-Iinclude -I.. -I../.. \
|
||||
$(CEREAL_CXXFLAGS) \
|
||||
$(ZMQ_FLAGS) \
|
||||
$(MESSAGING_FLAGS) \
|
||||
-c -o '$@' '$<'
|
||||
|
||||
.PHONY: clean
|
||||
|
|
|
|||
|
|
@ -1,11 +1,12 @@
|
|||
#include <stdio.h>
|
||||
#include <time.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <signal.h>
|
||||
#include <unistd.h>
|
||||
#include <sched.h>
|
||||
#include <sys/time.h>
|
||||
#include <string.h>
|
||||
#include <sys/cdefs.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/time.h>
|
||||
|
|
@ -14,7 +15,6 @@
|
|||
#include <assert.h>
|
||||
#include <pthread.h>
|
||||
|
||||
#include <zmq.h>
|
||||
#include <libusb-1.0/libusb.h>
|
||||
|
||||
#include <capnp/serialize.h>
|
||||
|
|
@ -25,6 +25,7 @@
|
|||
#include "common/params.h"
|
||||
#include "common/swaglog.h"
|
||||
#include "common/timing.h"
|
||||
#include "messaging.hpp"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
|
|
@ -36,6 +37,16 @@ namespace {
|
|||
|
||||
volatile sig_atomic_t do_exit = 0;
|
||||
|
||||
struct __attribute__((packed)) timestamp_t {
|
||||
uint16_t year;
|
||||
uint8_t month;
|
||||
uint8_t day;
|
||||
uint8_t weekday;
|
||||
uint8_t hour;
|
||||
uint8_t minute;
|
||||
uint8_t second;
|
||||
};
|
||||
|
||||
libusb_context *ctx = NULL;
|
||||
libusb_device_handle *dev_handle;
|
||||
pthread_mutex_t usb_lock;
|
||||
|
|
@ -48,8 +59,7 @@ bool is_pigeon = false;
|
|||
const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 24 * 3; // turn off charge after 3 days
|
||||
uint32_t no_ignition_cnt = 0;
|
||||
bool connected_once = false;
|
||||
uint8_t ignition_last = 0;
|
||||
bool safety_model_locked = false;
|
||||
bool ignition_last = false;
|
||||
|
||||
pthread_t safety_setter_thread_handle = -1;
|
||||
pthread_t pigeon_thread_handle = -1;
|
||||
|
|
@ -76,11 +86,9 @@ void *safety_setter_thread(void *s) {
|
|||
LOGW("got CarVin %s", value_vin);
|
||||
|
||||
// VIN query done, stop listening to OBDII
|
||||
if (!safety_model_locked) {
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, NULL, 0, TIMEOUT);
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
}
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, NULL, 0, TIMEOUT);
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
|
||||
char *value;
|
||||
size_t value_sz = 0;
|
||||
|
|
@ -103,7 +111,7 @@ void *safety_setter_thread(void *s) {
|
|||
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
|
||||
|
||||
auto safety_model = car_params.getSafetyModel();
|
||||
int safety_model = int(car_params.getSafetyModel());
|
||||
auto safety_param = car_params.getSafetyParam();
|
||||
LOGW("setting safety model: %d with param %d", safety_model, safety_param);
|
||||
|
||||
|
|
@ -115,7 +123,7 @@ void *safety_setter_thread(void *s) {
|
|||
// set if long_control is allowed by openpilot. Hardcoded to True for now
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdf, 1, 0, NULL, 0, TIMEOUT);
|
||||
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel(safety_model)), safety_param, NULL, 0, TIMEOUT);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_model, safety_param, NULL, 0, TIMEOUT);
|
||||
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
|
||||
|
|
@ -126,12 +134,14 @@ void *safety_setter_thread(void *s) {
|
|||
bool usb_connect() {
|
||||
int err;
|
||||
unsigned char hw_query[1] = {0};
|
||||
char *value_safety_model;
|
||||
size_t value_safety_model_sz = 0;
|
||||
int safety_model;
|
||||
const int result = read_db_value(NULL, "SafetyModelLock", &value_safety_model, &value_safety_model_sz);
|
||||
unsigned char fw_ver_buf[64];
|
||||
unsigned char serial_buf[16];
|
||||
const char *fw_ver;
|
||||
const char *serial;
|
||||
int fw_ver_sz = 0;
|
||||
int serial_sz = 0;
|
||||
|
||||
ignition_last = 0;
|
||||
ignition_last = false;
|
||||
|
||||
dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
|
||||
if (dev_handle == NULL) { goto fail; }
|
||||
|
|
@ -146,15 +156,26 @@ bool usb_connect() {
|
|||
libusb_control_transfer(dev_handle, 0xc0, 0xe5, 1, 0, NULL, 0, TIMEOUT);
|
||||
}
|
||||
|
||||
// check if safety mode is forced (needed to support gm)
|
||||
if (value_safety_model_sz > 0) {
|
||||
sscanf(value_safety_model, "%d", &safety_model);
|
||||
// sanity check that we are not setting all output
|
||||
assert(safety_model != (int)(cereal::CarParams::SafetyModel::ALL_OUTPUT));
|
||||
safety_model_locked = true;
|
||||
LOGW("Setting Locked Safety Model %s", value_safety_model);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel(safety_model)), 0, NULL, 0, TIMEOUT);
|
||||
// get panda fw
|
||||
err = libusb_control_transfer(dev_handle, 0xc0, 0xd6, 0, 0, fw_ver_buf, 64, TIMEOUT);
|
||||
if (err > 0) {
|
||||
fw_ver = (const char *)fw_ver_buf;
|
||||
fw_ver_sz = err;
|
||||
write_db_value(NULL, "PandaFirmware", fw_ver, fw_ver_sz);
|
||||
printf("panda fw: %.*s\n", fw_ver_sz, fw_ver);
|
||||
}
|
||||
else { goto fail; }
|
||||
|
||||
// get panda serial
|
||||
err = libusb_control_transfer(dev_handle, 0xc0, 0xd0, 0, 0, serial_buf, 16, TIMEOUT);
|
||||
|
||||
if (err > 0) {
|
||||
serial = (const char *)serial_buf;
|
||||
serial_sz = strnlen(serial, err);
|
||||
write_db_value(NULL, "PandaDongleId", serial, serial_sz);
|
||||
printf("panda serial: %.*s\n", serial_sz, serial);
|
||||
}
|
||||
else { goto fail; }
|
||||
|
||||
// power off ESP
|
||||
libusb_control_transfer(dev_handle, 0xc0, 0xd9, 0, 0, NULL, 0, TIMEOUT);
|
||||
|
|
@ -182,6 +203,38 @@ bool usb_connect() {
|
|||
}
|
||||
}
|
||||
|
||||
if (hw_type == cereal::HealthData::HwType::UNO){
|
||||
// Get time from system
|
||||
time_t rawtime;
|
||||
time(&rawtime);
|
||||
|
||||
struct tm * sys_time = gmtime(&rawtime);
|
||||
|
||||
// Get time from RTC
|
||||
timestamp_t rtc_time;
|
||||
libusb_control_transfer(dev_handle, 0xc0, 0xa0, 0, 0, (unsigned char*)&rtc_time, sizeof(rtc_time), TIMEOUT);
|
||||
|
||||
//printf("System: %d-%d-%d\t%d:%d:%d\n", 1900 + sys_time->tm_year, 1 + sys_time->tm_mon, sys_time->tm_mday, sys_time->tm_hour, sys_time->tm_min, sys_time->tm_sec);
|
||||
//printf("RTC: %d-%d-%d\t%d:%d:%d\n", rtc_time.year, rtc_time.month, rtc_time.day, rtc_time.hour, rtc_time.minute, rtc_time.second);
|
||||
|
||||
// Update system time from RTC if it looks off, and RTC time is good
|
||||
if (1900 + sys_time->tm_year < 2019 && rtc_time.year >= 2019){
|
||||
LOGE("System time wrong, setting from RTC");
|
||||
|
||||
struct tm new_time = { 0 };
|
||||
new_time.tm_year = rtc_time.year - 1900;
|
||||
new_time.tm_mon = rtc_time.month - 1;
|
||||
new_time.tm_mday = rtc_time.day;
|
||||
new_time.tm_hour = rtc_time.hour;
|
||||
new_time.tm_min = rtc_time.minute;
|
||||
new_time.tm_sec = rtc_time.second;
|
||||
|
||||
setenv("TZ","UTC",1);
|
||||
const struct timeval tv = {mktime(&new_time), 0};
|
||||
settimeofday(&tv, 0);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
fail:
|
||||
return false;
|
||||
|
|
@ -202,7 +255,7 @@ void handle_usb_issue(int err, const char func[]) {
|
|||
// TODO: check other errors, is simply retrying okay?
|
||||
}
|
||||
|
||||
void can_recv(void *s) {
|
||||
void can_recv(PubSocket *publisher) {
|
||||
int err;
|
||||
uint32_t data[RECV_SIZE/4];
|
||||
int recv;
|
||||
|
|
@ -256,10 +309,10 @@ void can_recv(void *s) {
|
|||
// send to can
|
||||
auto words = capnp::messageToFlatArray(msg);
|
||||
auto bytes = words.asBytes();
|
||||
zmq_send(s, bytes.begin(), bytes.size(), 0);
|
||||
publisher->send((char*)bytes.begin(), bytes.size());
|
||||
}
|
||||
|
||||
void can_health(void *s) {
|
||||
void can_health(PubSocket *publisher) {
|
||||
int cnt;
|
||||
int err;
|
||||
|
||||
|
|
@ -270,11 +323,13 @@ void can_health(void *s) {
|
|||
uint32_t can_send_errs;
|
||||
uint32_t can_fwd_errs;
|
||||
uint32_t gmlan_send_errs;
|
||||
uint8_t started;
|
||||
uint8_t ignition_line;
|
||||
uint8_t ignition_can;
|
||||
uint8_t controls_allowed;
|
||||
uint8_t gas_interceptor_detected;
|
||||
uint8_t car_harness_status;
|
||||
uint8_t usb_power_mode;
|
||||
uint8_t safety_model;
|
||||
} health;
|
||||
|
||||
// recv from board
|
||||
|
|
@ -289,7 +344,9 @@ void can_health(void *s) {
|
|||
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
|
||||
if (health.started == 0) {
|
||||
bool ignition = ((health.ignition_line != 0) || (health.ignition_can != 0));
|
||||
|
||||
if (!ignition) {
|
||||
no_ignition_cnt += 1;
|
||||
} else {
|
||||
no_ignition_cnt = 0;
|
||||
|
|
@ -305,7 +362,7 @@ void can_health(void *s) {
|
|||
#endif
|
||||
|
||||
// clear VIN, CarParams, and set new safety on car start
|
||||
if ((health.started != 0) && (ignition_last == 0)) {
|
||||
if (ignition && !ignition_last) {
|
||||
|
||||
int result = delete_db_value(NULL, "CarVin");
|
||||
assert((result == 0) || (result == ERR_NO_VALUE));
|
||||
|
|
@ -313,18 +370,46 @@ void can_health(void *s) {
|
|||
assert((result == 0) || (result == ERR_NO_VALUE));
|
||||
|
||||
// diagnostic only is the default, needed for VIN query
|
||||
if (!safety_model_locked) {
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::ELM327), 0, NULL, 0, TIMEOUT);
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
}
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::ELM327), 0, NULL, 0, TIMEOUT);
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
|
||||
if (safety_setter_thread_handle == -1) {
|
||||
err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
|
||||
assert(err == 0);
|
||||
}
|
||||
}
|
||||
|
||||
ignition_last = health.started;
|
||||
// Get fan RPM
|
||||
uint16_t fan_speed_rpm = 0;
|
||||
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
int sz = libusb_control_transfer(dev_handle, 0xc0, 0xb2, 0, 0, (unsigned char*)&fan_speed_rpm, sizeof(fan_speed_rpm), TIMEOUT);
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
|
||||
// Write to rtc once per minute when no ignition present
|
||||
if ((hw_type == cereal::HealthData::HwType::UNO) && !ignition && (no_ignition_cnt % 120 == 1)){
|
||||
// Get time from system
|
||||
time_t rawtime;
|
||||
time(&rawtime);
|
||||
|
||||
struct tm * sys_time = gmtime(&rawtime);
|
||||
|
||||
// Write time to RTC if it looks reasonable
|
||||
if (1900 + sys_time->tm_year >= 2019){
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xa1, (uint16_t)(1900 + sys_time->tm_year), 0, NULL, 0, TIMEOUT);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xa2, (uint16_t)(1 + sys_time->tm_mon), 0, NULL, 0, TIMEOUT);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xa3, (uint16_t)sys_time->tm_mday, 0, NULL, 0, TIMEOUT);
|
||||
// libusb_control_transfer(dev_handle, 0x40, 0xa4, (uint16_t)(1 + sys_time->tm_wday), 0, NULL, 0, TIMEOUT);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xa5, (uint16_t)sys_time->tm_hour, 0, NULL, 0, TIMEOUT);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xa6, (uint16_t)sys_time->tm_min, 0, NULL, 0, TIMEOUT);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xa7, (uint16_t)sys_time->tm_sec, 0, NULL, 0, TIMEOUT);
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
}
|
||||
}
|
||||
|
||||
ignition_last = ignition;
|
||||
|
||||
// create message
|
||||
capnp::MallocMessageBuilder msg;
|
||||
|
|
@ -336,10 +421,11 @@ void can_health(void *s) {
|
|||
healthData.setVoltage(health.voltage);
|
||||
healthData.setCurrent(health.current);
|
||||
if (spoofing_started) {
|
||||
healthData.setStarted(1);
|
||||
healthData.setIgnitionLine(1);
|
||||
} else {
|
||||
healthData.setStarted(health.started);
|
||||
healthData.setIgnitionLine(health.ignition_line);
|
||||
}
|
||||
healthData.setIgnitionCan(health.ignition_can);
|
||||
healthData.setControlsAllowed(health.controls_allowed);
|
||||
healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
|
||||
healthData.setHasGps(is_pigeon);
|
||||
|
|
@ -348,11 +434,13 @@ void can_health(void *s) {
|
|||
healthData.setGmlanSendErrs(health.gmlan_send_errs);
|
||||
healthData.setHwType(hw_type);
|
||||
healthData.setUsbPowerMode(cereal::HealthData::UsbPowerMode(health.usb_power_mode));
|
||||
healthData.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_model));
|
||||
healthData.setFanSpeedRpm(fan_speed_rpm);
|
||||
|
||||
// send to health
|
||||
auto words = capnp::messageToFlatArray(msg);
|
||||
auto bytes = words.asBytes();
|
||||
zmq_send(s, bytes.begin(), bytes.size(), 0);
|
||||
publisher->send((char*)bytes.begin(), bytes.size());
|
||||
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
|
||||
|
|
@ -363,24 +451,20 @@ void can_health(void *s) {
|
|||
}
|
||||
|
||||
|
||||
void can_send(void *s) {
|
||||
void can_send(SubSocket *subscriber) {
|
||||
int err;
|
||||
|
||||
// recv from sendcan
|
||||
zmq_msg_t msg;
|
||||
zmq_msg_init(&msg);
|
||||
err = zmq_msg_recv(&msg, s, 0);
|
||||
assert(err >= 0);
|
||||
Message * msg = subscriber->receive();
|
||||
|
||||
// format for board, make copy due to alignment issues, will be freed on out of scope
|
||||
auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
|
||||
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), msg->getData(), msg->getSize());
|
||||
|
||||
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||
if (nanos_since_boot() - event.getLogMonoTime() > 1e9) {
|
||||
//Older than 1 second. Dont send.
|
||||
zmq_msg_close(&msg);
|
||||
delete msg;
|
||||
return;
|
||||
}
|
||||
int msg_count = event.getCan().size();
|
||||
|
|
@ -403,7 +487,7 @@ void can_send(void *s) {
|
|||
}
|
||||
|
||||
// release msg
|
||||
zmq_msg_close(&msg);
|
||||
delete msg;
|
||||
|
||||
// send to board
|
||||
int sent;
|
||||
|
|
@ -428,15 +512,17 @@ void *can_send_thread(void *crap) {
|
|||
LOGD("start send thread");
|
||||
|
||||
// sendcan = 8017
|
||||
void *context = zmq_ctx_new();
|
||||
void *subscriber = sub_sock(context, "tcp://127.0.0.1:8017");
|
||||
Context * context = Context::create();
|
||||
SubSocket * subscriber = SubSocket::create(context, "sendcan");
|
||||
|
||||
|
||||
// drain sendcan to delete any stale messages from previous runs
|
||||
zmq_msg_t msg;
|
||||
zmq_msg_init(&msg);
|
||||
int err = 0;
|
||||
while(err >= 0) {
|
||||
err = zmq_msg_recv(&msg, subscriber, ZMQ_DONTWAIT);
|
||||
while (true){
|
||||
Message * msg = subscriber->receive(true);
|
||||
if (msg == NULL){
|
||||
break;
|
||||
}
|
||||
delete msg;
|
||||
}
|
||||
|
||||
// run as fast as messages come in
|
||||
|
|
@ -450,9 +536,8 @@ void *can_recv_thread(void *crap) {
|
|||
LOGD("start recv thread");
|
||||
|
||||
// can = 8006
|
||||
void *context = zmq_ctx_new();
|
||||
void *publisher = zmq_socket(context, ZMQ_PUB);
|
||||
zmq_bind(publisher, "tcp://*:8006");
|
||||
Context * c = Context::create();
|
||||
PubSocket * publisher = PubSocket::create(c, "can");
|
||||
|
||||
// run at 100hz
|
||||
const uint64_t dt = 10000000ULL;
|
||||
|
|
@ -479,9 +564,8 @@ void *can_recv_thread(void *crap) {
|
|||
void *can_health_thread(void *crap) {
|
||||
LOGD("start health thread");
|
||||
// health = 8011
|
||||
void *context = zmq_ctx_new();
|
||||
void *publisher = zmq_socket(context, ZMQ_PUB);
|
||||
zmq_bind(publisher, "tcp://*:8011");
|
||||
Context * c = Context::create();
|
||||
PubSocket * publisher = PubSocket::create(c, "health");
|
||||
|
||||
// run at 2hz
|
||||
while (!do_exit) {
|
||||
|
|
@ -491,6 +575,69 @@ void *can_health_thread(void *crap) {
|
|||
return NULL;
|
||||
}
|
||||
|
||||
void *hardware_control_thread(void *crap) {
|
||||
LOGD("start hardware control thread");
|
||||
Context * c = Context::create();
|
||||
SubSocket * thermal_sock = SubSocket::create(c, "thermal");
|
||||
SubSocket * driver_monitoring_sock = SubSocket::create(c, "driverMonitoring");
|
||||
|
||||
Poller * poller = Poller::create({thermal_sock, driver_monitoring_sock});
|
||||
|
||||
// Wait for hardware type to be set.
|
||||
while (hw_type == cereal::HealthData::HwType::UNKNOWN){
|
||||
usleep(100*1000);
|
||||
}
|
||||
// Only control fan speed on UNO
|
||||
if (hw_type != cereal::HealthData::HwType::UNO) return NULL;
|
||||
|
||||
|
||||
uint16_t prev_fan_speed = 999;
|
||||
uint16_t prev_ir_pwr = 999;
|
||||
|
||||
while (!do_exit) {
|
||||
for (auto sock : poller->poll(1000)){
|
||||
Message * msg = sock->receive();
|
||||
if (msg == NULL) continue;
|
||||
|
||||
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), msg->getData(), msg->getSize());
|
||||
|
||||
delete msg;
|
||||
|
||||
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||
|
||||
auto type = event.which();
|
||||
if(type == cereal::Event::THERMAL){
|
||||
uint16_t fan_speed = event.getThermal().getFanSpeed();
|
||||
if (fan_speed != prev_fan_speed){
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xb1, fan_speed, 0, NULL, 0, TIMEOUT);
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
|
||||
prev_fan_speed = fan_speed;
|
||||
}
|
||||
} else if (type == cereal::Event::DRIVER_MONITORING){
|
||||
uint16_t ir_pwr = 100.0 * event.getDriverMonitoring().getIrPwr();
|
||||
|
||||
if (ir_pwr != prev_ir_pwr){
|
||||
pthread_mutex_lock(&usb_lock);
|
||||
libusb_control_transfer(dev_handle, 0x40, 0xb0, ir_pwr, 0, NULL, 0, TIMEOUT);
|
||||
pthread_mutex_unlock(&usb_lock);
|
||||
|
||||
prev_ir_pwr = ir_pwr;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
delete poller;
|
||||
delete thermal_sock;
|
||||
delete c;
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
#define pigeon_send(x) _pigeon_send(x, sizeof(x)-1)
|
||||
|
||||
void hexdump(unsigned char *d, int l) {
|
||||
|
|
@ -582,7 +729,7 @@ void pigeon_init() {
|
|||
LOGW("panda GPS on");
|
||||
}
|
||||
|
||||
static void pigeon_publish_raw(void *publisher, unsigned char *dat, int alen) {
|
||||
static void pigeon_publish_raw(PubSocket *publisher, unsigned char *dat, int alen) {
|
||||
// create message
|
||||
capnp::MallocMessageBuilder msg;
|
||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
||||
|
|
@ -593,15 +740,14 @@ static void pigeon_publish_raw(void *publisher, unsigned char *dat, int alen) {
|
|||
// send to ubloxRaw
|
||||
auto words = capnp::messageToFlatArray(msg);
|
||||
auto bytes = words.asBytes();
|
||||
zmq_send(publisher, bytes.begin(), bytes.size(), 0);
|
||||
publisher->send((char*)bytes.begin(), bytes.size());
|
||||
}
|
||||
|
||||
|
||||
void *pigeon_thread(void *crap) {
|
||||
// ubloxRaw = 8042
|
||||
void *context = zmq_ctx_new();
|
||||
void *publisher = zmq_socket(context, ZMQ_PUB);
|
||||
zmq_bind(publisher, "tcp://*:8042");
|
||||
Context * context = Context::create();
|
||||
PubSocket * publisher = PubSocket::create(context, "ubloxRaw");
|
||||
|
||||
// run at ~100hz
|
||||
unsigned char dat[0x1000];
|
||||
|
|
@ -695,6 +841,11 @@ int main() {
|
|||
can_recv_thread, NULL);
|
||||
assert(err == 0);
|
||||
|
||||
pthread_t hardware_control_thread_handle;
|
||||
err = pthread_create(&hardware_control_thread_handle, NULL,
|
||||
hardware_control_thread, NULL);
|
||||
assert(err == 0);
|
||||
|
||||
// join threads
|
||||
|
||||
err = pthread_join(can_recv_thread_handle, NULL);
|
||||
|
|
|
|||
|
|
@ -12,7 +12,6 @@ import time
|
|||
|
||||
import selfdrive.messaging as messaging
|
||||
from common.realtime import Ratekeeper
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.swaglog import cloudlog
|
||||
from selfdrive.boardd.boardd import can_capnp_to_can_list
|
||||
from cereal import car
|
||||
|
|
@ -46,13 +45,13 @@ def can_list_to_can_capnp(can_msgs, msgtype='can'):
|
|||
def can_health():
|
||||
while 1:
|
||||
try:
|
||||
dat = handle.controlRead(usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE, 0xd2, 0, 0, 0x10)
|
||||
dat = handle.controlRead(usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE, 0xd2, 0, 0, 0x16)
|
||||
break
|
||||
except (USBErrorIO, USBErrorOverflow):
|
||||
cloudlog.exception("CAN: BAD HEALTH, RETRYING")
|
||||
v, i, started = struct.unpack("IIB", dat[0:9])
|
||||
# TODO: units
|
||||
return {"voltage": v, "current": i, "started": bool(started)}
|
||||
v, i = struct.unpack("II", dat[0:8])
|
||||
ign_line, ign_can = struct.unpack("BB", dat[20:22])
|
||||
return {"voltage": v, "current": i, "ignition_line": bool(ign_line), "ignition_can": bool(ign_can)}
|
||||
|
||||
def __parse_can_buffer(dat):
|
||||
ret = []
|
||||
|
|
@ -111,8 +110,8 @@ def boardd_mock_loop():
|
|||
can_init()
|
||||
handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'')
|
||||
|
||||
logcan = messaging.sub_sock(service_list['can'].port)
|
||||
sendcan = messaging.pub_sock(service_list['sendcan'].port)
|
||||
logcan = messaging.sub_sock('can')
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
while 1:
|
||||
tsc = messaging.drain_sock(logcan, wait_for_one=True)
|
||||
|
|
@ -150,24 +149,24 @@ def boardd_test_loop():
|
|||
cnt += 1
|
||||
|
||||
# *** main loop ***
|
||||
def boardd_loop(rate=200):
|
||||
def boardd_loop(rate=100):
|
||||
rk = Ratekeeper(rate)
|
||||
|
||||
can_init()
|
||||
|
||||
# *** publishes can and health
|
||||
logcan = messaging.pub_sock(service_list['can'].port)
|
||||
health_sock = messaging.pub_sock(service_list['health'].port)
|
||||
logcan = messaging.pub_sock('can')
|
||||
health_sock = messaging.pub_sock('health')
|
||||
|
||||
# *** subscribes to can send
|
||||
sendcan = messaging.sub_sock(service_list['sendcan'].port)
|
||||
sendcan = messaging.sub_sock('sendcan')
|
||||
|
||||
# drain sendcan to delete any stale messages from previous runs
|
||||
messaging.drain_sock(sendcan)
|
||||
|
||||
while 1:
|
||||
# health packet @ 1hz
|
||||
if (rk.frame%rate) == 0:
|
||||
# health packet @ 2hz
|
||||
if (rk.frame % (rate // 2)) == 0:
|
||||
health = can_health()
|
||||
msg = messaging.new_message()
|
||||
msg.init('health')
|
||||
|
|
@ -175,7 +174,8 @@ def boardd_loop(rate=200):
|
|||
# store the health to be logged
|
||||
msg.health.voltage = health['voltage']
|
||||
msg.health.current = health['current']
|
||||
msg.health.started = health['started']
|
||||
msg.health.ignitionLine = health['ignition_line']
|
||||
msg.health.ignitionCan = health['ignition_can']
|
||||
msg.health.controlsAllowed = True
|
||||
|
||||
health_sock.send(msg.to_bytes())
|
||||
|
|
@ -197,15 +197,15 @@ def boardd_loop(rate=200):
|
|||
rk.keep_time()
|
||||
|
||||
# *** main loop ***
|
||||
def boardd_proxy_loop(rate=200, address="192.168.2.251"):
|
||||
def boardd_proxy_loop(rate=100, address="192.168.2.251"):
|
||||
rk = Ratekeeper(rate)
|
||||
|
||||
can_init()
|
||||
|
||||
# *** subscribes can
|
||||
logcan = messaging.sub_sock(service_list['can'].port, addr=address)
|
||||
logcan = messaging.sub_sock('can', addr=address)
|
||||
# *** publishes to can send
|
||||
sendcan = messaging.pub_sock(service_list['sendcan'].port)
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
|
||||
# drain sendcan to delete any stale messages from previous runs
|
||||
messaging.drain_sock(sendcan)
|
||||
|
|
|
|||
|
|
@ -2,34 +2,37 @@
|
|||
import sys
|
||||
import time
|
||||
import signal
|
||||
import traceback
|
||||
from panda import Panda
|
||||
from multiprocessing import Pool
|
||||
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.boardd.boardd import can_capnp_to_can_list
|
||||
|
||||
def initializer():
|
||||
"""Ignore CTRL+C in the worker process.
|
||||
source: https://stackoverflow.com/a/44869451 """
|
||||
signal.signal(signal.SIGINT, signal.SIG_IGN)
|
||||
"""Ignore CTRL+C in the worker process.
|
||||
source: https://stackoverflow.com/a/44869451 """
|
||||
signal.signal(signal.SIGINT, signal.SIG_IGN)
|
||||
|
||||
def send_thread(serial):
|
||||
panda = Panda(serial)
|
||||
panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
|
||||
panda.set_can_loopback(False)
|
||||
try:
|
||||
panda = Panda(serial)
|
||||
panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
|
||||
panda.set_can_loopback(False)
|
||||
|
||||
can_sock = messaging.sub_sock(service_list['can'].port)
|
||||
can_sock = messaging.sub_sock('can')
|
||||
|
||||
while True:
|
||||
# Send messages one bus 0 and 1
|
||||
tsc = messaging.recv_one(can_sock)
|
||||
snd = can_capnp_to_can_list(tsc.can)
|
||||
snd = filter(lambda x: x[-1] <= 2, snd)
|
||||
panda.can_send_many(snd)
|
||||
while True:
|
||||
# Send messages one bus 0 and 1
|
||||
tsc = messaging.recv_one(can_sock)
|
||||
snd = can_capnp_to_can_list(tsc.can)
|
||||
snd = list(filter(lambda x: x[-1] <= 2, snd))
|
||||
panda.can_send_many(snd)
|
||||
|
||||
# Drain panda message buffer
|
||||
panda.can_recv()
|
||||
# Drain panda message buffer
|
||||
panda.can_recv()
|
||||
except Exception:
|
||||
traceback.print_exc()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
|
|
|||
|
|
@ -7,7 +7,6 @@ import time
|
|||
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from selfdrive.messaging import drain_sock, pub_sock, sub_sock
|
||||
from selfdrive.services import service_list
|
||||
|
||||
def get_test_string():
|
||||
return b"test"+os.urandom(10)
|
||||
|
|
@ -15,8 +14,8 @@ def get_test_string():
|
|||
BUS = 0
|
||||
|
||||
def main():
|
||||
rcv = sub_sock(service_list['can'].port) # port 8006
|
||||
snd = pub_sock(service_list['sendcan'].port) # port 8017
|
||||
rcv = sub_sock('can') # port 8006
|
||||
snd = pub_sock('sendcan') # port 8017
|
||||
time.sleep(0.3) # wait to bind before send/recv
|
||||
|
||||
for i in range(10):
|
||||
|
|
|
|||
|
|
@ -23,17 +23,6 @@ CFLAGS += -mcpu=cortex-a57
|
|||
CXXFLAGS += -mcpu=cortex-a57
|
||||
endif
|
||||
|
||||
ifeq ($(UNAME_S),Darwin)
|
||||
ZMQ_LIBS = -L/usr/local/lib -lzmq
|
||||
else ifeq ($(OPTEST),1)
|
||||
ZMQ_LIBS = -lzmq
|
||||
else ifeq ($(UNAME_M),x86_64)
|
||||
ZMQ_FLAGS = -I$(PHONELIBS)/zmq/x64/include
|
||||
ZMQ_LIBS = -L$(PHONELIBS)/zmq/x64/lib -l:libzmq.a
|
||||
else ifeq ($(UNAME_M),aarch64)
|
||||
ZMQ_LIBS = -l:libzmq.a -lgnustl_shared
|
||||
endif
|
||||
|
||||
OBJDIR = obj
|
||||
|
||||
OPENDBC_PATH := $(shell python3 -c 'import opendbc; print(opendbc.DBC_PATH)')
|
||||
|
|
@ -43,7 +32,7 @@ DBC_OBJS := $(patsubst $(OPENDBC_PATH)/%.dbc,$(OBJDIR)/%.o,$(DBC_SOURCES))
|
|||
DBC_CCS := $(patsubst $(OPENDBC_PATH)/%.dbc,dbc_out/%.cc,$(DBC_SOURCES))
|
||||
.SECONDARY: $(DBC_CCS)
|
||||
|
||||
LIBDBC_OBJS := $(OBJDIR)/dbc.o $(OBJDIR)/parser.o $(OBJDIR)/packer.o
|
||||
LIBDBC_OBJS := $(OBJDIR)/dbc.o $(OBJDIR)/parser.o $(OBJDIR)/packer.o $(OBJDIR)/common.o
|
||||
|
||||
CWD := $(shell pwd)
|
||||
|
||||
|
|
@ -64,8 +53,6 @@ libdbc.so:: $(LIBDBC_OBJS) $(DBC_OBJS)
|
|||
-I. -I../.. \
|
||||
$(CXXFLAGS) \
|
||||
$(LDFLAGS) \
|
||||
$(ZMQ_FLAGS) \
|
||||
$(ZMQ_LIBS) \
|
||||
$(CEREAL_CXXFLAGS) \
|
||||
$(CEREAL_LIBS)
|
||||
|
||||
|
|
@ -74,8 +61,8 @@ packer_impl.so: packer_impl.pyx packer_setup.py
|
|||
rm -rf build
|
||||
rm -f packer_impl.cpp
|
||||
|
||||
parser_pyx.so: parser_pyx_setup.py parser_pyx.pyx parser_pyx.pxd
|
||||
python3 $< build_ext --inplace
|
||||
parser_pyx.so: libdbc.so parser_pyx_setup.py parser_pyx.pyx common.pxd
|
||||
python3 parser_pyx_setup.py build_ext --inplace
|
||||
rm -rf build
|
||||
rm -f parser_pyx.cpp
|
||||
|
||||
|
|
@ -84,7 +71,6 @@ $(OBJDIR)/%.o: %.cc
|
|||
$(CXX) -fPIC -c -o '$@' $^ \
|
||||
-I. -I../.. \
|
||||
$(CXXFLAGS) \
|
||||
$(ZMQ_FLAGS) \
|
||||
$(CEREAL_CXXFLAGS) \
|
||||
|
||||
$(OBJDIR)/%.o: dbc_out/%.cc
|
||||
|
|
@ -92,13 +78,11 @@ $(OBJDIR)/%.o: dbc_out/%.cc
|
|||
$(CXX) -fPIC -c -o '$@' $^ \
|
||||
-I. -I../.. \
|
||||
$(CXXFLAGS) \
|
||||
$(ZMQ_FLAGS) \
|
||||
$(CEREAL_CXXFLAGS) \
|
||||
|
||||
dbc_out/%.cc: process_dbc.py dbc_template.cc $(OPENDBC_PATH)/%.dbc
|
||||
@echo "[ DBC GEN ] $@"
|
||||
@echo "Missing prereq $?"
|
||||
./process_dbc.py $(OPENDBC_PATH) dbc_out
|
||||
./process_dbc.py $(OPENDBC_PATH) '$@'
|
||||
|
||||
$(OBJDIR):
|
||||
mkdir -p $@
|
||||
|
|
@ -109,4 +93,5 @@ clean:
|
|||
rm -f dbc_out/*.cc
|
||||
rm -f dbcs.txt
|
||||
rm -f dbcs.csv
|
||||
rm -f *.so
|
||||
rm -rf $(OBJDIR)/*
|
||||
|
|
|
|||
|
|
@ -0,0 +1,165 @@
|
|||
#include "common.h"
|
||||
|
||||
unsigned int honda_checksum(unsigned int address, uint64_t d, int l) {
|
||||
d >>= ((8-l)*8); // remove padding
|
||||
d >>= 4; // remove checksum
|
||||
|
||||
int s = 0;
|
||||
while (address) { s += (address & 0xF); address >>= 4; }
|
||||
while (d) { s += (d & 0xF); d >>= 4; }
|
||||
s = 8-s;
|
||||
s &= 0xF;
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
unsigned int toyota_checksum(unsigned int address, uint64_t d, int l) {
|
||||
d >>= ((8-l)*8); // remove padding
|
||||
d >>= 8; // remove checksum
|
||||
|
||||
unsigned int s = l;
|
||||
while (address) { s += address & 0xff; address >>= 8; }
|
||||
while (d) { s += d & 0xff; d >>= 8; }
|
||||
|
||||
return s & 0xFF;
|
||||
}
|
||||
|
||||
// Static lookup table for fast computation of CRC8 poly 0x2F, aka 8H2F/AUTOSAR
|
||||
uint8_t crc8_lut_8h2f[256];
|
||||
|
||||
void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]) {
|
||||
uint8_t crc;
|
||||
int i, j;
|
||||
|
||||
for (i = 0; i < 256; i++) {
|
||||
crc = i;
|
||||
for (j = 0; j < 8; j++) {
|
||||
if ((crc & 0x80) != 0)
|
||||
crc = (uint8_t)((crc << 1) ^ poly);
|
||||
else
|
||||
crc <<= 1;
|
||||
}
|
||||
crc_lut[i] = crc;
|
||||
}
|
||||
}
|
||||
|
||||
void init_crc_lookup_tables() {
|
||||
// At init time, set up static lookup tables for fast CRC computation.
|
||||
|
||||
gen_crc_lookup_table(0x2F, crc8_lut_8h2f); // CRC-8 8H2F/AUTOSAR for Volkswagen
|
||||
}
|
||||
|
||||
unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) {
|
||||
// Volkswagen uses standard CRC8 8H2F/AUTOSAR, but they compute it with
|
||||
// a magic variable padding byte tacked onto the end of the payload.
|
||||
// https://www.autosar.org/fileadmin/user_upload/standards/classic/4-3/AUTOSAR_SWS_CRCLibrary.pdf
|
||||
|
||||
uint8_t *dat = (uint8_t *)&d;
|
||||
uint8_t crc = 0xFF; // Standard init value for CRC8 8H2F/AUTOSAR
|
||||
|
||||
// CRC the payload first, skipping over the first byte where the CRC lives.
|
||||
for (int i = 1; i < l; i++) {
|
||||
crc ^= dat[i];
|
||||
crc = crc8_lut_8h2f[crc];
|
||||
}
|
||||
|
||||
// Look up and apply the magic final CRC padding byte, which permutes by CAN
|
||||
// address, and additionally (for SOME addresses) by the message counter.
|
||||
uint8_t counter = dat[1] & 0x0F;
|
||||
switch(address) {
|
||||
case 0x86: // LWI_01 Steering Angle
|
||||
crc ^= (uint8_t[]){0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86}[counter];
|
||||
break;
|
||||
case 0x9F: // EPS_01 Electric Power Steering
|
||||
crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter];
|
||||
break;
|
||||
case 0xAD: // Getriebe_11 Automatic Gearbox
|
||||
crc ^= (uint8_t[]){0x3F,0x69,0x39,0xDC,0x94,0xF9,0x14,0x64,0xD8,0x6A,0x34,0xCE,0xA2,0x55,0xB5,0x2C}[counter];
|
||||
break;
|
||||
case 0xFD: // ESP_21 Electronic Stability Program
|
||||
crc ^= (uint8_t[]){0xB4,0xEF,0xF8,0x49,0x1E,0xE5,0xC2,0xC0,0x97,0x19,0x3C,0xC9,0xF1,0x98,0xD6,0x61}[counter];
|
||||
break;
|
||||
case 0x106: // ESP_05 Electronic Stability Program
|
||||
crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter];
|
||||
break;
|
||||
case 0x117: // ACC_10 Automatic Cruise Control
|
||||
crc ^= (uint8_t[]){0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC}[counter];
|
||||
break;
|
||||
case 0x122: // ACC_06 Automatic Cruise Control
|
||||
crc ^= (uint8_t[]){0x37,0x7D,0xF3,0xA9,0x18,0x46,0x6D,0x4D,0x3D,0x71,0x92,0x9C,0xE5,0x32,0x10,0xB9}[counter];
|
||||
break;
|
||||
case 0x126: // HCA_01 Heading Control Assist
|
||||
crc ^= (uint8_t[]){0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA}[counter];
|
||||
break;
|
||||
case 0x12B: // GRA_ACC_01 Steering wheel controls for ACC
|
||||
crc ^= (uint8_t[]){0x6A,0x38,0xB4,0x27,0x22,0xEF,0xE1,0xBB,0xF8,0x80,0x84,0x49,0xC7,0x9E,0x1E,0x2B}[counter];
|
||||
break;
|
||||
case 0x187: // EV_Gearshift "Gear" selection data for EVs with no gearbox
|
||||
crc ^= (uint8_t[]){0x7F,0xED,0x17,0xC2,0x7C,0xEB,0x44,0x21,0x01,0xFA,0xDB,0x15,0x4A,0x6B,0x23,0x05}[counter];
|
||||
break;
|
||||
case 0x30C: // ACC_02 Automatic Cruise Control
|
||||
crc ^= (uint8_t[]){0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F}[counter];
|
||||
break;
|
||||
case 0x3C0: // Klemmen_Status_01 ignition and starting status
|
||||
crc ^= (uint8_t[]){0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3}[counter];
|
||||
break;
|
||||
case 0x65D: // ESP_20 Electronic Stability Program
|
||||
crc ^= (uint8_t[]){0xAC,0xB3,0xAB,0xEB,0x7A,0xE1,0x3B,0xF7,0x73,0xBA,0x7C,0x9E,0x06,0x5F,0x02,0xD9}[counter];
|
||||
break;
|
||||
default: // As-yet undefined CAN message, CRC check expected to fail
|
||||
printf("Attempt to CRC check undefined Volkswagen message 0x%02X\n", address);
|
||||
crc ^= (uint8_t[]){0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}[counter];
|
||||
break;
|
||||
}
|
||||
crc = crc8_lut_8h2f[crc];
|
||||
|
||||
return crc ^ 0xFF; // Return after standard final XOR for CRC8 8H2F/AUTOSAR
|
||||
}
|
||||
|
||||
|
||||
unsigned int pedal_checksum(uint64_t d, int l) {
|
||||
uint8_t crc = 0xFF;
|
||||
uint8_t poly = 0xD5; // standard crc8
|
||||
|
||||
d >>= ((8-l)*8); // remove padding
|
||||
d >>= 8; // remove checksum
|
||||
|
||||
uint8_t *dat = (uint8_t *)&d;
|
||||
|
||||
int i, j;
|
||||
for (i = 0; i < l - 1; i++) {
|
||||
crc ^= dat[i];
|
||||
for (j = 0; j < 8; j++) {
|
||||
if ((crc & 0x80) != 0) {
|
||||
crc = (uint8_t)((crc << 1) ^ poly);
|
||||
}
|
||||
else {
|
||||
crc <<= 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
|
||||
uint64_t read_u64_be(const uint8_t* v) {
|
||||
return (((uint64_t)v[0] << 56)
|
||||
| ((uint64_t)v[1] << 48)
|
||||
| ((uint64_t)v[2] << 40)
|
||||
| ((uint64_t)v[3] << 32)
|
||||
| ((uint64_t)v[4] << 24)
|
||||
| ((uint64_t)v[5] << 16)
|
||||
| ((uint64_t)v[6] << 8)
|
||||
| (uint64_t)v[7]);
|
||||
}
|
||||
|
||||
uint64_t read_u64_le(const uint8_t* v) {
|
||||
return ((uint64_t)v[0]
|
||||
| ((uint64_t)v[1] << 8)
|
||||
| ((uint64_t)v[2] << 16)
|
||||
| ((uint64_t)v[3] << 24)
|
||||
| ((uint64_t)v[4] << 32)
|
||||
| ((uint64_t)v[5] << 40)
|
||||
| ((uint64_t)v[6] << 48)
|
||||
| ((uint64_t)v[7] << 56));
|
||||
}
|
||||
|
|
@ -4,22 +4,29 @@
|
|||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include <capnp/serialize.h>
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
#define MAX_BAD_COUNTER 5
|
||||
#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
|
||||
|
||||
// Helper functions
|
||||
unsigned int honda_checksum(unsigned int address, uint64_t d, int l);
|
||||
unsigned int toyota_checksum(unsigned int address, uint64_t d, int l);
|
||||
unsigned int pedal_checksum(unsigned int address, uint64_t d, int l);
|
||||
|
||||
void init_crc_lookup_tables();
|
||||
unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l);
|
||||
unsigned int pedal_checksum(uint64_t d, int l);
|
||||
uint64_t read_u64_be(const uint8_t* v);
|
||||
uint64_t read_u64_le(const uint8_t* v);
|
||||
|
||||
struct SignalPackValue {
|
||||
const char* name;
|
||||
double value;
|
||||
};
|
||||
|
||||
|
||||
struct SignalParseOptions {
|
||||
uint32_t address;
|
||||
const char* name;
|
||||
|
|
@ -82,6 +89,50 @@ struct DBC {
|
|||
size_t num_vals;
|
||||
};
|
||||
|
||||
|
||||
class MessageState {
|
||||
public:
|
||||
uint32_t address;
|
||||
unsigned int size;
|
||||
|
||||
std::vector<Signal> parse_sigs;
|
||||
std::vector<double> vals;
|
||||
|
||||
uint16_t ts;
|
||||
uint64_t seen;
|
||||
uint64_t check_threshold;
|
||||
|
||||
uint8_t counter;
|
||||
uint8_t counter_fail;
|
||||
|
||||
bool parse(uint64_t sec, uint16_t ts_, uint8_t * dat);
|
||||
bool update_counter_generic(int64_t v, int cnt_size);
|
||||
};
|
||||
|
||||
class CANParser {
|
||||
private:
|
||||
const int bus;
|
||||
|
||||
const DBC *dbc = NULL;
|
||||
std::unordered_map<uint32_t, MessageState> message_states;
|
||||
|
||||
public:
|
||||
bool can_valid = false;
|
||||
uint64_t last_sec = 0;
|
||||
|
||||
CANParser(int abus, const std::string& dbc_name,
|
||||
const std::vector<MessageParseOptions> &options,
|
||||
const std::vector<SignalParseOptions> &sigoptions);
|
||||
void UpdateCans(uint64_t sec, const capnp::List<cereal::CanData>::Reader& cans);
|
||||
void UpdateValid(uint64_t sec);
|
||||
void update_string(std::string data);
|
||||
std::vector<SignalValue> query_latest();
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
const DBC* dbc_lookup(const std::string& dbc_name);
|
||||
|
||||
void dbc_register(const DBC* dbc);
|
||||
|
|
|
|||
|
|
@ -0,0 +1,71 @@
|
|||
# distutils: language = c++
|
||||
#cython: language_level=3
|
||||
|
||||
from libc.stdint cimport uint32_t, uint64_t, uint16_t
|
||||
from libcpp.vector cimport vector
|
||||
from libcpp.map cimport map
|
||||
from libcpp.string cimport string
|
||||
from libcpp.unordered_set cimport unordered_set
|
||||
from libcpp cimport bool
|
||||
|
||||
|
||||
cdef extern from "common.h":
|
||||
|
||||
ctypedef enum SignalType:
|
||||
DEFAULT,
|
||||
HONDA_CHECKSUM,
|
||||
HONDA_COUNTER,
|
||||
TOYOTA_CHECKSUM,
|
||||
PEDAL_CHECKSUM,
|
||||
PEDAL_COUNTER
|
||||
|
||||
cdef struct Signal:
|
||||
const char* name
|
||||
int b1, b2, bo
|
||||
bool is_signed
|
||||
double factor, offset
|
||||
SignalType type
|
||||
|
||||
cdef struct Msg:
|
||||
const char* name
|
||||
uint32_t address
|
||||
unsigned int size
|
||||
size_t num_sigs
|
||||
const Signal *sigs
|
||||
|
||||
cdef struct Val:
|
||||
const char* name
|
||||
uint32_t address
|
||||
const char* def_val
|
||||
const Signal *sigs
|
||||
|
||||
cdef struct DBC:
|
||||
const char* name
|
||||
size_t num_msgs
|
||||
const Msg *msgs
|
||||
const Val *vals
|
||||
size_t num_vals
|
||||
|
||||
cdef struct SignalParseOptions:
|
||||
uint32_t address
|
||||
const char* name
|
||||
double default_value
|
||||
|
||||
|
||||
cdef struct MessageParseOptions:
|
||||
uint32_t address
|
||||
int check_frequency
|
||||
|
||||
cdef struct SignalValue:
|
||||
uint32_t address
|
||||
uint16_t ts
|
||||
const char* name
|
||||
double value
|
||||
|
||||
cdef const DBC* dbc_lookup(const string);
|
||||
|
||||
cdef cppclass CANParser:
|
||||
bool can_valid
|
||||
CANParser(int, string, vector[MessageParseOptions], vector[SignalParseOptions])
|
||||
void update_string(string)
|
||||
vector[SignalValue] query_latest()
|
||||
|
|
@ -27,14 +27,14 @@ const Signal sigs_{{address}}[] = {
|
|||
.type = SignalType::HONDA_COUNTER,
|
||||
{% elif checksum_type == "toyota" and sig.name == "CHECKSUM" %}
|
||||
.type = SignalType::TOYOTA_CHECKSUM,
|
||||
{% elif checksum_type == "volkswagen" and sig.name == "CHECKSUM" %}
|
||||
.type = SignalType::VOLKSWAGEN_CHECKSUM,
|
||||
{% elif checksum_type == "volkswagen" and sig.name == "COUNTER" %}
|
||||
.type = SignalType::VOLKSWAGEN_COUNTER,
|
||||
{% elif address in [512, 513] and sig.name == "CHECKSUM_PEDAL" %}
|
||||
.type = SignalType::PEDAL_CHECKSUM,
|
||||
{% elif address in [512, 513] and sig.name == "COUNTER_PEDAL" %}
|
||||
.type = SignalType::PEDAL_COUNTER,
|
||||
{% elif checksum_type == "volkswagen" and sig.name == "CHECKSUM" %}
|
||||
.type = SignalType::VOLKSWAGEN_CHECKSUM,
|
||||
{% elif checksum_type == "volkswagen" and sig.name == "COUNTER" %}
|
||||
.type = SignalType::VOLKSWAGEN_COUNTER,
|
||||
{% else %}
|
||||
.type = SignalType::DEFAULT,
|
||||
{% endif %}
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@ from cffi import FFI
|
|||
|
||||
can_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
libdbc_fn = os.path.join(can_dir, "libdbc.so")
|
||||
subprocess.check_call(["make"], cwd=can_dir)
|
||||
subprocess.check_call(["make", "-j3"], cwd=can_dir) # don't use all the cores to avoid overheating
|
||||
|
||||
ffi = FFI()
|
||||
ffi.cdef("""
|
||||
|
|
@ -79,10 +79,9 @@ typedef struct {
|
|||
|
||||
void* can_init(int bus, const char* dbc_name,
|
||||
size_t num_message_options, const MessageParseOptions* message_options,
|
||||
size_t num_signal_options, const SignalParseOptions* signal_options, bool sendcan,
|
||||
const char* tcp_addr, int timeout);
|
||||
size_t num_signal_options, const SignalParseOptions* signal_options);
|
||||
|
||||
int can_update(void* can, uint64_t sec, bool wait);
|
||||
void can_update_string(void *can, const char* dat, int len);
|
||||
|
||||
size_t can_query_latest(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
|
||||
|
||||
|
|
|
|||
|
|
@ -51,7 +51,6 @@ namespace {
|
|||
signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig;
|
||||
}
|
||||
}
|
||||
|
||||
init_crc_lookup_tables();
|
||||
}
|
||||
|
||||
|
|
@ -97,12 +96,10 @@ namespace {
|
|||
if (sig.type == SignalType::HONDA_CHECKSUM) {
|
||||
unsigned int chksm = honda_checksum(address, ret, message_lookup[address].size);
|
||||
ret = set_value(ret, sig, chksm);
|
||||
}
|
||||
else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
|
||||
} else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
|
||||
unsigned int chksm = toyota_checksum(address, ret, message_lookup[address].size);
|
||||
ret = set_value(ret, sig, chksm);
|
||||
}
|
||||
else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) {
|
||||
} else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) {
|
||||
// FIXME: Hackish fix for an endianness issue. The message is in reverse byte order
|
||||
// until later in the pack process. Checksums can be run backwards, CRCs not so much.
|
||||
// The correct fix is unclear but this works for the moment.
|
||||
|
|
@ -117,7 +114,6 @@ namespace {
|
|||
}
|
||||
|
||||
|
||||
|
||||
private:
|
||||
const DBC *dbc = NULL;
|
||||
std::map<std::pair<uint32_t, std::string>, Signal> signal_lookup;
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@ import os
|
|||
import subprocess
|
||||
|
||||
can_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
subprocess.check_call(["make", "packer_impl.so"], cwd=can_dir)
|
||||
subprocess.check_call(["make", "-j3", "packer_impl.so"], cwd=can_dir) # don't use all the cores to avoid overheating
|
||||
|
||||
from selfdrive.can.packer_impl import CANPacker
|
||||
assert CANPacker
|
||||
|
|
|
|||
|
|
@ -9,11 +9,7 @@
|
|||
#include <sys/mman.h>
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <algorithm>
|
||||
#include <unordered_map>
|
||||
|
||||
#include <zmq.h>
|
||||
|
||||
#include <capnp/serialize.h>
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
|
@ -24,661 +20,228 @@
|
|||
// #define DEBUG printf
|
||||
#define INFO printf
|
||||
|
||||
#define MAX_BAD_COUNTER 5
|
||||
|
||||
// Static lookup table for fast computation of CRC8 poly 0x2F, aka 8H2F/AUTOSAR
|
||||
uint8_t crc8_lut_8h2f[256];
|
||||
bool MessageState::parse(uint64_t sec, uint16_t ts_, uint8_t * dat) {
|
||||
uint64_t dat_le = read_u64_le(dat);
|
||||
uint64_t dat_be = read_u64_be(dat);
|
||||
|
||||
unsigned int honda_checksum(unsigned int address, uint64_t d, int l) {
|
||||
d >>= ((8-l)*8); // remove padding
|
||||
d >>= 4; // remove checksum
|
||||
for (int i=0; i < parse_sigs.size(); i++) {
|
||||
auto& sig = parse_sigs[i];
|
||||
int64_t tmp;
|
||||
|
||||
int s = 0;
|
||||
while (address) { s += (address & 0xF); address >>= 4; }
|
||||
while (d) { s += (d & 0xF); d >>= 4; }
|
||||
s = 8-s;
|
||||
s &= 0xF;
|
||||
|
||||
return s;
|
||||
}
|
||||
|
||||
unsigned int toyota_checksum(unsigned int address, uint64_t d, int l) {
|
||||
d >>= ((8-l)*8); // remove padding
|
||||
d >>= 8; // remove checksum
|
||||
|
||||
unsigned int s = l;
|
||||
while (address) { s += address & 0xff; address >>= 8; }
|
||||
while (d) { s += d & 0xff; d >>= 8; }
|
||||
|
||||
return s & 0xFF;
|
||||
}
|
||||
|
||||
unsigned int pedal_checksum(unsigned int address, uint64_t d, int l) {
|
||||
uint8_t crc = 0xFF;
|
||||
uint8_t poly = 0xD5; // standard crc8
|
||||
|
||||
d >>= ((8-l)*8); // remove padding
|
||||
d >>= 8; // remove checksum
|
||||
|
||||
uint8_t *dat = (uint8_t *)&d;
|
||||
|
||||
int i, j;
|
||||
for (i = 0; i < l - 1; i++) {
|
||||
crc ^= dat[i];
|
||||
for (j = 0; j < 8; j++) {
|
||||
if ((crc & 0x80) != 0) {
|
||||
crc = (uint8_t)((crc << 1) ^ poly);
|
||||
}
|
||||
else {
|
||||
crc <<= 1;
|
||||
}
|
||||
if (sig.is_little_endian){
|
||||
tmp = (dat_le >> sig.b1) & ((1ULL << sig.b2)-1);
|
||||
} else {
|
||||
tmp = (dat_be >> sig.bo) & ((1ULL << sig.b2)-1);
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[])
|
||||
{
|
||||
uint8_t crc;
|
||||
int i, j;
|
||||
|
||||
for (i = 0; i < 256; i++) {
|
||||
crc = i;
|
||||
for (j = 0; j < 8; j++) {
|
||||
if ((crc & 0x80) != 0)
|
||||
crc = (uint8_t)((crc << 1) ^ poly);
|
||||
else
|
||||
crc <<= 1;
|
||||
if (sig.is_signed) {
|
||||
tmp -= (tmp >> (sig.b2-1)) ? (1ULL << sig.b2) : 0; //signed
|
||||
}
|
||||
crc_lut[i] = crc;
|
||||
}
|
||||
}
|
||||
|
||||
void init_crc_lookup_tables()
|
||||
{
|
||||
// At init time, set up static lookup tables for fast CRC computation.
|
||||
DEBUG("parse 0x%X %s -> %lld\n", address, sig.name, tmp);
|
||||
|
||||
gen_crc_lookup_table(0x2F, crc8_lut_8h2f); // CRC-8 8H2F/AUTOSAR for Volkswagen
|
||||
}
|
||||
|
||||
unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l)
|
||||
{
|
||||
// Volkswagen uses standard CRC8 8H2F/AUTOSAR, but they compute it with
|
||||
// a magic variable padding byte tacked onto the end of the payload.
|
||||
// https://www.autosar.org/fileadmin/user_upload/standards/classic/4-3/AUTOSAR_SWS_CRCLibrary.pdf
|
||||
|
||||
uint8_t *dat = (uint8_t *)&d;
|
||||
uint8_t crc = 0xFF; // Standard init value for CRC8 8H2F/AUTOSAR
|
||||
|
||||
// CRC the payload first, skipping over the first byte where the CRC lives.
|
||||
for (int i = 1; i < l; i++) {
|
||||
crc ^= dat[i];
|
||||
crc = crc8_lut_8h2f[crc];
|
||||
}
|
||||
|
||||
// Look up and apply the magic final CRC padding byte, which permutes by CAN
|
||||
// address, and additionally (for SOME addresses) by the message counter.
|
||||
uint8_t counter = dat[1] & 0x0F;
|
||||
switch(address) {
|
||||
case 0x86: // LWI_01 Steering Angle
|
||||
crc ^= (uint8_t[]){0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86}[counter];
|
||||
break;
|
||||
case 0x9F: // EPS_01 Electric Power Steering
|
||||
crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter];
|
||||
break;
|
||||
case 0xAD: // Getriebe_11 Automatic Gearbox
|
||||
crc ^= (uint8_t[]){0x3F,0x69,0x39,0xDC,0x94,0xF9,0x14,0x64,0xD8,0x6A,0x34,0xCE,0xA2,0x55,0xB5,0x2C}[counter];
|
||||
break;
|
||||
case 0xFD: // ESP_21 Electronic Stability Program
|
||||
crc ^= (uint8_t[]){0xB4,0xEF,0xF8,0x49,0x1E,0xE5,0xC2,0xC0,0x97,0x19,0x3C,0xC9,0xF1,0x98,0xD6,0x61}[counter];
|
||||
break;
|
||||
case 0x106: // ESP_05 Electronic Stability Program
|
||||
crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter];
|
||||
break;
|
||||
case 0x117: // ACC_10 Automatic Cruise Control
|
||||
crc ^= (uint8_t[]){0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC}[counter];
|
||||
break;
|
||||
case 0x122: // ACC_06 Automatic Cruise Control
|
||||
crc ^= (uint8_t[]){0x37,0x7D,0xF3,0xA9,0x18,0x46,0x6D,0x4D,0x3D,0x71,0x92,0x9C,0xE5,0x32,0x10,0xB9}[counter];
|
||||
break;
|
||||
case 0x126: // HCA_01 Heading Control Assist
|
||||
crc ^= (uint8_t[]){0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA}[counter];
|
||||
break;
|
||||
case 0x12B: // GRA_ACC_01 Steering wheel controls for ACC
|
||||
crc ^= (uint8_t[]){0x6A,0x38,0xB4,0x27,0x22,0xEF,0xE1,0xBB,0xF8,0x80,0x84,0x49,0xC7,0x9E,0x1E,0x2B}[counter];
|
||||
break;
|
||||
case 0x187: // EV_Gearshift "Gear" selection data for EVs with no gearbox
|
||||
crc ^= (uint8_t[]){0x7F,0xED,0x17,0xC2,0x7C,0xEB,0x44,0x21,0x01,0xFA,0xDB,0x15,0x4A,0x6B,0x23,0x05}[counter];
|
||||
break;
|
||||
case 0x30C: // ACC_02 Automatic Cruise Control
|
||||
crc ^= (uint8_t[]){0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F}[counter];
|
||||
break;
|
||||
case 0x3C0: // Klemmen_Status_01 ignition and starting status
|
||||
crc ^= (uint8_t[]){0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3}[counter];
|
||||
break;
|
||||
case 0x65D: // ESP_20 Electronic Stability Program
|
||||
crc ^= (uint8_t[]){0xAC,0xB3,0xAB,0xEB,0x7A,0xE1,0x3B,0xF7,0x73,0xBA,0x7C,0x9E,0x06,0x5F,0x02,0xD9}[counter];
|
||||
break;
|
||||
default: // As-yet undefined CAN message, CRC check expected to fail
|
||||
INFO("Attempt to CRC check undefined Volkswagen message 0x%02X\n", address);
|
||||
crc ^= (uint8_t[]){0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}[counter];
|
||||
break;
|
||||
}
|
||||
crc = crc8_lut_8h2f[crc];
|
||||
|
||||
return crc ^ 0xFF; // Return after standard final XOR for CRC8 8H2F/AUTOSAR
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
uint64_t read_u64_be(const uint8_t* v) {
|
||||
return (((uint64_t)v[0] << 56)
|
||||
| ((uint64_t)v[1] << 48)
|
||||
| ((uint64_t)v[2] << 40)
|
||||
| ((uint64_t)v[3] << 32)
|
||||
| ((uint64_t)v[4] << 24)
|
||||
| ((uint64_t)v[5] << 16)
|
||||
| ((uint64_t)v[6] << 8)
|
||||
| (uint64_t)v[7]);
|
||||
}
|
||||
|
||||
uint64_t read_u64_le(const uint8_t* v) {
|
||||
return ((uint64_t)v[0]
|
||||
| ((uint64_t)v[1] << 8)
|
||||
| ((uint64_t)v[2] << 16)
|
||||
| ((uint64_t)v[3] << 24)
|
||||
| ((uint64_t)v[4] << 32)
|
||||
| ((uint64_t)v[5] << 40)
|
||||
| ((uint64_t)v[6] << 48)
|
||||
| ((uint64_t)v[7] << 56));
|
||||
}
|
||||
|
||||
|
||||
struct MessageState {
|
||||
uint32_t address;
|
||||
unsigned int size;
|
||||
|
||||
std::vector<Signal> parse_sigs;
|
||||
std::vector<double> vals;
|
||||
|
||||
uint16_t ts;
|
||||
uint64_t seen;
|
||||
uint64_t check_threshold;
|
||||
|
||||
uint8_t counter;
|
||||
uint8_t counter_fail;
|
||||
|
||||
bool parse(uint64_t sec, uint16_t ts_, uint64_t dat) {
|
||||
for (int i=0; i < parse_sigs.size(); i++) {
|
||||
auto& sig = parse_sigs[i];
|
||||
int64_t tmp;
|
||||
|
||||
if (sig.is_little_endian){
|
||||
tmp = (dat >> sig.b1) & ((1ULL << sig.b2)-1);
|
||||
} else {
|
||||
tmp = (dat >> sig.bo) & ((1ULL << sig.b2)-1);
|
||||
}
|
||||
|
||||
if (sig.is_signed) {
|
||||
tmp -= (tmp >> (sig.b2-1)) ? (1ULL << sig.b2) : 0; //signed
|
||||
}
|
||||
|
||||
DEBUG("parse 0x%X %s -> %lld\n", address, sig.name, tmp);
|
||||
|
||||
if (sig.type == SignalType::HONDA_CHECKSUM) {
|
||||
if (honda_checksum(address, dat, size) != tmp) {
|
||||
INFO("0x%X CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::HONDA_COUNTER) {
|
||||
if (!update_counter_generic(tmp, sig.b2)) {
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
|
||||
if (toyota_checksum(address, dat, size) != tmp) {
|
||||
INFO("0x%X CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) {
|
||||
if (volkswagen_crc(address, dat, size) != tmp) {
|
||||
INFO("0x%X CRC FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::VOLKSWAGEN_COUNTER) {
|
||||
if (!update_counter_generic(tmp, sig.b2)) {
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::PEDAL_CHECKSUM) {
|
||||
if (pedal_checksum(address, dat, size) != tmp) {
|
||||
INFO("0x%X PEDAL CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::PEDAL_COUNTER) {
|
||||
if (!update_counter_generic(tmp, sig.b2)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
vals[i] = tmp * sig.factor + sig.offset;
|
||||
}
|
||||
ts = ts_;
|
||||
seen = sec;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool update_counter_generic(int64_t v, int cnt_size) {
|
||||
uint8_t old_counter = counter;
|
||||
counter = v;
|
||||
if (((old_counter+1) & ((1 << cnt_size) -1)) != v) {
|
||||
counter_fail += 1;
|
||||
if (counter_fail > 1) {
|
||||
INFO("0x%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v);
|
||||
}
|
||||
if (counter_fail >= MAX_BAD_COUNTER) {
|
||||
if (sig.type == SignalType::HONDA_CHECKSUM) {
|
||||
if (honda_checksum(address, dat_be, size) != tmp) {
|
||||
INFO("0x%X CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::HONDA_COUNTER) {
|
||||
if (!update_counter_generic(tmp, sig.b2)) {
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
|
||||
if (toyota_checksum(address, dat_be, size) != tmp) {
|
||||
INFO("0x%X CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) {
|
||||
if (volkswagen_crc(address, dat_le, size) != tmp) {
|
||||
INFO("0x%X CRC FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::VOLKSWAGEN_COUNTER) {
|
||||
if (!update_counter_generic(tmp, sig.b2)) {
|
||||
INFO("0x%X CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::PEDAL_CHECKSUM) {
|
||||
if (pedal_checksum(dat_be, size) != tmp) {
|
||||
INFO("0x%X PEDAL CHECKSUM FAIL\n", address);
|
||||
return false;
|
||||
}
|
||||
} else if (sig.type == SignalType::PEDAL_COUNTER) {
|
||||
if (!update_counter_generic(tmp, sig.b2)) {
|
||||
return false;
|
||||
}
|
||||
} else if (counter_fail > 0) {
|
||||
counter_fail--;
|
||||
}
|
||||
return true;
|
||||
|
||||
vals[i] = tmp * sig.factor + sig.offset;
|
||||
}
|
||||
ts = ts_;
|
||||
seen = sec;
|
||||
|
||||
};
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
class CANParser {
|
||||
public:
|
||||
CANParser(int abus, const std::string& dbc_name,
|
||||
const std::vector<MessageParseOptions> &options,
|
||||
const std::vector<SignalParseOptions> &sigoptions,
|
||||
bool sendcan, const std::string& tcp_addr, int timeout=-1)
|
||||
: bus(abus) {
|
||||
// connect to can on 8006
|
||||
context = zmq_ctx_new();
|
||||
bool MessageState::update_counter_generic(int64_t v, int cnt_size) {
|
||||
uint8_t old_counter = counter;
|
||||
counter = v;
|
||||
if (((old_counter+1) & ((1 << cnt_size) -1)) != v) {
|
||||
counter_fail += 1;
|
||||
if (counter_fail > 1) {
|
||||
INFO("%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v);
|
||||
}
|
||||
if (counter_fail >= MAX_BAD_COUNTER) {
|
||||
return false;
|
||||
}
|
||||
} else if (counter_fail > 0) {
|
||||
counter_fail--;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
if (tcp_addr.length() > 0) {
|
||||
subscriber = zmq_socket(context, ZMQ_SUB);
|
||||
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
|
||||
zmq_setsockopt(subscriber, ZMQ_RCVTIMEO, &timeout, sizeof(int));
|
||||
|
||||
std::string tcp_addr_str;
|
||||
CANParser::CANParser(int abus, const std::string& dbc_name,
|
||||
const std::vector<MessageParseOptions> &options,
|
||||
const std::vector<SignalParseOptions> &sigoptions)
|
||||
: bus(abus) {
|
||||
|
||||
if (sendcan) {
|
||||
tcp_addr_str = "tcp://" + tcp_addr + ":8017";
|
||||
} else {
|
||||
tcp_addr_str = "tcp://" + tcp_addr + ":8006";
|
||||
}
|
||||
const char *tcp_addr_char = tcp_addr_str.c_str();
|
||||
dbc = dbc_lookup(dbc_name);
|
||||
assert(dbc);
|
||||
init_crc_lookup_tables();
|
||||
|
||||
zmq_connect(subscriber, tcp_addr_char);
|
||||
for (const auto& op : options) {
|
||||
MessageState state = {
|
||||
.address = op.address,
|
||||
// .check_frequency = op.check_frequency,
|
||||
};
|
||||
|
||||
// drain sendcan to delete any stale messages from previous runs
|
||||
zmq_msg_t msgDrain;
|
||||
zmq_msg_init(&msgDrain);
|
||||
int err = 0;
|
||||
while(err >= 0) {
|
||||
err = zmq_msg_recv(&msgDrain, subscriber, ZMQ_DONTWAIT);
|
||||
}
|
||||
} else {
|
||||
subscriber = NULL;
|
||||
// msg is not valid if a message isn't received for 10 consecutive steps
|
||||
if (op.check_frequency > 0) {
|
||||
state.check_threshold = (1000000000ULL / op.check_frequency) * 10;
|
||||
}
|
||||
|
||||
dbc = dbc_lookup(dbc_name);
|
||||
assert(dbc);
|
||||
init_crc_lookup_tables();
|
||||
|
||||
for (const auto& op : options) {
|
||||
MessageState state = {
|
||||
.address = op.address,
|
||||
// .check_frequency = op.check_frequency,
|
||||
};
|
||||
|
||||
// msg is not valid if a message isn't received for 10 consecutive steps
|
||||
if (op.check_frequency > 0) {
|
||||
state.check_threshold = (1000000000ULL / op.check_frequency) * 10;
|
||||
const Msg* msg = NULL;
|
||||
for (int i=0; i<dbc->num_msgs; i++) {
|
||||
if (dbc->msgs[i].address == op.address) {
|
||||
msg = &dbc->msgs[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!msg) {
|
||||
fprintf(stderr, "CANParser: could not find message 0x%X in dnc %s\n", op.address, dbc_name.c_str());
|
||||
assert(false);
|
||||
}
|
||||
|
||||
state.size = msg->size;
|
||||
|
||||
const Msg* msg = NULL;
|
||||
for (int i=0; i<dbc->num_msgs; i++) {
|
||||
if (dbc->msgs[i].address == op.address) {
|
||||
msg = &dbc->msgs[i];
|
||||
// track checksums and counters for this message
|
||||
for (int i=0; i<msg->num_sigs; i++) {
|
||||
const Signal *sig = &msg->sigs[i];
|
||||
if (sig->type != SignalType::DEFAULT) {
|
||||
state.parse_sigs.push_back(*sig);
|
||||
state.vals.push_back(0);
|
||||
}
|
||||
}
|
||||
|
||||
// track requested signals for this message
|
||||
for (const auto& sigop : sigoptions) {
|
||||
if (sigop.address != op.address) continue;
|
||||
|
||||
for (int i=0; i<msg->num_sigs; i++) {
|
||||
const Signal *sig = &msg->sigs[i];
|
||||
if (strcmp(sig->name, sigop.name) == 0
|
||||
&& sig->type == SignalType::DEFAULT) {
|
||||
state.parse_sigs.push_back(*sig);
|
||||
state.vals.push_back(sigop.default_value);
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!msg) {
|
||||
fprintf(stderr, "CANParser: could not find message 0x%X in dnc %s\n", op.address, dbc_name.c_str());
|
||||
assert(false);
|
||||
|
||||
}
|
||||
|
||||
message_states[state.address] = state;
|
||||
}
|
||||
}
|
||||
|
||||
void CANParser::UpdateCans(uint64_t sec, const capnp::List<cereal::CanData>::Reader& cans) {
|
||||
int msg_count = cans.size();
|
||||
uint64_t p;
|
||||
|
||||
DEBUG("got %d messages\n", msg_count);
|
||||
|
||||
// parse the messages
|
||||
for (int i = 0; i < msg_count; i++) {
|
||||
auto cmsg = cans[i];
|
||||
if (cmsg.getSrc() != bus) {
|
||||
// DEBUG("skip %d: wrong bus\n", cmsg.getAddress());
|
||||
continue;
|
||||
}
|
||||
auto state_it = message_states.find(cmsg.getAddress());
|
||||
if (state_it == message_states.end()) {
|
||||
// DEBUG("skip %d: not specified\n", cmsg.getAddress());
|
||||
continue;
|
||||
}
|
||||
|
||||
state.size = msg->size;
|
||||
if (cmsg.getDat().size() > 8) continue; //shouldnt ever happen
|
||||
uint8_t dat[8] = {0};
|
||||
memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size());
|
||||
|
||||
// track checksums and counters for this message
|
||||
for (int i=0; i<msg->num_sigs; i++) {
|
||||
const Signal *sig = &msg->sigs[i];
|
||||
if (sig->type != SignalType::DEFAULT) {
|
||||
state.parse_sigs.push_back(*sig);
|
||||
state.vals.push_back(0);
|
||||
}
|
||||
state_it->second.parse(sec, cmsg.getBusTime(), dat);
|
||||
}
|
||||
}
|
||||
|
||||
void CANParser::UpdateValid(uint64_t sec) {
|
||||
can_valid = true;
|
||||
for (const auto& kv : message_states) {
|
||||
const auto& state = kv.second;
|
||||
if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
|
||||
if (state.seen > 0) {
|
||||
DEBUG("%X TIMEOUT\n", state.address);
|
||||
}
|
||||
can_valid = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// track requested signals for this message
|
||||
for (const auto& sigop : sigoptions) {
|
||||
if (sigop.address != op.address) continue;
|
||||
void CANParser::update_string(std::string data) {
|
||||
// format for board, make copy due to alignment issues, will be freed on out of scope
|
||||
auto amsg = kj::heapArray<capnp::word>((data.length() / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), data.data(), data.length());
|
||||
|
||||
for (int i=0; i<msg->num_sigs; i++) {
|
||||
const Signal *sig = &msg->sigs[i];
|
||||
if (strcmp(sig->name, sigop.name) == 0
|
||||
&& sig->type == SignalType::DEFAULT) {
|
||||
state.parse_sigs.push_back(*sig);
|
||||
state.vals.push_back(sigop.default_value);
|
||||
break;
|
||||
}
|
||||
}
|
||||
// extract the messages
|
||||
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||
|
||||
}
|
||||
last_sec = event.getLogMonoTime();
|
||||
|
||||
message_states[state.address] = state;
|
||||
auto cans = event.getCan();
|
||||
UpdateCans(last_sec, cans);
|
||||
|
||||
UpdateValid(last_sec);
|
||||
}
|
||||
|
||||
|
||||
std::vector<SignalValue> CANParser::query_latest() {
|
||||
std::vector<SignalValue> ret;
|
||||
|
||||
for (const auto& kv : message_states) {
|
||||
const auto& state = kv.second;
|
||||
if (last_sec != 0 && state.seen != last_sec) continue;
|
||||
|
||||
for (int i=0; i<state.parse_sigs.size(); i++) {
|
||||
const Signal &sig = state.parse_sigs[i];
|
||||
ret.push_back((SignalValue){
|
||||
.address = state.address,
|
||||
.ts = state.ts,
|
||||
.name = sig.name,
|
||||
.value = state.vals[i],
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
void UpdateCans(uint64_t sec, const capnp::List<cereal::CanData>::Reader& cans) {
|
||||
int msg_count = cans.size();
|
||||
uint64_t p;
|
||||
|
||||
DEBUG("got %d messages\n", msg_count);
|
||||
|
||||
// parse the messages
|
||||
for (int i = 0; i < msg_count; i++) {
|
||||
auto cmsg = cans[i];
|
||||
if (cmsg.getSrc() != bus) {
|
||||
// DEBUG("skip %d: wrong bus\n", cmsg.getAddress());
|
||||
continue;
|
||||
}
|
||||
auto state_it = message_states.find(cmsg.getAddress());
|
||||
if (state_it == message_states.end()) {
|
||||
// DEBUG("skip %d: not specified\n", cmsg.getAddress());
|
||||
continue;
|
||||
}
|
||||
|
||||
if (cmsg.getDat().size() > 8) continue; //shouldnt ever happen
|
||||
uint8_t dat[8] = {0};
|
||||
memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size());
|
||||
|
||||
// Assumes all signals in the message are of the same type (little or big endian)
|
||||
// TODO: allow signals within the same message to have different endianess
|
||||
auto& sig = message_states[cmsg.getAddress()].parse_sigs[0];
|
||||
if (sig.is_little_endian) {
|
||||
p = read_u64_le(dat);
|
||||
} else {
|
||||
p = read_u64_be(dat);
|
||||
}
|
||||
|
||||
DEBUG(" proc %X: %llx\n", cmsg.getAddress(), p);
|
||||
|
||||
state_it->second.parse(sec, cmsg.getBusTime(), p);
|
||||
}
|
||||
}
|
||||
|
||||
void UpdateValid(uint64_t sec) {
|
||||
can_valid = true;
|
||||
for (const auto& kv : message_states) {
|
||||
const auto& state = kv.second;
|
||||
if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
|
||||
if (state.seen > 0) {
|
||||
DEBUG("%X TIMEOUT\n", state.address);
|
||||
}
|
||||
can_valid = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void update_string(std::string data) {
|
||||
// format for board, make copy due to alignment issues, will be freed on out of scope
|
||||
auto amsg = kj::heapArray<capnp::word>((data.length() / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), data.data(), data.length());
|
||||
|
||||
// extract the messages
|
||||
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||
|
||||
last_sec = event.getLogMonoTime();
|
||||
|
||||
auto cans = event.getCan();
|
||||
UpdateCans(last_sec, cans);
|
||||
|
||||
UpdateValid(last_sec);
|
||||
}
|
||||
|
||||
int update(uint64_t sec, bool wait) {
|
||||
int err;
|
||||
int result = 0;
|
||||
|
||||
// recv from can
|
||||
zmq_msg_t msg;
|
||||
zmq_msg_init(&msg);
|
||||
|
||||
// multiple recv is fine
|
||||
bool first = wait;
|
||||
while (subscriber != NULL) {
|
||||
if (first) {
|
||||
err = zmq_msg_recv(&msg, subscriber, 0);
|
||||
first = false;
|
||||
|
||||
// When we timeout on the first message, return error
|
||||
if (err < 0){
|
||||
result = -1;
|
||||
}
|
||||
} else {
|
||||
err = zmq_msg_recv(&msg, subscriber, ZMQ_DONTWAIT);
|
||||
}
|
||||
if (err < 0) break;
|
||||
|
||||
// format for board, make copy due to alignment issues, will be freed on out of scope
|
||||
auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
|
||||
|
||||
// extract the messages
|
||||
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||
|
||||
auto cans = event.getCan();
|
||||
UpdateCans(sec, cans);
|
||||
}
|
||||
|
||||
last_sec = sec;
|
||||
UpdateValid(sec);
|
||||
zmq_msg_close(&msg);
|
||||
return result;
|
||||
}
|
||||
|
||||
std::vector<SignalValue> query_latest() {
|
||||
std::vector<SignalValue> ret;
|
||||
|
||||
for (const auto& kv : message_states) {
|
||||
const auto& state = kv.second;
|
||||
if (last_sec != 0 && state.seen != last_sec) continue;
|
||||
|
||||
for (int i=0; i<state.parse_sigs.size(); i++) {
|
||||
const Signal &sig = state.parse_sigs[i];
|
||||
ret.push_back((SignalValue){
|
||||
.address = state.address,
|
||||
.ts = state.ts,
|
||||
.name = sig.name,
|
||||
.value = state.vals[i],
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool can_valid = false;
|
||||
uint64_t last_sec = 0;
|
||||
|
||||
private:
|
||||
const int bus;
|
||||
// zmq vars
|
||||
void *context = NULL;
|
||||
void *subscriber = NULL;
|
||||
|
||||
const DBC *dbc = NULL;
|
||||
std::unordered_map<uint32_t, MessageState> message_states;
|
||||
};
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
|
||||
void* can_init(int bus, const char* dbc_name,
|
||||
size_t num_message_options, const MessageParseOptions* message_options,
|
||||
size_t num_signal_options, const SignalParseOptions* signal_options,
|
||||
bool sendcan, const char* tcp_addr, int timeout) {
|
||||
CANParser* ret = new CANParser(bus, std::string(dbc_name),
|
||||
(message_options ? std::vector<MessageParseOptions>(message_options, message_options+num_message_options)
|
||||
: std::vector<MessageParseOptions>{}),
|
||||
(signal_options ? std::vector<SignalParseOptions>(signal_options, signal_options+num_signal_options)
|
||||
: std::vector<SignalParseOptions>{}), sendcan, std::string(tcp_addr), timeout);
|
||||
return (void*)ret;
|
||||
}
|
||||
|
||||
void* can_init_with_vectors(int bus, const char* dbc_name,
|
||||
std::vector<MessageParseOptions> message_options,
|
||||
std::vector<SignalParseOptions> signal_options,
|
||||
bool sendcan, const char* tcp_addr, int timeout) {
|
||||
CANParser* ret = new CANParser(bus, std::string(dbc_name),
|
||||
message_options,
|
||||
signal_options,
|
||||
sendcan, std::string(tcp_addr), timeout);
|
||||
return (void*)ret;
|
||||
}
|
||||
|
||||
int can_update(void* can, uint64_t sec, bool wait) {
|
||||
CANParser* cp = (CANParser*)can;
|
||||
return cp->update(sec, wait);
|
||||
}
|
||||
|
||||
void can_update_string(void *can, const char* dat, int len) {
|
||||
CANParser* cp = (CANParser*)can;
|
||||
cp->update_string(std::string(dat, len));
|
||||
}
|
||||
|
||||
size_t can_query_latest(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values) {
|
||||
CANParser* cp = (CANParser*)can;
|
||||
|
||||
if (out_can_valid) {
|
||||
*out_can_valid = cp->can_valid;
|
||||
}
|
||||
|
||||
const std::vector<SignalValue> values = cp->query_latest();
|
||||
if (out_values) {
|
||||
std::copy(values.begin(), values.begin()+std::min(out_values_size, values.size()), out_values);
|
||||
}
|
||||
return values.size();
|
||||
};
|
||||
|
||||
void can_query_latest_vector(void* can, bool *out_can_valid, std::vector<SignalValue> &values) {
|
||||
CANParser* cp = (CANParser*)can;
|
||||
if (out_can_valid) {
|
||||
*out_can_valid = cp->can_valid;
|
||||
}
|
||||
values = cp->query_latest();
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#ifdef TEST
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
CANParser cp(0, "honda_civic_touring_2016_can",
|
||||
std::vector<MessageParseOptions>{
|
||||
// address, check_frequency
|
||||
{0x14a, 100},
|
||||
{0x158, 100},
|
||||
{0x17c, 100},
|
||||
{0x191, 100},
|
||||
{0x1a4, 50},
|
||||
{0x326, 10},
|
||||
{0x1b0, 50},
|
||||
{0x1d0, 50},
|
||||
{0x305, 10},
|
||||
{0x324, 10},
|
||||
{0x405, 3},
|
||||
{0x18f, 0},
|
||||
{0x130, 0},
|
||||
{0x296, 0},
|
||||
{0x30c, 0},
|
||||
},
|
||||
std::vector<SignalParseOptions>{
|
||||
// sig_name, sig_address, default
|
||||
{0x158, "XMISSION_SPEED", 0},
|
||||
{0x1d0, "WHEEL_SPEED_FL", 0},
|
||||
{0x1d0, "WHEEL_SPEED_FR", 0},
|
||||
{0x1d0, "WHEEL_SPEED_RL", 0},
|
||||
{0x14a, "STEER_ANGLE", 0},
|
||||
{0x18f, "STEER_TORQUE_SENSOR", 0},
|
||||
{0x191, "GEAR", 0},
|
||||
{0x1b0, "WHEELS_MOVING", 1},
|
||||
{0x405, "DOOR_OPEN_FL", 1},
|
||||
{0x405, "DOOR_OPEN_FR", 1},
|
||||
{0x405, "DOOR_OPEN_RL", 1},
|
||||
{0x405, "DOOR_OPEN_RR", 1},
|
||||
{0x324, "CRUISE_SPEED_PCM", 0},
|
||||
{0x305, "SEATBELT_DRIVER_LAMP", 1},
|
||||
{0x305, "SEATBELT_DRIVER_LATCHED", 0},
|
||||
{0x17c, "BRAKE_PRESSED", 0},
|
||||
{0x130, "CAR_GAS", 0},
|
||||
{0x296, "CRUISE_BUTTONS", 0},
|
||||
{0x1a4, "ESP_DISABLED", 1},
|
||||
{0x30c, "HUD_LEAD", 0},
|
||||
{0x1a4, "USER_BRAKE", 0},
|
||||
{0x18f, "STEER_STATUS", 5},
|
||||
{0x1d0, "WHEEL_SPEED_RR", 0},
|
||||
{0x1b0, "BRAKE_ERROR_1", 1},
|
||||
{0x1b0, "BRAKE_ERROR_2", 1},
|
||||
{0x191, "GEAR_SHIFTER", 0},
|
||||
{0x326, "MAIN_ON", 0},
|
||||
{0x17c, "ACC_STATUS", 0},
|
||||
{0x17c, "PEDAL_GAS", 0},
|
||||
{0x296, "CRUISE_SETTING", 0},
|
||||
{0x326, "LEFT_BLINKER", 0},
|
||||
{0x326, "RIGHT_BLINKER", 0},
|
||||
{0x324, "COUNTER", 0},
|
||||
{0x17c, "ENGINE_RPM", 0},
|
||||
});
|
||||
|
||||
|
||||
|
||||
const std::string log_fn = "dats.bin";
|
||||
|
||||
int log_fd = open(log_fn.c_str(), O_RDONLY, 0);
|
||||
assert(log_fd >= 0);
|
||||
|
||||
off_t log_size = lseek(log_fd, 0, SEEK_END);
|
||||
lseek(log_fd, 0, SEEK_SET);
|
||||
|
||||
void* log_data = mmap(NULL, log_size, PROT_READ, MAP_PRIVATE, log_fd, 0);
|
||||
assert(log_data);
|
||||
|
||||
auto words = kj::arrayPtr((const capnp::word*)log_data, log_size/sizeof(capnp::word));
|
||||
while (words.size() > 0) {
|
||||
capnp::FlatArrayMessageReader reader(words);
|
||||
|
||||
auto evt = reader.getRoot<cereal::Event>();
|
||||
auto cans = evt.getCan();
|
||||
|
||||
cp.UpdateCans(0, cans);
|
||||
|
||||
words = kj::arrayPtr(reader.getEnd(), words.end());
|
||||
}
|
||||
|
||||
munmap(log_data, log_size);
|
||||
|
||||
close(log_fd);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -3,7 +3,7 @@ import subprocess
|
|||
|
||||
can_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
libdbc_fn = os.path.join(can_dir, "libdbc.so")
|
||||
subprocess.check_call(["make"], cwd=can_dir)
|
||||
subprocess.check_call(["make", "-j3"], cwd=can_dir) # don't use all the cores to avoid overheating
|
||||
|
||||
from selfdrive.can.parser_pyx import CANParser # pylint: disable=no-name-in-module, import-error
|
||||
assert CANParser
|
||||
|
|
|
|||
|
|
@ -1,98 +0,0 @@
|
|||
# distutils: language = c++
|
||||
#cython: language_level=3
|
||||
|
||||
from libc.stdint cimport uint32_t, uint64_t, uint16_t
|
||||
from libcpp.vector cimport vector
|
||||
from libcpp.map cimport map
|
||||
from libcpp.string cimport string
|
||||
from libcpp.unordered_set cimport unordered_set
|
||||
from libcpp cimport bool
|
||||
|
||||
ctypedef enum SignalType:
|
||||
DEFAULT,
|
||||
HONDA_CHECKSUM,
|
||||
HONDA_COUNTER,
|
||||
TOYOTA_CHECKSUM,
|
||||
PEDAL_CHECKSUM,
|
||||
PEDAL_COUNTER,
|
||||
VOLKSWAGEN_CHECKSUM,
|
||||
VOLKSWAGEN_COUNTER
|
||||
|
||||
cdef struct Signal:
|
||||
const char* name
|
||||
int b1, b2, bo
|
||||
bool is_signed
|
||||
double factor, offset
|
||||
SignalType type
|
||||
|
||||
|
||||
|
||||
cdef struct Msg:
|
||||
const char* name
|
||||
uint32_t address
|
||||
unsigned int size
|
||||
size_t num_sigs
|
||||
const Signal *sigs
|
||||
|
||||
cdef struct Val:
|
||||
const char* name
|
||||
uint32_t address
|
||||
const char* def_val
|
||||
const Signal *sigs
|
||||
|
||||
cdef struct DBC:
|
||||
const char* name
|
||||
size_t num_msgs
|
||||
const Msg *msgs
|
||||
const Val *vals
|
||||
size_t num_vals
|
||||
|
||||
cdef struct SignalParseOptions:
|
||||
uint32_t address
|
||||
const char* name
|
||||
double default_value
|
||||
|
||||
|
||||
cdef struct MessageParseOptions:
|
||||
uint32_t address
|
||||
int check_frequency
|
||||
|
||||
cdef struct SignalValue:
|
||||
uint32_t address
|
||||
uint16_t ts
|
||||
const char* name
|
||||
double value
|
||||
|
||||
ctypedef const DBC * (*dbc_lookup_func)(const char* dbc_name)
|
||||
ctypedef void* (*can_init_with_vectors_func)(int bus, const char* dbc_name,
|
||||
vector[MessageParseOptions] message_options,
|
||||
vector[SignalParseOptions] signal_options,
|
||||
bool sendcan,
|
||||
const char* tcp_addr,
|
||||
int timeout)
|
||||
ctypedef int (*can_update_func)(void* can, uint64_t sec, bool wait);
|
||||
ctypedef void (*can_update_string_func)(void* can, const char* dat, int len);
|
||||
ctypedef size_t (*can_query_latest_func)(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values);
|
||||
ctypedef void (*can_query_latest_vector_func)(void* can, bool *out_can_valid, vector[SignalValue] &values)
|
||||
|
||||
cdef class CANParser:
|
||||
cdef:
|
||||
void *can
|
||||
const DBC *dbc
|
||||
dbc_lookup_func dbc_lookup
|
||||
can_init_with_vectors_func can_init_with_vectors
|
||||
can_update_func can_update
|
||||
can_update_string_func can_update_string
|
||||
can_query_latest_vector_func can_query_latest_vector
|
||||
map[string, uint32_t] msg_name_to_address
|
||||
map[uint32_t, string] address_to_msg_name
|
||||
vector[SignalValue] can_values
|
||||
bool test_mode_enabled
|
||||
cdef public:
|
||||
string dbc_name
|
||||
dict vl
|
||||
dict ts
|
||||
bool can_valid
|
||||
int can_invalid_cnt
|
||||
|
||||
cdef unordered_set[uint32_t] update_vl(self)
|
||||
|
|
@ -1,7 +1,16 @@
|
|||
# distutils: language = c++
|
||||
# cython: c_string_encoding=ascii, language_level=3
|
||||
|
||||
from posix.dlfcn cimport dlopen, dlsym, RTLD_LAZY
|
||||
from libcpp.string cimport string
|
||||
from libcpp.vector cimport vector
|
||||
from libcpp cimport bool
|
||||
from libcpp.unordered_set cimport unordered_set
|
||||
from libc.stdint cimport uint32_t, uint64_t, uint16_t
|
||||
from libcpp.map cimport map
|
||||
|
||||
from common cimport CANParser as cpp_CANParser
|
||||
from common cimport SignalParseOptions, MessageParseOptions, dbc_lookup, SignalValue, DBC
|
||||
|
||||
|
||||
from libcpp cimport bool
|
||||
import os
|
||||
|
|
@ -9,25 +18,30 @@ import numbers
|
|||
|
||||
cdef int CAN_INVALID_CNT = 5
|
||||
|
||||
cdef class CANParser:
|
||||
def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr=b"", timeout=-1):
|
||||
self.test_mode_enabled = False
|
||||
can_dir = os.path.dirname(os.path.abspath(__file__))
|
||||
libdbc_fn = os.path.join(can_dir, "libdbc.so")
|
||||
libdbc_fn = str(libdbc_fn).encode('utf8')
|
||||
|
||||
cdef void *libdbc = dlopen(libdbc_fn, RTLD_LAZY)
|
||||
self.can_init_with_vectors = <can_init_with_vectors_func>dlsym(libdbc, 'can_init_with_vectors')
|
||||
self.dbc_lookup = <dbc_lookup_func>dlsym(libdbc, 'dbc_lookup')
|
||||
self.can_update = <can_update_func>dlsym(libdbc, 'can_update')
|
||||
self.can_update_string = <can_update_string_func>dlsym(libdbc, 'can_update_string')
|
||||
self.can_query_latest_vector = <can_query_latest_vector_func>dlsym(libdbc, 'can_query_latest_vector')
|
||||
cdef class CANParser:
|
||||
cdef:
|
||||
cpp_CANParser *can
|
||||
const DBC *dbc
|
||||
map[string, uint32_t] msg_name_to_address
|
||||
map[uint32_t, string] address_to_msg_name
|
||||
vector[SignalValue] can_values
|
||||
bool test_mode_enabled
|
||||
|
||||
cdef public:
|
||||
string dbc_name
|
||||
dict vl
|
||||
dict ts
|
||||
bool can_valid
|
||||
int can_invalid_cnt
|
||||
|
||||
def __init__(self, dbc_name, signals, checks=None, bus=0):
|
||||
if checks is None:
|
||||
checks = []
|
||||
|
||||
self.can_valid = True
|
||||
self.dbc_name = dbc_name
|
||||
self.dbc = self.dbc_lookup(dbc_name)
|
||||
self.dbc = dbc_lookup(dbc_name)
|
||||
self.vl = {}
|
||||
self.ts = {}
|
||||
|
||||
|
|
@ -78,15 +92,15 @@ cdef class CANParser:
|
|||
mpo.check_frequency = freq
|
||||
message_options_v.push_back(mpo)
|
||||
|
||||
self.can = self.can_init_with_vectors(bus, dbc_name, message_options_v, signal_options_v, sendcan, tcp_addr, timeout)
|
||||
self.can = new cpp_CANParser(bus, dbc_name, message_options_v, signal_options_v)
|
||||
self.update_vl()
|
||||
|
||||
cdef unordered_set[uint32_t] update_vl(self):
|
||||
cdef string sig_name
|
||||
cdef unordered_set[uint32_t] updated_val
|
||||
cdef bool valid = False
|
||||
|
||||
self.can_query_latest_vector(self.can, &valid, self.can_values)
|
||||
can_values = self.can.query_latest()
|
||||
valid = self.can.can_valid
|
||||
|
||||
# Update invalid flag
|
||||
self.can_invalid_cnt += 1
|
||||
|
|
@ -95,7 +109,7 @@ cdef class CANParser:
|
|||
self.can_valid = self.can_invalid_cnt < CAN_INVALID_CNT
|
||||
|
||||
|
||||
for cv in self.can_values:
|
||||
for cv in can_values:
|
||||
# Cast char * directly to unicde
|
||||
name = <unicode>self.address_to_msg_name[cv.address].c_str()
|
||||
cv_name = <unicode>cv.name
|
||||
|
|
@ -111,7 +125,7 @@ cdef class CANParser:
|
|||
return updated_val
|
||||
|
||||
def update_string(self, dat):
|
||||
self.can_update_string(self.can, dat, len(dat))
|
||||
self.can.update_string(dat)
|
||||
return self.update_vl()
|
||||
|
||||
def update_strings(self, strings):
|
||||
|
|
@ -122,8 +136,3 @@ cdef class CANParser:
|
|||
updated_vals.update(updated_val)
|
||||
|
||||
return updated_vals
|
||||
|
||||
def update(self, uint64_t sec, bool wait):
|
||||
r = (self.can_update(self.can, sec, wait) >= 0)
|
||||
updated_val = self.update_vl()
|
||||
return r, updated_val
|
||||
|
|
|
|||
|
|
@ -1,8 +1,10 @@
|
|||
import os
|
||||
import subprocess
|
||||
from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module
|
||||
|
||||
from Cython.Build import cythonize
|
||||
|
||||
from common.basedir import BASEDIR
|
||||
from common.cython_hacks import BuildExtWithoutPlatformSuffix
|
||||
|
||||
sourcefiles = ['parser_pyx.pyx']
|
||||
|
|
@ -12,12 +14,22 @@ ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pyl
|
|||
if ARCH == "aarch64":
|
||||
extra_compile_args += ["-Wno-deprecated-register"]
|
||||
|
||||
setup(name='Radard Thread',
|
||||
setup(name='CAN parser',
|
||||
cmdclass={'build_ext': BuildExtWithoutPlatformSuffix},
|
||||
ext_modules=cythonize(
|
||||
Extension(
|
||||
"parser_pyx",
|
||||
language="c++",
|
||||
sources=sourcefiles,
|
||||
extra_compile_args=extra_compile_args
|
||||
),
|
||||
nthreads=4))
|
||||
extra_compile_args=extra_compile_args,
|
||||
include_dirs=[
|
||||
BASEDIR,
|
||||
os.path.join(BASEDIR, 'phonelibs', 'capnp-cpp/include'),
|
||||
],
|
||||
extra_link_args=[
|
||||
os.path.join(BASEDIR, 'selfdrive', 'can', 'libdbc.so'),
|
||||
],
|
||||
)
|
||||
),
|
||||
nthreads=4,
|
||||
)
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
#!/usr/bin/env python3
|
||||
from __future__ import print_function
|
||||
import os
|
||||
import glob
|
||||
import sys
|
||||
|
||||
import jinja2
|
||||
|
|
@ -10,108 +10,96 @@ from common.dbc import dbc
|
|||
|
||||
def main():
|
||||
if len(sys.argv) != 3:
|
||||
print("usage: %s dbc_directory output_directory" % (sys.argv[0],))
|
||||
print("usage: %s dbc_directory output_filename" % (sys.argv[0],))
|
||||
sys.exit(0)
|
||||
|
||||
dbc_dir = sys.argv[1]
|
||||
out_dir = sys.argv[2]
|
||||
out_fn = sys.argv[2]
|
||||
|
||||
dbc_name = os.path.split(out_fn)[-1].replace('.cc', '')
|
||||
|
||||
template_fn = os.path.join(os.path.dirname(__file__), "dbc_template.cc")
|
||||
template_mtime = os.path.getmtime(template_fn)
|
||||
|
||||
this_file_mtime = os.path.getmtime(__file__)
|
||||
|
||||
with open(template_fn, "r") as template_f:
|
||||
template = jinja2.Template(template_f.read(), trim_blocks=True, lstrip_blocks=True)
|
||||
|
||||
for dbc_path in glob.iglob(os.path.join(dbc_dir, "*.dbc")):
|
||||
dbc_mtime = os.path.getmtime(dbc_path)
|
||||
dbc_fn = os.path.split(dbc_path)[1]
|
||||
dbc_name = os.path.splitext(dbc_fn)[0]
|
||||
can_dbc = dbc(dbc_path)
|
||||
out_fn = os.path.join(os.path.dirname(__file__), out_dir, dbc_name + ".cc")
|
||||
if os.path.exists(out_fn):
|
||||
out_mtime = os.path.getmtime(out_fn)
|
||||
else:
|
||||
out_mtime = 0
|
||||
can_dbc = dbc(os.path.join(dbc_dir, dbc_name + '.dbc'))
|
||||
|
||||
if dbc_mtime < out_mtime and template_mtime < out_mtime and this_file_mtime < out_mtime:
|
||||
continue #skip output is newer than template and dbc
|
||||
msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first
|
||||
for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.items()) if msg_sigs]
|
||||
|
||||
msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first
|
||||
for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.items()) if msg_sigs]
|
||||
def_vals = {a: set(b) for a,b in can_dbc.def_vals.items()} #remove duplicates
|
||||
def_vals = sorted(def_vals.items())
|
||||
|
||||
def_vals = {a: set(b) for a,b in can_dbc.def_vals.items()} #remove duplicates
|
||||
def_vals = sorted(def_vals.items())
|
||||
if can_dbc.name.startswith(("honda_", "acura_")):
|
||||
checksum_type = "honda"
|
||||
checksum_size = 4
|
||||
counter_size = 2
|
||||
checksum_start_bit = 3
|
||||
counter_start_bit = 5
|
||||
little_endian = False
|
||||
elif can_dbc.name.startswith(("toyota_", "lexus_")):
|
||||
checksum_type = "toyota"
|
||||
checksum_size = 8
|
||||
counter_size = None
|
||||
checksum_start_bit = 7
|
||||
counter_start_bit = None
|
||||
little_endian = False
|
||||
elif can_dbc.name.startswith(("vw_", "volkswagen_", "audi_", "seat_", "skoda_")):
|
||||
checksum_type = "volkswagen"
|
||||
checksum_size = 8
|
||||
counter_size = 4
|
||||
checksum_start_bit = 0
|
||||
counter_start_bit = 0
|
||||
little_endian = True
|
||||
else:
|
||||
checksum_type = None
|
||||
checksum_size = None
|
||||
counter_size = None
|
||||
checksum_start_bit = None
|
||||
counter_start_bit = None
|
||||
little_endian = None
|
||||
|
||||
if can_dbc.name.startswith(("honda_", "acura_")):
|
||||
checksum_type = "honda"
|
||||
checksum_size = 4
|
||||
counter_size = 2
|
||||
checksum_start_bit = 3
|
||||
counter_start_bit = 5
|
||||
little_endian = False
|
||||
elif can_dbc.name.startswith(("toyota_", "lexus_")):
|
||||
checksum_type = "toyota"
|
||||
checksum_size = 8
|
||||
counter_size = None
|
||||
checksum_start_bit = 7
|
||||
counter_start_bit = None
|
||||
little_endian = False
|
||||
elif can_dbc.name.startswith(("vw_", "volkswagen_", "audi_", "seat_", "skoda_")):
|
||||
checksum_type = "volkswagen"
|
||||
checksum_size = 8
|
||||
counter_size = 4
|
||||
checksum_start_bit = 0
|
||||
counter_start_bit = 0
|
||||
little_endian = True
|
||||
else:
|
||||
checksum_type = None
|
||||
checksum_size = None
|
||||
counter_size = None
|
||||
checksum_start_bit = None
|
||||
counter_start_bit = None
|
||||
little_endian = None
|
||||
# sanity checks on expected COUNTER and CHECKSUM rules, as packer and parser auto-compute those signals
|
||||
for address, msg_name, msg_size, sigs in msgs:
|
||||
dbc_msg_name = dbc_name + " " + msg_name
|
||||
for sig in sigs:
|
||||
if checksum_type is not None:
|
||||
# checksum rules
|
||||
if sig.name == "CHECKSUM":
|
||||
if sig.size != checksum_size:
|
||||
sys.exit("%s: CHECKSUM is not %d bits long" % (dbc_msg_name, checksum_size))
|
||||
if sig.start_bit % 8 != checksum_start_bit:
|
||||
sys.exit("%s: CHECKSUM starts at wrong bit" % dbc_msg_name)
|
||||
if little_endian != sig.is_little_endian:
|
||||
sys.exit("%s: CHECKSUM has wrong endianess" % dbc_msg_name)
|
||||
# counter rules
|
||||
if sig.name == "COUNTER":
|
||||
if counter_size is not None and sig.size != counter_size:
|
||||
sys.exit("%s: COUNTER is not %d bits long" % (dbc_msg_name, counter_size))
|
||||
if counter_start_bit is not None and sig.start_bit % 8 != counter_start_bit:
|
||||
print(counter_start_bit, sig.start_bit)
|
||||
sys.exit("%s: COUNTER starts at wrong bit" % dbc_msg_name)
|
||||
if little_endian != sig.is_little_endian:
|
||||
sys.exit("%s: COUNTER has wrong endianess" % dbc_msg_name)
|
||||
# pedal rules
|
||||
if address in [0x200, 0x201]:
|
||||
if sig.name == "COUNTER_PEDAL" and sig.size != 4:
|
||||
sys.exit("%s: PEDAL COUNTER is not 4 bits long" % dbc_msg_name)
|
||||
if sig.name == "CHECKSUM_PEDAL" and sig.size != 8:
|
||||
sys.exit("%s: PEDAL CHECKSUM is not 8 bits long" % dbc_msg_name)
|
||||
|
||||
# sanity checks on expected COUNTER and CHECKSUM rules, as packer and parser auto-compute those signals
|
||||
for address, msg_name, msg_size, sigs in msgs:
|
||||
dbc_msg_name = dbc_name + " " + msg_name
|
||||
for sig in sigs:
|
||||
if checksum_type is not None:
|
||||
# checksum rules
|
||||
if sig.name == "CHECKSUM":
|
||||
if sig.size != checksum_size:
|
||||
sys.exit("%s: CHECKSUM is not %d bits long" % (dbc_msg_name, checksum_size))
|
||||
if sig.start_bit % 8 != checksum_start_bit:
|
||||
sys.exit("%s: CHECKSUM starts at wrong bit" % dbc_msg_name)
|
||||
if little_endian != sig.is_little_endian:
|
||||
sys.exit("%s: CHECKSUM has wrong endianess" % dbc_msg_name)
|
||||
# counter rules
|
||||
if sig.name == "COUNTER":
|
||||
if counter_size is not None and sig.size != counter_size:
|
||||
sys.exit("%s: COUNTER is not %d bits long" % (dbc_msg_name, counter_size))
|
||||
if counter_start_bit is not None and sig.start_bit % 8 != counter_start_bit:
|
||||
print(counter_start_bit, sig.start_bit)
|
||||
sys.exit("%s: COUNTER starts at wrong bit" % dbc_msg_name)
|
||||
if little_endian != sig.is_little_endian:
|
||||
sys.exit("%s: COUNTER has wrong endianess" % dbc_msg_name)
|
||||
# pedal rules
|
||||
if address in [0x200, 0x201]:
|
||||
if sig.name == "COUNTER_PEDAL" and sig.size != 4:
|
||||
sys.exit("%s: PEDAL COUNTER is not 4 bits long" % dbc_msg_name)
|
||||
if sig.name == "CHECKSUM_PEDAL" and sig.size != 8:
|
||||
sys.exit("%s: PEDAL CHECKSUM is not 8 bits long" % dbc_msg_name)
|
||||
# Fail on duplicate message names
|
||||
c = Counter([msg_name for address, msg_name, msg_size, sigs in msgs])
|
||||
for name, count in c.items():
|
||||
if count > 1:
|
||||
sys.exit("%s: Duplicate message name in DBC file %s" % (dbc_name, name))
|
||||
|
||||
# Fail on duplicate message names
|
||||
c = Counter([msg_name for address, msg_name, msg_size, sigs in msgs])
|
||||
for name, count in c.items():
|
||||
if count > 1:
|
||||
sys.exit("%s: Duplicate message name in DBC file %s" % (dbc_name, name))
|
||||
parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, def_vals=def_vals, len=len)
|
||||
|
||||
parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, def_vals=def_vals, len=len)
|
||||
|
||||
with open(out_fn, "w") as out_f:
|
||||
out_f.write(parser_code)
|
||||
with open(out_fn, "w") as out_f:
|
||||
out_f.write(parser_code)
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
|
|
|||
|
|
@ -1,68 +0,0 @@
|
|||
import struct
|
||||
from selfdrive.can.libdbc_py import libdbc, ffi
|
||||
|
||||
|
||||
class CANPacker():
|
||||
def __init__(self, dbc_name):
|
||||
dbc_name = dbc_name.encode('utf8')
|
||||
self.packer = libdbc.canpack_init(dbc_name)
|
||||
self.dbc = libdbc.dbc_lookup(dbc_name)
|
||||
self.sig_names = {}
|
||||
self.name_to_address_and_size = {}
|
||||
|
||||
num_msgs = self.dbc[0].num_msgs
|
||||
for i in range(num_msgs):
|
||||
msg = self.dbc[0].msgs[i]
|
||||
|
||||
name = ffi.string(msg.name).decode('utf8')
|
||||
address = msg.address
|
||||
self.name_to_address_and_size[name] = (address, msg.size)
|
||||
self.name_to_address_and_size[address] = (address, msg.size)
|
||||
|
||||
def pack(self, addr, values, counter):
|
||||
values_thing = []
|
||||
for name, value in values.items():
|
||||
if name not in self.sig_names:
|
||||
self.sig_names[name] = ffi.new("char[]", name.encode('utf8'))
|
||||
|
||||
values_thing.append({
|
||||
'name': self.sig_names[name],
|
||||
'value': value
|
||||
})
|
||||
|
||||
values_c = ffi.new("SignalPackValue[]", values_thing)
|
||||
|
||||
return libdbc.canpack_pack(self.packer, addr, len(values_thing), values_c, counter)
|
||||
|
||||
def pack_bytes(self, addr, values, counter=-1):
|
||||
addr, size = self.name_to_address_and_size[addr]
|
||||
|
||||
val = self.pack(addr, values, counter)
|
||||
r = struct.pack(">Q", val)
|
||||
return addr, r[:size]
|
||||
|
||||
def make_can_msg(self, addr, bus, values, counter=-1):
|
||||
addr, msg = self.pack_bytes(addr, values, counter)
|
||||
return [addr, 0, msg, bus]
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
## little endian test
|
||||
cp = CANPacker("hyundai_santa_fe_2019_ccan")
|
||||
s = cp.pack_bytes(0x340, {
|
||||
"CR_Lkas_StrToqReq": -0.06,
|
||||
#"CF_Lkas_FcwBasReq": 1,
|
||||
"CF_Lkas_MsgCount": 7,
|
||||
"CF_Lkas_HbaSysState": 0,
|
||||
#"CF_Lkas_Chksum": 3,
|
||||
})
|
||||
s = cp.pack_bytes(0x340, {
|
||||
"CF_Lkas_MsgCount": 1,
|
||||
})
|
||||
# big endian test
|
||||
#cp = CANPacker("honda_civic_touring_2016_can_generated")
|
||||
#s = cp.pack_bytes(0xe4, {
|
||||
# "STEER_TORQUE": -2,
|
||||
#})
|
||||
print([hex(ord(v)) for v in s[1]])
|
||||
print(s[1].encode("hex"))
|
||||
|
|
@ -1,220 +0,0 @@
|
|||
import time
|
||||
import numbers
|
||||
|
||||
from selfdrive.can.libdbc_py import libdbc, ffi
|
||||
|
||||
CAN_INVALID_CNT = 5 # after so many consecutive CAN data with wrong checksum, counter or frequency, flag CAN invalidity
|
||||
|
||||
class CANParser():
|
||||
def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1", timeout=-1):
|
||||
if checks is None:
|
||||
checks = []
|
||||
|
||||
self.can_valid = True
|
||||
self.can_invalid_cnt = CAN_INVALID_CNT
|
||||
self.vl = {}
|
||||
self.ts = {}
|
||||
|
||||
self.dbc_name = dbc_name
|
||||
self.dbc = libdbc.dbc_lookup(dbc_name.encode('utf8'))
|
||||
self.msg_name_to_addres = {}
|
||||
self.address_to_msg_name = {}
|
||||
|
||||
num_msgs = self.dbc[0].num_msgs
|
||||
for i in range(num_msgs):
|
||||
msg = self.dbc[0].msgs[i]
|
||||
|
||||
name = ffi.string(msg.name).decode('utf8')
|
||||
address = msg.address
|
||||
|
||||
self.msg_name_to_addres[name] = address
|
||||
self.address_to_msg_name[address] = name
|
||||
|
||||
self.vl[address] = {}
|
||||
self.vl[name] = {}
|
||||
self.ts[address] = {}
|
||||
self.ts[name] = {}
|
||||
|
||||
# Convert message names into addresses
|
||||
for i in range(len(signals)):
|
||||
s = signals[i]
|
||||
if not isinstance(s[1], numbers.Number):
|
||||
s = (s[0], self.msg_name_to_addres[s[1]], s[2])
|
||||
signals[i] = s
|
||||
|
||||
for i in range(len(checks)):
|
||||
c = checks[i]
|
||||
if not isinstance(c[0], numbers.Number):
|
||||
c = (self.msg_name_to_addres[c[0]], c[1])
|
||||
checks[i] = c
|
||||
|
||||
sig_names = dict((name, ffi.new("char[]", name.encode('utf8'))) for name, _, _ in signals)
|
||||
|
||||
signal_options_c = ffi.new("SignalParseOptions[]", [
|
||||
{
|
||||
'address': sig_address,
|
||||
'name': sig_names[sig_name],
|
||||
'default_value': sig_default,
|
||||
} for sig_name, sig_address, sig_default in signals])
|
||||
|
||||
message_options = dict((address, 0) for _, address, _ in signals)
|
||||
message_options.update(dict(checks))
|
||||
|
||||
message_options_c = ffi.new("MessageParseOptions[]", [
|
||||
{
|
||||
'address': msg_address,
|
||||
'check_frequency': freq,
|
||||
} for msg_address, freq in message_options.items()])
|
||||
|
||||
self.can = libdbc.can_init(bus, dbc_name.encode('utf8'), len(message_options_c), message_options_c,
|
||||
len(signal_options_c), signal_options_c, sendcan, tcp_addr.encode('utf8'), timeout)
|
||||
|
||||
self.p_can_valid = ffi.new("bool*")
|
||||
|
||||
value_count = libdbc.can_query_latest(self.can, self.p_can_valid, 0, ffi.NULL)
|
||||
self.can_values = ffi.new("SignalValue[%d]" % value_count)
|
||||
self.update_vl(0)
|
||||
|
||||
def update_vl(self, sec):
|
||||
can_values_len = libdbc.can_query_latest(self.can, self.p_can_valid, len(self.can_values), self.can_values)
|
||||
assert can_values_len <= len(self.can_values)
|
||||
|
||||
self.can_invalid_cnt += 1
|
||||
if self.p_can_valid[0]:
|
||||
self.can_invalid_cnt = 0
|
||||
self.can_valid = self.can_invalid_cnt < CAN_INVALID_CNT
|
||||
|
||||
ret = set()
|
||||
for i in range(can_values_len):
|
||||
cv = self.can_values[i]
|
||||
address = cv.address
|
||||
# print("{0} {1}".format(hex(cv.address), ffi.string(cv.name)))
|
||||
name = ffi.string(cv.name).decode('utf8')
|
||||
self.vl[address][name] = cv.value
|
||||
self.ts[address][name] = cv.ts
|
||||
|
||||
sig_name = self.address_to_msg_name[address]
|
||||
self.vl[sig_name][name] = cv.value
|
||||
self.ts[sig_name][name] = cv.ts
|
||||
ret.add(address)
|
||||
return ret
|
||||
|
||||
def update(self, sec, wait):
|
||||
"""Returns if the update was successfull (e.g. no rcv timeout happened)"""
|
||||
r = libdbc.can_update(self.can, sec, wait)
|
||||
return bool(r >= 0), self.update_vl(sec)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
from common.realtime import sec_since_boot
|
||||
|
||||
radar_messages = range(0x430, 0x43A) + range(0x440, 0x446)
|
||||
# signals = zip(['LONG_DIST'] * 16 + ['NEW_TRACK'] * 16 + ['LAT_DIST'] * 16 +
|
||||
# ['REL_SPEED'] * 16, radar_messages * 4,
|
||||
# [255] * 16 + [1] * 16 + [0] * 16 + [0] * 16)
|
||||
# checks = zip(radar_messages, [20]*16)
|
||||
|
||||
# cp = CANParser("acura_ilx_2016_nidec", signals, checks, 1)
|
||||
|
||||
|
||||
# signals = [
|
||||
# ("XMISSION_SPEED", 0x158, 0), #sig_name, sig_address, default
|
||||
# ("WHEEL_SPEED_FL", 0x1d0, 0),
|
||||
# ("WHEEL_SPEED_FR", 0x1d0, 0),
|
||||
# ("WHEEL_SPEED_RL", 0x1d0, 0),
|
||||
# ("STEER_ANGLE", 0x14a, 0),
|
||||
# ("STEER_TORQUE_SENSOR", 0x18f, 0),
|
||||
# ("GEAR", 0x191, 0),
|
||||
# ("WHEELS_MOVING", 0x1b0, 1),
|
||||
# ("DOOR_OPEN_FL", 0x405, 1),
|
||||
# ("DOOR_OPEN_FR", 0x405, 1),
|
||||
# ("DOOR_OPEN_RL", 0x405, 1),
|
||||
# ("DOOR_OPEN_RR", 0x405, 1),
|
||||
# ("CRUISE_SPEED_PCM", 0x324, 0),
|
||||
# ("SEATBELT_DRIVER_LAMP", 0x305, 1),
|
||||
# ("SEATBELT_DRIVER_LATCHED", 0x305, 0),
|
||||
# ("BRAKE_PRESSED", 0x17c, 0),
|
||||
# ("CAR_GAS", 0x130, 0),
|
||||
# ("CRUISE_BUTTONS", 0x296, 0),
|
||||
# ("ESP_DISABLED", 0x1a4, 1),
|
||||
# ("HUD_LEAD", 0x30c, 0),
|
||||
# ("USER_BRAKE", 0x1a4, 0),
|
||||
# ("STEER_STATUS", 0x18f, 5),
|
||||
# ("WHEEL_SPEED_RR", 0x1d0, 0),
|
||||
# ("BRAKE_ERROR_1", 0x1b0, 1),
|
||||
# ("BRAKE_ERROR_2", 0x1b0, 1),
|
||||
# ("GEAR_SHIFTER", 0x191, 0),
|
||||
# ("MAIN_ON", 0x326, 0),
|
||||
# ("ACC_STATUS", 0x17c, 0),
|
||||
# ("PEDAL_GAS", 0x17c, 0),
|
||||
# ("CRUISE_SETTING", 0x296, 0),
|
||||
# ("LEFT_BLINKER", 0x326, 0),
|
||||
# ("RIGHT_BLINKER", 0x326, 0),
|
||||
# ("COUNTER", 0x324, 0),
|
||||
# ("ENGINE_RPM", 0x17C, 0)
|
||||
# ]
|
||||
# checks = [
|
||||
# (0x14a, 100), # address, frequency
|
||||
# (0x158, 100),
|
||||
# (0x17c, 100),
|
||||
# (0x191, 100),
|
||||
# (0x1a4, 50),
|
||||
# (0x326, 10),
|
||||
# (0x1b0, 50),
|
||||
# (0x1d0, 50),
|
||||
# (0x305, 10),
|
||||
# (0x324, 10),
|
||||
# (0x405, 3),
|
||||
# ]
|
||||
|
||||
# cp = CANParser("honda_civic_touring_2016_can_generated", signals, checks, 0)
|
||||
|
||||
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("GEAR", 956, 0x20),
|
||||
("BRAKE_PRESSED", 548, 0),
|
||||
("GAS_PEDAL", 705, 0),
|
||||
|
||||
("WHEEL_SPEED_FL", 170, 0),
|
||||
("WHEEL_SPEED_FR", 170, 0),
|
||||
("WHEEL_SPEED_RL", 170, 0),
|
||||
("WHEEL_SPEED_RR", 170, 0),
|
||||
("DOOR_OPEN_FL", 1568, 1),
|
||||
("DOOR_OPEN_FR", 1568, 1),
|
||||
("DOOR_OPEN_RL", 1568, 1),
|
||||
("DOOR_OPEN_RR", 1568, 1),
|
||||
("SEATBELT_DRIVER_UNLATCHED", 1568, 1),
|
||||
("TC_DISABLED", 951, 1),
|
||||
("STEER_ANGLE", 37, 0),
|
||||
("STEER_FRACTION", 37, 0),
|
||||
("STEER_RATE", 37, 0),
|
||||
("GAS_RELEASED", 466, 0),
|
||||
("CRUISE_STATE", 466, 0),
|
||||
("MAIN_ON", 467, 0),
|
||||
("SET_SPEED", 467, 0),
|
||||
("STEER_TORQUE_DRIVER", 608, 0),
|
||||
("STEER_TORQUE_EPS", 608, 0),
|
||||
("TURN_SIGNALS", 1556, 3), # 3 is no blinkers
|
||||
("LKA_STATE", 610, 0),
|
||||
]
|
||||
checks = [
|
||||
(548, 40),
|
||||
(705, 33),
|
||||
|
||||
(170, 80),
|
||||
(37, 80),
|
||||
(466, 33),
|
||||
(608, 50),
|
||||
]
|
||||
|
||||
cp = CANParser("toyota_rav4_2017_pt_generated", signals, checks, 0)
|
||||
|
||||
# print(cp.vl)
|
||||
|
||||
while True:
|
||||
cp.update(int(sec_since_boot()*1e9), True)
|
||||
# print(cp.vl)
|
||||
print(cp.ts)
|
||||
print(cp.can_valid)
|
||||
time.sleep(0.01)
|
||||
|
|
@ -1,35 +0,0 @@
|
|||
import unittest
|
||||
import random
|
||||
|
||||
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld
|
||||
from selfdrive.can.packer import CANPacker
|
||||
import selfdrive.car.chrysler.chryslercan as chryslercan
|
||||
|
||||
|
||||
class TestPackerMethods(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.chrysler_cp_old = CANPackerOld("chrysler_pacifica_2017_hybrid")
|
||||
self.chrysler_cp = CANPacker("chrysler_pacifica_2017_hybrid")
|
||||
|
||||
def test_correctness(self):
|
||||
# Test all commands, randomize the params.
|
||||
for _ in range(1000):
|
||||
gear = ('drive', 'reverse', 'low')[random.randint(0, 3) % 3]
|
||||
lkas_active = (random.randint(0, 2) % 2 == 0)
|
||||
hud_alert = random.randint(0, 6)
|
||||
hud_count = random.randint(0, 65536)
|
||||
lkas_car_model = random.randint(0, 65536)
|
||||
m_old = chryslercan.create_lkas_hud(self.chrysler_cp_old, gear, lkas_active, hud_alert, hud_count, lkas_car_model)
|
||||
m = chryslercan.create_lkas_hud(self.chrysler_cp, gear, lkas_active, hud_alert, hud_count, lkas_car_model)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
apply_steer = (random.randint(0, 2) % 2 == 0)
|
||||
moving_fast = (random.randint(0, 2) % 2 == 0)
|
||||
frame = random.randint(0, 65536)
|
||||
m_old = chryslercan.create_lkas_command(self.chrysler_cp_old, apply_steer, moving_fast, frame)
|
||||
m = chryslercan.create_lkas_command(self.chrysler_cp, apply_steer, moving_fast, frame)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,66 +0,0 @@
|
|||
import unittest
|
||||
import random
|
||||
|
||||
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld
|
||||
from selfdrive.can.packer import CANPacker
|
||||
import selfdrive.car.gm.gmcan as gmcan
|
||||
from selfdrive.car.gm.interface import CanBus as GMCanBus
|
||||
|
||||
|
||||
class TestPackerMethods(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.gm_cp_old = CANPackerOld("gm_global_a_powertrain")
|
||||
self.gm_cp = CANPacker("gm_global_a_powertrain")
|
||||
|
||||
self.ct6_cp_old = CANPackerOld("cadillac_ct6_chassis")
|
||||
self.ct6_cp = CANPacker("cadillac_ct6_chassis")
|
||||
|
||||
def test_correctness(self):
|
||||
# Test all cars' commands, randomize the params.
|
||||
for _ in range(1000):
|
||||
bus = random.randint(0, 65536)
|
||||
apply_steer = (random.randint(0, 2) % 2 == 0)
|
||||
idx = random.randint(0, 65536)
|
||||
lkas_active = (random.randint(0, 2) % 2 == 0)
|
||||
m_old = gmcan.create_steering_control(self.gm_cp_old, bus, apply_steer, idx, lkas_active)
|
||||
m = gmcan.create_steering_control(self.gm_cp, bus, apply_steer, idx, lkas_active)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
canbus = GMCanBus()
|
||||
apply_steer = (random.randint(0, 2) % 2 == 0)
|
||||
v_ego = random.randint(0, 65536)
|
||||
idx = random.randint(0, 65536)
|
||||
enabled = (random.randint(0, 2) % 2 == 0)
|
||||
m_old = gmcan.create_steering_control_ct6(self.ct6_cp_old, canbus, apply_steer, v_ego, idx, enabled)
|
||||
m = gmcan.create_steering_control_ct6(self.ct6_cp, canbus, apply_steer, v_ego, idx, enabled)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
bus = random.randint(0, 65536)
|
||||
throttle = random.randint(0, 65536)
|
||||
idx = random.randint(0, 65536)
|
||||
acc_engaged = (random.randint(0, 2) % 2 == 0)
|
||||
at_full_stop = (random.randint(0, 2) % 2 == 0)
|
||||
m_old = gmcan.create_gas_regen_command(self.gm_cp_old, bus, throttle, idx, acc_engaged, at_full_stop)
|
||||
m = gmcan.create_gas_regen_command(self.gm_cp, bus, throttle, idx, acc_engaged, at_full_stop)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
bus = random.randint(0, 65536)
|
||||
apply_brake = (random.randint(0, 2) % 2 == 0)
|
||||
idx = random.randint(0, 65536)
|
||||
near_stop = (random.randint(0, 2) % 2 == 0)
|
||||
at_full_stop = (random.randint(0, 2) % 2 == 0)
|
||||
m_old = gmcan.create_friction_brake_command(self.ct6_cp_old, bus, apply_brake, idx, near_stop, at_full_stop)
|
||||
m = gmcan.create_friction_brake_command(self.ct6_cp, bus, apply_brake, idx, near_stop, at_full_stop)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
bus = random.randint(0, 65536)
|
||||
acc_engaged = (random.randint(0, 2) % 2 == 0)
|
||||
target_speed_kph = random.randint(0, 65536)
|
||||
lead_car_in_sight = (random.randint(0, 2) % 2 == 0)
|
||||
m_old = gmcan.create_acc_dashboard_command(self.gm_cp_old, bus, acc_engaged, target_speed_kph, lead_car_in_sight)
|
||||
m = gmcan.create_acc_dashboard_command(self.gm_cp, bus, acc_engaged, target_speed_kph, lead_car_in_sight)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,56 +0,0 @@
|
|||
import unittest
|
||||
import random
|
||||
|
||||
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld
|
||||
from selfdrive.can.packer import CANPacker
|
||||
import selfdrive.car.honda.hondacan as hondacan
|
||||
from selfdrive.car.honda.values import HONDA_BOSCH
|
||||
from selfdrive.car.honda.carcontroller import HUDData
|
||||
|
||||
|
||||
class TestPackerMethods(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.honda_cp_old = CANPackerOld("honda_pilot_touring_2017_can_generated")
|
||||
self.honda_cp = CANPacker("honda_pilot_touring_2017_can_generated")
|
||||
|
||||
def test_correctness(self):
|
||||
# Test all commands, randomize the params.
|
||||
for _ in range(1000):
|
||||
has_relay = False
|
||||
car_fingerprint = HONDA_BOSCH[0]
|
||||
|
||||
apply_brake = (random.randint(0, 2) % 2 == 0)
|
||||
pump_on = (random.randint(0, 2) % 2 == 0)
|
||||
pcm_override = (random.randint(0, 2) % 2 == 0)
|
||||
pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0)
|
||||
fcw = random.randint(0, 65536)
|
||||
idx = random.randint(0, 65536)
|
||||
m_old = hondacan.create_brake_command(self.honda_cp_old, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay)
|
||||
m = hondacan.create_brake_command(self.honda_cp, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
apply_steer = (random.randint(0, 2) % 2 == 0)
|
||||
lkas_active = (random.randint(0, 2) % 2 == 0)
|
||||
idx = random.randint(0, 65536)
|
||||
m_old = hondacan.create_steering_control(self.honda_cp_old, apply_steer, lkas_active, car_fingerprint, idx, has_relay)
|
||||
m = hondacan.create_steering_control(self.honda_cp, apply_steer, lkas_active, car_fingerprint, idx, has_relay)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
pcm_speed = random.randint(0, 65536)
|
||||
hud = HUDData(random.randint(0, 65536), random.randint(0, 65536), 1, random.randint(0, 65536),
|
||||
0xc1, random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536), random.randint(0, 65536))
|
||||
idx = random.randint(0, 65536)
|
||||
is_metric = (random.randint(0, 2) % 2 == 0)
|
||||
m_old = hondacan.create_ui_commands(self.honda_cp_old, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay)
|
||||
m = hondacan.create_ui_commands(self.honda_cp, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
button_val = random.randint(0, 65536)
|
||||
idx = random.randint(0, 65536)
|
||||
m_old = hondacan.spam_buttons_command(self.honda_cp_old, button_val, idx, car_fingerprint, has_relay)
|
||||
m = hondacan.spam_buttons_command(self.honda_cp, button_val, idx, car_fingerprint, has_relay)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,70 +0,0 @@
|
|||
import unittest
|
||||
import random
|
||||
|
||||
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld
|
||||
from selfdrive.can.packer import CANPacker
|
||||
import selfdrive.car.hyundai.hyundaican as hyundaican
|
||||
from selfdrive.car.hyundai.values import CHECKSUM as hyundai_checksum
|
||||
|
||||
|
||||
class TestPackerMethods(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.hyundai_cp_old = CANPackerOld("hyundai_kia_generic")
|
||||
self.hyundai_cp = CANPacker("hyundai_kia_generic")
|
||||
|
||||
def test_correctness(self):
|
||||
# Test all commands, randomize the params.
|
||||
for _ in range(1000):
|
||||
# Hyundai
|
||||
car_fingerprint = hyundai_checksum["crc8"][0]
|
||||
apply_steer = (random.randint(0, 2) % 2 == 0)
|
||||
steer_req = (random.randint(0, 2) % 2 == 0)
|
||||
cnt = random.randint(0, 65536)
|
||||
enabled = (random.randint(0, 2) % 2 == 0)
|
||||
lkas11 = {
|
||||
"CF_Lkas_LdwsSysState": random.randint(0,65536),
|
||||
"CF_Lkas_SysWarning": random.randint(0,65536),
|
||||
"CF_Lkas_LdwsLHWarning": random.randint(0,65536),
|
||||
"CF_Lkas_LdwsRHWarning": random.randint(0,65536),
|
||||
"CF_Lkas_HbaLamp": random.randint(0,65536),
|
||||
"CF_Lkas_FcwBasReq": random.randint(0,65536),
|
||||
"CF_Lkas_ToiFlt": random.randint(0,65536),
|
||||
"CF_Lkas_HbaSysState": random.randint(0,65536),
|
||||
"CF_Lkas_FcwOpt": random.randint(0,65536),
|
||||
"CF_Lkas_HbaOpt": random.randint(0,65536),
|
||||
"CF_Lkas_FcwSysState": random.randint(0,65536),
|
||||
"CF_Lkas_FcwCollisionWarning": random.randint(0,65536),
|
||||
"CF_Lkas_FusionState": random.randint(0,65536),
|
||||
"CF_Lkas_FcwOpt_USM": random.randint(0,65536),
|
||||
"CF_Lkas_LdwsOpt_USM": random.randint(0,65536)
|
||||
}
|
||||
hud_alert = random.randint(0, 65536)
|
||||
keep_stock = (random.randint(0, 2) % 2 == 0)
|
||||
m_old = hyundaican.create_lkas11(self.hyundai_cp_old, car_fingerprint, apply_steer, steer_req, cnt, enabled,
|
||||
lkas11, hud_alert, keep_stock)
|
||||
m = hyundaican.create_lkas11(self.hyundai_cp, car_fingerprint, apply_steer, steer_req, cnt, enabled,
|
||||
lkas11, hud_alert, keep_stock)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
clu11 = {
|
||||
"CF_Clu_CruiseSwState": random.randint(0,65536),
|
||||
"CF_Clu_CruiseSwMain": random.randint(0,65536),
|
||||
"CF_Clu_SldMainSW": random.randint(0,65536),
|
||||
"CF_Clu_ParityBit1": random.randint(0,65536),
|
||||
"CF_Clu_VanzDecimal": random.randint(0,65536),
|
||||
"CF_Clu_Vanz": random.randint(0,65536),
|
||||
"CF_Clu_SPEED_UNIT": random.randint(0,65536),
|
||||
"CF_Clu_DetentOut": random.randint(0,65536),
|
||||
"CF_Clu_RheostatLevel": random.randint(0,65536),
|
||||
"CF_Clu_CluInfo": random.randint(0,65536),
|
||||
"CF_Clu_AmpInfo": random.randint(0,65536),
|
||||
"CF_Clu_AliveCnt1": random.randint(0,65536),
|
||||
}
|
||||
button = random.randint(0, 65536)
|
||||
m_old = hyundaican.create_clu11(self.hyundai_cp_old, clu11, button)
|
||||
m = hyundaican.create_clu11(self.hyundai_cp, clu11, button)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -0,0 +1,92 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import unittest
|
||||
|
||||
from selfdrive.can.parser import CANParser
|
||||
from selfdrive.can.packer import CANPacker
|
||||
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
|
||||
from selfdrive.car.honda.interface import CarInterface as HondaInterface
|
||||
from selfdrive.car.honda.values import CAR as HONDA_CAR
|
||||
from selfdrive.car.honda.values import DBC as HONDA_DBC
|
||||
|
||||
from selfdrive.car.subaru.interface import CarInterface as SubaruInterface
|
||||
from selfdrive.car.subaru.values import CAR as SUBARU_CAR
|
||||
from selfdrive.car.subaru.values import DBC as SUBARU_DBC
|
||||
|
||||
class TestCanParserPacker(unittest.TestCase):
|
||||
def test_civic(self):
|
||||
CP = HondaInterface.get_params(HONDA_CAR.CIVIC)
|
||||
|
||||
signals = [
|
||||
("STEER_TORQUE", "STEERING_CONTROL", 0),
|
||||
("STEER_TORQUE_REQUEST", "STEERING_CONTROL", 0),
|
||||
]
|
||||
checks = []
|
||||
|
||||
parser = CANParser(HONDA_DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
packer = CANPacker(HONDA_DBC[CP.carFingerprint]['pt'])
|
||||
|
||||
idx = 0
|
||||
|
||||
for steer in range(0, 255):
|
||||
for active in [1, 0]:
|
||||
values = {
|
||||
"STEER_TORQUE": steer,
|
||||
"STEER_TORQUE_REQUEST": active,
|
||||
}
|
||||
|
||||
msgs = packer.make_can_msg("STEERING_CONTROL", 0, values, idx)
|
||||
bts = can_list_to_can_capnp([msgs])
|
||||
|
||||
parser.update_string(bts)
|
||||
|
||||
self.assertAlmostEqual(parser.vl["STEERING_CONTROL"]["STEER_TORQUE"], steer)
|
||||
self.assertAlmostEqual(parser.vl["STEERING_CONTROL"]["STEER_TORQUE_REQUEST"], active)
|
||||
self.assertAlmostEqual(parser.vl["STEERING_CONTROL"]["COUNTER"], idx % 4)
|
||||
|
||||
idx += 1
|
||||
|
||||
def test_subaru(self):
|
||||
# Subuaru is little endian
|
||||
CP = SubaruInterface.get_params(SUBARU_CAR.IMPREZA)
|
||||
|
||||
signals = [
|
||||
("Counter", "ES_LKAS", 0),
|
||||
("LKAS_Output", "ES_LKAS", 0),
|
||||
("LKAS_Request", "ES_LKAS", 0),
|
||||
("SET_1", "ES_LKAS", 0),
|
||||
|
||||
]
|
||||
|
||||
checks = []
|
||||
|
||||
parser = CANParser(SUBARU_DBC[CP.carFingerprint]['pt'], signals, checks, 0)
|
||||
packer = CANPacker(SUBARU_DBC[CP.carFingerprint]['pt'])
|
||||
|
||||
idx = 0
|
||||
|
||||
for steer in range(0, 255):
|
||||
for active in [1, 0]:
|
||||
values = {
|
||||
"Counter": idx,
|
||||
"LKAS_Output": steer,
|
||||
"LKAS_Request": active,
|
||||
"SET_1": 1
|
||||
}
|
||||
|
||||
msgs = packer.make_can_msg("ES_LKAS", 0, values)
|
||||
bts = can_list_to_can_capnp([msgs])
|
||||
parser.update_string(bts)
|
||||
|
||||
self.assertAlmostEqual(parser.vl["ES_LKAS"]["LKAS_Output"], steer)
|
||||
self.assertAlmostEqual(parser.vl["ES_LKAS"]["LKAS_Request"], active)
|
||||
self.assertAlmostEqual(parser.vl["ES_LKAS"]["SET_1"], 1)
|
||||
self.assertAlmostEqual(parser.vl["ES_LKAS"]["Counter"], idx % 16)
|
||||
|
||||
idx += 1
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,37 +0,0 @@
|
|||
import unittest
|
||||
import random
|
||||
|
||||
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld
|
||||
from selfdrive.can.packer import CANPacker
|
||||
import selfdrive.car.subaru.subarucan as subarucan
|
||||
from selfdrive.car.subaru.values import CAR as subaru_car
|
||||
|
||||
|
||||
class TestPackerMethods(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.subaru_cp_old = CANPackerOld("subaru_global_2017")
|
||||
self.subaru_cp = CANPacker("subaru_global_2017")
|
||||
|
||||
def test_correctness(self):
|
||||
# Test all cars' commands, randomize the params.
|
||||
for _ in range(1000):
|
||||
apply_steer = (random.randint(0, 2) % 2 == 0)
|
||||
frame = random.randint(1, 65536)
|
||||
steer_step = random.randint(1, 65536)
|
||||
m_old = subarucan.create_steering_control(self.subaru_cp_old, subaru_car.IMPREZA, apply_steer, frame, steer_step)
|
||||
m = subarucan.create_steering_control(self.subaru_cp, subaru_car.IMPREZA, apply_steer, frame, steer_step)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
m_old = subarucan.create_steering_status(self.subaru_cp_old, subaru_car.IMPREZA, apply_steer, frame, steer_step)
|
||||
m = subarucan.create_steering_status(self.subaru_cp, subaru_car.IMPREZA, apply_steer, frame, steer_step)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
es_distance_msg = {}
|
||||
pcm_cancel_cmd = (random.randint(0, 2) % 2 == 0)
|
||||
m_old = subarucan.create_es_distance(self.subaru_cp_old, es_distance_msg, pcm_cancel_cmd)
|
||||
m = subarucan.create_es_distance(self.subaru_cp, es_distance_msg, pcm_cancel_cmd)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,86 +0,0 @@
|
|||
import unittest
|
||||
import random
|
||||
|
||||
from selfdrive.can.tests.packer_old import CANPacker as CANPackerOld
|
||||
from selfdrive.can.packer import CANPacker
|
||||
from selfdrive.car.toyota.toyotacan import (
|
||||
create_ipas_steer_command, create_steer_command, create_accel_command,
|
||||
create_fcw_command, create_ui_command
|
||||
)
|
||||
from common.realtime import sec_since_boot
|
||||
|
||||
|
||||
class TestPackerMethods(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.cp_old = CANPackerOld("toyota_rav4_hybrid_2017_pt_generated")
|
||||
self.cp = CANPacker("toyota_rav4_hybrid_2017_pt_generated")
|
||||
|
||||
def test_correctness(self):
|
||||
# Test all commands, randomize the params.
|
||||
for _ in range(1000):
|
||||
# Toyota
|
||||
steer = random.randint(-1, 1)
|
||||
enabled = (random.randint(0, 2) % 2 == 0)
|
||||
apgs_enabled = (random.randint(0, 2) % 2 == 0)
|
||||
m_old = create_ipas_steer_command(self.cp_old, steer, enabled, apgs_enabled)
|
||||
m = create_ipas_steer_command(self.cp, steer, enabled, apgs_enabled)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
steer = (random.randint(0, 2) % 2 == 0)
|
||||
steer_req = (random.randint(0, 2) % 2 == 0)
|
||||
raw_cnt = random.randint(1, 65536)
|
||||
m_old = create_steer_command(self.cp_old, steer, steer_req, raw_cnt)
|
||||
m = create_steer_command(self.cp, steer, steer_req, raw_cnt)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
accel = (random.randint(0, 2) % 2 == 0)
|
||||
pcm_cancel = (random.randint(0, 2) % 2 == 0)
|
||||
standstill_req = (random.randint(0, 2) % 2 == 0)
|
||||
lead = (random.randint(0, 2) % 2 == 0)
|
||||
m_old = create_accel_command(self.cp_old, accel, pcm_cancel, standstill_req, lead)
|
||||
m = create_accel_command(self.cp, accel, pcm_cancel, standstill_req, lead)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
fcw = random.randint(1, 65536)
|
||||
m_old = create_fcw_command(self.cp_old, fcw)
|
||||
m = create_fcw_command(self.cp, fcw)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
steer = (random.randint(0, 2) % 2 == 0)
|
||||
chime = random.randint(1, 65536)
|
||||
left_line = (random.randint(0, 2) % 2 == 0)
|
||||
right_line = (random.randint(0, 2) % 2 == 0)
|
||||
left_lane_depart = (random.randint(0, 2) % 2 == 0)
|
||||
right_lane_depart = (random.randint(0, 2) % 2 == 0)
|
||||
m_old = create_ui_command(self.cp_old, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
|
||||
m = create_ui_command(self.cp, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
|
||||
self.assertEqual(m_old, m)
|
||||
|
||||
def test_performance(self):
|
||||
n1 = sec_since_boot()
|
||||
recursions = 100000
|
||||
steer = (random.randint(0, 2) % 2 == 0)
|
||||
chime = random.randint(1, 65536)
|
||||
left_line = (random.randint(0, 2) % 2 == 0)
|
||||
right_line = (random.randint(0, 2) % 2 == 0)
|
||||
left_lane_depart = (random.randint(0, 2) % 2 == 0)
|
||||
right_lane_depart = (random.randint(0, 2) % 2 == 0)
|
||||
|
||||
for _ in range(recursions):
|
||||
create_ui_command(self.cp_old, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
|
||||
n2 = sec_since_boot()
|
||||
elapsed_old = n2 - n1
|
||||
|
||||
# print('Old API, elapsed time: {} secs'.format(elapsed_old))
|
||||
n1 = sec_since_boot()
|
||||
for _ in range(recursions):
|
||||
create_ui_command(self.cp, steer, chime, left_line, right_line, left_lane_depart, right_lane_depart)
|
||||
n2 = sec_since_boot()
|
||||
elapsed_new = n2 - n1
|
||||
# print('New API, elapsed time: {} secs'.format(elapsed_new))
|
||||
self.assertTrue(elapsed_new < elapsed_old / 2)
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,106 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
import os
|
||||
import unittest
|
||||
|
||||
import requests
|
||||
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.can.parser import CANParser as CANParserNew
|
||||
from selfdrive.can.tests.parser_old import CANParser as CANParserOld
|
||||
from selfdrive.car.honda.carstate import get_can_signals
|
||||
from selfdrive.car.honda.interface import CarInterface
|
||||
from selfdrive.car.honda.values import CAR, DBC
|
||||
from selfdrive.services import service_list
|
||||
from tools.lib.logreader import LogReader
|
||||
|
||||
BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/"
|
||||
DT = int(0.01 * 1e9) # ns
|
||||
|
||||
|
||||
def dict_keys_differ(dict1, dict2):
|
||||
keys1 = set(dict1.keys())
|
||||
keys2 = set(dict2.keys())
|
||||
|
||||
if keys1 != keys2:
|
||||
return True
|
||||
|
||||
for k in keys1:
|
||||
keys1 = set(dict1[k].keys())
|
||||
keys2 = set(dict2[k].keys())
|
||||
|
||||
if keys1 != keys2:
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
def dicts_vals_differ(dict1, dict2):
|
||||
for k_outer in dict1.keys():
|
||||
for k_inner in dict1[k_outer].keys():
|
||||
if dict1[k_outer][k_inner] != dict2[k_outer][k_inner]:
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
def run_route(route):
|
||||
can = messaging.pub_sock(service_list['can'].port)
|
||||
|
||||
CP = CarInterface.get_params(CAR.CIVIC)
|
||||
signals, checks = get_can_signals(CP)
|
||||
parser_old = CANParserOld(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1")
|
||||
parser_new = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1, tcp_addr="127.0.0.1")
|
||||
parser_string = CANParserNew(DBC[CP.carFingerprint]['pt'], signals, checks, 0, timeout=-1)
|
||||
|
||||
if dict_keys_differ(parser_old.vl, parser_new.vl):
|
||||
return False
|
||||
|
||||
lr = LogReader(route + ".bz2")
|
||||
|
||||
route_ok = True
|
||||
|
||||
for msg in lr:
|
||||
if msg.which() == 'can':
|
||||
t = msg.logMonoTime
|
||||
msg_bytes = msg.as_builder().to_bytes()
|
||||
can.send(msg_bytes)
|
||||
|
||||
_, updated_old = parser_old.update(t, True)
|
||||
_, updated_new = parser_new.update(t, True)
|
||||
updated_string = parser_string.update_string(msg_bytes)
|
||||
|
||||
if updated_old != updated_new:
|
||||
route_ok = False
|
||||
print(t, "Diff in seen")
|
||||
|
||||
if updated_new != updated_string:
|
||||
route_ok = False
|
||||
print(t, "Diff in seen string")
|
||||
|
||||
if dicts_vals_differ(parser_old.vl, parser_new.vl):
|
||||
print(t, "Diff in dict")
|
||||
route_ok = False
|
||||
|
||||
if dicts_vals_differ(parser_new.vl, parser_string.vl):
|
||||
print(t, "Diff in dict string")
|
||||
route_ok = False
|
||||
|
||||
return route_ok
|
||||
|
||||
class TestCanParser(unittest.TestCase):
|
||||
def setUp(self):
|
||||
self.routes = {
|
||||
CAR.CIVIC: "b0c9d2329ad1606b|2019-05-30--20-23-57"
|
||||
}
|
||||
|
||||
for route in self.routes.values():
|
||||
route_filename = route + ".bz2"
|
||||
if not os.path.isfile(route_filename):
|
||||
with open(route + ".bz2", "wb") as f:
|
||||
f.write(requests.get(BASE_URL + route_filename).content)
|
||||
|
||||
def test_parser_civic(self):
|
||||
self.assertTrue(run_route(self.routes[CAR.CIVIC]))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -1,6 +1,4 @@
|
|||
import os
|
||||
import zmq
|
||||
from cereal import car
|
||||
from common.params import Params
|
||||
from common.basedir import BASEDIR
|
||||
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars
|
||||
|
|
@ -9,17 +7,6 @@ from selfdrive.swaglog import cloudlog
|
|||
import selfdrive.messaging as messaging
|
||||
from selfdrive.car import gen_empty_fingerprint
|
||||
|
||||
|
||||
def get_one_can(logcan):
|
||||
while True:
|
||||
try:
|
||||
can = messaging.recv_one(logcan)
|
||||
if len(can.can) > 0:
|
||||
return can
|
||||
except zmq.error.Again:
|
||||
continue
|
||||
|
||||
|
||||
def get_startup_alert(car_recognized, controller_available):
|
||||
alert = 'startup'
|
||||
if not car_recognized:
|
||||
|
|
@ -69,14 +56,8 @@ def only_toyota_left(candidate_cars):
|
|||
# BOUNTY: every added fingerprint in selfdrive/car/*/values.py is a $100 coupon code on shop.comma.ai
|
||||
# **** for use live only ****
|
||||
def fingerprint(logcan, sendcan, has_relay):
|
||||
params = Params()
|
||||
car_params = params.get("CarParams")
|
||||
|
||||
if car_params is not None:
|
||||
# use already stored VIN: a new VIN query cannot be done, since panda isn't in ELM327 mode
|
||||
car_params = car.CarParams.from_bytes(car_params)
|
||||
vin = VIN_UNKNOWN if car_params.carVin == "" else car_params.carVin
|
||||
elif has_relay:
|
||||
if has_relay:
|
||||
# Vin query only reliably works thorugh OBDII
|
||||
vin = get_vin(logcan, sendcan, 1)
|
||||
else:
|
||||
|
|
@ -93,7 +74,7 @@ def fingerprint(logcan, sendcan, has_relay):
|
|||
done = False
|
||||
|
||||
while not done:
|
||||
a = get_one_can(logcan)
|
||||
a = messaging.get_one_can(logcan)
|
||||
|
||||
for can in a.can:
|
||||
# need to independently try to fingerprint both bus 0 and 1 to work
|
||||
|
|
|
|||
|
|
@ -1,16 +1,9 @@
|
|||
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
|
||||
from selfdrive.car.chrysler.values import ECU, CAR, SteerLimitParams
|
||||
from selfdrive.can.packer import CANPacker
|
||||
|
||||
class SteerLimitParams:
|
||||
STEER_MAX = 261 # 262 faults
|
||||
STEER_DELTA_UP = 3 # 3 is stock. 100 is fine. 200 is too much it seems
|
||||
STEER_DELTA_DOWN = 3 # no faults on the way down it seems
|
||||
STEER_ERROR_MAX = 80
|
||||
|
||||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, car_fingerprint, enable_camera):
|
||||
self.braking = False
|
||||
|
|
|
|||
|
|
@ -1,5 +1,13 @@
|
|||
from selfdrive.car import dbc_dict
|
||||
|
||||
class SteerLimitParams:
|
||||
STEER_MAX = 261 # 262 faults
|
||||
STEER_DELTA_UP = 3 # 3 is stock. 100 is fine. 200 is too much it seems
|
||||
STEER_DELTA_DOWN = 3 # no faults on the way down it seems
|
||||
STEER_ERROR_MAX = 80
|
||||
|
||||
|
||||
|
||||
class CAR:
|
||||
PACIFICA_2017_HYBRID = "CHRYSLER PACIFICA HYBRID 2017"
|
||||
PACIFICA_2018_HYBRID = "CHRYSLER PACIFICA HYBRID 2018"
|
||||
|
|
|
|||
|
|
@ -61,7 +61,6 @@ class CarInterface(CarInterfaceBase):
|
|||
candidate == CAR.CADILLAC_CT6
|
||||
ret.openpilotLongitudinalControl = ret.enableCamera
|
||||
tire_stiffness_factor = 0.444 # not optimized yet
|
||||
ret.safetyModelPassive = car.CarParams.SafetyModel.gmPassive
|
||||
|
||||
if candidate == CAR.VOLT:
|
||||
# supports stop and go, but initial engage must be above 18mph (which include conservatism)
|
||||
|
|
|
|||
|
|
@ -354,19 +354,3 @@ class CarState():
|
|||
|
||||
# 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
|
||||
|
||||
# carstate standalone tester
|
||||
if __name__ == '__main__':
|
||||
import zmq
|
||||
context = zmq.Context()
|
||||
|
||||
class CarParams():
|
||||
def __init__(self):
|
||||
self.carFingerprint = "HONDA CIVIC 2016 TOURING"
|
||||
self.enableGasInterceptor = 0
|
||||
CP = CarParams()
|
||||
CS = CarState(CP)
|
||||
|
||||
# while 1:
|
||||
# CS.update()
|
||||
# time.sleep(0.01)
|
||||
|
|
|
|||
|
|
@ -81,8 +81,9 @@ FINGERPRINTS = {
|
|||
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,
|
||||
}],
|
||||
# msg 1115 has seen with len 2 and 4, so ignore it
|
||||
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: 4, 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
|
||||
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, 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
|
||||
}],
|
||||
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
|
||||
|
|
|
|||
|
|
@ -2,19 +2,10 @@ from selfdrive.car import apply_std_steer_torque_limits
|
|||
from selfdrive.car.hyundai.hyundaican import create_lkas11, create_lkas12, \
|
||||
create_1191, create_1156, \
|
||||
create_clu11
|
||||
from selfdrive.car.hyundai.values import Buttons
|
||||
from selfdrive.car.hyundai.values import Buttons, SteerLimitParams
|
||||
from selfdrive.can.packer import CANPacker
|
||||
|
||||
|
||||
# Steer torque limits
|
||||
|
||||
class SteerLimitParams:
|
||||
STEER_MAX = 255 # 409 is the max, 255 is stock
|
||||
STEER_DELTA_UP = 3
|
||||
STEER_DELTA_DOWN = 7
|
||||
STEER_DRIVER_ALLOWANCE = 50
|
||||
STEER_DRIVER_MULTIPLIER = 2
|
||||
STEER_DRIVER_FACTOR = 1
|
||||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, car_fingerprint):
|
||||
|
|
|
|||
|
|
@ -9,6 +9,16 @@ def get_hud_alerts(visual_alert):
|
|||
else:
|
||||
return 0
|
||||
|
||||
# Steer torque limits
|
||||
|
||||
class SteerLimitParams:
|
||||
STEER_MAX = 255 # 409 is the max, 255 is stock
|
||||
STEER_DELTA_UP = 3
|
||||
STEER_DELTA_DOWN = 7
|
||||
STEER_DRIVER_ALLOWANCE = 50
|
||||
STEER_DRIVER_MULTIPLIER = 2
|
||||
STEER_DRIVER_FACTOR = 1
|
||||
|
||||
class CAR:
|
||||
ELANTRA = "HYUNDAI ELANTRA LIMITED ULTIMATE 2017"
|
||||
GENESIS = "HYUNDAI GENESIS 2018"
|
||||
|
|
|
|||
|
|
@ -13,6 +13,10 @@ class CarInterfaceBase():
|
|||
def calc_accel_override(a_ego, a_target, v_ego, v_target):
|
||||
return 1.
|
||||
|
||||
@staticmethod
|
||||
def compute_gb(accel, speed):
|
||||
raise NotImplementedError
|
||||
|
||||
@staticmethod
|
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay=False):
|
||||
raise NotImplementedError
|
||||
|
|
|
|||
|
|
@ -1,27 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
import sys
|
||||
from cereal import car
|
||||
from common.params import Params
|
||||
|
||||
# This script locks the safety model to a given value.
|
||||
# When the safety model is locked, boardd will preset panda to the locked safety model
|
||||
|
||||
# run example:
|
||||
# ./lock_safety_model.py gm
|
||||
|
||||
if __name__ == "__main__":
|
||||
|
||||
params = Params()
|
||||
|
||||
if len(sys.argv) < 2:
|
||||
params.delete("SafetyModelLock")
|
||||
print("Clear locked safety model")
|
||||
|
||||
else:
|
||||
safety_model = getattr(car.CarParams.SafetyModel, sys.argv[1])
|
||||
if type(safety_model) != int:
|
||||
raise Exception("Invalid safety model: " + sys.argv[1])
|
||||
if safety_model == car.CarParams.SafetyModel.allOutput:
|
||||
raise Exception("Locking the safety model to allOutput is not allowed")
|
||||
params.put("SafetyModelLock", str(safety_model))
|
||||
print("Locked safety model: " + sys.argv[1])
|
||||
|
|
@ -1,7 +1,6 @@
|
|||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.swaglog import cloudlog
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.car import gen_empty_fingerprint
|
||||
|
|
@ -16,15 +15,14 @@ LPG = 2 * 3.1415 * YAW_FR * TS / (1 + 2 * 3.1415 * YAW_FR * TS)
|
|||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
|
||||
self.CP = CP
|
||||
self.CC = CarController
|
||||
|
||||
cloudlog.debug("Using Mock Car Interface")
|
||||
|
||||
# TODO: subscribe to phone sensor
|
||||
self.sensor = messaging.sub_sock(service_list['sensorEvents'].port)
|
||||
self.gps = messaging.sub_sock(service_list['gpsLocation'].port)
|
||||
self.sensor = messaging.sub_sock('sensorEvents')
|
||||
self.gps = messaging.sub_sock('gpsLocation')
|
||||
|
||||
self.speed = 0.
|
||||
self.prev_speed = 0.
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#from common.numpy_fast import clip
|
||||
from selfdrive.car import apply_std_steer_torque_limits
|
||||
from selfdrive.car.subaru import subarucan
|
||||
from selfdrive.car.subaru.values import CAR, DBC
|
||||
from selfdrive.car.subaru.values import DBC
|
||||
from selfdrive.can.packer import CANPacker
|
||||
|
||||
|
||||
|
|
@ -11,10 +11,9 @@ class CarControllerParams():
|
|||
self.STEER_STEP = 2 # how often we update the steer cmd
|
||||
self.STEER_DELTA_UP = 50 # torque increase per refresh, 0.8s to max
|
||||
self.STEER_DELTA_DOWN = 70 # torque decrease per refresh
|
||||
if car_fingerprint == CAR.IMPREZA:
|
||||
self.STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting
|
||||
self.STEER_DRIVER_MULTIPLIER = 10 # weight driver torque heavily
|
||||
self.STEER_DRIVER_FACTOR = 1 # from dbc
|
||||
self.STEER_DRIVER_ALLOWANCE = 60 # allowed driver torque before start limiting
|
||||
self.STEER_DRIVER_MULTIPLIER = 10 # weight driver torque heavily
|
||||
self.STEER_DRIVER_FACTOR = 1 # from dbc
|
||||
|
||||
|
||||
|
||||
|
|
@ -30,7 +29,6 @@ class CarController():
|
|||
# Setup detection helper. Routes commands to
|
||||
# an appropriate CAN bus number.
|
||||
self.params = CarControllerParams(car_fingerprint)
|
||||
print(DBC)
|
||||
self.packer = CANPacker(DBC[car_fingerprint]['pt'])
|
||||
|
||||
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, left_line, right_line):
|
||||
|
|
|
|||
|
|
@ -123,7 +123,7 @@ class CarState():
|
|||
self.v_cruise_pcm *= CV.MPH_TO_KPH
|
||||
|
||||
v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
|
||||
# Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default
|
||||
# Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default
|
||||
if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
|
||||
self.v_ego_kf.x = [[v_wheel], [0.0]]
|
||||
|
||||
|
|
|
|||
|
|
@ -7,7 +7,7 @@ FINGERPRINTS = {
|
|||
CAR.IMPREZA: [{
|
||||
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1722: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8
|
||||
},
|
||||
# Crosstrek 2018 (same platform as Subaru)
|
||||
# Crosstrek 2018 (same platform as Impreza)
|
||||
{
|
||||
2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 256: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 372: 8, 544: 8, 545: 8, 546: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 811: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8
|
||||
}],
|
||||
|
|
|
|||
|
|
@ -2,11 +2,11 @@ from cereal import car
|
|||
from common.numpy_fast import clip, interp
|
||||
from selfdrive.car import apply_toyota_steer_torque_limits
|
||||
from selfdrive.car import create_gas_command
|
||||
from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
|
||||
from selfdrive.car.toyota.toyotacan import make_can_msg, \
|
||||
create_steer_command, create_ui_command, \
|
||||
create_ipas_steer_command, create_accel_command, \
|
||||
create_acc_cancel_command, create_fcw_command
|
||||
from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS, TSS2_CAR
|
||||
from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS, TSS2_CAR, SteerLimitParams
|
||||
from selfdrive.can.packer import CANPacker
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
|
@ -17,12 +17,6 @@ ACCEL_MAX = 1.5 # 1.5 m/s2
|
|||
ACCEL_MIN = -3.0 # 3 m/s2
|
||||
ACCEL_SCALE = max(ACCEL_MAX, -ACCEL_MIN)
|
||||
|
||||
# Steer torque limits
|
||||
class SteerLimitParams:
|
||||
STEER_MAX = 1500
|
||||
STEER_DELTA_UP = 10 # 1.5s time to peak torque
|
||||
STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
|
||||
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
|
||||
|
||||
# Steer angle limits (tested at the Crows Landing track and considered ok)
|
||||
ANGLE_MAX_BP = [0., 5.]
|
||||
|
|
@ -114,9 +108,8 @@ class CarController():
|
|||
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
def update(self, enabled, CS, frame, actuators,
|
||||
pcm_cancel_cmd, hud_alert, forwarding_camera, left_line,
|
||||
right_line, lead, left_lane_depart, right_lane_depart):
|
||||
def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert,
|
||||
left_line, right_line, lead, left_lane_depart, right_lane_depart):
|
||||
|
||||
# *** compute control surfaces ***
|
||||
|
||||
|
|
@ -223,10 +216,6 @@ class CarController():
|
|||
# This prevents unexpected pedal range rescaling
|
||||
can_sends.append(create_gas_command(self.packer, apply_gas, frame//2))
|
||||
|
||||
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
|
||||
for addr in TARGET_IDS:
|
||||
can_sends.append(create_video_target(frame//10, addr))
|
||||
|
||||
# ui mesg is at 100Hz but we send asap if:
|
||||
# - there is something to display
|
||||
# - there is something to stop displaying
|
||||
|
|
@ -253,19 +242,7 @@ class CarController():
|
|||
#*** static msgs ***
|
||||
|
||||
for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS:
|
||||
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars and not (ecu == ECU.CAM and forwarding_camera):
|
||||
# special cases
|
||||
if fr_step == 5 and ecu == ECU.CAM and bus == 1:
|
||||
cnt = (((frame // 5) % 7) + 1) << 5
|
||||
vl = bytes([cnt]) + vl
|
||||
elif addr in (0x489, 0x48a) and bus == 0:
|
||||
# add counter for those 2 messages (last 4 bits)
|
||||
cnt = ((frame // 100) % 0xf) + 1
|
||||
if addr == 0x48a:
|
||||
# 0x48a has a 8 preceding the counter
|
||||
cnt += 1 << 7
|
||||
vl += bytes([cnt])
|
||||
|
||||
if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars:
|
||||
can_sends.append(make_can_msg(addr, vl, bus, False))
|
||||
|
||||
return can_sends
|
||||
|
|
|
|||
|
|
@ -28,8 +28,6 @@ class CarInterface(CarInterfaceBase):
|
|||
self.cp = get_can_parser(CP)
|
||||
self.cp_cam = get_cam_can_parser(CP)
|
||||
|
||||
self.forwarding_camera = False
|
||||
|
||||
self.CC = None
|
||||
if CarController is not None:
|
||||
self.CC = CarController(self.cp.dbc_name, CP.carFingerprint, CP.enableCamera, CP.enableDsu, CP.enableApgs)
|
||||
|
|
@ -370,11 +368,9 @@ class CarInterface(CarInterfaceBase):
|
|||
|
||||
# events
|
||||
events = []
|
||||
if self.cp_cam.can_valid:
|
||||
self.forwarding_camera = True
|
||||
|
||||
if self.cp_cam.can_invalid_cnt >= 100 and self.CP.enableCamera:
|
||||
events.append(create_event('invalidGiraffeToyota', [ET.PERMANENT]))
|
||||
events.append(create_event('invalidGiraffeToyota', [ET.PERMANENT, ET.NO_ENTRY]))
|
||||
if not ret.gearShifter == GearShifter.drive and self.CP.enableDsu:
|
||||
events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
|
||||
if ret.doorOpen:
|
||||
|
|
@ -427,8 +423,8 @@ class CarInterface(CarInterfaceBase):
|
|||
def apply(self, c):
|
||||
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame,
|
||||
c.actuators, c.cruiseControl.cancel, c.hudControl.visualAlert,
|
||||
self.forwarding_camera, c.hudControl.leftLaneVisible,
|
||||
c.actuators, c.cruiseControl.cancel,
|
||||
c.hudControl.visualAlert, c.hudControl.leftLaneVisible,
|
||||
c.hudControl.rightLaneVisible, c.hudControl.leadVisible,
|
||||
c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
|
||||
|
||||
|
|
|
|||
|
|
@ -22,12 +22,6 @@ def make_can_msg(addr, dat, alt, cks=False):
|
|||
return [addr, 0, dat, alt]
|
||||
|
||||
|
||||
def create_video_target(frame, addr):
|
||||
counter = frame & 0xff
|
||||
msg = struct.pack("!BBBBBBB", counter, 0x03, 0xff, 0x00, 0x00, 0x00, 0x00)
|
||||
return make_can_msg(addr, msg, 1, True)
|
||||
|
||||
|
||||
def create_ipas_steer_command(packer, steer, enabled, apgs_enabled):
|
||||
"""Creates a CAN message for the Toyota Steer Command."""
|
||||
if steer < 0:
|
||||
|
|
|
|||
|
|
@ -1,5 +1,12 @@
|
|||
from selfdrive.car import dbc_dict
|
||||
|
||||
# Steer torque limits
|
||||
class SteerLimitParams:
|
||||
STEER_MAX = 1500
|
||||
STEER_DELTA_UP = 10 # 1.5s time to peak torque
|
||||
STEER_DELTA_DOWN = 25 # always lower than 45 otherwise the Rav4 faults (Prius seems ok with 50)
|
||||
STEER_ERROR_MAX = 350 # max delta between torque cmd and torque motor
|
||||
|
||||
class CAR:
|
||||
PRIUS = "TOYOTA PRIUS 2017"
|
||||
RAV4H = "TOYOTA RAV4 HYBRID 2017"
|
||||
|
|
@ -31,21 +38,6 @@ class ECU:
|
|||
|
||||
# addr: (ecu, cars, bus, 1/freq*100, vl)
|
||||
STATIC_MSGS = [
|
||||
(0x130, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 100, b'\x00\x00\x00\x00\x00\x00\x38'),
|
||||
(0x240, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x10\x01\x00\x10\x01\x00'),
|
||||
(0x241, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x10\x01\x00\x10\x01\x00'),
|
||||
(0x244, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x10\x01\x00\x10\x01\x00'),
|
||||
(0x245, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x10\x01\x00\x10\x01\x00'),
|
||||
(0x248, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 1, 5, b'\x00\x00\x00\x00\x00\x00\x01'),
|
||||
(0x367, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 40, b'\x06\x00'),
|
||||
(0x414, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, b'\x00\x00\x00\x00\x00\x00\x17\x00'),
|
||||
(0x466, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.HIGHLANDER, CAR.HIGHLANDERH), 1, 100, b'\x20\x20\xAD'),
|
||||
(0x466, ECU.CAM, (CAR.COROLLA, CAR.AVALON), 1, 100, b'\x24\x20\xB1'),
|
||||
(0x489, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x48a, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, b'\x00\x00\x00\x00\x00\x00\x00'),
|
||||
(0x48b, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON), 0, 100, b'\x66\x06\x08\x0a\x02\x00\x00\x00'),
|
||||
(0x4d3, ECU.CAM, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 0, 100, b'\x1C\x00\x00\x01\x00\x00\x00\x00'),
|
||||
|
||||
(0x128, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.AVALON), 1, 3, b'\xf4\x01\x90\x83\x00\x37'),
|
||||
(0x128, ECU.DSU, (CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.SIENNA, CAR.LEXUS_CTH), 1, 3, b'\x03\x00\x20\x00\x00\x52'),
|
||||
(0x141, ECU.DSU, (CAR.PRIUS, CAR.RAV4H, CAR.LEXUS_RXH, CAR.RAV4, CAR.COROLLA, CAR.HIGHLANDER, CAR.HIGHLANDERH, CAR.AVALON, CAR.SIENNA, CAR.LEXUS_CTH), 1, 2, b'\x00\x00\x00\x46'),
|
||||
|
|
@ -185,12 +177,11 @@ FINGERPRINTS = {
|
|||
36: 8, 37: 8, 114: 5, 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, 705: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 764: 8, 765: 8, 800: 8, 810: 2, 812: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 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, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1172: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1595: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1809: 8, 1816: 8, 1817: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1960: 8, 1981: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8
|
||||
}],
|
||||
CAR.COROLLAH_TSS2: [
|
||||
# 2019 Taiwan Altis Hybrid
|
||||
{
|
||||
# 2019 Taiwan Altis Hybrid
|
||||
{
|
||||
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 765: 8, 800: 8, 810: 2, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 896: 8, 898: 8, 918: 7, 921: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1082: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1172: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1745: 8, 1775: 8, 1779: 8
|
||||
}],
|
||||
CAR.LEXUS_ES_TSS2: [
|
||||
{
|
||||
CAR.LEXUS_ES_TSS2: [{
|
||||
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, 728: 8, 740: 5, 761: 8, 764: 8, 765: 8, 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, 882: 8, 885: 8, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 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, 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, 1649: 8, 1696: 8, 1775: 8, 1777: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8
|
||||
}],
|
||||
CAR.LEXUS_ESH_TSS2: [
|
||||
|
|
|
|||
|
|
@ -80,7 +80,7 @@ def get_vin(logcan, sendcan, bus, query_time=1.):
|
|||
|
||||
# 1s max of VIN query time
|
||||
while frame < query_time * 100:
|
||||
a = messaging.recv_one(logcan)
|
||||
a = messaging.get_one_can(logcan)
|
||||
|
||||
for can in a.can:
|
||||
vin_query.check_response(can)
|
||||
|
|
@ -92,7 +92,6 @@ def get_vin(logcan, sendcan, bus, query_time=1.):
|
|||
|
||||
|
||||
if __name__ == "__main__":
|
||||
from selfdrive.services import service_list
|
||||
logcan = messaging.sub_sock(service_list['can'].port)
|
||||
sendcan = messaging.pub_sock(service_list['sendcan'].port)
|
||||
logcan = messaging.sub_sock('can')
|
||||
sendcan = messaging.pub_sock('sendcan')
|
||||
print(get_vin(logcan, sendcan, 0))
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
|
||||
|
|
@ -0,0 +1,188 @@
|
|||
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.can.packer import CANPacker
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
class CarController():
|
||||
def __init__(self, canbus, car_fingerprint):
|
||||
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_gw = CANPacker(DBC[car_fingerprint]['pt'])
|
||||
|
||||
self.hcaSameTorqueCount = 0
|
||||
self.hcaEnabledFrameCount = 0
|
||||
self.graButtonStatesToSend = None
|
||||
self.graMsgSentCount = 0
|
||||
self.graMsgStartFramePrev = 0
|
||||
self.graMsgBusCounterPrev = 0
|
||||
|
||||
def update(self, enabled, CS, frame, actuators, visual_alert, audible_alert, leftLaneVisible, rightLaneVisible):
|
||||
""" Controls thread """
|
||||
|
||||
P = CarControllerParams
|
||||
|
||||
# Send CAN commands.
|
||||
can_sends = []
|
||||
canbus = self.canbus
|
||||
|
||||
#--------------------------------------------------------------------------
|
||||
# #
|
||||
# Prepare HCA_01 Heading Control Assist messages with steering torque. #
|
||||
# #
|
||||
#--------------------------------------------------------------------------
|
||||
|
||||
# The factory camera sends at 50Hz while steering and 1Hz when not. When
|
||||
# OP is active, Panda filters HCA_01 from the factory camera and OP emits
|
||||
# HCA_01 at 50Hz. Rate switching creates some confusion in Cabana and
|
||||
# doesn't seem to add value at this time. The rack will accept HCA_01 at
|
||||
# 100Hz if we want to control at finer resolution in the future.
|
||||
if frame % P.HCA_STEP == 0:
|
||||
|
||||
# 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):
|
||||
|
||||
# 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.
|
||||
apply_steer = int(round(actuators.steer * P.STEER_MAX))
|
||||
apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steeringTorque, P)
|
||||
|
||||
# FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending
|
||||
# a single frame with HCA disabled is an effective workaround.
|
||||
if apply_steer == 0:
|
||||
# We can usually reset the timer for free, just by disabling HCA
|
||||
# when apply_steer is exactly zero, which happens by chance during
|
||||
# many steer torque direction changes. This could be expanded with
|
||||
# a small dead-zone to capture all zero crossings, but not seeing a
|
||||
# major need at this time.
|
||||
hcaEnabled = False
|
||||
self.hcaEnabledFrameCount = 0
|
||||
else:
|
||||
self.hcaEnabledFrameCount += 1
|
||||
if self.hcaEnabledFrameCount >= 118 * (100 / P.HCA_STEP): # 118s
|
||||
# The Kansas I-70 Crosswind Problem: if we truly do need to steer
|
||||
# in one direction for > 360 seconds, we have to disable HCA for a
|
||||
# frame while actively steering. Testing shows we can just set the
|
||||
# disabled flag, and keep sending non-zero torque, which keeps the
|
||||
# Panda torque rate limiting safety happy. Do so 3x within the 360
|
||||
# second window for safety and redundancy.
|
||||
hcaEnabled = False
|
||||
self.hcaEnabledFrameCount = 0
|
||||
else:
|
||||
hcaEnabled = True
|
||||
# FAULT AVOIDANCE: HCA torque must not be static for > 6 seconds.
|
||||
# This is to detect the sending camera being stuck or frozen. OP
|
||||
# can trip this on a curve if steering is saturated. Avoid this by
|
||||
# reducing torque 0.01 Nm for one frame. Do so 3x within the 6
|
||||
# second period for safety and redundancy.
|
||||
if self.apply_steer_last == apply_steer:
|
||||
self.hcaSameTorqueCount += 1
|
||||
if self.hcaSameTorqueCount > 1.9 * (100 / P.HCA_STEP): # 1.9s
|
||||
apply_steer -= (1, -1)[apply_steer < 0]
|
||||
self.hcaSameTorqueCount = 0
|
||||
else:
|
||||
self.hcaSameTorqueCount = 0
|
||||
|
||||
else:
|
||||
# Continue sending HCA_01 messages, with the enable flags turned off.
|
||||
hcaEnabled = False
|
||||
apply_steer = 0
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
idx = (frame / P.HCA_STEP) % 16
|
||||
can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_gw, canbus.gateway, apply_steer,
|
||||
idx, hcaEnabled))
|
||||
|
||||
#--------------------------------------------------------------------------
|
||||
# #
|
||||
# Prepare LDW_02 HUD messages with lane borders, confidence levels, and #
|
||||
# the LKAS status LED. #
|
||||
# #
|
||||
#--------------------------------------------------------------------------
|
||||
|
||||
# The factory camera emits this message at 10Hz. When OP is active, Panda
|
||||
# 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
|
||||
|
||||
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_gw, canbus.gateway, hcaEnabled,
|
||||
CS.steeringPressed, hud_alert, leftLaneVisible,
|
||||
rightLaneVisible))
|
||||
|
||||
#--------------------------------------------------------------------------
|
||||
# #
|
||||
# Prepare GRA_ACC_01 ACC control messages with button press events. #
|
||||
# #
|
||||
#--------------------------------------------------------------------------
|
||||
|
||||
# The car sends this message at 33hz. OP sends it on-demand only for
|
||||
# virtual button presses.
|
||||
#
|
||||
# First create any virtual button press event needed by openpilot, to sync
|
||||
# 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:
|
||||
# Cancel ACC if it's engaged with OP disengaged.
|
||||
self.graButtonStatesToSend = BUTTON_STATES.copy()
|
||||
self.graButtonStatesToSend["cancel"] = True
|
||||
elif enabled and CS.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.
|
||||
self.graButtonStatesToSend = BUTTON_STATES.copy()
|
||||
self.graButtonStatesToSend["resumeCruise"] = True
|
||||
|
||||
# OP/Panda can see this message but can't filter it when integrated at the
|
||||
# R242 LKAS camera. It could do so if integrated at the J533 gateway, but
|
||||
# we need a generalized solution that works for either. The message is
|
||||
# counter-protected, so we need to time our transmissions very precisely
|
||||
# to achieve fast and fault-free switching between message flows accepted
|
||||
# at the J428 ACC radar.
|
||||
#
|
||||
# Example message flow on the bus, frequency of 33Hz (GRA_ACC_STEP):
|
||||
#
|
||||
# CAR: 0 1 2 3 4 5 6 7 8 9 A B C D E F 0 1 2 3 4 5 6
|
||||
# EON: 3 4 5 6 7 8 9 A B C D E F 0 1 2 GG^
|
||||
#
|
||||
# If OP needs to send a button press, it waits to see a GRA_ACC_01 message
|
||||
# counter change, and then immediately follows up with the next increment.
|
||||
# The OP message will be sent within about 1ms of the car's message, which
|
||||
# is about 2ms before the car's next message is expected. OP sends for an
|
||||
# arbitrary duration of 16 messages / ~0.5 sec, in lockstep with each new
|
||||
# message from the car.
|
||||
#
|
||||
# Because OP's counter is synced to the car, J428 immediately accepts the
|
||||
# OP messages as valid. Further messages from the car get discarded as
|
||||
# duplicates without a fault. When OP stops sending, the extra time gap
|
||||
# (GG) to the next valid car message is less than 1 * GRA_ACC_STEP. J428
|
||||
# tolerates the gap just fine and control returns to the car immediately.
|
||||
|
||||
if CS.graMsgBusCounter != self.graMsgBusCounterPrev:
|
||||
self.graMsgBusCounterPrev = CS.graMsgBusCounter
|
||||
if self.graButtonStatesToSend is not None:
|
||||
if self.graMsgSentCount == 0:
|
||||
self.graMsgStartFramePrev = frame
|
||||
idx = (CS.graMsgBusCounter + 1) % 16
|
||||
can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_gw, canbus.extended, self.graButtonStatesToSend, CS, idx))
|
||||
self.graMsgSentCount += 1
|
||||
if self.graMsgSentCount >= 16:
|
||||
self.graButtonStatesToSend = None
|
||||
self.graMsgSentCount = 0
|
||||
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,229 @@
|
|||
import numpy as np
|
||||
from cereal import car
|
||||
from common.kalman.simple_kalman import KF1D
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.can.parser import CANParser
|
||||
from selfdrive.can.can_define import CANDefine
|
||||
from selfdrive.car.volkswagen.values import DBC, BUTTON_STATES
|
||||
from selfdrive.car.volkswagen.carcontroller import CarControllerParams
|
||||
|
||||
GEAR = car.CarState.GearShifter
|
||||
|
||||
def get_mqb_gateway_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
|
||||
("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
|
||||
("Motor_20", 50), # From J623 Engine control module
|
||||
("GRA_ACC_01", 33), # From J??? steering wheel control buttons
|
||||
("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.gateway)
|
||||
|
||||
def get_mqb_extended_can_parser(CP, canbus):
|
||||
|
||||
signals = [
|
||||
# sig_name, sig_address, default
|
||||
("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
|
||||
]
|
||||
|
||||
checks = [
|
||||
# sig_address, frequency
|
||||
("ACC_06", 50), # From J428 ACC radar control module
|
||||
("ACC_02", 17), # From J428 ACC radar control module
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.extended)
|
||||
|
||||
def parse_gear_shifter(gear, vals):
|
||||
# Return mapping of gearshift position to selected gear.
|
||||
|
||||
val_to_capnp = {'P': GEAR.park, 'R': GEAR.reverse, 'N': GEAR.neutral,
|
||||
'D': GEAR.drive, 'E': GEAR.eco, 'S': GEAR.sport, 'T': GEAR.manumatic}
|
||||
try:
|
||||
return val_to_capnp[vals[gear]]
|
||||
except KeyError:
|
||||
return "unknown"
|
||||
|
||||
class CarState():
|
||||
def __init__(self, CP, canbus):
|
||||
# initialize can parser
|
||||
self.CP = CP
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
|
||||
|
||||
self.shifter_values = self.can_define.dv["Getriebe_11"]['GE_Fahrstufe']
|
||||
|
||||
self.buttonStates = BUTTON_STATES.copy()
|
||||
|
||||
# vEgo Kalman filter
|
||||
dt = 0.01
|
||||
self.v_ego_kf = KF1D(x0=[[0.], [0.]],
|
||||
A=[[1., dt], [0., 1.]],
|
||||
C=[1., 0.],
|
||||
K=[[0.12287673], [0.29666309]])
|
||||
|
||||
def update(self, gw_cp, ex_cp):
|
||||
# Update vehicle speed and acceleration from ABS wheel speeds.
|
||||
self.wheelSpeedFL = gw_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
self.wheelSpeedFR = gw_cp.vl["ESP_19"]['ESP_VR_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
self.wheelSpeedRL = gw_cp.vl["ESP_19"]['ESP_HL_Radgeschw_02'] * CV.KPH_TO_MS
|
||||
self.wheelSpeedRR = gw_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]))
|
||||
v_ego_x = self.v_ego_kf.update(self.vEgoRaw)
|
||||
self.vEgo = float(v_ego_x[0])
|
||||
self.aEgo = float(v_ego_x[1])
|
||||
self.standstill = self.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 = gw_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1,-1)[int(gw_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
|
||||
self.steeringRate = gw_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1,-1)[int(gw_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
|
||||
self.steeringTorque = gw_cp.vl["EPS_01"]['Driver_Strain'] * (1,-1)[int(gw_cp.vl["EPS_01"]['Driver_Strain_VZ'])]
|
||||
self.steeringPressed = abs(self.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
|
||||
self.yawRate = gw_cp.vl["ESP_02"]['ESP_Gierrate'] * (1,-1)[int(gw_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD
|
||||
|
||||
# Update gas, brakes, and gearshift.
|
||||
self.gas = gw_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0
|
||||
self.gasPressed = self.gas > 0
|
||||
self.brake = gw_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects
|
||||
self.brakePressed = bool(gw_cp.vl["ESP_05"]['ESP_Fahrer_bremst'])
|
||||
self.brakeLights = bool(gw_cp.vl["ESP_05"]['ESP_Status_Bremsdruck'])
|
||||
|
||||
# Update gear and/or clutch position data.
|
||||
can_gear_shifter = int(gw_cp.vl["Getriebe_11"]['GE_Fahrstufe'])
|
||||
self.gearShifter = parse_gear_shifter(can_gear_shifter, self.shifter_values)
|
||||
|
||||
# Update door and trunk/hatch lid open status.
|
||||
self.doorOpen = any([gw_cp.vl["Gateway_72"]['ZV_FT_offen'],
|
||||
gw_cp.vl["Gateway_72"]['ZV_BT_offen'],
|
||||
gw_cp.vl["Gateway_72"]['ZV_HFS_offen'],
|
||||
gw_cp.vl["Gateway_72"]['ZV_HBFS_offen'],
|
||||
gw_cp.vl["Gateway_72"]['ZV_HD_offen']])
|
||||
|
||||
# Update seatbelt fastened status.
|
||||
self.seatbeltUnlatched = False if gw_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.
|
||||
# We use the speed preference for OP.
|
||||
self.displayMetricUnits = not gw_cp.vl["Einheiten_01"]["KBI_MFA_v_Einheit_02"]
|
||||
|
||||
# Update ACC radar status.
|
||||
accStatus = ex_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:
|
||||
# ACC okay and enabled, but not currently engaged
|
||||
self.accFault = False
|
||||
self.accAvailable = True
|
||||
self.accEnabled = 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
|
||||
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
|
||||
|
||||
# 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 = ex_cp.vl["ACC_02"]['SetSpeed']
|
||||
if self.accSetSpeed > 90: self.accSetSpeed = 0
|
||||
|
||||
# Update control button states for turn signals and ACC controls.
|
||||
self.buttonStates["leftBlinker"] = bool(gw_cp.vl["Gateway_72"]['BH_Blinker_li'])
|
||||
self.buttonStates["leftBlinker"] = bool(gw_cp.vl["Gateway_72"]['BH_Blinker_re'])
|
||||
self.buttonStates["accelCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Hoch'])
|
||||
self.buttonStates["decelCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Runter'])
|
||||
self.buttonStates["cancel"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Abbrechen'])
|
||||
self.buttonStates["setCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Setzen'])
|
||||
self.buttonStates["resumeCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Wiederaufnahme'])
|
||||
self.buttonStates["gapAdjustCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Verstellung_Zeitluecke'])
|
||||
|
||||
# Read ACC hardware button type configuration info that has to pass thru
|
||||
# to the radar. Ends up being different for steering wheel buttons vs
|
||||
# third stalk type controls.
|
||||
self.graHauptschalter = gw_cp.vl["GRA_ACC_01"]['GRA_Hauptschalter']
|
||||
self.graTypHauptschalter = gw_cp.vl["GRA_ACC_01"]['GRA_Typ_Hauptschalter']
|
||||
self.graButtonTypeInfo = gw_cp.vl["GRA_ACC_01"]['GRA_ButtonTypeInfo']
|
||||
self.graTipStufe2 = gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Stufe_2']
|
||||
# Pick up the GRA_ACC_01 CAN message counter so we can sync to it for
|
||||
# later cruise-control button spamming.
|
||||
self.graMsgBusCounter = gw_cp.vl["GRA_ACC_01"]['COUNTER']
|
||||
|
||||
# Check to make sure the electric power steering rack is configured to
|
||||
# accept and respond to HCA_01 messages and has not encountered a fault.
|
||||
self.steeringFault = not gw_cp.vl["EPS_01"]["HCA_Ready"]
|
||||
|
||||
# Additional safety checks performed in CarInterface.
|
||||
self.parkingBrakeSet = bool(gw_cp.vl["Kombi_01"]['KBI_Handbremse']) # FIXME: need to include an EPB check as well
|
||||
self.stabilityControlDisabled = gw_cp.vl["ESP_21"]['ESP_Tastung_passiv']
|
||||
|
||||
|
|
@ -0,0 +1,242 @@
|
|||
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_gateway_can_parser, get_mqb_extended_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:
|
||||
gateway = 0
|
||||
extended = 2
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
def __init__(self, CP, CarController):
|
||||
self.CP = CP
|
||||
self.CC = None
|
||||
|
||||
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.gw_cp = get_mqb_gateway_can_parser(CP, CANBUS)
|
||||
self.ex_cp = get_mqb_extended_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(), vin="", has_relay=False):
|
||||
ret = car.CarParams.new_message()
|
||||
|
||||
ret.carFingerprint = candidate
|
||||
ret.isPandaBlack = has_relay
|
||||
ret.carVin = vin
|
||||
|
||||
if candidate == CAR.GOLF:
|
||||
# Set common MQB parameters that will apply globally
|
||||
ret.carName = "volkswagen"
|
||||
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
|
||||
# Steer torque is strongly rate limit and max value is decently high. Off to avoid false positives
|
||||
ret.steerLimitAlert = False
|
||||
|
||||
# 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.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
|
||||
# whether the driver assist map breakpoints have any direct bearing on
|
||||
# HCA assist torque, but if they're good breakpoints for the driver,
|
||||
# they're probably good breakpoints for HCA as well. OP won't be driving
|
||||
# 250kph/155mph but it provides interpolation scaling above 100kmh/62mph.
|
||||
ret.lateralTuning.pid.kpBP = [0., 15 * CV.KPH_TO_MS, 50 * CV.KPH_TO_MS]
|
||||
ret.lateralTuning.pid.kiBP = [0., 15 * CV.KPH_TO_MS, 50 * CV.KPH_TO_MS]
|
||||
|
||||
# FIXME: Per-vehicle parameters need to be reintegrated.
|
||||
# For the time being, per-vehicle stuff is being archived since we
|
||||
# can't auto-detect very well yet. Now that tuning is figured out,
|
||||
# averaged params should work reasonably on a range of cars. Owners
|
||||
# can tweak here, as needed, until we have car type auto-detection.
|
||||
|
||||
ret.mass = 1700 + STD_CARGO_KG
|
||||
ret.wheelbase = 2.75
|
||||
ret.centerToFront = ret.wheelbase * 0.45
|
||||
ret.steerRatio = 15.6
|
||||
ret.lateralTuning.pid.kf = 0.00006
|
||||
ret.lateralTuning.pid.kpV = [0.15, 0.25, 0.60]
|
||||
ret.lateralTuning.pid.kiV = [0.05, 0.05, 0.05]
|
||||
tire_stiffness_factor = 0.6
|
||||
|
||||
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
|
||||
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,
|
||||
tire_stiffness_factor=tire_stiffness_factor)
|
||||
|
||||
return ret
|
||||
|
||||
# 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
|
||||
self.gw_cp.update_strings(can_strings)
|
||||
self.ex_cp.update_strings(can_strings)
|
||||
self.CS.update(self.gw_cp, self.ex_cp)
|
||||
ret.canValid = self.gw_cp.can_valid and self.ex_cp.can_valid
|
||||
|
||||
# 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.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:
|
||||
if self.CS.buttonStates[button] != self.buttonStatesPrev[button]:
|
||||
be = car.CarState.ButtonEvent.new_message()
|
||||
be.type = button
|
||||
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]))
|
||||
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:
|
||||
events.append(create_event('pcmEnable', [ET.ENABLE]))
|
||||
|
||||
ret.events = events
|
||||
ret.buttonEvents = buttonEvents
|
||||
ret.canMonoTimes = canMonoTimes
|
||||
|
||||
# update previous car states
|
||||
self.gasPressedPrev = ret.gasPressed
|
||||
self.brakePressedPrev = ret.brakePressed
|
||||
self.cruiseStateEnabledPrev = 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()
|
||||
|
||||
def apply(self, c):
|
||||
can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
|
||||
c.hudControl.visualAlert,
|
||||
c.hudControl.audibleAlert,
|
||||
c.hudControl.leftLaneVisible,
|
||||
c.hudControl.rightLaneVisible)
|
||||
self.frame += 1
|
||||
return can_sends
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
#!/usr/bin/env python3
|
||||
from selfdrive.car.interfaces import RadarInterfaceBase
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
pass
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
from selfdrive.car import dbc_dict
|
||||
|
||||
class CarControllerParams:
|
||||
HCA_STEP = 2 # HCA_01 message frequency 50Hz
|
||||
LDW_STEP = 10 # LDW_02 message frequency 10Hz
|
||||
GRA_ACC_STEP = 3 # GRA_ACC_01 message frequency 33Hz
|
||||
|
||||
GRA_VBP_STEP = 100 # Send ACC virtual button presses once a second
|
||||
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))
|
||||
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
|
||||
|
||||
BUTTON_STATES = {
|
||||
"leftBlinker": False,
|
||||
"rightBlinker": False,
|
||||
"accelCruise": False,
|
||||
"decelCruise": False,
|
||||
"cancel": False,
|
||||
"setCruise": False,
|
||||
"resumeCruise": False,
|
||||
"gapAdjustCruise": False
|
||||
}
|
||||
|
||||
MQB_LDW_MESSAGES = {
|
||||
"none": 0, # Nothing to display
|
||||
"laneAssistUnavailChime": 1, # "Lane Assist currently not available." with chime
|
||||
"laneAssistUnavailNoSensorChime": 3, # "Lane Assist not available. No sensor view." with chime
|
||||
"laneAssistTakeOverUrgent": 4, # "Lane Assist: Please Take Over Steering" with urgent beep
|
||||
"emergencyAssistUrgent": 6, # "Emergency Assist: Please Take Over Steering" with urgent beep
|
||||
"laneAssistTakeOverChime": 7, # "Lane Assist: Please Take Over Steering" with chime
|
||||
"laneAssistTakeOverSilent": 8, # "Lane Assist: Please Take Over Steering" silent
|
||||
"emergencyAssistChangingLanes": 9, # "Emergency Assist: Changing lanes..." with urgent beep
|
||||
"laneAssistDeactivated": 10, # "Lane Assist deactivated." silent with persistent icon afterward
|
||||
}
|
||||
|
||||
class CAR:
|
||||
GOLF = "Volkswagen Golf"
|
||||
|
||||
FINGERPRINTS = {
|
||||
CAR.GOLF: [
|
||||
# 76b83eb0245de90e|2019-10-21--17-40-42 - jyoung8607 car
|
||||
{64: 8, 134: 8, 159: 8, 173: 8, 178: 8, 253: 8, 257: 8, 260: 8, 262: 8, 264: 8, 278: 8, 279: 8, 283: 8, 286: 8, 288: 8, 289: 8, 290: 8, 294: 8, 299: 8, 302: 8, 346: 8, 385: 8, 418: 8, 427: 8, 668: 8, 679: 8, 681: 8, 695: 8, 779: 8, 780: 8, 783: 8, 792: 8, 795: 8, 804: 8, 806: 8, 807: 8, 808: 8, 809: 8, 870: 8, 896: 8, 897: 8, 898: 8, 901: 8, 917: 8, 919: 8, 949: 8, 958: 8, 960: 4, 981: 8, 987: 8, 988: 8, 991: 8, 997: 8, 1000: 8, 1019: 8, 1120: 8, 1122: 8, 1123: 8, 1124: 8, 1153: 8, 1162: 8, 1175: 8, 1312: 8, 1385: 8, 1413: 8, 1440: 5, 1514: 8, 1515: 8, 1520: 8, 1600: 8, 1601: 8, 1603: 8, 1605: 8, 1624: 8, 1626: 8, 1629: 8, 1631: 8, 1646: 8, 1648: 8, 1712: 6, 1714: 8, 1716: 8, 1717: 8, 1719: 8, 1720: 8, 1721: 8
|
||||
}],
|
||||
}
|
||||
|
||||
class ECU:
|
||||
CAM = 0
|
||||
|
||||
ECU_FINGERPRINT = {
|
||||
ECU.CAM: [294, 919], # HCA_01 Heading Control Assist, LDW_02 Lane Departure Warning
|
||||
}
|
||||
|
||||
DBC = {
|
||||
CAR.GOLF: dbc_dict('vw_mqb_2010', None),
|
||||
}
|
||||
|
|
@ -0,0 +1,52 @@
|
|||
# CAN controls for MQB platform Volkswagen, Audi, Skoda and SEAT.
|
||||
# PQ35/PQ46/NMS, and any future MLB, to come later.
|
||||
|
||||
def create_mqb_steering_control(packer, bus, apply_steer, idx, lkas_enabled):
|
||||
values = {
|
||||
"SET_ME_0X3": 0x3,
|
||||
"Assist_Torque": abs(apply_steer),
|
||||
"Assist_Requested": lkas_enabled,
|
||||
"Assist_VZ": 1 if apply_steer < 0 else 0,
|
||||
"HCA_Available": 1,
|
||||
"HCA_Standby": not lkas_enabled,
|
||||
"HCA_Active": lkas_enabled,
|
||||
"SET_ME_0XFE": 0xFE,
|
||||
"SET_ME_0X07": 0x07,
|
||||
}
|
||||
return packer.make_can_msg("HCA_01", bus, values, idx)
|
||||
|
||||
def create_mqb_hud_control(packer, bus, hca_enabled, steering_pressed, hud_alert, leftLaneVisible, rightLaneVisible):
|
||||
|
||||
if hca_enabled:
|
||||
leftlanehud = 3 if leftLaneVisible else 1
|
||||
rightlanehud = 3 if rightLaneVisible else 1
|
||||
else:
|
||||
leftlanehud = 2 if leftLaneVisible else 1
|
||||
rightlanehud = 2 if rightLaneVisible else 1
|
||||
|
||||
values = {
|
||||
"LDW_Unknown": 2, # FIXME: possible speed or attention relationship
|
||||
"Kombi_Lamp_Orange": 1 if hca_enabled and steering_pressed else 0,
|
||||
"Kombi_Lamp_Green": 1 if hca_enabled and not steering_pressed else 0,
|
||||
"Left_Lane_Status": leftlanehud,
|
||||
"Right_Lane_Status": rightlanehud,
|
||||
"Alert_Message": hud_alert,
|
||||
}
|
||||
return packer.make_can_msg("LDW_02", bus, values)
|
||||
|
||||
def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx):
|
||||
values = {
|
||||
"GRA_Hauptschalter": CS.graHauptschalter,
|
||||
"GRA_Abbrechen": buttonStatesToSend["cancel"],
|
||||
"GRA_Tip_Setzen": buttonStatesToSend["setCruise"],
|
||||
"GRA_Tip_Hoch": buttonStatesToSend["accelCruise"],
|
||||
"GRA_Tip_Runter": buttonStatesToSend["decelCruise"],
|
||||
"GRA_Tip_Wiederaufnahme": buttonStatesToSend["resumeCruise"],
|
||||
"GRA_Verstellung_Zeitluecke": 3 if buttonStatesToSend["gapAdjustCruise"] else 0,
|
||||
"GRA_Typ_Hauptschalter": CS.graTypHauptschalter,
|
||||
"GRA_Codierung": 2,
|
||||
"GRA_Tip_Stufe_2": CS.graTipStufe2,
|
||||
"GRA_ButtonTypeInfo": CS.graButtonTypeInfo
|
||||
}
|
||||
|
||||
return packer.make_can_msg("GRA_ACC_01", bus, values, idx)
|
||||
|
|
@ -9,6 +9,7 @@
|
|||
#include <unistd.h>
|
||||
#include <dirent.h>
|
||||
#include <sys/file.h>
|
||||
#include <sys/stat.h>
|
||||
|
||||
#include <map>
|
||||
#include <string>
|
||||
|
|
@ -109,6 +110,12 @@ int write_db_value(const char* params_path, const char* key, const char* value,
|
|||
goto cleanup;
|
||||
}
|
||||
|
||||
// change permissions to 0666 for apks
|
||||
result = fchmod(tmp_fd, 0666);
|
||||
if (result < 0) {
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
// fsync to force persist the changes.
|
||||
result = fsync(tmp_fd);
|
||||
if (result < 0) {
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@ inline bool starts_with(std::string s, std::string prefix) {
|
|||
template<typename ... Args>
|
||||
inline std::string string_format( const std::string& format, Args ... args ) {
|
||||
size_t size = snprintf( nullptr, 0, format.c_str(), args ... ) + 1;
|
||||
std::unique_ptr<char[]> buf( new char[ size ] );
|
||||
std::unique_ptr<char[]> buf( new char[ size ] );
|
||||
snprintf( buf.get(), size, format.c_str(), args ... );
|
||||
return std::string( buf.get(), buf.get() + size - 1 );
|
||||
}
|
||||
|
|
@ -32,7 +32,7 @@ inline std::string read_file(std::string fn) {
|
|||
|
||||
inline std::string tohex(const uint8_t* buf, size_t buf_size) {
|
||||
std::unique_ptr<char[]> hexbuf(new char[buf_size*2+1]);
|
||||
for (int i=0; i<buf_size; i++) {
|
||||
for (size_t i=0; i < buf_size; i++) {
|
||||
sprintf(&hexbuf[i*2], "%02x", buf[i]);
|
||||
}
|
||||
hexbuf[buf_size*2] = 0;
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
#define COMMA_VERSION "0.6.5-release"
|
||||
#define COMMA_VERSION "0.6.6-release"
|
||||
|
|
|
|||
|
|
@ -1,9 +1,7 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
import gc
|
||||
import json
|
||||
import capnp
|
||||
import zmq
|
||||
from cereal import car, log
|
||||
from common.numpy_fast import clip
|
||||
from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL
|
||||
|
|
@ -11,9 +9,8 @@ from common.profiler import Profiler
|
|||
from common.params import Params, put_nonblocking
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.config import Conversions as CV
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
||||
from selfdrive.car.car_helpers import get_car, get_startup_alert, get_one_can
|
||||
from selfdrive.car.car_helpers import get_car, get_startup_alert
|
||||
from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
|
||||
from selfdrive.controls.lib.drive_helpers import get_events, \
|
||||
create_event, \
|
||||
|
|
@ -55,12 +52,12 @@ def events_to_bytes(events):
|
|||
return ret
|
||||
|
||||
|
||||
def data_sample(CI, CC, sm, can_poller, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
|
||||
def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
|
||||
driver_status, state, mismatch_counter, params):
|
||||
"""Receive data from sockets and create events for battery, temperature and disk space"""
|
||||
|
||||
# Update carstate from CAN and create events
|
||||
can_strs = messaging.drain_sock_raw_poller(can_poller, can_sock, wait_for_one=True)
|
||||
can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True)
|
||||
CS = CI.update(CC, can_strs)
|
||||
|
||||
sm.update(0)
|
||||
|
|
@ -441,18 +438,16 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
|
|||
sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \
|
||||
'gpsLocation'], ignore_alive=['gpsLocation'])
|
||||
|
||||
can_poller = zmq.Poller()
|
||||
|
||||
if can_sock is None:
|
||||
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
|
||||
can_sock = messaging.sub_sock(service_list['can'].port, timeout=can_timeout)
|
||||
can_poller.register(can_sock)
|
||||
can_sock = messaging.sub_sock('can', timeout=can_timeout)
|
||||
|
||||
# wait for health and CAN packets
|
||||
hw_type = messaging.recv_one(sm.sock['health']).health.hwType
|
||||
has_relay = hw_type in [HwType.blackPanda, HwType.uno]
|
||||
print("Waiting for CAN messages...")
|
||||
get_one_can(can_sock)
|
||||
messaging.get_one_can(can_sock)
|
||||
|
||||
CI, CP = get_car(can_sock, pm.sock['sendcan'], has_relay)
|
||||
|
||||
|
|
@ -461,7 +456,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
|
|||
controller_available = CP.enableCamera and CI.CC is not None and not passive
|
||||
read_only = not car_recognized or not controller_available or CP.dashcamOnly
|
||||
if read_only:
|
||||
CP.safetyModel = CP.safetyModelPassive
|
||||
CP.safetyModel = car.CarParams.SafetyModel.noOutput
|
||||
|
||||
# Write CarParams for radard and boardd safety mode
|
||||
params.put("CarParams", CP.to_bytes())
|
||||
|
|
@ -509,9 +504,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
|
|||
# controlsd is driven by can recv, expected at 100Hz
|
||||
rk = Ratekeeper(100, print_delay_threshold=None)
|
||||
|
||||
# FIXME: offroad alerts should not be created with negative severity
|
||||
connectivity_alert = params.get("Offroad_ConnectivityNeeded", encoding='utf8')
|
||||
internet_needed = connectivity_alert is not None and json.loads(connectivity_alert)["severity"] >= 0
|
||||
internet_needed = params.get("Offroad_ConnectivityNeeded", encoding='utf8') is not None
|
||||
|
||||
prof = Profiler(False) # off by default
|
||||
|
||||
|
|
@ -521,7 +514,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None):
|
|||
|
||||
# Sample data and compute car events
|
||||
CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\
|
||||
data_sample(CI, CC, sm, can_poller, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
|
||||
data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery,
|
||||
driver_status, state, mismatch_counter, params)
|
||||
prof.checkpoint("Sample")
|
||||
|
||||
|
|
|
|||
|
|
@ -403,63 +403,63 @@ ALERTS = [
|
|||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Controls Failed",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
|
||||
|
||||
Alert(
|
||||
"controlsMismatch",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Controls Mismatch",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
|
||||
|
||||
Alert(
|
||||
"canError",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"CAN Error: Check Connections",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
|
||||
|
||||
Alert(
|
||||
"steerUnavailable",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"LKAS Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
|
||||
|
||||
Alert(
|
||||
"brakeUnavailable",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Cruise Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
|
||||
|
||||
Alert(
|
||||
"gasUnavailable",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Gas Fault: Restart the Car",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
|
||||
|
||||
Alert(
|
||||
"reverseGear",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Reverse Gear",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
|
||||
|
||||
Alert(
|
||||
"cruiseDisabled",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Cruise Is Off",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
|
||||
|
||||
Alert(
|
||||
"plannerError",
|
||||
"TAKE CONTROL IMMEDIATELY",
|
||||
"Planner Solution Error",
|
||||
AlertStatus.critical, AlertSize.full,
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 1., 3., 4.),
|
||||
Priority.HIGHEST, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, 2.2, 3., 4.),
|
||||
|
||||
# not loud cancellations (user is in control)
|
||||
Alert(
|
||||
|
|
@ -631,6 +631,13 @@ ALERTS = [
|
|||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"invalidGiraffeToyotaNoEntry",
|
||||
"openpilot Unavailable",
|
||||
"Visit comma.ai/tg",
|
||||
AlertStatus.normal, AlertSize.mid,
|
||||
Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.),
|
||||
|
||||
Alert(
|
||||
"commIssueNoEntry",
|
||||
"openpilot Unavailable",
|
||||
|
|
|
|||
|
|
@ -15,5 +15,13 @@
|
|||
"Offroad_ConnectivityNeeded": {
|
||||
"text": "Connect to internet to check for updates. openpilot won't engage.",
|
||||
"severity": 1
|
||||
},
|
||||
"Offroad_PandaFirmwareMismatch": {
|
||||
"text": "Unexpected panda firmware version. System won't start. Reboot your EON to reflash panda.",
|
||||
"severity": 1
|
||||
},
|
||||
"Offroad_InvalidTime": {
|
||||
"text": "Invalid date and time settings, system won't start. Connect to internet to set time.",
|
||||
"severity": 1
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@ import numpy as np
|
|||
from cereal import log
|
||||
from common.realtime import DT_CTRL
|
||||
from common.numpy_fast import clip
|
||||
from selfdrive.car.toyota.carcontroller import SteerLimitParams
|
||||
from selfdrive.car.toyota.values import SteerLimitParams
|
||||
from selfdrive.car import apply_toyota_steer_torque_limits
|
||||
from selfdrive.controls.lib.drive_helpers import get_steer_max
|
||||
|
||||
|
|
|
|||
|
|
@ -3,8 +3,6 @@ import importlib
|
|||
import math
|
||||
from collections import defaultdict, deque
|
||||
|
||||
import zmq
|
||||
|
||||
import selfdrive.messaging as messaging
|
||||
from cereal import car
|
||||
from common.params import Params
|
||||
|
|
@ -13,7 +11,6 @@ from selfdrive.config import RADAR_TO_CAMERA
|
|||
from selfdrive.controls.lib.cluster.fastcluster_py import \
|
||||
cluster_points_centroid
|
||||
from selfdrive.controls.lib.radar_helpers import Cluster, Track
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.swaglog import cloudlog
|
||||
|
||||
DEBUG = False
|
||||
|
|
@ -189,11 +186,8 @@ def radard_thread(sm=None, pm=None, can_sock=None):
|
|||
cloudlog.info("radard is importing %s", CP.carName)
|
||||
RadarInterface = importlib.import_module('selfdrive.car.%s.radar_interface' % CP.carName).RadarInterface
|
||||
|
||||
can_poller = zmq.Poller()
|
||||
|
||||
if can_sock is None:
|
||||
can_sock = messaging.sub_sock(service_list['can'].port)
|
||||
can_poller.register(can_sock)
|
||||
can_sock = messaging.sub_sock('can')
|
||||
|
||||
if sm is None:
|
||||
sm = messaging.SubMaster(['model', 'controlsState', 'liveParameters'])
|
||||
|
|
@ -210,7 +204,7 @@ def radard_thread(sm=None, pm=None, can_sock=None):
|
|||
has_radar = not CP.radarOffCan
|
||||
|
||||
while 1:
|
||||
can_strings = messaging.drain_sock_raw_poller(can_poller, can_sock, wait_for_one=True)
|
||||
can_strings = messaging.drain_sock_raw(can_sock, wait_for_one=True)
|
||||
rr = RI.update(can_strings)
|
||||
|
||||
if rr is None:
|
||||
|
|
|
|||
|
|
@ -6,11 +6,10 @@ from collections import defaultdict
|
|||
|
||||
import selfdrive.messaging as messaging
|
||||
from common.realtime import sec_since_boot
|
||||
from selfdrive.services import service_list
|
||||
|
||||
|
||||
def can_printer(bus=0, max_msg=None, addr="127.0.0.1"):
|
||||
logcan = messaging.sub_sock(service_list['can'].port, addr=addr)
|
||||
logcan = messaging.sub_sock('can', addr=addr)
|
||||
|
||||
start = sec_since_boot()
|
||||
lp = sec_since_boot()
|
||||
|
|
|
|||
|
|
@ -1,20 +1,15 @@
|
|||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import argparse
|
||||
import zmq
|
||||
import json
|
||||
from hexdump import hexdump
|
||||
from threading import Thread
|
||||
|
||||
from cereal import log
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.services import service_list
|
||||
|
||||
def run_server(socketio):
|
||||
socketio.run(app, host='0.0.0.0', port=4000)
|
||||
|
||||
if __name__ == "__main__":
|
||||
poller = zmq.Poller()
|
||||
poller = messaging.Poller()
|
||||
|
||||
parser = argparse.ArgumentParser(description='Sniff a communcation socket')
|
||||
parser.add_argument('--pipe', action='store_true')
|
||||
|
|
@ -22,57 +17,25 @@ if __name__ == "__main__":
|
|||
parser.add_argument('--json', action='store_true')
|
||||
parser.add_argument('--dump-json', action='store_true')
|
||||
parser.add_argument('--no-print', action='store_true')
|
||||
parser.add_argument('--proxy', action='store_true', help='republish on localhost')
|
||||
parser.add_argument('--map', action='store_true')
|
||||
parser.add_argument('--addr', default='127.0.0.1')
|
||||
parser.add_argument('--values', help='values to monitor (instead of entire event)')
|
||||
parser.add_argument("socket", type=str, nargs='*', help="socket name")
|
||||
args = parser.parse_args()
|
||||
|
||||
republish_socks = {}
|
||||
|
||||
for m in args.socket if len(args.socket) > 0 else service_list:
|
||||
if m in service_list:
|
||||
port = service_list[m].port
|
||||
elif m.isdigit():
|
||||
port = int(m)
|
||||
else:
|
||||
print("service not found")
|
||||
sys.exit(-1)
|
||||
sock = messaging.sub_sock(port, poller, addr=args.addr)
|
||||
if args.proxy:
|
||||
republish_socks[sock] = messaging.pub_sock(port)
|
||||
|
||||
if args.map:
|
||||
from flask.ext.socketio import SocketIO #pylint: disable=no-name-in-module, import-error
|
||||
from flask import Flask
|
||||
app = Flask(__name__)
|
||||
socketio = SocketIO(app, async_mode='threading')
|
||||
server_thread = Thread(target=run_server, args=(socketio,))
|
||||
server_thread.daemon = True
|
||||
server_thread.start()
|
||||
print('server running')
|
||||
sock = messaging.sub_sock(m, poller, addr=args.addr)
|
||||
|
||||
values = None
|
||||
if args.values:
|
||||
values = [s.strip().split(".") for s in args.values.split(",")]
|
||||
|
||||
while 1:
|
||||
polld = poller.poll(timeout=1000)
|
||||
for sock, mode in polld:
|
||||
if mode != zmq.POLLIN:
|
||||
continue
|
||||
msg = sock.recv()
|
||||
polld = poller.poll(1000)
|
||||
for sock in polld:
|
||||
msg = sock.receive()
|
||||
evt = log.Event.from_bytes(msg)
|
||||
if sock in republish_socks:
|
||||
republish_socks[sock].send(msg)
|
||||
if args.map and evt.which() == 'liveLocation':
|
||||
print('send loc')
|
||||
socketio.emit('location', {
|
||||
'lat': evt.liveLocation.lat,
|
||||
'lon': evt.liveLocation.lon,
|
||||
'alt': evt.liveLocation.alt,
|
||||
})
|
||||
|
||||
if not args.no_print:
|
||||
if args.pipe:
|
||||
sys.stdout.write(msg)
|
||||
|
|
|
|||
|
|
@ -12,16 +12,15 @@
|
|||
# until all messages are received at least once
|
||||
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.services import service_list
|
||||
|
||||
logcan = messaging.sub_sock(service_list['can'].port)
|
||||
logcan = messaging.sub_sock('can')
|
||||
msgs = {}
|
||||
while True:
|
||||
lc = messaging.recv_sock(logcan, True)
|
||||
for c in lc.can:
|
||||
# read also msgs sent by EON on CAN bus 0x80 and filter out the
|
||||
# addr with more than 11 bits
|
||||
if c.src%0x80 == 0 and c.address < 0x800:
|
||||
if c.src in [0, 2] and c.address < 0x800:
|
||||
msgs[c.address] = len(c.dat)
|
||||
|
||||
fingerprint = ', '.join("%d: %d" % v for v in sorted(msgs.items()))
|
||||
|
|
|
|||
|
|
@ -3,7 +3,6 @@ import matplotlib
|
|||
matplotlib.use('TkAgg')
|
||||
|
||||
import sys
|
||||
from selfdrive.services import service_list
|
||||
import selfdrive.messaging as messaging
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
|
@ -58,9 +57,9 @@ def mpc_vwr_thread(addr="127.0.0.1"):
|
|||
|
||||
|
||||
# *** log ***
|
||||
livempc = messaging.sub_sock(service_list['liveMpc'].port, addr=addr)
|
||||
model = messaging.sub_sock(service_list['model'].port, addr=addr)
|
||||
path_plan_sock = messaging.sub_sock(service_list['pathPlan'].port, addr=addr)
|
||||
livempc = messaging.sub_sock('liveMpc', addr=addr)
|
||||
model = messaging.sub_sock('model', addr=addr)
|
||||
path_plan_sock = messaging.sub_sock('pathPlan', addr=addr)
|
||||
|
||||
while 1:
|
||||
lMpc = messaging.recv_sock(livempc, wait=True)
|
||||
|
|
|
|||
|
|
@ -4,7 +4,6 @@ import matplotlib
|
|||
matplotlib.use('TkAgg')
|
||||
|
||||
import sys
|
||||
from selfdrive.services import service_list
|
||||
import selfdrive.messaging as messaging
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
|
|
@ -16,8 +15,8 @@ N = 21
|
|||
|
||||
def plot_longitudinal_mpc(addr="127.0.0.1"):
|
||||
# *** log ***
|
||||
livempc = messaging.sub_sock(service_list['liveLongitudinalMpc'].port, addr=addr, conflate=True)
|
||||
radarstate = messaging.sub_sock(service_list['radarState'].port, addr=addr, conflate=True)
|
||||
livempc = messaging.sub_sock('liveLongitudinalMpc', addr=addr, conflate=True)
|
||||
radarstate = messaging.sub_sock('radarState', addr=addr, conflate=True)
|
||||
|
||||
plt.ion()
|
||||
fig = plt.figure()
|
||||
|
|
|
|||
|
|
@ -15,28 +15,25 @@ WARN_FLAGS = -Werror=implicit-function-declaration \
|
|||
|
||||
CFLAGS = -std=gnu11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS) -Wall
|
||||
CXXFLAGS = -std=c++11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS) -Wall
|
||||
ZMQ_LIBS = -l:libczmq.a -l:libzmq.a
|
||||
|
||||
MESSAGING_FLAGS = -I$(BASEDIR)/selfdrive/messaging
|
||||
MESSAGING_LIBS = $(BASEDIR)/selfdrive/messaging/messaging.a
|
||||
|
||||
EXTRA_LIBS = -lpthread
|
||||
|
||||
ifeq ($(ARCH),aarch64)
|
||||
CFLAGS += -mcpu=cortex-a57
|
||||
CXXFLAGS += -mcpu=cortex-a57
|
||||
ZMQ_LIBS += -lgnustl_shared
|
||||
EXTRA_LIBS += -llog -luuid -lgnustl_shared
|
||||
endif
|
||||
|
||||
|
||||
JSON_FLAGS = -I$(PHONELIBS)/json/src
|
||||
JSON11_FLAGS = -I$(PHONELIBS)/json11
|
||||
|
||||
EXTRA_LIBS = -lpthread
|
||||
|
||||
ifeq ($(ARCH),x86_64)
|
||||
ZMQ_FLAGS = -I$(BASEDIR)/phonelibs/zmq/x64/include
|
||||
ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib \
|
||||
-l:libczmq.a -l:libzmq.a
|
||||
ZMQ_SHARED_LIBS = -L$(BASEDIR)/external/zmq/lib \
|
||||
-lczmq -lzmq
|
||||
else
|
||||
EXTRA_LIBS += -llog -luuid
|
||||
endif
|
||||
|
||||
.PHONY: all
|
||||
|
|
@ -64,32 +61,28 @@ OBJS = ublox_msg.o \
|
|||
|
||||
DEPS := $(OBJS:.o=.d) ubloxd.d ubloxd_test.d
|
||||
|
||||
liblocationd.so: $(LOC_OBJS)
|
||||
liblocationd.so: $(LOC_OBJS) $(MESSAGING_LIBS)
|
||||
@echo "[ LINK ] $@"
|
||||
$(CXX) -shared -o '$@' $^ \
|
||||
$(CEREAL_LIBS) \
|
||||
$(ZMQ_SHARED_LIBS) \
|
||||
$(EXTRA_LIBS)
|
||||
|
||||
paramsd: $(LOC_OBJS)
|
||||
paramsd: $(LOC_OBJS) $(MESSAGING_LIBS)
|
||||
@echo "[ LINK ] $@"
|
||||
$(CXX) -fPIC -o '$@' $^ \
|
||||
$(CEREAL_LIBS) \
|
||||
$(ZMQ_LIBS) \
|
||||
$(EXTRA_LIBS)
|
||||
|
||||
ubloxd: ubloxd.o $(OBJS)
|
||||
ubloxd: ubloxd.o $(OBJS) $(MESSAGING_LIBS)
|
||||
@echo "[ LINK ] $@"
|
||||
$(CXX) -fPIC -o '$@' $^ \
|
||||
$(CEREAL_LIBS) \
|
||||
$(ZMQ_LIBS) \
|
||||
$(EXTRA_LIBS)
|
||||
|
||||
ubloxd_test: ubloxd_test.o $(OBJS)
|
||||
ubloxd_test: ubloxd_test.o $(OBJS) $(MESSAGING_LIBS)
|
||||
@echo "[ LINK ] $@"
|
||||
$(CXX) -fPIC -o '$@' $^ \
|
||||
$(CEREAL_LIBS) \
|
||||
$(ZMQ_LIBS) \
|
||||
$(EXTRA_LIBS)
|
||||
|
||||
%.o: %.cc
|
||||
|
|
@ -98,6 +91,7 @@ ubloxd_test: ubloxd_test.o $(OBJS)
|
|||
-Iinclude -I.. -I../.. \
|
||||
$(CEREAL_CXXFLAGS) \
|
||||
$(ZMQ_FLAGS) \
|
||||
$(MESSAGING_FLAGS) \
|
||||
$(JSON11_FLAGS) \
|
||||
$(JSON_FLAGS) \
|
||||
-I../ \
|
||||
|
|
|
|||
|
|
@ -31,7 +31,7 @@ void Localizer::update_state(const Eigen::Matrix<double, 1, 4> &C, const double
|
|||
|
||||
void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reader sensor_events, double current_time) {
|
||||
for (cereal::SensorEventData::Reader sensor_event : sensor_events){
|
||||
if (sensor_event.getSensor() == 5) {
|
||||
if (sensor_event.getSensor() == 5 && sensor_event.getType() == 16) {
|
||||
sensor_data_time = current_time;
|
||||
double meas = -sensor_event.getGyroUncalibrated().getV()[0];
|
||||
update_state(C_gyro, R_gyro, current_time, meas);
|
||||
|
|
|
|||
|
|
@ -1,7 +1,10 @@
|
|||
#include <future>
|
||||
#include <iostream>
|
||||
#include <cassert>
|
||||
#include <csignal>
|
||||
#include <unistd.h>
|
||||
|
||||
|
||||
#include <czmq.h>
|
||||
#include <capnp/message.h>
|
||||
#include <capnp/serialize-packed.h>
|
||||
|
||||
|
|
@ -13,32 +16,28 @@
|
|||
#include "common/params.h"
|
||||
#include "common/timing.h"
|
||||
|
||||
#include "messaging.hpp"
|
||||
#include "locationd_yawrate.h"
|
||||
#include "params_learner.h"
|
||||
|
||||
const int num_polls = 3;
|
||||
|
||||
void sigpipe_handler(int sig) {
|
||||
LOGE("SIGPIPE received");
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char *argv[]) {
|
||||
auto ctx = zmq_ctx_new();
|
||||
auto controls_state_sock = sub_sock(ctx, "tcp://127.0.0.1:8007");
|
||||
auto sensor_events_sock = sub_sock(ctx, "tcp://127.0.0.1:8003");
|
||||
auto camera_odometry_sock = sub_sock(ctx, "tcp://127.0.0.1:8066");
|
||||
signal(SIGPIPE, (sighandler_t)sigpipe_handler);
|
||||
|
||||
auto live_parameters_sock = zsock_new_pub("@tcp://*:8064");
|
||||
assert(live_parameters_sock);
|
||||
auto live_parameters_sock_raw = zsock_resolve(live_parameters_sock);
|
||||
Context * c = Context::create();
|
||||
SubSocket * controls_state_sock = SubSocket::create(c, "controlsState");
|
||||
SubSocket * sensor_events_sock = SubSocket::create(c, "sensorEvents");
|
||||
SubSocket * camera_odometry_sock = SubSocket::create(c, "cameraOdometry");
|
||||
PubSocket * live_parameters_sock = PubSocket::create(c, "liveParameters");
|
||||
Poller * poller = Poller::create({controls_state_sock, sensor_events_sock, camera_odometry_sock});
|
||||
|
||||
int err;
|
||||
Localizer localizer;
|
||||
|
||||
zmq_pollitem_t polls[num_polls] = {{0}};
|
||||
polls[0].socket = controls_state_sock;
|
||||
polls[0].events = ZMQ_POLLIN;
|
||||
polls[1].socket = camera_odometry_sock;
|
||||
polls[1].events = ZMQ_POLLIN;
|
||||
polls[2].socket = sensor_events_sock;
|
||||
polls[2].events = ZMQ_POLLIN;
|
||||
|
||||
// Read car params
|
||||
char *value;
|
||||
size_t value_sz = 0;
|
||||
|
|
@ -98,96 +97,84 @@ int main(int argc, char *argv[]) {
|
|||
// Main loop
|
||||
int save_counter = 0;
|
||||
while (true){
|
||||
int ret = zmq_poll(polls, num_polls, 100);
|
||||
for (auto s : poller->poll(-1)){
|
||||
Message * msg = s->receive();
|
||||
|
||||
if (ret == 0){
|
||||
continue;
|
||||
} else if (ret < 0){
|
||||
break;
|
||||
}
|
||||
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), msg->getData(), msg->getSize());
|
||||
|
||||
for (int i=0; i < num_polls; i++) {
|
||||
if (polls[i].revents) {
|
||||
zmq_msg_t msg;
|
||||
err = zmq_msg_init(&msg);
|
||||
assert(err == 0);
|
||||
err = zmq_msg_recv(&msg, polls[i].socket, 0);
|
||||
assert(err >= 0);
|
||||
capnp::FlatArrayMessageReader capnp_msg(amsg);
|
||||
cereal::Event::Reader event = capnp_msg.getRoot<cereal::Event>();
|
||||
|
||||
// make copy due to alignment issues, will be freed on out of scope
|
||||
auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
|
||||
zmq_msg_close(&msg);
|
||||
localizer.handle_log(event);
|
||||
|
||||
capnp::FlatArrayMessageReader capnp_msg(amsg);
|
||||
cereal::Event::Reader event = capnp_msg.getRoot<cereal::Event>();
|
||||
auto which = event.which();
|
||||
// Throw vision failure if posenet and odometric speed too different
|
||||
if (which == cereal::Event::CAMERA_ODOMETRY){
|
||||
if (std::abs(localizer.posenet_speed - localizer.car_speed) > std::max(0.4 * localizer.car_speed, 5.0)) {
|
||||
posenet_invalid_count++;
|
||||
} else {
|
||||
posenet_invalid_count = 0;
|
||||
}
|
||||
} else if (which == cereal::Event::CONTROLS_STATE){
|
||||
save_counter++;
|
||||
|
||||
localizer.handle_log(event);
|
||||
double yaw_rate = -localizer.x[0];
|
||||
bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle);
|
||||
|
||||
auto which = event.which();
|
||||
// Throw vision failure if posenet and odometric speed too different
|
||||
if (which == cereal::Event::CAMERA_ODOMETRY){
|
||||
if (std::abs(localizer.posenet_speed - localizer.car_speed) > std::max(0.4 * localizer.car_speed, 5.0)) {
|
||||
posenet_invalid_count++;
|
||||
} else {
|
||||
posenet_invalid_count = 0;
|
||||
}
|
||||
} else if (which == cereal::Event::CONTROLS_STATE){
|
||||
save_counter++;
|
||||
// TODO: Fix in replay
|
||||
double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time;
|
||||
double camera_odometry_age = localizer.controls_state_time - localizer.camera_odometry_time;
|
||||
|
||||
double yaw_rate = -localizer.x[0];
|
||||
bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle);
|
||||
double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
|
||||
double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
|
||||
|
||||
// TODO: Fix in replay
|
||||
double sensor_data_age = localizer.controls_state_time - localizer.sensor_data_time;
|
||||
double camera_odometry_age = localizer.controls_state_time - localizer.camera_odometry_time;
|
||||
capnp::MallocMessageBuilder msg;
|
||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
||||
event.setLogMonoTime(nanos_since_boot());
|
||||
auto live_params = event.initLiveParameters();
|
||||
live_params.setValid(valid);
|
||||
live_params.setYawRate(localizer.x[0]);
|
||||
live_params.setGyroBias(localizer.x[2]);
|
||||
live_params.setSensorValid(sensor_data_age < 5.0);
|
||||
live_params.setAngleOffset(angle_offset_degrees);
|
||||
live_params.setAngleOffsetAverage(angle_offset_average_degrees);
|
||||
live_params.setStiffnessFactor(learner.x);
|
||||
live_params.setSteerRatio(learner.sR);
|
||||
live_params.setPosenetSpeed(localizer.posenet_speed);
|
||||
live_params.setPosenetValid((posenet_invalid_count < 4) && (camera_odometry_age < 5.0));
|
||||
|
||||
double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao;
|
||||
double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao;
|
||||
auto words = capnp::messageToFlatArray(msg);
|
||||
auto bytes = words.asBytes();
|
||||
live_parameters_sock->send((char*)bytes.begin(), bytes.size());
|
||||
|
||||
capnp::MallocMessageBuilder msg;
|
||||
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
|
||||
event.setLogMonoTime(nanos_since_boot());
|
||||
auto live_params = event.initLiveParameters();
|
||||
live_params.setValid(valid);
|
||||
live_params.setYawRate(localizer.x[0]);
|
||||
live_params.setGyroBias(localizer.x[2]);
|
||||
live_params.setSensorValid(sensor_data_age < 5.0);
|
||||
live_params.setAngleOffset(angle_offset_degrees);
|
||||
live_params.setAngleOffsetAverage(angle_offset_average_degrees);
|
||||
live_params.setStiffnessFactor(learner.x);
|
||||
live_params.setSteerRatio(learner.sR);
|
||||
live_params.setPosenetSpeed(localizer.posenet_speed);
|
||||
live_params.setPosenetValid((posenet_invalid_count < 4) && (camera_odometry_age < 5.0));
|
||||
// Save parameters every minute
|
||||
if (save_counter % 6000 == 0) {
|
||||
json11::Json json = json11::Json::object {
|
||||
{"carVin", vin},
|
||||
{"carFingerprint", fingerprint},
|
||||
{"steerRatio", learner.sR},
|
||||
{"stiffnessFactor", learner.x},
|
||||
{"angleOffsetAverage", angle_offset_average_degrees},
|
||||
};
|
||||
|
||||
auto words = capnp::messageToFlatArray(msg);
|
||||
auto bytes = words.asBytes();
|
||||
zmq_send(live_parameters_sock_raw, bytes.begin(), bytes.size(), ZMQ_DONTWAIT);
|
||||
|
||||
// Save parameters every minute
|
||||
if (save_counter % 6000 == 0) {
|
||||
json11::Json json = json11::Json::object {
|
||||
{"carVin", vin},
|
||||
{"carFingerprint", fingerprint},
|
||||
{"steerRatio", learner.sR},
|
||||
{"stiffnessFactor", learner.x},
|
||||
{"angleOffsetAverage", angle_offset_average_degrees},
|
||||
};
|
||||
|
||||
std::string out = json.dump();
|
||||
std::async(std::launch::async,
|
||||
[out]{
|
||||
write_db_value(NULL, "LiveParameters", out.c_str(), out.length());
|
||||
});
|
||||
}
|
||||
std::string out = json.dump();
|
||||
std::async(std::launch::async,
|
||||
[out]{
|
||||
write_db_value(NULL, "LiveParameters", out.c_str(), out.length());
|
||||
});
|
||||
}
|
||||
}
|
||||
delete msg;
|
||||
}
|
||||
}
|
||||
|
||||
zmq_close(controls_state_sock);
|
||||
zmq_close(sensor_events_sock);
|
||||
zmq_close(camera_odometry_sock);
|
||||
zmq_close(live_parameters_sock_raw);
|
||||
delete live_parameters_sock;
|
||||
delete controls_state_sock;
|
||||
delete camera_odometry_sock;
|
||||
delete sensor_events_sock;
|
||||
delete poller;
|
||||
delete c;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -720,12 +720,11 @@ class UBlox:
|
|||
|
||||
self.dev = PandaSerial(self.panda, 1, self.baudrate)
|
||||
elif grey:
|
||||
from selfdrive.services import service_list
|
||||
import selfdrive.messaging as messaging
|
||||
|
||||
class BoarddSerial():
|
||||
def __init__(self):
|
||||
self.ubloxRaw = messaging.sub_sock(service_list['ubloxRaw'].port)
|
||||
self.ubloxRaw = messaging.sub_sock('ubloxRaw')
|
||||
self.buf = ""
|
||||
|
||||
def read(self, n):
|
||||
|
|
|
|||
|
|
@ -10,7 +10,6 @@ import sys
|
|||
from cereal import log
|
||||
from common import realtime
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.services import service_list
|
||||
from selfdrive.locationd.test.ephemeris import EphemerisData, GET_FIELD_U
|
||||
|
||||
panda = os.getenv("PANDA") is not None # panda directly connected
|
||||
|
|
@ -270,8 +269,8 @@ def main(gctx=None):
|
|||
nav_frame_buffer[0][i] = {}
|
||||
|
||||
|
||||
gpsLocationExternal = messaging.pub_sock(service_list['gpsLocationExternal'].port)
|
||||
ubloxGnss = messaging.pub_sock(service_list['ubloxGnss'].port)
|
||||
gpsLocationExternal = messaging.pub_sock('gpsLocationExternal')
|
||||
ubloxGnss = messaging.pub_sock('ubloxGnss')
|
||||
|
||||
dev = init_reader()
|
||||
while True:
|
||||
|
|
|
|||
|
|
@ -6,7 +6,6 @@ from common import realtime
|
|||
from selfdrive.locationd.test.ubloxd import gen_raw, gen_solution
|
||||
import zmq
|
||||
import selfdrive.messaging as messaging
|
||||
from selfdrive.services import service_list
|
||||
|
||||
|
||||
unlogger = os.getenv("UNLOGGER") is not None # debug prints
|
||||
|
|
@ -14,10 +13,10 @@ unlogger = os.getenv("UNLOGGER") is not None # debug prints
|
|||
def main(gctx=None):
|
||||
poller = zmq.Poller()
|
||||
|
||||
gpsLocationExternal = messaging.pub_sock(service_list['gpsLocationExternal'].port)
|
||||
ubloxGnss = messaging.pub_sock(service_list['ubloxGnss'].port)
|
||||
gpsLocationExternal = messaging.pub_sock('gpsLocationExternal')
|
||||
ubloxGnss = messaging.pub_sock('ubloxGnss')
|
||||
|
||||
# ubloxRaw = messaging.sub_sock(service_list['ubloxRaw'].port, poller)
|
||||
# ubloxRaw = messaging.sub_sock('ubloxRaw', poller)
|
||||
|
||||
# buffer with all the messages that still need to be input into the kalman
|
||||
while 1:
|
||||
|
|
|
|||
|
|
@ -16,7 +16,6 @@
|
|||
#include <vector>
|
||||
#include <algorithm>
|
||||
|
||||
#include <zmq.h>
|
||||
#include <capnp/serialize.h>
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include "messaging.hpp"
|
||||
|
||||
#define min(x, y) ((x) <= (y) ? (x) : (y))
|
||||
|
||||
|
|
@ -144,6 +145,6 @@ namespace ublox {
|
|||
|
||||
}
|
||||
|
||||
typedef int (*poll_ubloxraw_msg_func)(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg);
|
||||
typedef int (*send_gps_event_func)(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags);
|
||||
typedef Message * (*poll_ubloxraw_msg_func)(Poller *poller);
|
||||
typedef int (*send_gps_event_func)(PubSocket *s, const void *buf, size_t len);
|
||||
int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func);
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@
|
|||
#include <map>
|
||||
#include <vector>
|
||||
|
||||
#include <zmq.h>
|
||||
#include "messaging.hpp"
|
||||
#include <capnp/serialize.h>
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
|
|
@ -27,17 +27,19 @@
|
|||
|
||||
const long ZMQ_POLL_TIMEOUT = 1000; // In miliseconds
|
||||
|
||||
int poll_ubloxraw_msg(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg) {
|
||||
int err;
|
||||
zmq_pollitem_t item = {.socket = subscriber, .events = ZMQ_POLLIN};
|
||||
err = zmq_poll (&item, 1, ZMQ_POLL_TIMEOUT);
|
||||
if(err <= 0)
|
||||
return err;
|
||||
return zmq_msg_recv(msg, subscriber, 0);
|
||||
Message * poll_ubloxraw_msg(Poller * poller) {
|
||||
auto p = poller->poll(ZMQ_POLL_TIMEOUT);
|
||||
|
||||
if (p.size()) {
|
||||
return p[0]->receive();
|
||||
} else {
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
int send_gps_event(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags) {
|
||||
return zmq_send(s, buf, len, flags);
|
||||
|
||||
int send_gps_event(PubSocket *s, const void *buf, size_t len) {
|
||||
return s->send((char*)buf, len);
|
||||
}
|
||||
|
||||
int main() {
|
||||
|
|
|
|||
|
|
@ -15,7 +15,7 @@
|
|||
#include <map>
|
||||
#include <vector>
|
||||
|
||||
#include <zmq.h>
|
||||
#include "messaging.hpp"
|
||||
#include <capnp/serialize.h>
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
|
|
@ -39,30 +39,24 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
|
|||
signal(SIGTERM, (sighandler_t) set_do_exit);
|
||||
|
||||
UbloxMsgParser parser;
|
||||
void *context = zmq_ctx_new();
|
||||
void *gpsLocationExternal = zmq_socket(context, ZMQ_PUB);
|
||||
zmq_bind(gpsLocationExternal, "tcp://*:8032");
|
||||
void *ubloxGnss = zmq_socket(context, ZMQ_PUB);
|
||||
zmq_bind(ubloxGnss, "tcp://*:8033");
|
||||
// ubloxRaw = 8042
|
||||
void *subscriber = zmq_socket(context, ZMQ_SUB);
|
||||
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);
|
||||
zmq_connect(subscriber, "tcp://127.0.0.1:8042");
|
||||
|
||||
Context * c = Context::create();
|
||||
PubSocket * gpsLocationExternal = PubSocket::create(c, "gpsLocationExternal");
|
||||
PubSocket * ubloxGnss = PubSocket::create(c, "ubloxGnss");
|
||||
SubSocket * ubloxRaw = SubSocket::create(c, "ubloxRaw");
|
||||
Poller * poller = Poller::create({ubloxRaw});
|
||||
|
||||
|
||||
while (!do_exit) {
|
||||
zmq_msg_t msg;
|
||||
zmq_msg_init(&msg);
|
||||
int err = poll_func(gpsLocationExternal, ubloxGnss, subscriber, &msg);
|
||||
if(err < 0) {
|
||||
LOGE_100("zmq_poll error %s in %s", strerror(errno ), __FUNCTION__);
|
||||
break;
|
||||
} else if(err == 0) {
|
||||
continue;
|
||||
}
|
||||
// format for board, make copy due to alignment issues, will be freed on out of scope
|
||||
auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
|
||||
Message * msg = poll_func(poller);
|
||||
if (msg == NULL) continue;
|
||||
|
||||
auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
|
||||
memcpy(amsg.begin(), msg->getData(), msg->getSize());
|
||||
|
||||
capnp::FlatArrayMessageReader cmsg(amsg);
|
||||
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
|
||||
|
||||
const uint8_t *data = event.getUbloxRaw().begin();
|
||||
size_t len = event.getUbloxRaw().size();
|
||||
size_t bytes_consumed = 0;
|
||||
|
|
@ -76,7 +70,7 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
|
|||
auto words = parser.gen_solution();
|
||||
if(words.size() > 0) {
|
||||
auto bytes = words.asBytes();
|
||||
send_func(parser.msg_class(), parser.msg_id(), gpsLocationExternal, bytes.begin(), bytes.size(), 0);
|
||||
send_func(gpsLocationExternal, bytes.begin(), bytes.size());
|
||||
}
|
||||
} else
|
||||
LOGW("Unknown nav msg id: 0x%02X", parser.msg_id());
|
||||
|
|
@ -86,14 +80,14 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
|
|||
auto words = parser.gen_raw();
|
||||
if(words.size() > 0) {
|
||||
auto bytes = words.asBytes();
|
||||
send_func(parser.msg_class(), parser.msg_id(), ubloxGnss, bytes.begin(), bytes.size(), 0);
|
||||
send_func(ubloxGnss, bytes.begin(), bytes.size());
|
||||
}
|
||||
} else if(parser.msg_id() == MSG_RXM_SFRBX) {
|
||||
//LOGD("MSG_RXM_SFRBX");
|
||||
auto words = parser.gen_nav_data();
|
||||
if(words.size() > 0) {
|
||||
auto bytes = words.asBytes();
|
||||
send_func(parser.msg_class(), parser.msg_id(), ubloxGnss, bytes.begin(), bytes.size(), 0);
|
||||
send_func(ubloxGnss, bytes.begin(), bytes.size());
|
||||
}
|
||||
} else
|
||||
LOGW("Unknown rxm msg id: 0x%02X", parser.msg_id());
|
||||
|
|
@ -103,11 +97,14 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func)
|
|||
}
|
||||
bytes_consumed += bytes_consumed_this_time;
|
||||
}
|
||||
zmq_msg_close(&msg);
|
||||
delete msg;
|
||||
}
|
||||
zmq_close(subscriber);
|
||||
zmq_close(gpsLocationExternal);
|
||||
zmq_close(ubloxGnss);
|
||||
zmq_ctx_destroy(context);
|
||||
|
||||
delete poller;
|
||||
delete ubloxRaw;
|
||||
delete ubloxGnss;
|
||||
delete gpsLocationExternal;
|
||||
delete c;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -16,7 +16,8 @@
|
|||
#include <vector>
|
||||
#include <iostream>
|
||||
|
||||
#include <zmq.h>
|
||||
#include "messaging.hpp"
|
||||
#include "impl_zmq.hpp"
|
||||
#include <capnp/serialize.h>
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
|
||||
|
|
@ -27,6 +28,7 @@
|
|||
#include "ublox_msg.h"
|
||||
|
||||
using namespace ublox;
|
||||
extern volatile sig_atomic_t do_exit;
|
||||
|
||||
void write_file(std::string fpath, uint8_t *data, int len) {
|
||||
FILE* f = fopen(fpath.c_str(), "wb");
|
||||
|
|
@ -43,40 +45,37 @@ static size_t consumed = 0U;
|
|||
static uint8_t *data = NULL;
|
||||
static int save_idx = 0;
|
||||
static std::string prefix;
|
||||
static void *gps_sock, *ublox_gnss_sock;
|
||||
|
||||
int poll_ubloxraw_msg(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg) {
|
||||
gps_sock = gpsLocationExternal;
|
||||
ublox_gnss_sock = ubloxGnss;
|
||||
Message * poll_ubloxraw_msg(Poller * poller) {
|
||||
assert(poller);
|
||||
|
||||
size_t consuming = min(len - consumed, 128);
|
||||
if(consumed < len) {
|
||||
// create message
|
||||
capnp::MallocMessageBuilder msg_builder;
|
||||
cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>();
|
||||
event.setLogMonoTime(nanos_since_boot());
|
||||
|
||||
auto ublox_raw = event.initUbloxRaw(consuming);
|
||||
memcpy(ublox_raw.begin(), (void *)(data + consumed), consuming);
|
||||
|
||||
auto words = capnp::messageToFlatArray(msg_builder);
|
||||
auto bytes = words.asBytes();
|
||||
zmq_msg_init_size (msg, bytes.size());
|
||||
memcpy (zmq_msg_data(msg), (void *)bytes.begin(), bytes.size());
|
||||
|
||||
Message * msg = new ZMQMessage();
|
||||
msg->init((char*)bytes.begin(), bytes.size());
|
||||
consumed += consuming;
|
||||
return 1;
|
||||
} else
|
||||
return -1;
|
||||
return msg;
|
||||
} else {
|
||||
do_exit = 1;
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
int send_gps_event(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags) {
|
||||
if(msg_cls == CLASS_NAV && msg_id == MSG_NAV_PVT)
|
||||
assert(s == gps_sock);
|
||||
else if(msg_cls == CLASS_RXM && msg_id == MSG_RXM_RAW)
|
||||
assert(s == ublox_gnss_sock);
|
||||
else if(msg_cls == CLASS_RXM && msg_id == MSG_RXM_SFRBX)
|
||||
assert(s == ublox_gnss_sock);
|
||||
else
|
||||
assert(0);
|
||||
int send_gps_event(PubSocket *s, const void *buf, size_t len) {
|
||||
assert(s);
|
||||
write_file(prefix + "/" + std::to_string(save_idx), (uint8_t *)buf, len);
|
||||
save_idx ++;
|
||||
save_idx++;
|
||||
return len;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -14,12 +14,14 @@ WARN_FLAGS = -Werror=implicit-function-declaration \
|
|||
|
||||
CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
|
||||
CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
|
||||
ZMQ_LIBS = -l:libczmq.a -l:libzmq.a
|
||||
|
||||
MESSAGING_FLAGS = -I$(BASEDIR)/selfdrive/messaging
|
||||
MESSAGING_LIBS = $(BASEDIR)/selfdrive/messaging/messaging.a
|
||||
|
||||
ifeq ($(ARCH),aarch64)
|
||||
CFLAGS += -mcpu=cortex-a57
|
||||
CXXFLAGS += -mcpu=cortex-a57
|
||||
ZMQ_LIBS += -lgnustl_shared
|
||||
EXTRA_LIBS += -lgnustl_shared
|
||||
endif
|
||||
|
||||
|
||||
|
|
@ -33,11 +35,11 @@ OBJS = logcatd.o \
|
|||
|
||||
DEPS := $(OBJS:.o=.d)
|
||||
|
||||
logcatd: $(OBJS)
|
||||
logcatd: $(OBJS) $(MESSAGING_LIBS)
|
||||
@echo "[ LINK ] $@"
|
||||
$(CXX) -fPIC -o '$@' $^ \
|
||||
$(CEREAL_LIBS) \
|
||||
$(ZMQ_LIBS) \
|
||||
$(EXTRA_LIBS) \
|
||||
-llog
|
||||
|
||||
%.o: %.cc
|
||||
|
|
@ -45,7 +47,7 @@ logcatd: $(OBJS)
|
|||
$(CXX) $(CXXFLAGS) \
|
||||
-I$(PHONELIBS)/android_system_core/include \
|
||||
$(CEREAL_CXXFLAGS) \
|
||||
$(ZMQ_FLAGS) \
|
||||
$(MESSAGING_FLAGS) \
|
||||
-I../ \
|
||||
-I../../ \
|
||||
-c -o '$@' '$<'
|
||||
|
|
|
|||
|
|
@ -7,10 +7,10 @@
|
|||
#include <log/logger.h>
|
||||
#include <log/logprint.h>
|
||||
|
||||
#include <zmq.h>
|
||||
#include <capnp/serialize.h>
|
||||
#include "common/timing.h"
|
||||
#include "cereal/gen/cpp/log.capnp.h"
|
||||
#include "messaging.hpp"
|
||||
|
||||
int main() {
|
||||
int err;
|
||||
|
|
@ -28,10 +28,8 @@ int main() {
|
|||
// struct logger *kernel_logger = android_logger_open(logger_list, LOG_ID_KERNEL);
|
||||
// assert(kernel_logger);
|
||||
|
||||
void *context = zmq_ctx_new();
|
||||
void *publisher = zmq_socket(context, ZMQ_PUB);
|
||||
err = zmq_bind(publisher, "tcp://*:8020");
|
||||
assert(err == 0);
|
||||
Context * c = Context::create();
|
||||
PubSocket * androidLog = PubSocket::create(c, "androidLog");
|
||||
|
||||
while (1) {
|
||||
log_msg log_msg;
|
||||
|
|
@ -60,10 +58,13 @@ int main() {
|
|||
|
||||
auto words = capnp::messageToFlatArray(msg);
|
||||
auto bytes = words.asBytes();
|
||||
zmq_send(publisher, bytes.begin(), bytes.size(), 0);
|
||||
androidLog->send((char*)bytes.begin(), bytes.size());
|
||||
}
|
||||
|
||||
android_logger_list_close(logger_list);
|
||||
|
||||
delete c;
|
||||
delete androidLog;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,6 +24,9 @@ CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) \
|
|||
|
||||
ZMQ_LIBS = -l:libczmq.a -l:libzmq.a
|
||||
|
||||
MESSAGING_FLAGS = -I$(BASEDIR)/selfdrive/messaging
|
||||
MESSAGING_LIBS = $(BASEDIR)/selfdrive/messaging/messaging.a
|
||||
|
||||
ifeq ($(ARCH),aarch64)
|
||||
CFLAGS += -mcpu=cortex-a57
|
||||
CXXFLAGS += -mcpu=cortex-a57
|
||||
|
|
@ -78,12 +81,12 @@ YAML_LIBS = $(PHONELIBS)/yaml-cpp/x64/lib/libyaml-cpp.a
|
|||
else
|
||||
OBJS += encoder.o \
|
||||
raw_logger.o
|
||||
EXTRA_LIBS = -lcutils -llog
|
||||
EXTRA_LIBS = -lcutils -llog -lgnustl_shared
|
||||
endif
|
||||
|
||||
DEPS := $(OBJS:.o=.d)
|
||||
|
||||
loggerd: $(OBJS)
|
||||
loggerd: $(OBJS) $(MESSAGING_LIBS)
|
||||
@echo "[ LINK ] $@"
|
||||
$(CXX) -fPIC -o '$@' $^ \
|
||||
$(LIBYUV_LIBS) \
|
||||
|
|
@ -104,6 +107,7 @@ loggerd: $(OBJS)
|
|||
$(CEREAL_CXXFLAGS) \
|
||||
$(LIBYUV_FLAGS) \
|
||||
$(ZMQ_FLAGS) \
|
||||
$(MESSAGING_FLAGS) \
|
||||
$(OPENMAX_FLAGS) \
|
||||
$(YAML_FLAGS) \
|
||||
$(BZIP_FLAGS) \
|
||||
|
|
|
|||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue